# How to calculate error for inverse kinematics?

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.

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.

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