Skip to content
2 changes: 2 additions & 0 deletions ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,8 @@ struct OptimalControlProblem {
std::unique_ptr<StateConstraintCollection> preJumpEqualityConstraintPtr;
/** Final equality constraints */
std::unique_ptr<StateConstraintCollection> finalEqualityConstraintPtr;
/** (Hard) inequality constraints **/
std::unique_ptr<StateInputConstraintCollection> inequalityConstraintPtr;

/* Lagrangians */
/** Lagrangian for intermediate equality constraints */
Expand Down
8 changes: 8 additions & 0 deletions ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,11 @@ struct PerformanceIndex {
*/
scalar_t equalityConstraintsSSE = 0.0;

/** Sum of Squared Error (SSE) of hard inequality constraints, used by the SQP solver.
* Inequality constraints are satisfied if positive, so there is only error if the constraints are negative.
*/
scalar_t inequalityConstraintsSSE = 0.0;

/** Sum of equality Lagrangians:
* - Final: penalty for violation in state equality constraints
* - PreJumps: penalty for violation in state equality constraints
Expand All @@ -76,6 +81,7 @@ struct PerformanceIndex {
this->cost += rhs.cost;
this->dynamicsViolationSSE += rhs.dynamicsViolationSSE;
this->equalityConstraintsSSE += rhs.equalityConstraintsSSE;
this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE;
this->equalityLagrangian += rhs.equalityLagrangian;
this->inequalityLagrangian += rhs.inequalityLagrangian;
return *this;
Expand All @@ -93,6 +99,7 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) {
std::swap(lhs.cost, rhs.cost);
std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE);
std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE);
std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE);
std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian);
std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian);
}
Expand All @@ -109,6 +116,7 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe
stream << std::setw(indentation) << "";
stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE;
stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n';
stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n';

stream << std::setw(indentation) << "";
stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian;
Expand Down
7 changes: 7 additions & 0 deletions ocs2_oc/src/oc_problem/OptimalControlProblem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ OptimalControlProblem::OptimalControlProblem()
stateEqualityConstraintPtr(new StateConstraintCollection),
preJumpEqualityConstraintPtr(new StateConstraintCollection),
finalEqualityConstraintPtr(new StateConstraintCollection),
/* Inequality constraints */
inequalityConstraintPtr(new StateInputConstraintCollection),
/* Lagrangians */
equalityLagrangianPtr(new StateInputCostCollection),
stateEqualityLagrangianPtr(new StateCostCollection),
Expand Down Expand Up @@ -82,6 +84,8 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other)
stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()),
preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()),
finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()),
/* Inequality constraints */
inequalityConstraintPtr(other.inequalityConstraintPtr->clone()),
/* Lagrangians */
equalityLagrangianPtr(other.equalityLagrangianPtr->clone()),
stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()),
Expand Down Expand Up @@ -130,6 +134,9 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept {
preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr);
finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr);

/* Inequality constraints */
inequalityConstraintPtr.swap(other.inequalityConstraintPtr);

/* Lagrangians */
equalityLagrangianPtr.swap(other.equalityLagrangianPtr);
stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ class LeggedRobotInterface final : public RobotInterface {
std::pair<scalar_t, RelaxedBarrierPenalty::Config> loadFrictionConeSettings(const std::string& taskFile, bool verbose) const;
std::unique_ptr<StateInputCost> getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient,
const RelaxedBarrierPenalty::Config& barrierPenaltyConfig);
std::unique_ptr<StateInputConstraint> getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient);
std::unique_ptr<StateInputConstraint> getZeroForceConstraint(size_t contactPointIndex);
std::unique_ptr<StateInputConstraint> getZeroVelocityConstraint(const EndEffectorKinematics<scalar_t>& eeKinematics,
size_t contactPointIndex, bool useAnalyticalGradients);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -174,8 +174,9 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile
modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));
}

problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
// problemPtr_->softConstraintPtr->add(footName + "_frictionCone",
// getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig));
problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getHardFrictionConeConstraint(i, frictionCoefficient));
problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i));
problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity",
getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints));
Expand Down Expand Up @@ -321,6 +322,13 @@ std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeConstraint(
return std::unique_ptr<StateInputCost>(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty)));
}

