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 x_dot here in the equation below.

When x_dot had only positions to take into account, and had no orientations this was quite simple. But now when x_dot 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.

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 dividing each result with 100. But this seems a bit complicated. Are there any better ways to address this issue? Any ideas would be highly appreciated!

Answer 1

You need to represent the orientation of your end effector as a 3 by 3 rotation matrix. You the calculate the orientation of the end effector at the current joint vector (Theta) and then by adding a small increment to each element in the joint vector (6 increments in your case, since six joints). In the simple case, where you were interested in the change in tip position that results from each small joint change, you calculated the change in X, Y, and Z positions which was a simple vector subtraction of the position at theta, and for each perturbed theta. To do the same for angle, you you need to find the rotation matrix R that takes the 3x3@Theta (A) to the 3x3@ThetaPrime(B).

since A*R=B

AInvAR=AInv*B

AInve*A = Identity

R=AInv*B

From R, you can extract delta roll,pitch,yaw Euler angles. The formula is here

https://pdfs.semanticscholar.org/6681/37fa4b875d890f446e689eea1e334bcf6bf6.pdf

The theta values are the change in yaw, pitch and roll caused by a change in each theta.