Feature/hard inequality constraints #35
Conversation
Hard inequality constraints added to HPIPM interface, with one unit test and basic testing using the legged robot example.
This commit fixes issue leggedrobotics#35 - CentroidalModelPinocchioMappingTpl::getOcs2Jacobian is called with Jv a 0×Nv matrix. This causes a runtime error in line 164 of pinocchio mapping. - Subsequently PinocchioEndEffectorKinematics::getPositionLinearApproximation is broken when calling getOcs2Jacobian. - By changing Jv to a zero matrix of size 3×Nv, the runtime error is fixed.
|
|
||
| // Combine equality and inequality constraints and extract raw data | ||
| for (int k = 0; k <= N; k++) { | ||
| // Make block diagonal C = [[C_eq, 0 ], |
There was a problem hiding this comment.
I think they should be stacked without the zeros, the size of x, the rhs dimension of C should not change:
C = [[C_eq],
[C_inq]]
same for D
I would suggest to load them into the final matrix directly though
There was a problem hiding this comment.
Yep, you're right. By loading them into the final matrix directly, do you mean I should avoid having the intermediate C_eq and C_ineq matrices and just build the C_con matrix directly from the constraint objects (same for D)?
There was a problem hiding this comment.
Exactly, we should avoid copying around the data too much. Ideally, if only one of the constraint types is used, we just assign the pointers and don't need a copy at all.
|
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) |
Currently, slacks can be turned on for all constraints or none. Their penalization and bounds are the same for all slacks and can be set via settings.
* testCircularKinematics with an inequality constraint (which does force the trajectory to change), without and without projection of the equality constraint * testConstrained: analogous to testUnconstrained, which verifies that the result of an empty constraint and no constraint at all are the same. testConstrained also tests that the constraint is satisfied throughout the trajectory.
|
Okay, a number of new changes:
Let me know what you think. |
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_catkinpackage 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
inequalityConstraintsSSEfor merit/performance index and line search purposes. I've addedinequalityConstraintsSSE, 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 existingequalityConstraintsSSE.