@@ -35,72 +35,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
3535
3636#include " ocs2_ipm/IpmSolver.h"
3737
38+ #include < ocs2_core/constraint/LinearStateConstraint.h>
39+ #include < ocs2_core/constraint/LinearStateInputConstraint.h>
3840#include < ocs2_core/initialization/DefaultInitializer.h>
3941#include < ocs2_oc/test/EXP0.h>
4042
4143using namespace ocs2 ;
4244
43- class EXP0_StateIneqConstraints final : public StateConstraint {
44- public:
45- EXP0_StateIneqConstraints (const vector_t & xmin, const vector_t & xmax)
46- : StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {}
47- ~EXP0_StateIneqConstraints () override = default ;
48-
49- EXP0_StateIneqConstraints* clone () const override { return new EXP0_StateIneqConstraints (*this ); }
50-
51- size_t getNumConstraints (scalar_t time) const override { return 4 ; }
52-
53- vector_t getValue (scalar_t t, const vector_t & x, const PreComputation&) const override {
54- vector_t e (4 );
55- e.head (2 ) = x - xmin_;
56- e.tail (2 ) = xmax_ - x;
57- return e;
58- }
59-
60- VectorFunctionLinearApproximation getLinearApproximation (scalar_t t, const vector_t & x, const PreComputation& preComp) const override {
61- VectorFunctionLinearApproximation e;
62- e.f = getValue (t, x, preComp);
63- e.dfdx = matrix_t::Zero (4 , x.size ());
64- e.dfdx .topLeftCorner (2 , 2 ) = matrix_t::Identity (2 , 2 );
65- e.dfdx .bottomRightCorner (2 , 2 ) = -matrix_t::Identity (2 , 2 );
66- return e;
67- }
68-
69- private:
70- vector_t xmin_, xmax_;
71- };
72-
73- class EXP0_StateInputIneqConstraints final : public StateInputConstraint {
74- public:
75- EXP0_StateInputIneqConstraints (scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {}
76- ~EXP0_StateInputIneqConstraints () override = default ;
77-
78- EXP0_StateInputIneqConstraints* clone () const override { return new EXP0_StateInputIneqConstraints (*this ); }
79-
80- size_t getNumConstraints (scalar_t time) const override { return 2 ; }
81-
82- vector_t getValue (scalar_t t, const vector_t & x, const vector_t & u, const PreComputation&) const override {
83- vector_t e (2 );
84- e << (u.coeff (0 ) - umin_), (umax_ - u.coeff (0 ));
85- return e;
86- }
87-
88- VectorFunctionLinearApproximation getLinearApproximation (scalar_t t, const vector_t & x, const vector_t & u,
89- const PreComputation& preComp) const override {
90- VectorFunctionLinearApproximation e;
91- e.f = getValue (t, x, u, preComp);
92- e.dfdx = matrix_t::Zero (2 , x.size ());
93- e.dfdu = (matrix_t (2 , 1 ) << 1 , -1 ).finished ();
94- return e;
95- }
96-
97- private:
98- scalar_t umin_, umax_;
99- };
100-
10145TEST (Exp0Test, Unconstrained) {
102- static constexpr size_t STATE_DIM = 2 ;
103- static constexpr size_t INPUT_DIM = 1 ;
46+ constexpr size_t STATE_DIM = 2 ;
47+ constexpr size_t INPUT_DIM = 1 ;
10448
10549 // Solver settings
10650 const auto settings = []() {
@@ -141,8 +85,27 @@ TEST(Exp0Test, Unconstrained) {
14185}
14286
14387TEST (Exp0Test, Constrained) {
144- static constexpr size_t STATE_DIM = 2 ;
145- static constexpr size_t INPUT_DIM = 1 ;
88+ constexpr size_t STATE_DIM = 2 ;
89+ constexpr size_t INPUT_DIM = 1 ;
90+
91+ auto getStateBoxConstraint = [&](const vector_t & minState, const vector_t & maxState) {
92+ constexpr size_t numIneqConstraint = 2 * STATE_DIM;
93+ const vector_t e = (vector_t (numIneqConstraint) << -minState, maxState).finished ();
94+ const matrix_t C =
95+ (matrix_t (numIneqConstraint, STATE_DIM) << matrix_t::Identity (STATE_DIM, STATE_DIM), -matrix_t::Identity (STATE_DIM, STATE_DIM))
96+ .finished ();
97+ return std::make_unique<LinearStateConstraint>(std::move (e), std::move (C));
98+ };
99+
100+ auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) {
101+ constexpr size_t numIneqConstraint = 2 * INPUT_DIM;
102+ const vector_t e = (vector_t (numIneqConstraint) << -minInput, maxInput).finished ();
103+ const matrix_t C = matrix_t::Zero (numIneqConstraint, STATE_DIM);
104+ const matrix_t D =
105+ (matrix_t (numIneqConstraint, INPUT_DIM) << matrix_t::Identity (INPUT_DIM, INPUT_DIM), -matrix_t::Identity (INPUT_DIM, INPUT_DIM))
106+ .finished ();
107+ return std::make_unique<LinearStateInputConstraint>(std::move (e), std::move (C), std::move (D));
108+ };
146109
147110 // Solver settings
148111 const auto settings = []() {
@@ -173,14 +136,11 @@ TEST(Exp0Test, Constrained) {
173136 // add inequality constraints
174137 const scalar_t umin = -7.5 ;
175138 const scalar_t umax = 7.5 ;
176- auto stateInputIneqConstraint = std::make_unique<EXP0_StateInputIneqConstraints>(umin, umax);
177- problem.inequalityConstraintPtr ->add (" ubound" std::move (stateInputIneqConstraint));
139+ problem.inequalityConstraintPtr ->add (" ubound" getInputBoxConstraint (umin, umax));
178140 const vector_t xmin = (vector_t (2 ) << -7.5 , -7.5 ).finished ();
179141 const vector_t xmax = (vector_t (2 ) << 7.5 , 7.5 ).finished ();
180- auto stateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax);
181- auto finalStateIneqConstraint = std::make_unique<EXP0_StateIneqConstraints>(xmin, xmax);
182- problem.stateInequalityConstraintPtr ->add (" xbound" std::move (stateIneqConstraint));
183- problem.finalInequalityConstraintPtr ->add (" xbound" std::move (finalStateIneqConstraint));
142+ problem.stateInequalityConstraintPtr ->add (" xbound" getStateBoxConstraint (xmin, xmax));
143+ problem.finalInequalityConstraintPtr ->add (" xbound" getStateBoxConstraint (xmin, xmax));
184144
185145 const scalar_t startTime = 0.0 ;
186146 const scalar_t finalTime = 2.0 ;
@@ -196,18 +156,16 @@ TEST(Exp0Test, Constrained) {
196156 const auto primalSolution = solver.primalSolution (finalTime);
197157
198158 // check constraint satisfaction
199- for (const auto & e : primalSolution.stateTrajectory_ ) {
200- if (e.size () > 0 ) {
201- ASSERT_TRUE (e.coeff (0 ) >= xmin.coeff (0 ));
202- ASSERT_TRUE (e.coeff (1 ) >= xmin.coeff (1 ));
203- ASSERT_TRUE (e.coeff (0 ) <= xmax.coeff (0 ));
204- ASSERT_TRUE (e.coeff (1 ) <= xmax.coeff (1 ));
159+ for (const auto & x : primalSolution.stateTrajectory_ ) {
160+ if (x.size () > 0 ) {
161+ ASSERT_TRUE ((x - xmin).minCoeff () >= 0 );
162+ ASSERT_TRUE ((xmax - x).minCoeff () >= 0 );
205163 }
206164 }
207- for (const auto & e : primalSolution.inputTrajectory_ ) {
208- if (e .size () > 0 ) {
209- ASSERT_TRUE (e. coeff (0 ) >= umin );
210- ASSERT_TRUE (e. coeff (0 ) <= umax );
165+ for (const auto & u : primalSolution.inputTrajectory_ ) {
166+ if (u .size () > 0 ) {
167+ ASSERT_TRUE (u (0 ) - umin >= 0 );
168+ ASSERT_TRUE (umax - u (0 ) >= 0 );
211169 }
212170 }
213171}
0 commit comments