Feature/hard inequality constraints

[adamheins]

With reference to #30, this in an in-progress PR for implementing hard inequality constraint support via the HPIPM interface. I've done a basic implementation of general inequality constraints, which seems to work with (1) the friction cone constraints in the legged robot example, and (2) a new test in the hpipm_catkin package with random inequality constraints. At this point I'm just looking for some feedback, in particular with respect to any further testing I can do/add or really just anything I've missed/got wrong/can improve.

The largest remaining task (as mentioned in the original issue), is supporting box inequality constraints directly, since these can be efficiently handled by HPIPM. I've been thinking about this, and it seems to me that we'd need a new type of constraint (say BoxConstraint, presumably specialized to State-only and StateInput like the existing constraints), because the box constraints in HPIPM are (in general) two-sided and I don't believe two-sided constraints are supported directly in OCS2 at the moment. I'm very happy to hear any thoughts on this.

Another item I'm not entirely sure about is handling the inequalityConstraintsSSE for merit/performance index and line search purposes. I've added inequalityConstraintsSSE, which is computed as the sum of squares of the violated inequality constraints (i.e. when the constraint value is negative), but otherwise just treat it in exactly the same way as the existing equalityConstraintsSSE.

[rubengrandia]

Hi, thanks for this work, and sorry for getting back to you so late.

I think you are on the right track in integrating this. I agree that we should probably introduce a double sided specification for inequalities, and add a specific interface for box constraints as well. I would also suggest to use the slack variable interface of HPIPM, since QP subproblems might be infeasible with the linearized constraints.

On the implementation I think you missed the projection of the inequality contraints. Look for "changeOfInputVariables" in MultipleShootingTranscription.cpp. The change of variables needs to be applied to the linearized inequality contraints, just as for the dynamics.

In terms of testing I suggest to add one test to testCircularKinematics.cpp. This would have revealed the fact that the projection is missing. And a second test for a problem with perfect quadratic cost, linear dynamics, and linear inequality constraints. (the constrained version of testUnconstrained.cpp)

[adamheins]

Okay, a number of new changes:

1. Fixed the construction of the constraint matrices in the HPIPM interface, and also reduced data copying significantly.
2. New tests in ocs2_sqp, as suggested. In particular, there are new tests in testCircularKinematics.cpp which add an inequality constraint. I've verified that this does in fact change the resulting trajectory. There are tests with and without projection of the equality constraint. In addition, I added a testConstrained.cpp analogous to testUnconstrained.cpp. Like the latter, it first verifies that an empty constraint and null constraint behave the same. Further, it includes a test to verify constraint satisfaction for a random linear inequality constraint.
3. When creating the new circular kinematics tests, I did run into some feasibility problems so I created a preliminary interface for the slack variables. At the moment information about the slacks is just specified in the HpipmSettings object; I expect we ultimately may wish to handle this differently/in a more full-featured way.

Let me know what you think.