SQP (Sequential Quadratic Programming):


This method linearizes the constraints and uses a second derivative of the objective function to approach a local minimum. SQP obeys boundaries at all iterations, making it the more conservative method. SQP is relevant for medium scale problems, such as ours.


Interior Point Method:


Interior Point involves computing a gradient which moves away from a set of linearized constraints. Interior point is the default algorithm used by Matlab's function fmincon, but is typically used for problems of higher dimensionality than ours. This method also obeys boundaries for all evaluations.


Active Set:


Active Set is a more agressive algorithm. Designed for medium scale problems, it takes larger steps and only ignores subsets of constraints during evaluation intervals. This must be accounted for, and can cause problems when using a barrier in your penalty function. Proceed with caution.


CMA-ES


CMA-ES (Covariance Matrix Adaptation - Evolution Strategy) is a popular new method in the optimization community. It involves producing a population of evaluation points. Within one iteration, the fittest points from a population are selected, and a new set is created based off a gausian distribution of the fittest points. As a dimension converges towards a locally optimum value, its standard deviation will converge towards zero. When the diagonal elements of the covariance matrix all converge to zero a solution has been determined, this works well on non smooth manifolds, where other methods may get caught on weaker local minima.

The joint angles used to produce these 7 Configurations will make up our initial guesses. From these guesses each method will search for locally optimim solutions for 7 targeted wrist poses. These 7 targets correspond to wrist poses of the starting configurations above. That means that there will be 7 uninteresting experiments, where the wrist starts at a local minimum.


Here we add a note that target 7 is not identical to the pose in configuration 7. We extend the position of the target outside the workspace to observe Atlas' behavior when it cannot reach a feasible solution. The code for our simulations can be found here.

--------------------------------------------------------------


Results:

A composition of the data from these experiments can be perused here. The data includes the number of function evaluation, objective function score, and the success of each simulation. There are 4 sheets, 1 for each method.


We also logged the paths taken from initial to final point, creating a set of animations for each test. This allowed for us to make some qualitative observations about the data. Next we'll go over the insights we gained.


Analysis:

1: Approach Characteristics

Right away we noticed that the algorithms act quite differently. Observe the difference in approach when moving from Configuration 2 to Target 4.     










                              sqp24                                          interiorpoint24                                      activeset24


These approaches correlate with the algorithm characteristics described in the Methods section. SQP works to resolve its wrist constraint while obeying all other constraints. Once resolving it briefly adjusts its center of mass and joint angle deviations to arrive at a locally optimal solution. Interior point is a bit more agressive. It converges towards resolving the wrist pose and feet constraints simultaneously. This is a characteristic of the method and allows it to search along a more easily navigable objective funciton manifold while resolving its violation of the wrist constraint. This is a feature which makes the interior point method suitable for higher dimensions. The interior point method is more likely to find a solution in high dimensional space than Sequential Quadratic Programming. Because SQP doesn't diverge from the constraints it currently obeys, it can get caught on sections of a manifold that are over-constrained. It may therefor not obtain a local minima, even when a solution exists. An example of this can be seen in Analysis Part 3.  The Active Set shows a chaotic approach. It initially ignores center of mass and feet restrictions. It even appears to violate the joint limits, which SQP and interior point are designed not to do. 


Similar approach characteristics can be observed when moving from Configuration 3 to Target 6.










                            sqp36                                           interiorpoint36                                        activeset36​​

Our task for Assignment 1 is to place Atlas' right wrist at a set of position-orientation combinations while optimizing for a set of configuration qualities we deem important. In addition to our wrist placement, we are looking to control our robots center of mass and avoid proximity to joint limits, without picking up Atlas’ feet.

Here is the optimization framework in which we will search for locally optimal configurations.

Dynamic Optimization: Assignment 1

Quan Nguyen, Brian Bittner


For Assignment 1, we are provided the forward kinematics and joint constraints for Atlas, a humanoid produced by Boston Dynamics. As participants in the DARPA Robotics Challenge, CMU is working to enable Atlas to perform a series of tasks, including those necessary for arriving at a plant, opening doors, navigating rough terrain, and opening/closing valves.

Experimentation:

We will run an equivalent set of experiments for each method tested. The uniqueness of each test will involve the starting configuration (an array of joint velocities) and the targeted pose of the wrist. While it is intuitive to see what positions are reachable within the workspace, it is a bit ambiguous as to whether a rotation matrix/quaternion for the wrist joint will be obtainable. By optimizing for wrist positions, we observe the wrist orientations at configurations to get a sense of what poses are feasible. Here is the set of configurations we will use for our testing.


Configurations:​​ (click on figure for description)

Penalty Function (Objective Function):

