CMU RI 16-745: Dynamic Optimization: Assignment 1

This assignment explores using function optimization to do inverse kinematics.

We are expecting you to use Matlab.

Part 1: You will write a Matlab subroutine that chooses joint angles for a version of the Atlas robot that

Puts the right wrist (r_arm_mwx joint) at a specified x, y, z and orientation target. If you can reach the target with the desired orientation without falling over, the error should be zero. Otherwise, the robot should try to minimize a weighted sum of the position and orientation error without falling over. [Equality constraints on position and orientation, or penalty function if can't reach].
Keeps the both feet on the ground in the original position. No stepping is allowed. [Equality constraints on position and orientation].
Keeps the center of mass over the base of support, as far away from the edge of the polygon of support as possible. If you don't move the feet from the symmetric standard standing position, the base of support is a rectangle where the width is 0.302m, and the length is 0.262m, centered on the origin. [Inequality constraints AND penalty function].
Obeys joint limits (robot.j(i).limits) and keeps joints away from joint limits as much as possible. [Inequality constraints AND penalty function].

Part 2: 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 3: 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?

BONUS: Use automatic differentiation to find the derivatives of the criterion and the constraints, and then use the derivatives in the optimization. Does it 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. Google "matlab automatic differentiation" for automatic differentiation tools. You can also do it manually (It's not that hard for forward kinematics).

Matlab code we provide: Everything as a zip file. The geometry of the robot is listed in drc4.m along with other useful stuff. A forward kinematics routine is provided: drc_forward_kinematics.m. There is a very simple drawing program draw.m that might help you debug. It draws a robot stick figure with the current angles, given drc_forward_kinematics() has been called. The notes.txt file is notes on how to use the code. generate_axis_angle_rotation_matrix.m and q_to_R.m are programs to generate rotation matrices from other orientation representations.

Here is a Matlab script from Seungmoon Song that draws the ATLAS robot. You can run the "test.m" file in the "anim" folder. Unfortunately, this needs to be debugged with the current model, and the meshes do not match the current robot, especially for the head location.

What to turn in?

I am expecting you to use Matlab for this assignment. 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 snagaval@andrew.xxx. [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?

Questions

The TA will have office hours after class Monday and Wednesday.