CS325  Lab 5 - Inverse Kinematics

Objectives:

• Be able to compute and use inverse kinematics for Armand

1. Download lab5.r3.  This is an incomplete program in which a position array for the tool is provided and the moveAngles routine moves each of the joint angles in order to place the tool at that position and orientation.
    
2. In order to complete this code you will need the following inverse kinematics :
   
   theta1 = atan2(p2, p1)
   theta234 = atan2(-(c1*r13 + s1*r23), -r33)  ;; global pitch from the joint coupling
   
   b1 = c1*p1 +s1*p2 - a4*c234 +d5*s234
   b2 = d1 - a4*s234 - d5*c234 - p3
   z = b1*b1 + b2*b2
   
   theta3 = acos((z - a2*a2 - a3*a3) / (2 * a2* a3))
   theta2 = atan2((a2 + a3*c3)*b2 - a3*s3 *b1, (a2 + a3*c3)*b1 + a3*s3*b2)
   theta4 = theta234 - theta2 - theta3
   theta5 = atan2(s1*r11 - c1*r21, s1*r12 - c1*r22)
   
   The values of the arrays a and d can be found in the file armand.m
    
3. Determine appropriate test values and test your code thoroughly.
    
4. Demo your program for me and email your code to jill.zimmerman@goucher.edu