CMU RI 16-745: Dynamic Optimization: Assignment 1

The assignment now includes site licensed and SDFAST generated code, which I can't just throw onto the web. Email me if you don't remember the class/DRC login or password.

This assignment explores using function optimization.

Part 1: You will write a subroutine that chooses joint angles for the DRC robot that

Puts the right wrist (r_arm_mwx joint) at a specified x, y, z target. You are scored on the Euclidean distance between the wrist and the target. If you can reach the target without falling over, the error should be zero. Otherwise, the robot should try to minimize the error without falling over.
Keeps the both feet on the ground in the original position. No stepping is allowed.
Keeps the center of mass over the base of support. 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 (numbers corrected 25-Jan-2013, thanks smaeta!).
Does something reasonable with all the joints of the humanoid.
Joint limits are now provided (tree.j(i).limits) if you want to obey those. This is optional.

Part 2: Analytically compute first derivatives of the objective function and any constraints. Use that in your optimization. From the help for fminunc: Use the GradObj option to specify that FUN also returns a second output argument G that is the partial derivatives of the function df/dX, at the point X. See this documentation for fmincon.

BONUS: Try computing the second derivative of the criterion and constraints.

DOUBLE BONUS: Do it all in C. Use SNOPT or optimization code you can find on the web as the optimizer. We have a site license for SNOPT in the Robotics Insitute, so don't distribute it to others. The manual is available from here. For my most recent installation of SNOPT on a 64bit Linux Ubuntu 12.04 machine, here are my notes:

1) Look at INSTALL.x86-64
2) export F77=/usr/bin/x86_64-linux-gnu-gfortran
3) make FFLAGS="-O -fPIC" CFLAGS="-O -fPIC" CXXFLAGS="-O -fPIC"
4) In sn10mach.f
external etime -> external real  etime
5) In cppsrc/snoptProblem.cc
#include <stdlib.h> // CGA

Here are some examples of using SNOPT: example 1 and example 2. If you have trouble installing SNOPT in Windows, ask junggon at cs.cmu.edu.

New and Improved Software Wed Jan 23 20:24:14 EST 2013

The geometry of the robot is listed in drc.m along with other useful stuff. A debugged forward kinematics routine is now provided: drc_forward_kinematics.m. I also provide C routines that do the same thing. See README.TXT
Everything as a zip file.

Here is a little drawing program that might help you debug. It draws a robot stick figure with the current angles, given drc_forward_kinematics() has been called.

Hints for Part 2:

1) Try automatic differentiation on your matlab code. Background info, another paper, another paper, a class lecture, Commercial, same, maybe there is a demo version, Free 1, issue with matrices?, Free 2, Free? 3, Free 4, Trial version available, and a list. There is more, google "matlab automatic differentiation".

2) Try automatic differentiation on my C code. Google "automatic differentiation".

3) Do it manually. You will have a criterion that is computed by iterating from the base link outward along the robot "tree". Using the chain rule, the first derivative will involve a pass from the tips of the tree towards the base computing the derivative of the criterion with respect to each variable met along the way.

What to turn in?

You can use any type of computer/OS/language you want. 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. [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?