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

We won't actually model or take into account the gripper part.

Matlab template for your function.

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.

Matlab template for your function.

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 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?