Lab 4 - Inverse Kinematics

Inverse Kinematics Lecture

Inverse Kinematics lecture by Jaden Clark, April 24, 2023.


Lab Instructions

Goal: 1) Learn how to compute inverse kinematics 2) Become familiar with simulation-to-real pipeline.

../_images/teleop.jpeg

TODO: picture of sim and real robot together

Step 1. Code inverse kinematics

  1. Implement ik_cost in reacher_kinematics.py as the squared-norm of the error between the position returned by FK(guess) and end_effector_pos.

  2. Implement calculate_jacobian in reacher_kinematics.py using the finite differencing method we covered in lecture. A good perturbation size is small, like 0.001.

  3. Implement calculate_inverse_kinematics in reacher_kinematics.py. Play around with different step sizes, from 1 to 100, to see what works for you.

Step 2. Test the consistency between forward kinematics and inverse kinematics

  1. Test your code by taking some reachable (x,y,z) point in space (think hard about what’s reachable!), using your IK function to get the corresponding joint angles, then passing them to your FK function to retrieve the original (x,yz). These should match as long as the original point was reachable.

The reason we’re doing this IK -> FK consistency test and not a FK -> IK consistency test is that for any reachable point in space, the robot can flip its “elbow” joint up or down to get to that point in space, resulting in different joint angles.

Step 3. Test IK in simulation

  1. Use the simulator to visualize your IK: Make the white sphere go to an arbitrary point in space by modifying reacher_manual_control.py (see line 95)

  2. Use your IK code to figure out the joint angles, and then set the simulated robot’s joint commands to those angles (see line 81).

  3. Like in lab 3, the white sphere should follow the red sphere of the robot.

Step 4. Deploy your IK to your robot

  1. Look at the code that sets the actual robot motors, specifically line 86 and 87.

  2. Change the joint angles sent to the hardware to those calculated by your IK.

  3. Deploy to robot. Remember to pass the --run_on_robot flag when you run your program.

Step 5. Make your two robot arms match each other’s end-effector positions

  1. Calculate the cartesian end-effector position of the right arm using FK.

  2. Use this result to calculate the cartesian position of the right arm’s end-effector relative to the base of the left arm.

  3. Disable the right arm’s torque by de-activating the motors in the right arm. [TODO link code line number]

  4. Deploy to robot and sanity check the reported position

  5. Figure out what to add/subtract from the right arm’s position to get the corresponding position relative to the left arm.

  6. Deploy to robot and sanity check that the position relative to the left arm is correct.

  7. Use your IK to move the left arm to this position in the simulator. Check that the left arm doesn’t freak out.

  8. Deploy to robot!

  9. The result you should get is that the left arm and right arm are touching or very close at the end effector! You can put an object between the end-effectors and now you have a multi-robot manipulator!

Notes: * The code is set up to control both arms, but currently we are commanding the second robot arm’s actuators to stay at zero. * We can move the second arm by setting the values in the 3rd column (index 2) of the full_actions 2d array. See line 86 as an example. * You might need to change the sign of some of your joint angle commands because the second arm is a right-sided leg instead of a left-sided leg.

(Old) IK Lecture