# Using Optimization To Do Inverse Kinematics

This assignment explores using function optimization to do inverse kinematics for a snake robot with a parallel jaw (two fingered) gripper for a head. We want you to be able to position control this robot to reach into a cluttered environment and grasp an object. The base of the robot is bolted to the coordinate frame origin pointing along the global X axis, with the second axis aligned with the global Y (horizontal) axis and the third axis aligned with the global Z (vertical) axis. Here is an example of such a robot that avoids obstables by feel.

Click on image to get high resolution version.

Part 1: You will write a program that chooses joint angles for the snake robot that

[[link lengths]' [min roll limits]' [max roll limits]' [min pitch limits]' [max pitch limits]' [min yaw limits]' [max yaw limits]']
• Puts the tip (end of the last link) at a specified x, y, z, and quaternion target. The argument is a seven element vector:
[x y z q0 q1 q2 q3]
where the zero quaternion is [1 0 0 0].
• If the target is not within reach, you should indicate that, and get the tip as close as possible to the target. (What does "close" mean?)
• Obeys joint limits and keeps joints away from joint limits as much as possible. (Hard inequality constraints AND cost/penalty functions).
• Stays away from obstacles, described as a list of spheres: (x, y, z, radius).
We won't actually model or take into account the gripper part.

Part 2: Analytic Derivatives: Find a systematic way to compute the derivative of the tip position and orientation with respect to each set of joint angles for the 3D case. Note that the robot is recursive. If you can compute this for one joint/link, you can compute it for all of them. Does providing a derivative to the optimization routines you used in Part 1 improve performance? Faster optimization? Better answers? See this documentation for fmincon for how the criterion and constraint functions can return a value, a gradient, and possibly a Hessian in the Input Arguments section. Feel free to use any automatic differentiation software you can find. Google "matlab automatic differentiation" for automatic differentiation tools, for example.

Part 3: In Matlab's fmincon, one can use several algorithms: interior-point (default), trust-region-reflective (needs objective function gradient and only one type of equality constraint), sqp, and active-set. See the fmincon documentation to see how to set the options variable using optimoptions().

We would also like you to try CMA-ES. Matlab implementations are available at here.

How does performance change when using the interior-point, sqp, active-set and CMA-ES algorithms, in terms of answer quality and run time?

Part 4: For many positions, there are multiple local minimum. Given a position and orientation target, can you find multiple distinct local minima, and provide the user with a choice of local minima? Can you provide a non-trivial 2nd and 3rd best answer?

## What to turn in?

We require you to use Matlab for this assignment. Use these function names and templates - inputs, outputs for each function (part1.m and part2.m). You should use Matlab graphics to draw pictures of your robot and the obstacles, that allows rotation of the picture so it can be viewed from any angle (modify inverse kinematics class examples and use plot3()). You can work in groups or alone. Generate a web page describing what you did (one per group). Include links to your source and any compiled code in either .zip, .tar, or .tar.gz format. Be sure to list the names of all the members of your group. Mail the URL of your web page to cga@cmu.xxx and the TAs. [You complete the address, we are trying to avoid spam.] The writeup is more important than the code. What did you do? Why did it work? What didn't work and why?