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