Skip to content

Commit 88efbd9

Browse files
mayatakamayataka
committed
refactored tests
1 parent 549ed19 commit 88efbd9

File tree

3 files changed

+142
-268
lines changed

3 files changed

+142
-268
lines changed

‎ocs2_ipm/test/Exp0Test.cpp‎

Lines changed: 36 additions & 78 deletions
Original file line numberDiff line numberDiff line change
@@ -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

4143
using 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-
10145
TEST(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

14387
TEST(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
}

‎ocs2_ipm/test/Exp1Test.cpp‎

Lines changed: 41 additions & 83 deletions
Original file line numberDiff line numberDiff line change
@@ -35,69 +35,13 @@ 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/EXP1.h>
4042

4143
using namespace ocs2;
4244

43-
class EXP1_StateIneqConstraints final : public StateConstraint {
44-
public:
45-
EXP1_StateIneqConstraints(const vector_t& xmin, const vector_t& xmax)
46-
: StateConstraint(ConstraintOrder::Linear), xmin_(xmin), xmax_(xmax) {}
47-
~EXP1_StateIneqConstraints() override = default;
48-
49-
EXP1_StateIneqConstraints* clone() const override { return new EXP1_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 EXP1_StateInputIneqConstraints final : public StateInputConstraint {
74-
public:
75-
EXP1_StateInputIneqConstraints(scalar_t umin, scalar_t umax) : StateInputConstraint(ConstraintOrder::Linear), umin_(umin), umax_(umax) {}
76-
~EXP1_StateInputIneqConstraints() override = default;
77-
78-
EXP1_StateInputIneqConstraints* clone() const override { return new EXP1_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-
10145
class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint {
10246
public:
10347
EXP1_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax)
@@ -110,26 +54,26 @@ class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint {
11054

11155
vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override {
11256
vector_t e(2);
113-
e << (x.coeff(0) * u.coeff(0) - xumin_), (xumax_ - x.coeff(1) * u.coeff(0));
57+
e << (x(0) * u(0) - xumin_), (xumax_ - x(1) * u(0));
11458
return e;
11559
}
11660

11761
VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u,
11862
const PreComputation& preComp) const override {
119-
VectorFunctionLinearApproximation e;
63+
VectorFunctionLinearApproximation e(2, x.size(), u.size());
12064
e.f = getValue(t, x, u, preComp);
121-
e.dfdx = (matrix_t(2, 2) << u.coeff(0), 0, 0, -u.coeff(0)).finished();
122-
e.dfdu = (matrix_t(2, 1) << x.coeff(0), -x.coeff(1)).finished();
65+
e.dfdx << u(0), 0, 0, -u(0);
66+
e.dfdu << x(0), -x(1);
12367
return e;
12468
}
12569

12670
private:
127-
scalar_t xumin_, xumax_;
71+
const scalar_t xumin_, xumax_;
12872
};
12973

13074
TEST(Exp1Test, Unconstrained) {
131-
static constexpr size_t STATE_DIM = 2;
132-
static constexpr size_t INPUT_DIM = 1;
75+
constexpr size_t STATE_DIM = 2;
76+
constexpr size_t INPUT_DIM = 1;
13377

13478
// Solver settings
13579
const auto settings = []() {
@@ -170,8 +114,27 @@ TEST(Exp1Test, Unconstrained) {
170114
}
171115

172116
TEST(Exp1Test, Constrained) {
173-
static constexpr size_t STATE_DIM = 2;
174-
static constexpr size_t INPUT_DIM = 1;
117+
constexpr size_t STATE_DIM = 2;
118+
constexpr size_t INPUT_DIM = 1;
119+
120+
auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) {
121+
constexpr size_t numIneqConstraint = 2 * STATE_DIM;
122+
const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished();
123+
const matrix_t C =
124+
(matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM))
125+
.finished();
126+
return std::make_unique<LinearStateConstraint>(std::move(e), std::move(C));
127+
};
128+
129+
auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) {
130+
constexpr size_t numIneqConstraint = 2 * INPUT_DIM;
131+
const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished();
132+
const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM);
133+
const matrix_t D =
134+
(matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM))
135+
.finished();
136+
return std::make_unique<LinearStateInputConstraint>(std::move(e), std::move(C), std::move(D));
137+
};
175138

176139
// Solver settings
177140
const auto settings = []() {
@@ -202,14 +165,11 @@ TEST(Exp1Test, Constrained) {
202165
// add inequality constraints
203166
const scalar_t umin = -1.0;
204167
const scalar_t umax = 1.0;
205-
auto stateInputIneqConstraint = std::make_unique<EXP1_StateInputIneqConstraints>(umin, umax);
206-
problem.inequalityConstraintPtr->add("ubound", std::move(stateInputIneqConstraint));
168+
problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax));
207169
const vector_t xmin = (vector_t(2) << -0.0, -0.0).finished();
208170
const vector_t xmax = (vector_t(2) << 3.0, 4.0).finished();
209-
auto stateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax);
210-
auto finalStateIneqConstraint = std::make_unique<EXP1_StateIneqConstraints>(xmin, xmax);
211-
problem.stateInequalityConstraintPtr->add("xbound", std::move(stateIneqConstraint));
212-
problem.finalInequalityConstraintPtr->add("xbound", std::move(finalStateIneqConstraint));
171+
problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax));
172+
problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax));
213173

214174
const scalar_t startTime = 0.0;
215175
const scalar_t finalTime = 3.0;
@@ -225,18 +185,16 @@ TEST(Exp1Test, Constrained) {
225185
const auto primalSolution = solver.primalSolution(finalTime);
226186

227187
// check constraint satisfaction
228-
for (const auto& e : primalSolution.stateTrajectory_) {
229-
if (e.size() > 0) {
230-
ASSERT_TRUE(e.coeff(0) >= xmin.coeff(0));
231-
ASSERT_TRUE(e.coeff(1) >= xmin.coeff(1));
232-
ASSERT_TRUE(e.coeff(0) <= xmax.coeff(0));
233-
ASSERT_TRUE(e.coeff(1) <= xmax.coeff(1));
188+
for (const auto& x : primalSolution.stateTrajectory_) {
189+
if (x.size() > 0) {
190+
ASSERT_TRUE((x - xmin).minCoeff() >= 0);
191+
ASSERT_TRUE((xmax - x).minCoeff() >= 0);
234192
}
235193
}
236-
for (const auto& e : primalSolution.inputTrajectory_) {
237-
if (e.size() > 0) {
238-
ASSERT_TRUE(e.coeff(0) >= umin);
239-
ASSERT_TRUE(e.coeff(0) <= umax);
194+
for (const auto& u : primalSolution.inputTrajectory_) {
195+
if (u.size() > 0) {
196+
ASSERT_TRUE(u(0) - umin >= 0);
197+
ASSERT_TRUE(umax - u(0) >= 0);
240198
}
241199
}
242200
}

0 commit comments

Comments
 (0)