We quantify configuration characteristics into a single score. In this case, accumulating points for score is bad s we are working to minimize its value. Within this context we will implement soft constraints and barrier functions.

Soft Constraints:
A soft constraint can be understood as a reinforcement to stricter (hard) constraints which reward the solutions which more closely obey the constraint. We impose weighted penalties on these 3 distinct configuration qualities.

  • Angle displacement from initial configuration.
  • Center of Mass displacement from origin.
  • Distance of joint angles from boundaries.


Barrier Functions:
Barrier functions are a special type of soft constraint which can be used to more heavily reinforce a boundary in the penalty function. We use a barrier function to keep our robot from tipping over, as this could cause serious damage for a real system.

  • We score the inverse natural log of the center of mass distance from the edge.

For very close points, this drives our penalty function to infiniti. If an initial configuration lies outside the barrier, it loses all functionality. More on this later.

Hard Constraints:

Independent of the penalty function, hard constraints involve a set of restrictions supplied to the optimizer. We invoke a set of equality and inequality constraints on our system.

Equality Constraints:

  • Right wrist position and orientation.
  • Position of both feet.


Inequality Constraints

  • Center of Mass cannot exceed displacement from origin.


Boundaries:

  • Upper and lower boundaries on joint angles.

Boundaries allow us to directly restrict the configuration variables we optimize for. By providing lower and upper bounds for each configuration variable (joint angles in this case) we greatly reduce the configuration space through which we search for optimum.​


Explained mathematically here.


Methods:

We offer a brief description of the qualitative nature of the optimization algorithms we will compare.

2: Approach Similarity
Conversely we observe that in some cases there is little variation in approach across the methods. This is a product of objective function manifold shape. Depending on the manifold shape, there may be a direct, obvious route along which to converge to a local minima. For example, if you are right next to a local minima, all of the algorithms will likely step right to it. This is a nice feature of the active set and interior point methods, in that neither takes an unnecessarily complicated route, despite their aggressive nature. We provide an example where the robot moves from its zero-angle Configuration 1 to the reaching forward Target 3.








                            sqp13                                           interiorpoint13                                        activeset13​​

3:  Limitations of SQP

As we spoke of earlier, Sequential Quadratic Programming typically converges while adhering very closely to the constraints. This could be described as a controlled approach toward a local minimum. It seeks to fix its wrist constraint issue while maintaining the constraints it already adheres to. This can be seen as a positive quality for those who dislike methods which disregard constraints over certain iterations.

That said, it seems that SQP's controlled approach can yield negative consequences. For example, when moving from Configuration 2 to Target 5, SQP is unable to obtain a local minimum, while interior point and active set are.










                            sqp25                                           interiorpoint25                                        activeset25 

4: Active Set and Barrier Functions

​As has been discussed, active set well iteratively ignore sets of constraints, leading to unpredictable configuration evaluations. This is problematic for our barrier function which requires estimates to remain within correct bounds to function properly. When an evaluation is made on the wrong side of the barrier function, it can drive the computation toward producing a singular or near-singular Hessian, at which point active set crashes. Correspondingly, we reran our active set simulations, removing the logarithmic barrier function. 









                                    active-set_barrier                                 active-set_barrier-removed

5: Target Outside of Workspace

A component of the assignment was to ensure that, in the case of an unreachable target, that Atlas would configure itself such that it minimizes variation in pose while obeying all other constraints. As expected, SQP is great for this. SQP will always reach out towards the target until it encounteries the boundary of its workspace or will tip over. This is reflective of its conservative nature, as explained throughout the analysis. The active set and interior point methods are not good at providing this functionality. For example, Target 7 is strategically placed such that you must exceed your center of mass boundaries to achieve it (assuming feet are still planted). Both methods try to consolidate the center of mass and pose constraints simultaneously, but get caught in between. They output their local minima outside the center of mass boundaries, reporting an infeasible solution. Therefor, these algorithms would not be safe to run on Atlas unless you wrote extensive additional code to enforce some priority in constraint choice. You could also throw out that solution and rerun with SQP or another more conservative algorithm.


6. Run Time

SQP turns out to be the fastest approach, using the least number of function evaluations across the set of simuations. This may be because it works directly towards resolving its wrist constraints, while the others experiment by sampling the space more fully. Active set is the clear winner amongst these three in terms of run time. This is most likely because Active set spends a good bit of time experimenting with combinations of constraints. This may allow it to find better solutions in some cases, but overall you are paying for it in compuation time, by a about an order of magnitude.


7. Local Minima

