Differential Inverse Kinematic Controller ROS

Question

Inverse differential Kinematic - Problem with Orientation

Hey guys,

currently I am working to control my UR-Robot. My goal is to control the robot with inverse differential kinematics : 1.) vel_q = J^-1 * (x_goal - x_act)

I am able to get the actual position via Moveit Service GetPositionFK. I translate the quaternion to euler with tf_transformation.

Furthermore I subscribe and publish the jacobian matrix with movegroup_commander.

My problem is that the Robot is moving (gazebo) to the target position (x,y,z) but does not find the correct orientation. The result is that the robot goes literally crazy.

Can somebody tell me maybe what is my mistake or what can I improve? Is the method x_target - x_act = error wrong? Should I maybe use x_act - x_previous / t_act - t_previous to calculate the tcp velocity?

Thanks in advance!

Answer 1

As discussed here, the orientation error is not simply one orientation minus another. It is best to convert your orientations into rotation matrices (which can be obtained from both Euler angles and quaternions), and use the following formula:

$$ \mathbf{e}_{o} = 0.5 \cdot \sum^{3}_{i=1} \mathbf{r}_{a,i} \times \mathbf{r}_{d,i} $$

where $\mathbf{r}_{a,i}$ represents the $i^{\text{th}}$ column of the rotation matrix $\mathbf{R}_{a}$ representing the current (actual) orientation, $\mathbf{r}_{d,i}$ represents the $i^{\text{th}}$ column of the rotation matrix $\mathbf{R}_{d}$ representing the goal (desired) orientation, and $\times$ represents the cross product. So, the error vector is a 6 by 1 vector where the first three entries are the differences between the current and desired x, y, and z positions - and the last three entries are $\mathbf{e}_{o}$.