CMU RI 16-745: Dynamic Optimization: Assignment 2

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 trajectory optimization.

Part 1: You will write a function that can optimize a trajectory that goes to a specified position (x, y, and z) target from the rest robot position 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. The center of pressure of each foot needs to remain within the foot, and the friction coefficient of 1 should not be violated. Given a total or net force and torque applied at some point on the foot, the center of pressure is the location on the bottom of the foot where applying the same force in the same direction with no torque results in the same force and torque effect. Note that moving a force results in a torque = r x F The foot's width is 0.124m, and length 0.262m.
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.
Does something reasonable with all the joints of the humanoid.
Obeys joint limits (robot.j(i).angle_limits) including a knee stop at zero.
Obeys velocity limits (robot.j(i).velocity_limits).
Obeys torque limits (robot.j(i).torque_limits).
and the move is completed in 0.5 seconds.

Part 2A: You will optimize a set of trajectories that start at a grid of states, and returns to the rest position, obeying the above constraints, except instead of a desired time, the sum of squared joint torques and deviations from the goal position are minimized.

Part 2B: You will interpolate over this set of trajectories to provide a policy that maps any double support state to an action. BONUS: Test your policy using the DRC simulator (send email to cga for up-to-date model code), an ODE model, or using SDFAST (see below). You need to be running Ubuntu 12.04 to use the official DRC simulator. Instructions on setting up ODE. The ODE model is here. Send ODE model questions to Dongil Choi (doyle.choi.cdi at gmail).

BONUS: Part 3: Repeat Part 1 with the robot moving as fast as possible.

The updated (and with fields renamed somewhat) Matlab description of the robot is here.

We modeled the robot several ways including using a floating base in SDFAST, a commercially available dynamics package. You can download our code so far from sdfast7.zip. A manual for SDFAST is available from the download from www.sdfast.com or by googling "sdfast manual". The example program you want to look at to see how inverse dynamics is done is: air-inverse-dynamics.c

It is possible to do everything in Matlab. However, to do this fast, you might want to do it 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 32bit Linux Ubuntu 12.04 machine, here are my notes.

1) I needed to symbolic link /usr/bin/gmake to /usr/bin/make
2) I needed to install the package fort77
3) In cppsrc/snoptProblem.cc
#include <stdlib.h> // CGA

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

You need to (in snopt7)

sudo make install
sudo cp ./include/f2c.h /usr/local/include
sudo cp ./cexamples/snfilewrapper.h /usr/local/include
sudo cp ./cexamples/snopt.h /usr/local/include
sudo cp ./cexamples/snfilewrapper.o /usr/local/lib
sudo cp ./lib/libf2c.a /usr/local/lib

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

Here is a start on recursive Newton Euler inverse dynamics standalone code (air.c is the most elaborate), as well as quintic spline stuff (planar.c): kin-dyn5.zip This will be upgraded, so keep an eye on the version number (currently 5).

To optimize quickly, you might want to use analytic derivatives in your criterion and constraint function. BONUS: Try computing the second derivative of the criterion and constraints.

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?