In Response to Question 3, we consider a situation where we want to supply a user with a set of local minimum options, given a desired wrist pose. We suggest running a set of fmincon simulations from a large set of initial guesses that span the space as fully as possible, while balancing computation time requirements. Run fmincon from these points with sqp, interior point, and active set. 


As can be seen in the data, starting point and method applied can have a significant impact on the solution we arrive at. For example, when moving from Configuration 1 to Target 3, each method finds a separate local minima. 


Additionally, during 7 approaches to Target 3, SQP finds 5 separate local minima.It is not always the case that spanning the space well will provide multiple minima. This is all heavily dependent on the shape of the objective function manifold. For example, the interior point method finds the same local minima for all seven approaches to Target 4.


Once a set of local minima are obtained, some computation should be done to get a sense of the angular displacement each configuration has from each other. This will provide a sense of qualitatively or visually different minima. 


You cannot report possession of all minima. You would need to exhaustively search from all possible guessing points. Since we have no way of knowing if another minimum exists, we cannot report a global minimum.


CMA-ES
Analysis of CMA-ES is being assessed separately for a few reasons. The previous three methods were all interfaced with MATLAB's Optimization Toolbox, specifically the function fmincon. To implement CMA-ES, we use starter code from the Inria Research Institution. We ran into some implementation obstacles which we will discuss below. 


When interfacing to fmincon, you can explicitly provide the constraints as its own argument. This provides fmincon a context of where these constraints. In CMA-ES, to implement a hard constraint, one simply returns NaN (not a number) in the penalty function for that evaluation. The optimizer is then blind to these hard constraints when generating populations. When testing with the same hard constraints used for the methods above, we generated no states that fell within the constraints! Our problem was overconstrained. We would be incredibly lucky if a randomly selected state (of joint angles) provided the correct wrist position, wrist orientation, left foot position or right foot position, let alone all of them at once!


After a helpul discussion with the TA, we realized we had to convert our hard constraints to soft constraints, to implicity constrain our system. This allows an initial tolerance, and provides a gradient which will allow states to converge towards meeting all desired constraints. 


We converted the following hard constraints to components of our penalty function.

  • Right and Left Foot Positions
  • Wrist Pose


After running under this new optimization framework, our simulations converged to meaningful local minima. At first we observed foot error in the final configuration. We fixed this by simply increasing the weight in the scoring function for foot error.


We used the default population 4 + 3*ln(N) where N is the dimensionality of the space. This population size (14) allowed us to arrive at our solution. We could have increased the popuation size if we felt that 14 states were not enough to gain meaningful context for the shape of the manifold in that region. Time of computation is controlled to grow less than linearly with population size, which is a nice feature of the optimization framework.


We chose a standard deviation of the one fourth the interval of each joint limits. This would provide an intial gaussian which spans the interval of each joint limit. [-2sigma, 2sigma]. 


CMA-ES Analysis:

 CMA-ES is particularly good at avoiding local minima caused by roughness in manifold. This allows the algorithm to avoid weak local minima.


Here CMA-ES finds a feasible solution when moving from Configuration 2 to Target 5, as SQP is not able to (see Analysis Part 3).










A last impressions about CMA-ES: This appears to be a promising method for generating strong local minima. Unfortunately this significantly drives up computation time for each simulation. Looking through sheet 4 of the data file in our results section, you can see that the number of function evalutions required is sinificantly higher than all other methods. The output configurations were scored through the penalty function used in the fmincon implementations for comparison. The configurations determined by CMAES were consistently larger than minima found in other configurations. This is  a product of the implementation of our new soft constraints. We set the weights for these fairly high, so the optimizer may have cared less about what was previously deemed important.


Using an i7 processor, switching from the fmincon implementations to CMA-ES drove CPU utilization from 12% to 72% suggesting CMA-ES is better parallelized.

 

Conclusion:

We constructed a framework in which to optimally achieve wrist poses for Atlas. We provided a set of penalty functions and constraints (hard, soft, barriers, boundaries, etc..). These allowed us to maintain Atlas' foot position, center of mass, and angle displacement while placing its wrist.


Using MATLAB's fmincon, we were able to search for optimal configurations of our ATLAS robot with three separate methods (sql, interior point, active set). Each has its own strengths and weaknesses, along with fields of applicability. Most notable SQP's conservative nature makes it suitable for integration to direct testing on the robot. It will not tip it over. Something additional to not about SQP is that it can struggle with over-constraint issues while the active set and interior point methods succeed. 


Additionally, when using active set, pay attention to your barrier functions. Active set has large enough step sizes that it may evaluate outside the barrier while ignoring an inequality constraint, as we experienced.


This lab provided some great insight to the algorithms implemented in fmincon. It would be interesting to do a comparison on high dimensionality optimization problems.