Find end effector pose from joint state

Hi, I am trying to get the end effector Cartesian point from a list of joint states. I used a solution I found in a different answer here (https://answers.ros.org/question/2806...) and used the pykdl_utils package (http://wiki.ros.org/pykdl_utils) It worked fine but what I got back from there was a 4*4 matrix, and im not sure how to convert this matrix to position and orientation coordinates that I can use in moveit.

Thanks

edit retag close merge delete

Sort by » oldest newest most voted The 4x4 matrix you have received is known as a DH-Matrix it contains a 3x3 rotation matrix and a 3 element translation vector in such a way that the combined transformation can be applied using a single matrix multiplication.

If you want to extract just the translation from this you'll want to extract the first three rows from the final column, this will give you a 3D position vector. If the DH-Matrix you've got is a transformation from the base frame to the end effector frame then this position vector will actually point from the end effector to the origin of the base link, so it will be the opposite of what you might expect. To get the location of the end effector in the base_link frame you'll need to subtract it from the zero vector.

Hope this helps.

more