diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index 2a50d98b7..baba62d24 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -75,6 +75,8 @@ struct OptimalControlProblem { std::unique_ptr preJumpEqualityConstraintPtr; /** Final equality constraints */ std::unique_ptr finalEqualityConstraintPtr; + /** (Hard) inequality constraints **/ + std::unique_ptr inequalityConstraintPtr; /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h index ee45978ee..2137acb8d 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h @@ -56,6 +56,11 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; + /** Sum of Squared Error (SSE) of hard inequality constraints, used by the SQP solver. + * Inequality constraints are satisfied if positive, so there is only error if the constraints are negative. + */ + scalar_t inequalityConstraintsSSE = 0.0; + /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -76,6 +81,7 @@ struct PerformanceIndex { this->cost += rhs.cost; this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; + this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; this->equalityLagrangian += rhs.equalityLagrangian; this->inequalityLagrangian += rhs.inequalityLagrangian; return *this; @@ -93,6 +99,7 @@ inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { std::swap(lhs.cost, rhs.cost); std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); + std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); } @@ -109,6 +116,7 @@ inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& pe stream << std::setw(indentation) << ""; stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n'; stream << std::setw(indentation) << ""; stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index cd5c45102..f50e64370 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,6 +50,8 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), + /* Inequality constraints */ + inequalityConstraintPtr(new StateInputConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputCostCollection), stateEqualityLagrangianPtr(new StateCostCollection), @@ -82,6 +84,8 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), + /* Inequality constraints */ + inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -130,6 +134,9 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); + /* Inequality constraints */ + inequalityConstraintPtr.swap(other.inequalityConstraintPtr); + /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); 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_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..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,6 +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); 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..d7bff7ab4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -174,8 +174,9 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd)); } - problemPtr_->softConstraintPtr->add(footName + "_frictionCone", - getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + // problemPtr_->softConstraintPtr->add(footName + "_frictionCone", + // getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getHardFrictionConeConstraint(i, frictionCoefficient)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints)); @@ -321,6 +322,13 @@ 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) { + FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); + std::unique_ptr frictionConeConstraintPtr( + new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_)); + return frictionConeConstraintPtr; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index c876217c7..d87c2811a 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. @@ -85,7 +86,8 @@ class HpipmInterface { */ 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, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory, bool verbose = false); /** * Return the Riccati cost-to-go for the previously solved problem. 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 425644c90..5ebf6ac98 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h @@ -81,12 +81,16 @@ 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. + * @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* constraints, + 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 65408a455..1f25e5c3c 100644 --- a/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/src/HpipmInterface.cpp @@ -145,7 +145,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."); @@ -156,7 +157,13 @@ class HpipmInterface::Impl { } if (constraints != nullptr) { if (constraints->size() != ocpSize_.numStages + 1) { - throw std::runtime_error("[HpipmInterface] Inconsistent size of constraints: " + std::to_string(constraints->size()) + " with " + + throw std::runtime_error("[HpipmInterface] Inconsistent size of equality constraints: " + std::to_string(constraints->size()) + " with " + + std::to_string(ocpSize_.numStages + 1) + " nodes."); + } + } + if (ineqConstraints != nullptr) { + if (ineqConstraints->size() != ocpSize_.numStages + 1) { + throw std::runtime_error("[HpipmInterface] Inconsistent size of inequality constraints: " + std::to_string(ineqConstraints->size()) + " with " + std::to_string(ocpSize_.numStages + 1) + " nodes."); } } @@ -165,9 +172,10 @@ 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, 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); @@ -221,48 +229,147 @@ 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 C(N + 1); + std::vector D(N + 1); + std::vector d(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); - std::vector boundData; // Declare at this scope to keep the data alive while HPIPM has the pointers - 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 - 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(); - } + 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; + } + + 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(); + + // 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_); + + // 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 = 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(); + 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 = 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(); + // 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; + } + + 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(); + + // 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_); + } + + // No need to give CC[0] since initial state is given + if (k > 0) { + CC[k] = constr[k].dfdx.data(); + } + + if (k < N) { + DD[k] = constr[k].dfdu.data(); + } } } + // === 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; @@ -270,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) { @@ -533,9 +633,11 @@ 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, + vector_array_t& stateTrajectory, vector_array_t& inputTrajectory, bool verbose) { - return pImpl_->solve(x0, dynamics, cost, constraints, stateTrajectory, inputTrajectory, verbose); + return pImpl_->solve(x0, dynamics, cost, constraints, ineqConstraints, stateTrajectory, inputTrajectory, verbose); } std::vector 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..2eed9adf5 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, + bool useSlack) { const int numStages = dynamics.size(); OcpSize problemSize(dynamics.size()); @@ -67,9 +69,21 @@ 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, 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, 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, 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, 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, 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, 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, false); 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, 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, xSol, uSol, true); ASSERT_EQ(status, hpipm_status::SUCCESS); // Get Riccati info from hpipm interface 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/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h index 5a908e2c0..52a09cdb9 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h @@ -158,8 +158,9 @@ 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_; // 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..32d8d1fdf 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h @@ -45,6 +45,7 @@ struct Transcription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; VectorFunctionLinearApproximation constraintsProjection; }; @@ -79,6 +80,7 @@ struct TerminalTranscription { PerformanceIndex performance; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** @@ -105,6 +107,7 @@ struct EventTranscription { VectorFunctionLinearApproximation dynamics; ScalarFunctionQuadraticApproximation cost; VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp index 298f527cc..fd06b7f1e 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_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, 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); + eqConstraintPtr = &constraints_; } + const bool hasInequalityConstraints = !ocpDefinitions_.front().inequalityConstraintPtr->empty(); + if (hasInequalityConstraints) { + ineqConstraintPtr = &ineqConstraints_; + } + + hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, eqConstraintPtr, ineqConstraintPtr, settings_.hpipmSettings.use_slack)); + status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, eqConstraintPtr, ineqConstraintPtr, deltaXSol, deltaUSol, settings_.printSolverStatus); + if (status != hpipm_status::SUCCESS) { throw std::runtime_error("[MultipleShootingSolver] Failed to solve QP"); } @@ -358,6 +366,7 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec dynamics_.resize(N); cost_.resize(N + 1); constraints_.resize(N + 1); + ineqConstraints_.resize(N + 1); constraintsProjection_.resize(N); std::atomic_int timeIndex{0}; @@ -376,6 +385,7 @@ 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); constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); } else { // Normal, intermediate node @@ -387,6 +397,7 @@ 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); constraintsProjection_[i] = std::move(result.constraintsProjection); } @@ -399,6 +410,7 @@ 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); } // Accumulate! Same worker might run multiple tasks @@ -470,7 +482,7 @@ scalar_t MultipleShootingSolver::trajectoryNorm(const vector_array_t& v) { } scalar_t MultipleShootingSolver::totalConstraintViolation(const PerformanceIndex& performance) const { - 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 994a50c1d..1a93c1de9 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp @@ -46,6 +46,7 @@ Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlP auto& performance = transcription.performance; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; auto& projection = transcription.constraintsProjection; // Dynamics @@ -63,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); @@ -81,6 +82,21 @@ 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(); + + // Adapt if using projection + if (projectStateInputEqualityConstraints) { + changeOfInputVariables(ineqConstraints, projection.dfdu, projection.dfdx, projection.f); + } + } + } + return transcription; } @@ -100,7 +116,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) { @@ -108,6 +124,15 @@ 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.inequalityConstraintsSSE += dt * ineqConstraints.cwiseMin(0.0).squaredNorm(); + } + } + return performance; } @@ -117,6 +142,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont auto& performance = transcription.performance; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; optimalControlProblem.preComputationPtr->requestFinal(request, t, x); @@ -125,6 +151,7 @@ TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalCont performance.cost = cost.f; constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } @@ -148,6 +175,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro auto& dynamics = transcription.dynamics; auto& cost = transcription.cost; auto& constraints = transcription.constraints; + auto& ineqConstraints = transcription.ineqConstraints; constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); @@ -163,6 +191,7 @@ EventTranscription setupEventNode(const OptimalControlProblem& optimalControlPro performance.cost = cost.f; constraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); + ineqConstraints = VectorFunctionLinearApproximation::Zero(0, x.size(), 0); return transcription; } 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()); + } +} 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