I am implementing inverse kinematics on a robot, and I am confused about how to calculate error from my desired pose to my current estimated pose. Both are orocos-kdl frames that consist of a 3x3 position matrix, 3x1 rotation matrix, and a 1x4 homogeneous transform matrix at the bottom.I simply don't know what the procedure is to calculate the error between those two frames. Is it simply to multiply it by its own inverse? Subtraction of KDL frames isn't something that c++ allows me to do.

This error needs to be in a form that I can multiply by the psedoinverse jacobian to get a 6x1 matrix of changes to make to my working joint angle value.


1 Answer 1


First, you don't seem to correctly define pose, homogeneous transformation matrices etc.

A 'pose' is defined by a position and an orientation. Position is typically represented by [x,y,z]' coordinates and rotation is typically represented by:

  • 3 values (e.g. some Euler angles representation or Roll-Pitch-Yaw), or
  • 4 values (e.g. a quaternion), or
  • 9 values (3x3 rotation matrix).

There are pros and cons to each representation but this is out of the scope of this question.

A homogeneous transform matrix is a 4x4 matrix of the form [R, p; 0, 0, 0, 1] with R a 3x3 rotation matrix and p a 3x1 position vector.

Then to answer your question:

KDL provides diff() functions for this purpose, see here in the code.

Mind you, you will also need to have a good understanding of the concept of a twist (6x1 vector holding translational and rotational velocities), and of the reference point and reference frame of a twist.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.