Rick Shanor
16-745: Dynamic Optimization
Assignment 1

Using Function Optimization to do Inverse Kinematics

Part 1: Choosing Joint Angles


In order to place the right wrist of the atlas robot at a specified x,y,z and orientation, I wrote a Matlab subroutine that optimized each joint angle of the atlas robot based on several motion constraints. These specific constraints are outlined in the section below. I defined these constraints as penalty functions, upper/lower variable bounds, and finally nonlinear equality and inequality constraints in the fmincon function.

Penalty Function Constraints:

The right wrist must be placed at a specified x,y,z and orientation target

The joints should be as far from the joint limits as possible

The center of mass should be as close to the origin as possible

These constraints are explicitly defined in the file minFun.m, which can be found in the zip file downloadable at the end of this page. In addition, by tweaking the weight of the penalty function variables such that the position and orientation constraints are the highest weighted, we can ensure that if possible the robot will reach the goal.

Boundary Constraints (Implementation of Inequality Constraints):

I defined upper and lower bound constraints in the fmincon function. Specifically, I created vectors as follows:

The bounds are defined in the file main_rshanor.m based on the limits in drm.4

Equality Constraints:

No stepping is allowed. Thus, the final position and orientation of the feet must be the same as the initial position and orientation. These are simply defined in the file footConstraints.m as

Inequality Constraints:

The center of mass must be over the base of support. This forces the optimizer to create as stable a configuration as possible. Implementation of these inequality constraints can be found in footConstraints.m.

Preparing Variables for Testing:

First, I generated a series of goal points and orientations permeating about the base configuration. These configurations will serve as a baseline as I move forward comparing the different optimization algorithms. I am trying to standardize the process for comparison.


Next, I ran the fmincon optimization routine on the Atlas robot for these 15 points. The 4 figures below depict the results. Specifically, green dots indicate that the Atlas robot was able to achieve the desired position and orientation. The red dots indicate that the right wrist could not achieve the desired space. This optimization routine could achieve most position/orientations located in front of the robot. Usually results could not be found where the robot was trying to either reach behind itself or trying to reach a point further than it can reach without moving the feet.



Below is a video of the Atlas robot attempting to achieve these desired positions. In addition, for debugging purposes, I added a bounding box and the center of mass projected onto the floor, showing that the center of mass constraint holds.


If the embedded link does not work, please find the video uploaded at: http://youtu.be/S3TS-CXROn4

Results – Fixed Point with Varying Orientation:

The following 6 images depict the Atlas robot attempting to reach the same point with different desired orientations. This helps to see how the optimization routine performs when the goal is unreachable. Specifically, we see the optimizer trying to balance between reaching the desired position and the desired orientation. In addition, we see how the position constraints significantly outweigh the center of mass constraints and the joint angle constraints. By changing the desired orientation, the joints and center of mass shift all over. However, the robot always arrives close to the goal location.



orientation2 orientation6


Part 2: Comparing Optimization Algorithms


In part 2, I ran my optimization subroutine through several different algorithms. Specifically, I first tested the three different algorithms built into Matlab (interior-point, trust-region-reflective, and sqp) on the inverse kinematics routine. Next, I also implemented the CMA-ES (Covariance Matrix Adaptation Evolution Strategy) optimization package. I tested all these algorithms on the same set of 15 points generated for part 1 to allow for accurate comparison between methods.

Comparing Run Times and Optimization Scores of fmincon Options:


The table on the left shows the runtime of the three different optimization routines within fmincon. The fastest optimizer is the ‘active-set’ option, which takes less than half the time to find a local minimum. The slowest optimizer, ‘SQP’, has virtually the same performance as the ‘Active’ set on every point. However, the interior-point function algorithm appears to find a noticeably ‘more optimal’ point based on the penalty function. There are a few cases in (point 6 and point 8) where the ‘interior-point’ function is able to find a local minimum which reaches the goal where the two other algorithms do not. Some of these instances are discussed in more detail in the next section.

Comparing Solutions of fmincon Options:


The above example shows the results of ‘SQP’ (left) and ‘Interior-Point’ (right). As shown in the penalty function score chart, the ‘Interior-Point’ method finds a significantly better optimum than the ‘SQP’ function. Looking at the picture above, you see that the first method positions the robot almost reaching backward and around trying to achieve the desired position and orientation. This method, however, is penalized for poor orientation and joint angle selection. However, in the ‘Interior-Point’ method, the robot leans sideways and back. In this configuration, the desired point still is not achieved but better joint angles and orientation give this position a better penalty score.



The images above are the result of running ‘Active-Set’ and ‘Interior-Point’ optimizations on the Atlas robot. In both optimizations, the goal position is achieved with a similar penalty function cost. However, the legs joints end up in slightly different positions, with ‘Interior-Point’ having a slightly lower cost due to joint angle penalties and center of mass constraints. In reality, there is little different between the two results.



This final example shows an instance where ‘Interior-Point’ is able to barely reach the goal position where ‘SQP’ cannot. By bending the ‘knee joint’ slightly forward rather than slightly backward, the Atlas robot can shift its center of mass just enough to reach the goal position without breaking joint angle constraints.