std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient) {
FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
std::unique_ptr<FrictionConeConstraint> frictionConeConstraintPtr(
new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_));
return frictionConeConstraintPtr;
}

/******************************************************************************************************/
/******************************************************************************************************/
/******************************************************************************************************/
Expand Down
6 changes: 4 additions & 2 deletions ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,8 @@ class HpipmInterface {
* @param x0 : Initial state (deviation).
* @param dynamics : Linearized approximation of the discrete dynamics.
* @param cost : Quadratic approximation of the cost.
* @param constraints : Linearized approximation of constraints, all constraints are mapped to inequality constraints in HPIPM.
* @param constraints : Linearized approximation of equality constraints, all constraints are mapped to inequality constraints in HPIPM.
* @param ineqConstraints : Linearized approximation of inequality constraints.
* @param [out] stateTrajectory : Solution state (deviation) trajectory.
* @param [out] inputTrajectory : Solution input (deviation) trajectory.
* @param verbose : Prints the HPIPM iteration statistics if true.
Expand All @@ -85,7 +86,8 @@ class HpipmInterface {
*/
hpipm_status solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<VectorFunctionLinearApproximation>* constraints,
vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose = false);
std::vector<VectorFunctionLinearApproximation>* ineqConstraints, vector_array_t& stateTrajectory,
vector_array_t& inputTrajectory, bool verbose = false);

