CMU RI 16-745: Dynamic Optimization: Assignment 1

This assignment explores using function optimization to do inverse kinematics.

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 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. 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). [Inequality constraints].

Part 2: For many positions, there are multiple local minimum. Given a position and orientation target, can you find multiple local minima, and provide the user with a choice of local minima?

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 that might help you debug. It draws a robot stick figure with the current angles, given drc_forward_kinematics() has been called.

I found this paper on a metric for how close two rotations are. Metrics for 3D Rotations: Comparison and Analysis, Du Q. Huynh, J Math Imaging Vis (2009) 35: 155-164.

Here is an Matlab script from Seungmoon Song that draws the ATLAS robot. You can run the "test.m" file in the "anim" folder. Unfortunately the meshes do not match the current robot, especially for the head location.

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?

Bug Fix

In the original notes.txt file, I said the root variables were:

% set root variables
robot.j(1).position_w = [ 0 0 0.92712 ];
robot.j(1).rotation = q_to_R( [ 0 0 0 1 ] );

The correct root variables are:

% set root variables
robot.j(1).position_w = [ 0 0 0.92712 ];
robot.l(1).orientation = q_to_R( [ 0 0 0 1 ] );