From 905ab691d3848ce643df4fc39dbb3c251d2ade89 Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Thu, 5 May 2022 17:05:09 -0400 Subject: [PATCH 01/10] Initial work on hard inequality constraints. Hard inequality constraints added to HPIPM interface, with one unit test and basic testing using the legged robot example. --- .../oc_problem/OptimalControlProblem.h | 4 + .../src/oc_problem/OptimalControlProblem.cpp | 10 ++ .../ocs2_legged_robot/LeggedRobotInterface.h | 2 + .../src/LeggedRobotInterface.cpp | 15 ++- .../launch/legged_robot_sqp.launch | 2 +- .../include/hpipm_catkin/HpipmInterface.h | 7 +- .../include/hpipm_catkin/OcpSize.h | 6 +- ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp | 123 ++++++++++++++---- ocs2_sqp/hpipm_catkin/src/OcpSize.cpp | 17 ++- .../hpipm_catkin/test/testHpipmInterface.cpp | 74 +++++++++-- .../include/ocs2_sqp/MultipleShootingSolver.h | 4 +- .../ocs2_sqp/MultipleShootingTranscription.h | 9 +- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 17 ++- .../src/MultipleShootingTranscription.cpp | 23 ++++ 14 files changed, 262 insertions(+), 51 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index 2a50d98b7..b30706424 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -75,6 +75,10 @@ struct OptimalControlProblem { std::unique_ptr preJumpEqualityConstraintPtr; /** Final equality constraints */ std::unique_ptr finalEqualityConstraintPtr; + /** Inequality constraints **/ + std::unique_ptr inequalityConstraintPtr; + /** Box inequality constraints **/ + std::unique_ptr boxConstraintPtr; /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index cd5c45102..a0e5e3299 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,6 +50,9 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), + /* Inequality constraints */ + inequalityConstraintPtr(new StateInputConstraintCollection), + boxConstraintPtr(new StateInputConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputCostCollection), stateEqualityLagrangianPtr(new StateCostCollection), @@ -82,6 +85,9 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), + /* Inequality constraints */ + inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), + boxConstraintPtr(other.boxConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -130,6 +136,10 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); + /* Inequality constraints */ + inequalityConstraintPtr.swap(other.inequalityConstraintPtr); + boxConstraintPtr.swap(other.boxConstraintPtr); + /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index fa9c388e3..deff44519 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -95,6 +95,8 @@ class LeggedRobotInterface final : public RobotInterface { std::pair loadFrictionConeSettings(const std::string& taskFile, bool verbose) const; std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); + std::unique_ptr getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); std::unique_ptr getZeroForceConstraint(size_t contactPointIndex); std::unique_ptr getZeroVelocityConstraint(const EndEffectorKinematics& eeKinematics, size_t contactPointIndex, bool useAnalyticalGradients); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 29815cf88..9f0d7d115 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -174,8 +174,11 @@ 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, barrierPenaltyConfig)); + problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints)); @@ -321,6 +324,14 @@ std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint( return std::unique_ptr(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty))); } +std::unique_ptr LeggedRobotInterface::getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { + FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); + std::unique_ptr frictionConeConstraintPtr( + new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_)); + return frictionConeConstraintPtr; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch index 36f6fe62f..3a4a8d284 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch @@ -34,7 +34,7 @@ + output="screen" launch-prefix="gdb -ex run --args"/> diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index c876217c7..17232789e 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -83,9 +83,12 @@ class HpipmInterface { * NAN_SOL = NaN in computations; * INCONS_EQ = Unconsistent equality constraints; */ + // TODO this needs to be modified to accept inequality and box constraints hpipm_status solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, std::vector* constraints, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose = false); + std::vector* ineqConstraints, + std::vector* boxConstraints, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory, bool verbose = false); /** * Return the Riccati cost-to-go for the previously solved problem. @@ -127,4 +130,4 @@ class HpipmInterface { std::unique_ptr pImpl_; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h index 425644c90..d5f045c44 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -86,7 +86,9 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept; */ OcpSize extractSizesFromProblem(const std::vector& dynamics, const std::vector& cost, - const std::vector* constraints); + const std::vector* constraints, + const std::vector* ineqConstraints, + const std::vector* boxConstraints); } // namespace hpipm_interface -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp index 65408a455..e451ad7d2 100644 --- a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp @@ -31,6 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include + extern "C" { #include #include @@ -165,7 +167,9 @@ class HpipmInterface::Impl { hpipm_status solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, std::vector* constraints, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose) { + std::vector* ineqConstraints, + std::vector* boxConstraints, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory, bool verbose) { const int N = ocpSize_.numStages; verifySizes(x0, dynamics, cost, constraints); @@ -221,48 +225,110 @@ 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 CC(N + 1, nullptr); - std::vector DD(N + 1, nullptr); - std::vector llg(N + 1, nullptr); - std::vector uug(N + 1, nullptr); - std::vector boundData; // Declare at this scope to keep the data alive while HPIPM has the pointers + + // Equality constraints for each stage: + // C * x + D * u == b + std::vector C_eq(N + 1); + std::vector D_eq(N + 1); + std::vector 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 >= b + std::vector C_ineq(N + 1); + std::vector D_ineq(N + 1); + std::vector d_ineq(N + 1); + + if (ineqConstraints != nullptr) { + auto& constr = *ineqConstraints; + + // k = 0 + // No need to specify C_ineq[0] since initial state is given + 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; + } + } + + 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 C_con(N + 1); + std::vector D_con(N + 1); + std::vector d_con(N + 1); + std::vector upper_bound_mask(N + 1); + + // Raw data for each stage + std::vector CC(N + 1, nullptr); + std::vector DD(N + 1, nullptr); + std::vector llg(N + 1, nullptr); + std::vector uug(N + 1, nullptr); + + // Combine equality and inequality constraints and extract raw data + for (int k = 0; k <= N; k++) { + 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(); + + 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; @@ -533,9 +599,12 @@ void HpipmInterface::resize(OcpSize ocpSize) { hpipm_status HpipmInterface::solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, - std::vector* constraints, vector_array_t& stateTrajectory, + std::vector* constraints, + std::vector* ineqConstraints, + std::vector* boxConstraints, + 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, boxConstraints, stateTrajectory, inputTrajectory, verbose); } std::vector HpipmInterface::getRiccatiCostToGo(const VectorFunctionLinearApproximation& dynamics0, diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp index 53d5263fb..c946f5029 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp @@ -48,7 +48,9 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { OcpSize extractSizesFromProblem(const std::vector& dynamics, const std::vector& cost, - const std::vector* constraints) { + const std::vector* constraints, + const std::vector* ineqConstraints, + const std::vector* boxConstraints) { const int numStages = dynamics.size(); OcpSize problemSize(dynamics.size()); @@ -67,9 +69,20 @@ OcpSize extractSizesFromProblem(const std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -93,11 +93,11 @@ TEST(test_hpiphm_interface, solve_after_resize) { // Solve! std::vector xSol; std::vector uSol; - hpipmInterface.solve(x0, system, cost, nullptr, xSol, uSol, true); + hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); // Solve again! hpipmInterface.resize(ocpSize); - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -143,7 +143,7 @@ TEST(test_hpiphm_interface, knownSolution) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(xSolGiven[0], system, cost, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(xSolGiven[0], system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Check! @@ -151,7 +151,7 @@ TEST(test_hpiphm_interface, knownSolution) { ASSERT_TRUE(ocs2::isEqual(uSolGiven, uSol, 1e-9)); } -TEST(test_hpiphm_interface, with_constraints) { +TEST(test_hpiphm_interface, with_equality_constraints) { // Initialize without size ocs2::HpipmInterface hpipmInterface; @@ -186,7 +186,7 @@ TEST(test_hpiphm_interface, with_constraints) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, &constraints, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, &constraints, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -205,6 +205,62 @@ TEST(test_hpiphm_interface, with_constraints) { } } +TEST(test_hpiphm_interface, with_inequality_constraints) { + // Initialize without size + ocs2::HpipmInterface hpipmInterface; + + int nx = 3; + int nu = 2; + int nc = 1; + int N = 5; + + // Problem setup + ocs2::vector_t x0 = ocs2::vector_t::Random(nx); + std::vector system; + std::vector ineqConstraints; + std::vector cost; + for (int k = 0; k < N; k++) { + system.emplace_back(ocs2::getRandomDynamics(nx, nu)); + cost.emplace_back(ocs2::getRandomCost(nx, nu)); + ineqConstraints.emplace_back(ocs2::getRandomConstraints(nx, nu, nc)); + } + cost.emplace_back(ocs2::getRandomCost(nx, 0)); + ineqConstraints.emplace_back(ocs2::getRandomConstraints(nx, 0, nc)); + + // Resize Interface + ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + std::fill(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), nc); + + // Set one of the constraints to empty + ineqConstraints[1] = ocs2::VectorFunctionLinearApproximation(); + ocpSize.numIneqConstraints[1] = 0; + + hpipmInterface.resize(ocpSize); + + // Solve! + std::vector xSol; + std::vector uSol; + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, &ineqConstraints, nullptr, xSol, uSol, true); + ASSERT_EQ(status, hpipm_status::SUCCESS); + + // Initial condition + ASSERT_TRUE(xSol[0].isApprox(x0)); + + // Check dynamic feasibility + for (int k = 0; k < N; k++) { + ASSERT_TRUE(xSol[k + 1].isApprox(system[k].dfdx * xSol[k] + system[k].dfdu * uSol[k] + system[k].f, 1e-9)); + } + + // Check constraints + for (int k = 0; k < N; k++) { + if (ineqConstraints[k].f.size() > 0) { + ocs2::vector_t lhs = -ineqConstraints[k].f; + ocs2::vector_t rhs = ineqConstraints[k].dfdx * xSol[k] + ineqConstraints[k].dfdu * uSol[k]; + ASSERT_TRUE((lhs.array() <= rhs.array() + 1e-9).all()); + } + } +} + TEST(test_hpiphm_interface, noInputs) { // Initialize without size ocs2::HpipmInterface hpipmInterface; @@ -241,13 +297,13 @@ TEST(test_hpiphm_interface, noInputs) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; - const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr); + const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr, nullptr, nullptr); hpipmInterface.resize(ocpSize); // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(xSolGiven.front(), system, cost, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(xSolGiven.front(), system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Check! @@ -310,7 +366,7 @@ TEST(test_hpiphm_interface, retrieveRiccati) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Get Riccati info from hpipm interface diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 5a908e2c0..6e5422fc4 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -158,8 +158,10 @@ class MultipleShootingSolver : public SolverBase { // LQ approximation std::vector dynamics_; std::vector cost_; - std::vector constraints_; + std::vector constraints_; // equality constraints std::vector constraintsProjection_; + std::vector ineqConstraints_; + std::vector boxConstraints_; // Iteration performance log std::vector performanceIndeces_; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index 92985b915..aadac149f 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -45,7 +45,10 @@ struct Transcription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; + VectorFunctionLinearApproximation boxConstraints; VectorFunctionLinearApproximation constraintsProjection; + // TODO need to add ineq and box here too }; /** @@ -79,6 +82,8 @@ struct TerminalTranscription { PerformanceIndex performance; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; + VectorFunctionLinearApproximation boxConstraints; }; /** @@ -105,6 +110,8 @@ struct EventTranscription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; + VectorFunctionLinearApproximation boxConstraints; }; /** @@ -127,4 +134,4 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon const vector_t& x_next); } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 298f527cc..897ccbb63 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -250,11 +250,11 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu hpipm_status status; const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_, &ineqConstraints_, &boxConstraints_)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, &ineqConstraints_, &boxConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr, nullptr, nullptr)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, nullptr, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); } if (status != hpipm_status::SUCCESS) { @@ -358,6 +358,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec dynamics_.resize(N); cost_.resize(N + 1); constraints_.resize(N + 1); + ineqConstraints_.resize(N + 1); + boxConstraints_.resize(N + 1); constraintsProjection_.resize(N); std::atomic_int timeIndex{0}; @@ -376,6 +378,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); + ineqConstraints_[i] = std::move(result.ineqConstraints); + boxConstraints_[i] = std::move(result.boxConstraints); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node @@ -387,6 +391,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec dynamics_[i] = std::move(result.dynamics); cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); + ineqConstraints_[i] = std::move(result.ineqConstraints); + boxConstraints_[i] = std::move(result.boxConstraints); constraintsProjection_[i] = std::move(result.constraintsProjection); } @@ -399,6 +405,8 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec workerPerformance += result.performance; cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); + ineqConstraints_[i] = std::move(result.ineqConstraints); + boxConstraints_[i] = std::move(result.boxConstraints); } // Accumulate! Same worker might run multiple tasks @@ -470,6 +478,7 @@ scalar_t MultipleShootingSolver::trajectoryNorm(const vector_array_t& v) { } scalar_t MultipleShootingSolver::totalConstraintViolation(const PerformanceIndex& performance) const { + // TODO: adam: update this to include inequality violations return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); } diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 994a50c1d..ec8fb2b5d 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -46,6 +46,8 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP auto& performance = transcription.performance; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; + auto& boxConstraints = transcription.boxConstraints; auto& projection = transcription.constraintsProjection; // Dynamics @@ -81,6 +83,10 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP } } + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + ineqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + } + return transcription; } @@ -108,6 +114,14 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt } } + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + const vector_t ineqConstraints = optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); + if (ineqConstraints.size() > 0) { + // constraint is only a violation if negative + performance.equalityConstraintsSSE += dt * ineqConstraints.cwiseMin(0.0).squaredNorm(); + } + } + return performance; } @@ -117,6 +131,8 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont auto& performance = transcription.performance; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; + auto& boxConstraints = transcription.boxConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -125,6 +141,8 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont performance.cost = cost.f; constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + boxConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } @@ -148,7 +166,10 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; + auto& boxConstraints = transcription.boxConstraints; + // TODO: adam: modify request here? constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -163,6 +184,8 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro performance.cost = cost.f; constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + boxConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } From cbdb5e707f944c23b02cd6de3c4c164c19cde08c Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Thu, 5 May 2022 17:25:44 -0400 Subject: [PATCH 02/10] Remove box constraint API (for now). --- .../oc_problem/OptimalControlProblem.h | 4 +--- .../src/oc_problem/OptimalControlProblem.cpp | 3 --- .../include/hpipm_catkin/HpipmInterface.h | 3 +-- .../include/hpipm_catkin/OcpSize.h | 6 +++--- ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp | 19 ++++++++++++------- ocs2_sqp/hpipm_catkin/src/OcpSize.cpp | 9 +-------- .../hpipm_catkin/test/testHpipmInterface.cpp | 18 +++++++++--------- .../include/ocs2_sqp/MultipleShootingSolver.h | 1 - .../ocs2_sqp/MultipleShootingTranscription.h | 4 ---- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 12 ++++-------- .../src/MultipleShootingTranscription.cpp | 6 ------ 11 files changed, 31 insertions(+), 54 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index b30706424..baba62d24 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -75,10 +75,8 @@ struct OptimalControlProblem { std::unique_ptr preJumpEqualityConstraintPtr; /** Final equality constraints */ std::unique_ptr finalEqualityConstraintPtr; - /** Inequality constraints **/ + /** (Hard) inequality constraints **/ std::unique_ptr inequalityConstraintPtr; - /** Box inequality constraints **/ - std::unique_ptr boxConstraintPtr; /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index a0e5e3299..f50e64370 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -52,7 +52,6 @@ OptimalControlProblem::OptimalControlProblem() finalEqualityConstraintPtr(new StateConstraintCollection), /* Inequality constraints */ inequalityConstraintPtr(new StateInputConstraintCollection), - boxConstraintPtr(new StateInputConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputCostCollection), stateEqualityLagrangianPtr(new StateCostCollection), @@ -87,7 +86,6 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), /* Inequality constraints */ inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), - boxConstraintPtr(other.boxConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -138,7 +136,6 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { /* Inequality constraints */ inequalityConstraintPtr.swap(other.inequalityConstraintPtr); - boxConstraintPtr.swap(other.boxConstraintPtr); /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index 17232789e..2cc9a2f63 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -86,8 +86,7 @@ class HpipmInterface { // TODO this needs to be modified to accept inequality and box constraints hpipm_status solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, std::vector* constraints, - std::vector* ineqConstraints, - std::vector* boxConstraints, vector_array_t& stateTrajectory, + std::vector* ineqConstraints, vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose = false); /** diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h index d5f045c44..308077c4e 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -81,14 +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& dynamics, const std::vector& cost, const std::vector* constraints, - const std::vector* ineqConstraints, - const std::vector* boxConstraints); + const std::vector* ineqConstraints); } // namespace hpipm_interface } // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp index e451ad7d2..c4324f281 100644 --- a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp @@ -147,7 +147,8 @@ class HpipmInterface::Impl { void verifySizes(const vector_t& x0, std::vector& dynamics, std::vector& cost, - std::vector* constraints) const { + std::vector* constraints, + std::vector* 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."); @@ -158,7 +159,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."); } } @@ -167,11 +174,10 @@ class HpipmInterface::Impl { hpipm_status solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, std::vector* constraints, - std::vector* ineqConstraints, - std::vector* boxConstraints, vector_array_t& stateTrajectory, + std::vector* 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 AA(N, nullptr); @@ -601,10 +607,9 @@ hpipm_status HpipmInterface::solve(const vector_t& x0, std::vector& cost, std::vector* constraints, std::vector* ineqConstraints, - std::vector* boxConstraints, vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose) { - return pImpl_->solve(x0, dynamics, cost, constraints, ineqConstraints, boxConstraints, stateTrajectory, inputTrajectory, verbose); + return pImpl_->solve(x0, dynamics, cost, constraints, ineqConstraints, stateTrajectory, inputTrajectory, verbose); } std::vector HpipmInterface::getRiccatiCostToGo(const VectorFunctionLinearApproximation& dynamics0, diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp index c946f5029..9c9ed8077 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp @@ -49,8 +49,7 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { OcpSize extractSizesFromProblem(const std::vector& dynamics, const std::vector& cost, const std::vector* constraints, - const std::vector* ineqConstraints, - const std::vector* boxConstraints) { + const std::vector* ineqConstraints) { const int numStages = dynamics.size(); OcpSize problemSize(dynamics.size()); @@ -74,12 +73,6 @@ OcpSize extractSizesFromProblem(const std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -93,11 +93,11 @@ TEST(test_hpiphm_interface, solve_after_resize) { // Solve! std::vector xSol; std::vector uSol; - hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + hpipmInterface.solve(x0, system, cost, nullptr, nullptr, xSol, uSol, true); // Solve again! hpipmInterface.resize(ocpSize); - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -143,7 +143,7 @@ TEST(test_hpiphm_interface, knownSolution) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(xSolGiven[0], system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(xSolGiven[0], system, cost, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Check! @@ -186,7 +186,7 @@ TEST(test_hpiphm_interface, with_equality_constraints) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, &constraints, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, &constraints, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -240,7 +240,7 @@ TEST(test_hpiphm_interface, with_inequality_constraints) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, &ineqConstraints, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, &ineqConstraints, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Initial condition @@ -297,13 +297,13 @@ TEST(test_hpiphm_interface, noInputs) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; - const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr, nullptr, nullptr); + const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr, nullptr); hpipmInterface.resize(ocpSize); // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(xSolGiven.front(), system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(xSolGiven.front(), system, cost, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Check! @@ -366,7 +366,7 @@ TEST(test_hpiphm_interface, retrieveRiccati) { // Solve! std::vector xSol; std::vector uSol; - const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, nullptr, xSol, uSol, true); + const auto status = hpipmInterface.solve(x0, system, cost, nullptr, nullptr, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Get Riccati info from hpipm interface diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 6e5422fc4..52a09cdb9 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -161,7 +161,6 @@ class MultipleShootingSolver : public SolverBase { std::vector constraints_; // equality constraints std::vector constraintsProjection_; std::vector ineqConstraints_; - std::vector boxConstraints_; // Iteration performance log std::vector performanceIndeces_; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h index aadac149f..413c72a27 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -46,9 +46,7 @@ struct Transcription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; VectorFunctionLinearApproximation ineqConstraints; - VectorFunctionLinearApproximation boxConstraints; VectorFunctionLinearApproximation constraintsProjection; - // TODO need to add ineq and box here too }; /** @@ -83,7 +81,6 @@ struct TerminalTranscription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; VectorFunctionLinearApproximation ineqConstraints; - VectorFunctionLinearApproximation boxConstraints; }; /** @@ -111,7 +108,6 @@ struct EventTranscription { ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; VectorFunctionLinearApproximation ineqConstraints; - VectorFunctionLinearApproximation boxConstraints; }; /** diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 897ccbb63..cfd42f181 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -250,11 +250,11 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu hpipm_status status; const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_, &ineqConstraints_, &boxConstraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, &ineqConstraints_, &boxConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_, &ineqConstraints_)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, &ineqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr, nullptr, nullptr)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, nullptr, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr, nullptr)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); } if (status != hpipm_status::SUCCESS) { @@ -359,7 +359,6 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_.resize(N + 1); constraints_.resize(N + 1); ineqConstraints_.resize(N + 1); - boxConstraints_.resize(N + 1); constraintsProjection_.resize(N); std::atomic_int timeIndex{0}; @@ -379,7 +378,6 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); ineqConstraints_[i] = std::move(result.ineqConstraints); - boxConstraints_[i] = std::move(result.boxConstraints); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node @@ -392,7 +390,6 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); ineqConstraints_[i] = std::move(result.ineqConstraints); - boxConstraints_[i] = std::move(result.boxConstraints); constraintsProjection_[i] = std::move(result.constraintsProjection); } @@ -406,7 +403,6 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec cost_[i] = std::move(result.cost); constraints_[i] = std::move(result.constraints); ineqConstraints_[i] = std::move(result.ineqConstraints); - boxConstraints_[i] = std::move(result.boxConstraints); } // Accumulate! Same worker might run multiple tasks diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index ec8fb2b5d..6844c2205 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -47,7 +47,6 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP auto& cost = transcription.cost; auto& constraints = transcription.constraints; auto& ineqConstraints = transcription.ineqConstraints; - auto& boxConstraints = transcription.boxConstraints; auto& projection = transcription.constraintsProjection; // Dynamics @@ -132,7 +131,6 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont auto& cost = transcription.cost; auto& constraints = transcription.constraints; auto& ineqConstraints = transcription.ineqConstraints; - auto& boxConstraints = transcription.boxConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -142,7 +140,6 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); - boxConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } @@ -167,9 +164,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& cost = transcription.cost; auto& constraints = transcription.constraints; auto& ineqConstraints = transcription.ineqConstraints; - auto& boxConstraints = transcription.boxConstraints; - // TODO: adam: modify request here? constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -185,7 +180,6 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); - boxConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } From 2a8577bf96981653faf133d57f6569c76749854c Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Thu, 5 May 2022 18:00:23 -0400 Subject: [PATCH 03/10] Separate inequalityConstraintsSSE from equalityConstraintsSSE. --- .../include/ocs2_oc/oc_solver/PerformanceIndex.h | 12 ++++++++++++ ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp | 3 +-- .../ocs2_sqp/src/MultipleShootingTranscription.cpp | 13 ++++++++++--- ocs2_sqp/ocs2_sqp/test/testTranscription.cpp | 4 ++-- 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index ee45978ee..90cd912ee 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -56,6 +56,15 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; + /** Sum of Squared Error (SSE) of inequality constraints: + * - Final: squared norm of violation in state equality constraints + * - PreJumps: sum of squared norm of violation in state equality constraints + * - Intermediates: Integral of squared norm violation in state/state-input equality constraints + * + * 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 @@ -76,6 +85,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; @@ -93,6 +103,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); } @@ -109,6 +120,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; diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index cfd42f181..d05dfe8c9 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -474,8 +474,7 @@ scalar_t MultipleShootingSolver::trajectoryNorm(const vector_array_t& v) { } scalar_t MultipleShootingSolver::totalConstraintViolation(const PerformanceIndex& performance) const { - // TODO: adam: update this to include inequality violations - return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); + return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE + performance.inequalityConstraintsSSE); } multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIndex& baseline, diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index 6844c2205..ce048a57b 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -64,7 +64,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP cost *= dt; performance.cost = cost.f; - // Constraints + // Equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 constraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); @@ -82,8 +82,14 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP } } + // Inequality constraints if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} >= 0 ineqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + if (ineqConstraints.f.size() > 0) { + // constraint is only a violation if negative + performance.inequalityConstraintsSSE = dt * ineqConstraints.f.cwiseMin(0.0).squaredNorm(); + } } return transcription; @@ -105,7 +111,7 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt // Costs performance.cost = dt * computeCost(optimalControlProblem, t, x, u); - // Constraints + // Equality constraints if (!optimalControlProblem.equalityConstraintPtr->empty()) { const vector_t constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); if (constraints.size() > 0) { @@ -113,11 +119,12 @@ PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& opt } } + // Inequality constraints if (!optimalControlProblem.inequalityConstraintPtr->empty()) { const vector_t ineqConstraints = optimalControlProblem.inequalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); if (ineqConstraints.size() > 0) { // constraint is only a violation if negative - performance.equalityConstraintsSSE += dt * ineqConstraints.cwiseMin(0.0).squaredNorm(); + performance.inequalityConstraintsSSE += dt * ineqConstraints.cwiseMin(0.0).squaredNorm(); } } diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp index f3c6fdce3..9ee7bee57 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp @@ -38,8 +38,8 @@ namespace { /** Helper to compare if two performance indices are identical */ bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { return lhs.merit == rhs.merit && lhs.cost == rhs.cost && lhs.dynamicsViolationSSE == rhs.dynamicsViolationSSE && - lhs.equalityConstraintsSSE == rhs.equalityConstraintsSSE && lhs.equalityLagrangian == rhs.equalityLagrangian && - lhs.inequalityLagrangian == rhs.inequalityLagrangian; + lhs.equalityConstraintsSSE == rhs.equalityConstraintsSSE && lhs.inequalityConstraintsSSE == rhs.inequalityConstraintsSSE && + lhs.equalityLagrangian == rhs.equalityLagrangian && lhs.inequalityLagrangian == rhs.inequalityLagrangian; } } // namespace From 8455ccde292c515cd94cfedc8ed37766b2b3dbc9 Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Fri, 6 May 2022 13:21:07 -0400 Subject: [PATCH 04/10] Clean up: formatting, comments, etc. --- ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h | 6 +----- .../include/ocs2_legged_robot/LeggedRobotInterface.h | 3 +-- .../ocs2_legged_robot/src/LeggedRobotInterface.cpp | 7 ++----- .../launch/legged_robot_sqp.launch | 2 +- .../include/hpipm_catkin/HpipmInterface.h | 5 +++-- ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h | 1 + ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp | 12 +++++++----- ocs2_sqp/hpipm_catkin/src/OcpSize.cpp | 1 + .../ocs2_sqp/src/MultipleShootingTranscription.cpp | 1 + 9 files changed, 18 insertions(+), 20 deletions(-) diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index 90cd912ee..2137acb8d 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -56,11 +56,7 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; - /** Sum of Squared Error (SSE) of inequality constraints: - * - Final: squared norm of violation in state equality constraints - * - PreJumps: sum of squared norm of violation in state equality constraints - * - Intermediates: Integral of squared norm violation in state/state-input equality constraints - * + /** 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; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index deff44519..2e95f520d 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -95,8 +95,7 @@ class LeggedRobotInterface final : public RobotInterface { std::pair loadFrictionConeSettings(const std::string& taskFile, bool verbose) const; std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); - std::unique_ptr getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); + std::unique_ptr getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient); std::unique_ptr getZeroForceConstraint(size_t contactPointIndex); std::unique_ptr getZeroVelocityConstraint(const EndEffectorKinematics& eeKinematics, size_t contactPointIndex, bool useAnalyticalGradients); diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 9f0d7d115..d7bff7ab4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -176,9 +176,7 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile // problemPtr_->softConstraintPtr->add(footName + "_frictionCone", // getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); - problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", - getHardFrictionConeConstraint(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)); @@ -324,8 +322,7 @@ std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint( return std::unique_ptr(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty))); } -std::unique_ptr LeggedRobotInterface::getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { +std::unique_ptr LeggedRobotInterface::getHardFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); std::unique_ptr frictionConeConstraintPtr( new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_)); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch index 3a4a8d284..36f6fe62f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch @@ -34,7 +34,7 @@ + output="screen" launch-prefix=""/> diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index 2cc9a2f63..56d42c1e6 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -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. @@ -83,7 +84,6 @@ class HpipmInterface { * NAN_SOL = NaN in computations; * INCONS_EQ = Unconsistent equality constraints; */ - // TODO this needs to be modified to accept inequality and box constraints hpipm_status solve(const vector_t& x0, std::vector& dynamics, std::vector& cost, std::vector* constraints, std::vector* ineqConstraints, vector_array_t& stateTrajectory, @@ -130,3 +130,4 @@ class HpipmInterface { }; } // namespace ocs2 + diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h index 308077c4e..2dd194daa 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -92,3 +92,4 @@ OcpSize extractSizesFromProblem(const std::vector -#include - extern "C" { #include #include @@ -233,7 +231,8 @@ class HpipmInterface::Impl { // === Constraints === // Equality constraints for each stage: - // C * x + D * u == b + // C * x + D * u == d + // Declare at this scope to keep data alive while HPIPM has the pointers std::vector C_eq(N + 1); std::vector D_eq(N + 1); std::vector d_eq(N + 1); @@ -266,7 +265,7 @@ class HpipmInterface::Impl { } // Inequality constraints for each stage: - // C * x + D * u >= b + // C * x + D * u >= d std::vector C_ineq(N + 1); std::vector D_ineq(N + 1); std::vector d_ineq(N + 1); @@ -275,7 +274,6 @@ class HpipmInterface::Impl { auto& constr = *ineqConstraints; // k = 0 - // No need to specify C_ineq[0] since initial state is given if (constr[0].f.size() > 0) { d_ineq[0] = -constr[0].f; d_ineq[0].noalias() -= constr[0].dfdx * x0; @@ -291,6 +289,7 @@ class HpipmInterface::Impl { } } + // k = N if (constr[N].f.size() > 0) { d_ineq[N] = -constr[N].f; C_ineq[N] = constr[N].dfdx; @@ -311,11 +310,14 @@ class HpipmInterface::Impl { // Combine equality and inequality constraints and extract raw data for (int k = 0; k <= N; k++) { + // Make block diagonal C = [[C_eq, 0 ], + // [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]; diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp index 9c9ed8077..d1c0dfd74 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp @@ -79,3 +79,4 @@ OcpSize extractSizesFromProblem(const std::vector Date: Fri, 6 May 2022 13:27:03 -0400 Subject: [PATCH 05/10] Fix line endings. --- ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h | 3 +-- ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h | 3 +-- ocs2_sqp/hpipm_catkin/src/OcpSize.cpp | 3 +-- .../ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h | 2 +- ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp | 3 +-- 5 files changed, 5 insertions(+), 9 deletions(-) diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index 56d42c1e6..d87c2811a 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -129,5 +129,4 @@ class HpipmInterface { std::unique_ptr pImpl_; }; -} // namespace ocs2 - +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h index 2dd194daa..55123c42d 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -91,5 +91,4 @@ OcpSize extractSizesFromProblem(const std::vector* ineqConstraints); } // namespace hpipm_interface -} // namespace ocs2 - +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp index d1c0dfd74..65718df9a 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp @@ -78,5 +78,4 @@ OcpSize extractSizesFromProblem(const std::vector Date: Fri, 6 May 2022 13:28:25 -0400 Subject: [PATCH 06/10] Fix line endings take 2. --- ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index ed8f7d401..ce048a57b 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -207,4 +207,4 @@ PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalCon } } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 From 8047e3ab763d1048cd88c9da5fffda4067e39b9b Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Tue, 19 Jul 2022 16:43:37 -0400 Subject: [PATCH 07/10] Fix arrangement of constraint matrices, reduce data copying. --- ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp | 168 +++++++++---------- 1 file changed, 76 insertions(+), 92 deletions(-) diff --git a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp index 0dec4e669..5a5452439 100644 --- a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp @@ -229,112 +229,96 @@ class HpipmInterface::Impl { qq[N] = cost[N].dfdx.data(); // === Constraints === + std::vector C(N + 1); + std::vector D(N + 1); + std::vector d(N + 1); + std::vector upper_bound_mask(N + 1); - // 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 C_eq(N + 1); - std::vector D_eq(N + 1); - std::vector d_eq(N + 1); - - if (constraints != nullptr) { - auto& constr = *constraints; - - // k = 0 - // No need to specify C_eq[0] since initial state is given - if (constr[0].f.size() > 0) { - d_eq[0] = -constr[0].f; - d_eq[0].noalias() -= constr[0].dfdx * x0; - D_eq[0] = constr[0].dfdu; - } + // Raw data for each stage + std::vector CC(N + 1, nullptr); + std::vector DD(N + 1, nullptr); + std::vector llg(N + 1, nullptr); + std::vector uug(N + 1, nullptr); - // k = 1 -> (N-1) - for (int k = 1; k < N; k++) { - if (constr[k].f.size() > 0) { - d_eq[k] = -constr[k].f; - C_eq[k] = constr[k].dfdx; - D_eq[k] = constr[k].dfdu; + bool hasEqualityConstraints = (constraints != nullptr); + bool hasInequalityConstraints = (ineqConstraints != nullptr); + + // If we have both constraints, we need to copy the data in order to concatentate it + // Equality constraints are of the form + // C * x + D * u == d, + // inequalities are + // C * x + D * u >= d. + if (hasEqualityConstraints && hasInequalityConstraints) { + auto& eqConstr = *constraints; + auto& ineqConstr = *ineqConstraints; + + for (int k = 0; k <= N; k++) { + size_t ne = eqConstr[k].f.size(); + size_t ni = ineqConstr[k].f.size(); + size_t n = ne + ni; + if (n == 0) { + continue; } - } - // k = N, no inputs - if (constr[N].f.size() > 0) { - d_eq[N] = -constr[N].f; - C_eq[N] = constr[N].dfdx; - } - } + d[k].resize(n); + d[k] << -eqConstr[k].f, -ineqConstr[k].f; + if (k == 0) { + d[k].head(ne).noalias() -= eqConstr[k].dfdx * x0; + d[k].tail(ni).noalias() -= ineqConstr[k].dfdx * x0; + } + llg[k] = d[k].data(); + uug[k] = d[k].data(); - // Inequality constraints for each stage: - // C * x + D * u >= d - std::vector C_ineq(N + 1); - std::vector D_ineq(N + 1); - std::vector d_ineq(N + 1); + // Mask out upper bounds for the inequality constraints + upper_bound_mask[k].setZero(n); + upper_bound_mask[k].head(ne).setOnes(); + d_ocp_qp_set_ug_mask(k, upper_bound_mask[k].data(), &qp_); - if (ineqConstraints != nullptr) { - auto& constr = *ineqConstraints; + // No need to give CC[0] since initial state is given + if (k > 0) { + C[k].resize(n, eqConstr[k].dfdx.cols()); + C[k] << eqConstr[k].dfdx, ineqConstr[k].dfdx; + CC[k] = C[k].data(); + } - // 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; + if (k < N) { + D[k].resize(n, eqConstr[k].dfdu.cols()); + D[k] << eqConstr[k].dfdu, ineqConstr[k].dfdu; + DD[k] = D[k].data(); + } } - // 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; + // If we only have one or the other, we can directly take pointers to data + } else if (hasEqualityConstraints || hasInequalityConstraints) { + auto& constr = hasEqualityConstraints ? *constraints : *ineqConstraints; + + for (int k = 0; k <= N; k++) { + if (constr[k].f.size() == 0) { + continue; } - } - // k = N - if (constr[N].f.size() > 0) { - d_ineq[N] = -constr[N].f; - C_ineq[N] = constr[N].dfdx; - } - } + d[k] = -constr[k].f; + if (k == 0) { + d[k].noalias() -= constr[k].dfdx * x0; + } + llg[k] = d[k].data(); + uug[k] = d[k].data(); - // Combined equality and inequality constraints for each stage - std::vector C_con(N + 1); - std::vector D_con(N + 1); - std::vector d_con(N + 1); - std::vector upper_bound_mask(N + 1); + // If the constraints are inequalities, mask out the upper bound + if (hasInequalityConstraints) { + upper_bound_mask[k].setZero(d[k].rows()); + d_ocp_qp_set_ug_mask(k, upper_bound_mask[k].data(), &qp_); + } - // Raw data for each stage - std::vector CC(N + 1, nullptr); - std::vector DD(N + 1, nullptr); - std::vector llg(N + 1, nullptr); - std::vector uug(N + 1, nullptr); + // No need to give CC[0] since initial state is given + if (k > 0) { + CC[k] = constr[k].dfdx.data(); + } - // Combine equality and inequality constraints and extract raw data - for (int k = 0; k <= N; k++) { - // Make block diagonal C = [[C_eq, 0 ], - // [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_); + if (k < N) { + DD[k] = constr[k].dfdu.data(); + } + } } // === Unused === From 0f2cc27bd6d5cab385ca38dfb690dafa91cbfefa Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Tue, 19 Jul 2022 20:02:38 -0400 Subject: [PATCH 08/10] Handle implications of equality constraint projection on the inequality constraints. --- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 18 +++++++++++++----- .../src/MultipleShootingTranscription.cpp | 7 ++++++- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index d05dfe8c9..6bf081279 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp @@ -248,15 +248,23 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu auto& deltaXSol = solution.deltaXSol; auto& deltaUSol = solution.deltaUSol; hpipm_status status; + + std::vector* eqConstraintPtr = nullptr; + std::vector* ineqConstraintPtr = nullptr; + const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_, &ineqConstraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, &ineqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); - } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr, nullptr)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); + eqConstraintPtr = &constraints_; } + const bool hasInequalityConstraints = !ocpDefinitions_.front().inequalityConstraintPtr->empty(); + if (hasInequalityConstraints) { + ineqConstraintPtr = &ineqConstraints_; + } + + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_, &ineqConstraints_)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, &ineqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); + if (status != hpipm_status::SUCCESS) { throw std::runtime_error("[MultipleShootingSolver] Failed to solve QP"); } diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp index ce048a57b..1a93c1de9 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -87,8 +87,13 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} >= 0 ineqConstraints = optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); if (ineqConstraints.f.size() > 0) { - // constraint is only a violation if negative + // Constraint is only a violation if negative performance.inequalityConstraintsSSE = dt * ineqConstraints.f.cwiseMin(0.0).squaredNorm(); + + // Adapt if using projection + if (projectStateInputEqualityConstraints) { + changeOfInputVariables(ineqConstraints, projection.dfdu, projection.dfdx, projection.f); + } } } From 0e28f67697e8746887e4b18dcdb814b193bd507f Mon Sep 17 00:00:00 2001 From: Adam Heins Date: Wed, 20 Jul 2022 14:50:28 -0400 Subject: [PATCH 09/10] Prelimary interface for HPIPM slack variables. 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. --- .../hpipm_catkin/HpipmInterfaceSettings.h | 13 ++++- .../include/hpipm_catkin/OcpSize.h | 6 +- ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp | 58 ++++++++++++++++--- ocs2_sqp/hpipm_catkin/src/OcpSize.cpp | 12 +++- .../hpipm_catkin/test/testHpipmInterface.cpp | 2 +- .../ocs2_sqp/src/MultipleShootingSolver.cpp | 4 +- 6 files changed, 79 insertions(+), 16 deletions(-) diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterfaceSettings.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterfaceSettings.h index fa32f205a..dfb137241 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterfaceSettings.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterfaceSettings.h @@ -54,9 +54,20 @@ struct Settings { int warm_start = 0; int pred_corr = 1; int ric_alg = 0; // square root ricatti recursion + + // Slacks - soften constraints + // TODO we probably want more control over which constraints get slacks, + // perhaps in its own structure + bool use_slack = false; + scalar_t slack_upper_L2_penalty = 1e2; + scalar_t slack_lower_L2_penalty = 1e2; + scalar_t slack_upper_L1_penalty = 0; + scalar_t slack_lower_L1_penalty = 0; + scalar_t slack_upper_low_bound = 0; + scalar_t slack_lower_low_bound = 0; }; std::ostream& operator<<(std::ostream& stream, const Settings& settings); } // namespace hpipm_interface -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h index 55123c42d..5ebf6ac98 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -83,12 +83,14 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept; * @param cost : Quadratic approximation of the cost. * @param constraints : Linearized approximation of equality constraints, all constraints are mapped to inequality constraints in HPIPM. * @param ineqConstraints : Linearized approximation of inequality constraints. + * @param useSlack : True if slack variables should be used. * @return Derived sizes */ OcpSize extractSizesFromProblem(const std::vector& dynamics, const std::vector& cost, const std::vector* constraints, - const std::vector* ineqConstraints); + const std::vector* ineqConstraints, + bool useSlack); } // namespace hpipm_interface -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp index 5a5452439..1f25e5c3c 100644 --- a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp @@ -321,6 +321,55 @@ class HpipmInterface::Impl { } } + // === Slacks === + // L2 slack penalties - only diagonal is stored + std::vector Zl(N + 1); + std::vector Zu(N + 1); + + // L1 slack penalties + std::vector zl(N + 1); + std::vector zu(N + 1); + + // Lower bounds on lower and upper slacks + std::vector sl(N + 1); + std::vector su(N + 1); + + // Slack index variables: used to control which constraints we actually + // want to apply the slacks to. + std::vector idxs(N + 1); + + // Raw slack data + std::vector ZZl(N + 1, nullptr); + std::vector ZZu(N + 1, nullptr); + std::vector zzu(N + 1, nullptr); + std::vector zzl(N + 1, nullptr); + std::vector lls(N + 1, nullptr); + std::vector lus(N + 1, nullptr); + std::vector iidxs(N + 1, nullptr); + + if (settings_.use_slack) { + for (int k = 0; k <= N; k++) { + size_t n = ocpSize_.numIneqSlack[k]; + Zl[k].setConstant(n, settings_.slack_lower_L2_penalty); + Zu[k].setConstant(n, settings_.slack_upper_L2_penalty); + zl[k].setConstant(n, settings_.slack_lower_L1_penalty); + zu[k].setConstant(n, settings_.slack_upper_L1_penalty); + sl[k].setConstant(n, settings_.slack_lower_low_bound); + su[k].setConstant(n, settings_.slack_upper_low_bound); + + // set indices to [0, ..., n - 1] + idxs[k].setLinSpaced(n, 0, n - 1); + + ZZl[k] = Zl[k].data(); + ZZu[k] = Zu[k].data(); + zzl[k] = zl[k].data(); + zzu[k] = zu[k].data(); + lls[k] = sl[k].data(); + lus[k] = su[k].data(); + iidxs[k] = idxs[k].data(); + } + } + // === Unused === int** hidxbx = nullptr; scalar_t** hlbx = nullptr; @@ -328,17 +377,10 @@ class HpipmInterface::Impl { int** hidxbu = nullptr; scalar_t** hlbu = nullptr; scalar_t** hubu = nullptr; - scalar_t** hZl = nullptr; - scalar_t** hZu = nullptr; - scalar_t** hzl = nullptr; - scalar_t** hzu = nullptr; - int** hidxs = nullptr; - scalar_t** hlls = nullptr; - scalar_t** hlus = nullptr; // === Set and solve === d_ocp_qp_set_all(AA.data(), BB.data(), bb.data(), QQ.data(), SS.data(), RR.data(), qq.data(), rr.data(), hidxbx, hlbx, hubx, hidxbu, - hlbu, hubu, CC.data(), DD.data(), llg.data(), uug.data(), hZl, hZu, hzl, hzu, hidxs, hlls, hlus, &qp_); + hlbu, hubu, CC.data(), DD.data(), llg.data(), uug.data(), ZZl.data(), ZZu.data(), zzl.data(), zzu.data(), iidxs.data(), lls.data(), lus.data(), &qp_); d_ocp_qp_ipm_solve(&qp_, &qpSol_, &arg_, &workspace_); if (verbose) { diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp index 65718df9a..2eed9adf5 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp @@ -49,7 +49,8 @@ bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { OcpSize extractSizesFromProblem(const std::vector& dynamics, const std::vector& cost, const std::vector* constraints, - const std::vector* ineqConstraints) { + const std::vector* ineqConstraints, + bool useSlack) { const int numStages = dynamics.size(); OcpSize problemSize(dynamics.size()); @@ -74,8 +75,15 @@ OcpSize extractSizesFromProblem(const std::vector Date: Wed, 20 Jul 2022 14:51:46 -0400 Subject: [PATCH 10/10] Add new tests for inequality constraints. * 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. --- .../ocs2_oc/test/circular_kinematics.h | 44 ++++ ocs2_sqp/ocs2_sqp/CMakeLists.txt | 3 +- .../ocs2_sqp/test/testCircularKinematics.cpp | 73 ++----- ocs2_sqp/ocs2_sqp/test/testConstrained.cpp | 199 ++++++++++++++++++ 4 files changed, 267 insertions(+), 52 deletions(-) create mode 100644 ocs2_sqp/ocs2_sqp/test/testConstrained.cpp diff --git a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h index e5f7e159b..72ba370fe 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h +++ b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h @@ -117,6 +117,39 @@ class CircularKinematicsConstraints final : public StateInputConstraint { } }; +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/** + * Adds a constraint on the state, such that the first coordinate x(0) >= 0.75, which causes the + * trajectory to deviate from the unconstrained version. + */ +class CircularKinematicsInequalityConstraints final : public StateInputConstraint { + public: + CircularKinematicsInequalityConstraints() : StateInputConstraint(ConstraintOrder::Linear) {} + ~CircularKinematicsInequalityConstraints() override = default; + + CircularKinematicsInequalityConstraints* clone() const override { return new CircularKinematicsInequalityConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 1; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(1); + e << x(0) - 0.75; + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e; + e.f = getValue(t, x, u, preComp); + e.dfdx.setZero(1, 2); + e.dfdx(0, 0) = 1; + e.dfdu.setZero(1, 2); + return e; + } +}; + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -136,4 +169,15 @@ inline OptimalControlProblem createCircularKinematicsProblem(const std::string& return problem; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +inline OptimalControlProblem createCircularKinematicsProblemWithInequality(const std::string& libraryFolder) { + // Add an inequality constraint to the problem + OptimalControlProblem problem = createCircularKinematicsProblem(libraryFolder); + std::unique_ptr constraint(new CircularKinematicsInequalityConstraints()); + problem.inequalityConstraintPtr->add("ineqConstraint", std::move(constraint)); + return problem; +} + } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 5a672b97b..d01f78697 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -102,6 +102,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp + test/testConstrained.cpp test/testDiscretization.cpp test/testProjection.cpp test/testSwitchedProblem.cpp @@ -113,4 +114,4 @@ target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES} gtest_main -) \ No newline at end of file +) diff --git a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp index 05e1d9897..88129037f 100644 --- a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp @@ -35,10 +35,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -TEST(test_circular_kinematics, solve_projected_EqConstraints) { - // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/sqp_test_generated"); +void circular_kinematics_test(const ocs2::OptimalControlProblem& problem, bool useProjection) { // Initializer ocs2::DefaultInitializer zeroInitializer(2); @@ -46,12 +44,13 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { ocs2::multiple_shooting::Settings settings; settings.dt = 0.01; settings.sqpIteration = 20; - settings.projectStateInputEqualityConstraints = true; + settings.projectStateInputEqualityConstraints = useProjection; settings.useFeedbackPolicy = true; settings.printSolverStatistics = true; settings.printSolverStatus = true; settings.printLinesearch = true; settings.nThreads = 1; + settings.hpipmSettings.use_slack = true; // Additional problem definitions const ocs2::scalar_t startTime = 0.0; @@ -78,6 +77,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { const auto performance = solver.getPerformanceIndeces(); ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + ASSERT_LT(performance.inequalityConstraintsSSE, 1e-6); // Check feedback controller for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { @@ -89,55 +89,26 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { } } -TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { - // optimal control problem +TEST(test_circular_kinematics, solve_projected_EqConstraints) { + const bool useProjection = true; ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/sqp_test_generated"); + circular_kinematics_test(problem, useProjection); +} - // Initializer - ocs2::DefaultInitializer zeroInitializer(2); - - // Solver settings - ocs2::multiple_shooting::Settings settings; - settings.dt = 0.01; - settings.sqpIteration = 20; - settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities - settings.useFeedbackPolicy = true; - settings.printSolverStatistics = true; - settings.printSolverStatus = true; - settings.printLinesearch = true; - - // Additional problem definitions - const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t finalTime = 1.0; - const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 - - // Solve - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); - solver.run(startTime, initState, finalTime); - - // Inspect solution - const auto primalSolution = solver.primalSolution(finalTime); - for (int i = 0; i < primalSolution.timeTrajectory_.size(); i++) { - std::cout << "time: " << primalSolution.timeTrajectory_[i] << "\t state: " << primalSolution.stateTrajectory_[i].transpose() - << "\t input: " << primalSolution.inputTrajectory_[i].transpose() << std::endl; - } - - // Check initial condition - ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); - ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); - ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); +TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { + const bool useProjection = false; + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/sqp_test_generated"); + circular_kinematics_test(problem, useProjection); +} - // Check constraint satisfaction. - const auto performance = solver.getPerformanceIndeces(); - ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); - ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); +TEST(test_circular_kinematics, solve_projected_EqConstraints_with_IneqConstraints) { + const bool useProjection = true; + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblemWithInequality("/tmp/sqp_test_generated"); + circular_kinematics_test(problem, useProjection); +} - // Check feedback controller - for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { - const auto t = primalSolution.timeTrajectory_[i]; - const auto& x = primalSolution.stateTrajectory_[i]; - const auto& u = primalSolution.inputTrajectory_[i]; - // Feed forward part - ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); - } +TEST(test_circular_kinematics, solve_EqConstraints_with_IneqConstraints) { + const bool useProjection = false; + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblemWithInequality("/tmp/sqp_test_generated"); + circular_kinematics_test(problem, useProjection); } diff --git a/ocs2_sqp/ocs2_sqp/test/testConstrained.cpp b/ocs2_sqp/ocs2_sqp/test/testConstrained.cpp new file mode 100644 index 000000000..eee6b4cd2 --- /dev/null +++ b/ocs2_sqp/ocs2_sqp/test/testConstrained.cpp @@ -0,0 +1,199 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +******************************************************************************/ + +#include + +#include "ocs2_sqp/MultipleShootingSolver.h" + +#include + +#include +#include + +namespace ocs2 { +namespace { + +std::pair> solveWithFeedbackSetting( + bool feedback, bool addConstraint, const VectorFunctionLinearApproximation& dynamicsMatrices, + const ScalarFunctionQuadraticApproximation& costMatrices, + const VectorFunctionLinearApproximation& constraintMatrices) { + int n = dynamicsMatrices.dfdu.rows(); + int m = dynamicsMatrices.dfdu.cols(); + + ocs2::OptimalControlProblem problem; + + // System + problem.dynamicsPtr = getOcs2Dynamics(dynamicsMatrices); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(costMatrices)); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(costMatrices)); + + // Reference Managaer + ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); + std::shared_ptr referenceManagerPtr(new ReferenceManager(targetTrajectories)); + + problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); + + // Constraint + if (addConstraint) { + problem.inequalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(constraintMatrices)); + } + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::multiple_shooting::Settings settings; + settings.dt = 0.05; + settings.sqpIteration = 10; + settings.projectStateInputEqualityConstraints = true; + settings.useFeedbackPolicy = feedback; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Ones(n); + + // Construct solver + ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_constrained, withFeedback) { + int n = 3; + int m = 2; + int nc = 0; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto constraints = ocs2::getRandomConstraints(n, m, nc); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(true, true, dynamics, costs, constraints); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(true, false, dynamics, costs, constraints); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithEmptyConstraint.second.back().inequalityConstraintsSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().inequalityConstraintsSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} + +TEST(test_constrained, noFeedback) { + int n = 3; + int m = 2; + int nc = 0; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto constraints = ocs2::getRandomConstraints(n, m, nc); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(false, true, dynamics, costs, constraints); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(false, false, dynamics, costs, constraints); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithEmptyConstraint.second.back().inequalityConstraintsSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().inequalityConstraintsSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} + +TEST(test_constrained, constraintSatisfaction) { + int n = 3; + int m = 2; + int nc = 1; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto constraints = ocs2::getRandomConstraints(n, m, nc); + const auto solution = ocs2::solveWithFeedbackSetting(true, true, dynamics, costs, constraints); + + ASSERT_LE(solution.second.size(), 2); + ASSERT_LT(solution.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solution.second.back().inequalityConstraintsSSE, tol); + + const auto& primal = solution.first; + for (int i = 0; i < primal.timeTrajectory_.size(); i++) { + const auto t = primal.timeTrajectory_[i]; + const auto& x = primal.stateTrajectory_[i]; + const auto& u = primal.inputTrajectory_[i]; + + // Check that the constraint is satisfied. + const auto constraint_value = constraints.f + constraints.dfdx * x + constraints.dfdu * u; + ASSERT_TRUE((constraint_value.array() >= 0).all()); + } +}