/**
* Return the Riccati cost-to-go for the previously solved problem.
Expand Down
6 changes: 4 additions & 2 deletions ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,14 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept;
*
* @param dynamics : Linearized approximation of the discrete dynamics.
* @param cost : Quadratic approximation of the cost.
* @param constraints : Linearized approximation of constraints, all constraints are mapped to inequality constraints in HPIPM.
* @param constraints : Linearized approximation of equality constraints, all constraints are mapped to inequality constraints in HPIPM.
* @param ineqConstraints : Linearized approximation of inequality constraints.
* @return Derived sizes
*/
OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics,
const std::vector<ScalarFunctionQuadraticApproximation>& cost,
const std::vector<VectorFunctionLinearApproximation>* constraints);
const std::vector<VectorFunctionLinearApproximation>* constraints,
const std::vector<VectorFunctionLinearApproximation>* ineqConstraints);

} // namespace hpipm_interface
} // namespace ocs2
136 changes: 106 additions & 30 deletions ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,8 @@ class HpipmInterface::Impl {

void verifySizes(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
std::vector<ScalarFunctionQuadraticApproximation>& cost,
std::vector<VectorFunctionLinearApproximation>* constraints) const {
std::vector<VectorFunctionLinearApproximation>* constraints,
std::vector<VectorFunctionLinearApproximation>* ineqConstraints) const {
if (dynamics.size() != ocpSize_.numStages) {
throw std::runtime_error("[HpipmInterface] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " +
std::to_string(ocpSize_.numStages) + " number of stages.");
Expand All @@ -156,7 +157,13 @@ class HpipmInterface::Impl {
}
if (constraints != nullptr) {
if (constraints->size() != ocpSize_.numStages + 1) {
throw std::runtime_error("[HpipmInterface] Inconsistent size of constraints: " + std::to_string(constraints->size()) + " with " +
throw std::runtime_error("[HpipmInterface] Inconsistent size of equality constraints: " + std::to_string(constraints->size()) + " with " +
std::to_string(ocpSize_.numStages + 1) + " nodes.");
}
}
if (ineqConstraints != nullptr) {
if (ineqConstraints->size() != ocpSize_.numStages + 1) {
throw std::runtime_error("[HpipmInterface] Inconsistent size of inequality constraints: " + std::to_string(ineqConstraints->size()) + " with " +
std::to_string(ocpSize_.numStages + 1) + " nodes.");
}
}
Expand All @@ -165,9 +172,10 @@ class HpipmInterface::Impl {

hpipm_status solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
std::vector<ScalarFunctionQuadraticApproximation>& cost, std::vector<VectorFunctionLinearApproximation>* constraints,
vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose) {
std::vector<VectorFunctionLinearApproximation>* ineqConstraints, vector_array_t& stateTrajectory,
vector_array_t& inputTrajectory, bool verbose) {
const int N = ocpSize_.numStages;
verifySizes(x0, dynamics, cost, constraints);
verifySizes(x0, dynamics, cost, constraints, ineqConstraints);

// === Dynamics ===
std::vector<scalar_t*> AA(N, nullptr);
Expand Down Expand Up @@ -221,48 +229,114 @@ class HpipmInterface::Impl {
qq[N] = cost[N].dfdx.data();

// === Constraints ===
// for ocs2 --> C*dx + D*du + e = 0
// for hpipm --> ug >= C*dx + D*du >= lg
std::vector<scalar_t*> CC(N + 1, nullptr);
std::vector<scalar_t*> DD(N + 1, nullptr);
std::vector<scalar_t*> llg(N + 1, nullptr);
std::vector<scalar_t*> uug(N + 1, nullptr);
std::vector<ocs2::vector_t> boundData; // Declare at this scope to keep the data alive while HPIPM has the pointers

// Equality constraints for each stage:
// C * x + D * u == d
// Declare at this scope to keep data alive while HPIPM has the pointers
std::vector<matrix_t> C_eq(N + 1);
std::vector<matrix_t> D_eq(N + 1);
std::vector<vector_t> d_eq(N + 1);

if (constraints != nullptr) {
auto& constr = *constraints;
boundData.resize(N + 1);

// k = 0, eliminate initial state
// numState[0] = 0 --> No need to specify C[0] here
// k = 0
// No need to specify C_eq[0] since initial state is given
if (constr[0].f.size() > 0) {
boundData[0] = -constr[0].f;
boundData[0].noalias() -= constr[0].dfdx * x0;
llg[0] = boundData[0].data();
uug[0] = boundData[0].data();
DD[0] = constr[0].dfdu.data();
d_eq[0] = -constr[0].f;
d_eq[0].noalias() -= constr[0].dfdx * x0;
D_eq[0] = constr[0].dfdu;
}

// k = 1 -> (N-1)
for (int k = 1; k < N; k++) {
if (constr[k].f.size() > 0) {
CC[k] = constr[k].dfdx.data();
DD[k] = constr[k].dfdu.data();
boundData[k] = -constr[k].f;
llg[k] = boundData[k].data();
uug[k] = boundData[k].data();
d_eq[k] = -constr[k].f;
C_eq[k] = constr[k].dfdx;
D_eq[k] = constr[k].dfdu;
}
}

// k = N, no inputs
if (constr[N].f.size() > 0) {
CC[N] = constr[N].dfdx.data();
boundData[N] = -constr[N].f;
llg[N] = boundData[N].data();
uug[N] = boundData[N].data();
d_eq[N] = -constr[N].f;
C_eq[N] = constr[N].dfdx;
}
}

// Inequality constraints for each stage:
// C * x + D * u >= d
std::vector<matrix_t> C_ineq(N + 1);
std::vector<matrix_t> D_ineq(N + 1);
std::vector<vector_t> d_ineq(N + 1);

if (ineqConstraints != nullptr) {
auto& constr = *ineqConstraints;

// k = 0
if (constr[0].f.size() > 0) {
d_ineq[0] = -constr[0].f;
d_ineq[0].noalias() -= constr[0].dfdx * x0;
D_ineq[0] = constr[0].dfdu;
}

// k = 1 -> (N-1)
for (int k = 1; k < N; k++) {
if (constr[k].f.size() > 0) {
C_ineq[k] = constr[k].dfdx;
D_ineq[k] = constr[k].dfdu;
d_ineq[k] = -constr[k].f;
}
}

// k = N
if (constr[N].f.size() > 0) {
d_ineq[N] = -constr[N].f;
C_ineq[N] = constr[N].dfdx;
}
}

// Combined equality and inequality constraints for each stage
std::vector<matrix_t> C_con(N + 1);
std::vector<matrix_t> D_con(N + 1);
std::vector<vector_t> d_con(N + 1);
std::vector<vector_t> upper_bound_mask(N + 1);

// Raw data for each stage
std::vector<scalar_t*> CC(N + 1, nullptr);
std::vector<scalar_t*> DD(N + 1, nullptr);
std::vector<scalar_t*> llg(N + 1, nullptr);
std::vector<scalar_t*> uug(N + 1, nullptr);

// Combine equality and inequality constraints and extract raw data
for (int k = 0; k <= N; k++) {
// Make block diagonal C = [[C_eq, 0 ],
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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)?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

// [0, C_ineq]]
C_con[k].setZero(C_eq[k].rows() + C_ineq[k].rows(), C_eq[k].cols() + C_ineq[k].cols());
C_con[k].topLeftCorner(C_eq[k].rows(), C_eq[k].cols()) = C_eq[k];
C_con[k].bottomRightCorner(C_ineq[k].rows(), C_ineq[k].cols()) = C_ineq[k];
CC[k] = C_con[k].data();

// Make block diagonal D
D_con[k].setZero(D_eq[k].rows() + D_ineq[k].rows(), D_eq[k].cols() + D_ineq[k].cols());
D_con[k].topLeftCorner(D_eq[k].rows(), D_eq[k].cols()) = D_eq[k];
D_con[k].bottomRightCorner(D_ineq[k].rows(), D_ineq[k].cols()) = D_ineq[k];
DD[k] = D_con[k].data();

// Equality constraints have both upper and lower bounds (which are the
// same). The inequality constraints have only lower bounds: we set it
// the same as the lower bound, but then mask out the inequalities
// constraints to make them unbounded.
d_con[k].setZero(d_eq[k].rows() + d_ineq[k].rows());
d_con[k] << d_eq[k], d_ineq[k];
llg[k] = d_con[k].data();
uug[k] = d_con[k].data();

upper_bound_mask[k].setZero(d_eq[k].rows() + d_ineq[k].rows());
upper_bound_mask[k].head(d_eq[k].rows()).setOnes();
d_ocp_qp_set_ug_mask(k, upper_bound_mask[k].data(), &qp_);
}

// === Unused ===
int** hidxbx = nullptr;
scalar_t** hlbx = nullptr;
Expand Down Expand Up @@ -533,9 +607,11 @@ void HpipmInterface::resize(OcpSize ocpSize) {

hpipm_status HpipmInterface::solve(const vector_t& x0, std::vector<VectorFunctionLinearApproximation>& dynamics,
std::vector<ScalarFunctionQuadraticApproximation>& cost,
std::vector<VectorFunctionLinearApproximation>* constraints, vector_array_t& stateTrajectory,
std::vector<VectorFunctionLinearApproximation>* constraints,
std::vector<VectorFunctionLinearApproximation>* ineqConstraints,
vector_array_t& stateTrajectory,
vector_array_t& inputTrajectory, bool verbose) {
return pImpl_->solve(x0, dynamics, cost, constraints, stateTrajectory, inputTrajectory, verbose);
return pImpl_->solve(x0, dynamics, cost, constraints, ineqConstraints, stateTrajectory, inputTrajectory, verbose);
}

std::vector<ScalarFunctionQuadraticApproximation> HpipmInterface::getRiccatiCostToGo(const VectorFunctionLinearApproximation& dynamics0,
Expand Down
8 changes: 7 additions & 1 deletion ocs2_sqp/hpipm_catkin/src/OcpSize.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept {

OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximation>& dynamics,
const std::vector<ScalarFunctionQuadraticApproximation>& cost,
const std::vector<VectorFunctionLinearApproximation>* constraints) {
const std::vector<VectorFunctionLinearApproximation>* constraints,
const std::vector<VectorFunctionLinearApproximation>* ineqConstraints) {
const int numStages = dynamics.size();

OcpSize problemSize(dynamics.size());
Expand All @@ -67,6 +68,11 @@ OcpSize extractSizesFromProblem(const std::vector<VectorFunctionLinearApproximat
problemSize.numIneqConstraints[k] = (*constraints)[k].f.size();
}
}
if (ineqConstraints != nullptr) {
for (int k = 0; k < numStages + 1; k++) {
problemSize.numIneqConstraints[k] += (*ineqConstraints)[k].f.size();
}
}

return problemSize;
}
Expand Down
Loading