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