(5) Jhanani - Hand control / QP optimization

Part I: Trying out controller tasks

   In the beginning of this project, quite some time was spent in getting familiar with the ROS and Gazebo environments. There were extensive tutorials to get through, but they were well structured. After an ease of navigation was established, by assignment I was part of the motion control team where I tried to write my own controller classes based on existing code that used whole body space control. In particular, I worked on controlling hand position and orientation. I wrote code to append the desired hand movements to an existing task list. At first, we worked on the old Valkyrie model, where the pelvis was fixed so as to maintain balance. Later, we shifted to working on the R5 model where also initially we had the pelvis fixed as we tried out hand controller tasks.

Challenges:

(1) Did not know about task prioritization in multi-task control, and once I had the right order of assigning tasks (joint position specification took priority over others), I was able to detect movement of the hands.

(2) This was particularly true later in the balance control code, because the balance control took higher priority over all others.

In the following gif, we have balance control turned off, and the pelvis is fixed to the world. We see the movement of the right hand of the robot in a periodic manner as set by the task.

Part II: Trying QP solver for balance task

To maintain balance, the robot needs to control the position and acceleration of the center of gravity (CoM). Originally, the WBOSC used contact constraints and a corresponding CoM control task to maintain stability. We began to develop a balancing controller that did this by directly calculating the reaction force on the ground that would keep up stability and thus maintain the center of mass in a desired position. The problem of calculating the reaction force is actually an quadratic programming problem, which is described as follows:

Note that the problem described above is a quadratic convex problem, and there are equality and inequality contraints. The inequality constraints come from the need to have the robot not penetrate the ground at any time, and also have the total weight of the robot be equal to the sum of all reaction forces at all times.

For the solution to the quadratic programming problem, Donghyun initially employed the highly versatile NLopt non-linear programming solver suite. There are different options for the solvers in this suite, depending on the type of problem and the constraints.

Challenges:

(1) NLopt was unable to sustain balance in standing because there was no feasible solution for the reaction force after a few time steps.

(2) The real time factor of the simulation was also low (~0.2 to 0.3) and this was not desirable in view of walking and performing other tasks. 

We felt that using NLopt for a convex programming problem crated a lot of unnecessary overhead in the solution process. NLopt is a tool for non-linear programming problems, and there is actually a statement in the user manual which said that for convex programming problems, we are better off using solvers meant precisely for that purpose. For these reasons, we started hunting for a convex programming solver that would not fail and be faster than NLopt. We also decided to relax the equality constraint and minimize the norm of the difference between the robot weight and total reaction force. In other words, we modified the objective function (which was still quadratic in the reaction force) to include the weight equality constraint.

The convex programming solvers that were available for use in Ubuntu were CPLEX (IBM proprietary software), Gurobi and MOSEK, which are also proprietary but free for academic use. These are standard convex optimization solvers. We found another one called qpOASES which uses an active set method for specifically solving quadratic optimization problems and was developed with the aim of being fast enough for use with on-board computation devices. At first, we did a comparative study of MOSEK, qpOASES and NLopt on a sample quadratic programming problem of a smaller dimension than the actual one. We found that while MOSEK was not as fast as expected, qpOASES was actually much faster than the NLopt solver and yielded solutions of the same accuracy.

The above figure shows a snippet of results comparing different runs of the QP solvers for a problem of state dimension three. Note that NLopt is faster or slower than qpOASES depending on the solver used. The solver SLSQP, while faster, did not hold for long periods in double support while the solver COBYLA was able to sustain balance longer, though at a cost of being slower to compute. Armed with this knowledge, we then implemented the balance control reaction force calculator using qpOASES as the quadratic programming solver. The implementation took some time to debug because it had to be integrated with the existing balance controller.

The following observations could be made with respect to the performance of qpOASES as a solver:

(1) We again met with the same issues of the simulation real time factor being very low. This was a set back since we had expected an improvement over the previous solver.

(2) Also, the solution yielded by qpOASES was deemed infeasible in the presence of equality constraints after a few steps (same problem as with NLopt).

(3) If the equality constraint was incorporated into the objective function, eventually the sum of the reaction force deviated from the weight by large amounts, sometimes even showing cyclic variations.

(4) After a lot of debugging, and changing parameters, qpOASES finally worked for just double support for a fixed CoM. However, there was no consistency in the solution which prompted the use of Gurobi as the quadratic programming solver in subsequent calculations. 

(5) There was a lot of numerical instability in the solver, since it behaved well for certain combination of the parameters and weights which in reality should not change the solution feasibility. 

Future work: State machine for qual 2 of src:

After the ihmc controllers for the src were released, we realized that much of the low-level controller building was of no direct relevance any more. In essence, we needed to write a ROS wrapper for the controllers provided to us. Apart from runs of the controllers, we are interested in modeling and developing high-level autonomy for this robot to complete the qualification tasks. The first step towards that is to create a state-machine model of the qualification tasks. Specifically, the required state machine is a language acceptor, where the language is the series of control commands that leads to completion of the task.

Some atmoic propositions that could be a part of the specification of this state machine are:

(a) Harness is detached, (b) Home pose is active, (c) Robot is upright, (d) Robot before the first red line, (e) Button was aligned/pulled, (f) Door is open, (g) Time elapsed is less than "t".

If we are doing a line search on time to finish the task, it might be useful to have time in the specifications so we can do a line search and reject language strings that take more time than the current best time. But the challenge there is to correlate the execution of each step to a specific time taken. 

The actions are thereby: (1) step left (2) step right (3) step forward (4) step backward (5) do nothing (6) extend hand (7) move feet to regain balance/ go home. 

In the following weeks, we will formalize these high-level specifications so that can we can do some model-verification and input evaluation using state machine synthesis tools.