Inverse kinematics with end effector orientation?

Question

I'm trying to implement an inverse kinematics solver, but this time even with the end effector's orientation. I succeeded with the case when the end effector only requires the position.

I learned that in this case, you can construct the Jacobian Matrix like this, where $w_i$ is the $i^{th}$ rotation axis in global space and $p_i$ is the vector from the $i^{th}$ axis to the target position.

The problem is when I have to calculate the $\dot{x}$ here in the equation below.

$$ J \dot \theta = \dot x $$

When $\dot{x}$ had only positions to take into account, and no orientations this was quite simple, since the $\dot{x}$ vector was calculated as (target_location - current_end_effector_location) * (delta_time = 0.01). But now when $\dot{x}$ requires 6 entries (position, orientation), I don't know what I should do for the orientation part. I've been using euler angles for representing orientation in my program until now.

The idea I've got at the moment is just subtracting current end effector's yaw, pitch, and roll with the target's yaw, pitch, and roll, and multiplying each result with $\Delta t$. But this seems a bit complicated and not right. Are there any better ways to address this issue? Any ideas would be highly appreciated!

---

In case it helps, the forward kinematic map looks like this (T stands for the translation matrix relative to parent node)

(x,y,z) = Root_T * Shoulder_T * Shoulder_Rx(q_0) * Shoulder_Ry(q_1) * 
    Shoulder_Rz(q_2) * Elbow_T * Elbow_Rx(q_3) * Hand_T * Hand_Rx(q_4) * Position

Answer 1

The same as for positions, the angular components of $\dot x$ are angular velocities.

• Expressing them as motion difference over time is generally correct. The unit of measurment in this case needs to be rad/s.
• The other problem can come from the how the rotations are expressed. There are many conventions on how rotations can be expressed and Euler angles are also ambigous (if not further specified). However, from the formulation of $J$ it seems that you should use angular velocities arount the fixed reference frames, so extrinsic Euler Angles. The rotation order should be x, y, and z as far as I can tell from the forward kinematic map you have posted.

Answer 2

If the three wrist axes intersect at a common point, your problem can be simplified using wrist partitioning. With this method, you compute the inverse kinematics to the wrist center (also the intersection point of the three wrist axes). These equations will not involve the wrist angles - they only require you to use the position vector. Once you have that, you can solve for the inverse wrist angles using only the relative angles between the end effector frame and the forearm frame. These equations will not have any terms that include the arm rotations.