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

• Is positioned using a sequence of Euler (roll, pitch, yaw) angles and link lengths. Roll is defined as the axis generated by the previous (proximal) link. Positive roll is defined by the link X axis along the link towards the distal end. Pitch is about an axis that is horizontal in the next (distal) link coordinate frame. Positive pitch is defined by a horizontal Y axis, defined so that the Z axis is up in link coordinates. Yaw is about an axis that is perpendicular to roll and pitch) in the next (distal) link coordinate frame. Positive yaw is defined by the link Z axis (up). We will specify the link lengths and limits on the Euler angles as 7 vector arguments to your program:
  [[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).

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?

Questions