# 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