# 3 DOF Inverse Kinematics Implementation: What's wrong with my code?

I am currently trying to implement an Inverse Kinematics solver for Baxter's arm using only 3 pitch DOF (that is why the yGoal value is redundant, that is the axis of revolution). I for the most part copied the slide pseudocode at page 26 of http://graphics.cs.cmu.edu/nsp/course/15-464/Fall09/handouts/IK.pdf .

def sendArm(xGoal, yGoal, zGoal):
invJacob = np.matrix([[3.615, 0, 14.0029], [-2.9082, 0, -16.32], [-3.4001, 0, -17.34]])
ycurrent = 0
while xcurrent != xGoal:
theta1 = left.joint_angle(lj[1])
theta2 = left.joint_angle(lj[3])
theta3 = left.joint_angle(lj[5])
xcurrent, zcurrent = forwardKinematics(theta1, theta2, theta3)
xIncrement = xGoal - xcurrent
zIncrement = zGoal - zCurrent
increMatrix = np.matrix([[xIncrement], [0], [zIncrement]])
change = np.dot(invJacob, increMatrix)
left.set_joint_positions({lj[1]: currentPosition + change.index(0)/10}) #First pitch joint
left.set_joint_positions({lj[3]: currentPosition + change.index(1)/10}) #Second pitch
left.set_joint_positions({lj[5]: currentPosition + change.index(2)/10}) #Third Pitch joint

def forwardKinematics(theta1, theta2, theta3):
xcurrent = 370.8 * sine(theta1) + 374 * sine(theta1+theta2) + 229 * sine(theta1+theta2+theta3)
zcurrent = 370.8 * cos(theta1) + 374 * cos(theta1+theta2) + 229 * cos(theta1+theta2+theta3)
return xcurrent, zcurrent


Here is my logic in writing this: I first calculated the Jacobian 3x3 matrix by taking the derivative of each equation seen in the forwardKinematics method, arriving at:

[370cos(theta1) + 374cos(theta1+theta2) .....

0 0 0

-370sin(theta1)-374sin(theta1+theta2)-...... ]

In order to arrive at numerical values, I inputted a delta theta change for theta1,2 and 3 of 0.1 radians. I arrived at a Jacobian of numbers:

[0.954 0.586 .219

0.0000 0.000 0.0000

-.178 -.142 -0.0678]

I then input this matrix into a pseudoinverse solver, and came up with the values you see in the invJacob matrix in the code I posted. I then multiplied this by the difference between the goal and where the end effector is currently at. I then applied a tenth of this value into each of the joints, to make small steps toward the goal. However, this just goes into an infinite loop and my numbers are way off what they should be. Where did I go wrong? Is a complete rewrite of this implementation necessary? Thank you for all your help.

The (inverse) Jacobi Matrix is pose dependent. It is not constant, it is dependent on pose (joint angles or TCP coords, based on formulation) and so it characterizes a pose not a robot. By implementing it as a constant you basicly assume that the same joint velocity will always cause the same TCP velocity. In reality it is clear that joint positions play an important role in how joint velocities are transformed to the TCP.

I recommend you generate the (inverse) Jacobi matrix in a symbolic form, similarly as your forward kinematics, dependend on variables (joint angles of TCP cooridnates) and write a function that calculates the value of the matrix for each current pose. Then recalculate the matrix in every iteration and it should be fine.

Furthermore, in this form, if you have a 3x3 matrix, you can easily calculate its inverse, you do not need a pseudoinverse. Later, if you have 7 DOFs you might...

• Thank you for your help! I'll do what you suggested and update my findings in a couple hours.
– Iche
Nov 5, 2015 at 17:37
• I'm having a problem. I am trying to generate the inverse of the Jacobian matrix I listed earlier in symbolic form using MATLAB, but the inverse gives me a FAIL! result, and the pseudoinverse gives me garbage. Would it be fine to turn the Jacobian matrix into numbers, and then find the inverse of that Jacobian?
– Iche
Nov 5, 2015 at 20:04
• Yes, if you comoute the jacobi for the current pose and invert it as values it is OK. If you use symbolcs toolbox in matlab make sure to add the assimptions that your symbols (variables) are real, you should be able to invert it.
– 50k4
Nov 5, 2015 at 22:31
• Oh, you have a line of zeros in the marrix? That might be the problem....
– 50k4
Nov 5, 2015 at 22:35