Comparing fmincon and CMA-ES

I also implemented minimization parameters to run on the CMA-ES optimizer. Overall, the results between optimizers is comparable in terms of accuracy. However, fmincon tends to find slightly better solutions and was able to achieve the correct position and orientation significantly more frequently. However, the CMA-ES optimizer results tended to be relatively close. However, the biggest drawback of the CMA-ES optimizer was run time. The CME-ES optimizer was slower by a factor of 10 compared to the fmincon function. In addition, penalty constraints were not as intuitive to implement. I had to generally find methods of forcing constraints into the penalty function. Lower and upper bound constraints were still built in options in the CMA-ES  optimizer. Below is a video of the CMA-ES optimizer trying to solve the inverse kinematics problem.


If the embedded link does not work, please find the video uploaded at: https://www.youtube.com/edit?o=U&video_id=8KYpLXoTmKQ

Part 3: Comparing Local Minimum: Varying Starting Location and Other Parameters

Varying Starting Location to Find Local Minimum

First, I tried to use the resulting joint angles from the first optimization in order to find local minimum. Specifically, I wrote an algorithm which would take robot positions based on final locations of the Atlas robot when trying to reach desired points in the previous exercises. From these various starting configurations, I attempted to command the robot to the same final position in hopes of achieving different local minima. In most cases, however, I found that the fmincon function found the same minimum from many starting configurations. In fact, I had to manually change the joint angles to extreme conditions in order to achieve a variety of local minimum. Results from my testing can be shown below.

Test 1: Starting Configurations (Left) and Local Minimum (Right)




Attempting to achieve the same desired position and orientation in the three trials above led to very similar local minimum. As you can see, the robot can’t quite achieve the desired configuration. However, starting from the third configuration leads to a local minimum that optimizes position over orientation, where in the first two the orientation is close to desired. In addition, in each configuration you see slight different behavior in the left arm of the Atlas robot, finding a different position to balance the center of gravity of the robot.

Test 2: Starting Configurations (Left) and Local Minimum (Right)




The three different local minimum in this case are more drastically different than in the first example. There is only one instance in which the robot actually found a ‘good’ local minimum. In the other two examples, the fmincon function solver ended quickly, determining no better local minimum could be found. One reason for this is that I manually drove the joints starting angles so that the center of mass was outside the center of mass. From there, the solver took extreme steps to meet this inequality constraint. In doing this, the solver drove to a local minimum from where the solver could not improve to achieve a better (but possible) solution.

In all my testing, I found that from the base configuration, as well as for most reasonable configurations, the solver arrived at a very similar local minimum. As shown in example 1, these minimum only very slightly at some joints but with similar end effector configurations. However, manually driving the robot outside standard operating ranges showed to have the effect that sometimes any close-to-optimal solution could not be found.

In order to present these various options to the user, the solver must aggressively change starting parameters. Another method of presenting local minimum would be to solve the Inverse Kinematics problem once, update the constraints based on this one solution, then solve for other similar solutions. This approach would not, however, lead to drastically different local minimum.

Effect of Penalty Function Weights on Results

Finally, I experimented with how changing the different weights in the penalty function would affect the local minimum. Some of these penalties may or may not be more important to specific scenarios.

Starting from the base configuration above I tweaked the parameters to force different inverse kinematics results.


Balanced Parameters (Left) and Increased Center of Mass Penalty (Right)

In this example, by boosting the Center of Mass parameter we are able to ensure the center of mass is basically at the origin. However, the Atlas robot is further from the desired position and doesn’t ‘extend’ the same way it does initially.


Slight Increase (Left) and Large Increase (Right) in the Joint Angle Penalty Weight

As expected, increasing the joint angle penalty caused the robot to ‘straighten out’ toward initial configuration. However, this has the inverse result of moving away from the goal. In addition, we see behavior such as the left arm does not bend to try and shift the canter of mass nearly as much. This indicates that the joint angle penalty is larger than the penalty of the center of mass.

In order to present these various options to an end user, it would be important to understand how much each penalty mattered. Specifically, individual user may care more about orientation (reaching out for a high-five), more about position (reaching a light switch), or more about both position and orientation (turning a doorknob). In addition, depending on terrain the center of mass penalty may need to be more significant to ensure the robot doesn’t fall over. Finally, power usage constraints may drive how important it is to limit joint motion. Finally, it is important that these weights are all relative to each other in the penalty function. There is no hard rule about how to best set weights.

About the Code

The code can be found and downloaded at: https://www.dropbox.com/s/61q2yv60ptsle25/hw1_turnin.zip?dl=0

The most helpful files and functions are as follows:

main_turnin.m: Run this to observe fmincon or CMA-ES optimization run on a series of positions. Use optFlag = 1 for CMA-ES otherwise fmincon

minFun.m: Contains the penalty scoring function called by the fmincon optimizer

footConstraints.m: Constraint function enforcing stationary location

minFunCMASE.m: Penalty function edited to run with CMAES optimizer

main_rshanor.m: Script used to test routines, generate plots, test run times, etc in completion of this assignment