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)
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.
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
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
[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?
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,
Matlab template for your function.
Part 3: In Matlab's fmincon, one can use several algorithms:
trust-region-reflective (needs objective function gradient and only
one type of equality constraint),
See the fmincon
documentation to see how to set the options variable using
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
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
Be sure to list the names of all the members of your group.
Mail the URL of your web page to email@example.com 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?