CMU RI 16-745: Dynamic Optimization: Assignment 2 - 05/14/2014(Venkat Rajagopalan)
This assignment explores using of trajectory optimization to do a sit-to-stand maneuver on a 3 link arm.
Part 1: The task is to design a sub-routine that chose joint angles for the three links while meeting the followng constraints
-
Generate a trajectory such that the links form an inverted pendulum in the final position.
-
Keeps the torque limit at the "ankle" to be +/-100 N-m and +/- 400 N-m for the "knee" and the hip
-
Does not allow the "hip" to dip below the initial configuration
The following image shows the robot at it starting configuration
Algorithmic Approach
The SNOPT library was used for the trajectory optimization.
Joint space trajectories were parameterized as a cubic hermite form spline. The "knots" represented initial and terminal conditions for position, velocity and accelerations of the joints
Experiments were conducted with two different starting conditions. The robot was assumed to be sitting on an a) imaginary chair on the left and b) imaginary chair on the right
Optimization was done on a) all the joints
Joint angle limits were passed in as the lower and upper bounds to the optimizer
Penalty functions sitToStand.c (line 585) - four different criteria were chosen to be penalized and are described below
- Distance to goal orientation was modeled as a L2 squared norm. The difference in angles were wrapped around 0 radians Dp = (x_d - x)' * (x_d - x)
- Per joint torques were computed and were penalized only if they exceeded their limits. An L2 squared normwas used for the penalty Tp = (t_d - t)' * (t_d - t)
- Large velocities and accelerations as well as their deviation from the terminal conditions were penalized as a L2 squared Norm. Vp = v * v and Ap = a * a
- The total penalty was a weighted sum of all the above penalties Tp = 1.0*Dp + 1000.0*Tp + 1.0*Vp + 1.0*Ap
Results
Final configuration of the robot
Videos:
SitToStand on a chair sitting to the left SitToStand_FromLeft.avi
SitToStand on a chair sitting to the right SitToStand_FromRight.avi
