Skip to content

Commit 23d2265

Browse files
author
earlaud
committed
Add discrete interpollator and fix tests
1 parent 14023ac commit 23d2265

File tree

3 files changed

+160
-92
lines changed

3 files changed

+160
-92
lines changed

‎include/simple-mpc/interpolator.hpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,12 @@ namespace simple_mpc
4343
const std::vector<Eigen::VectorXd> & vs,
4444
Eigen::Ref<Eigen::VectorXd> v_interp);
4545

46+
void interpolateDiscrete(
47+
const double delay,
48+
const double timestep,
49+
const std::vector<Eigen::VectorXd> & vs,
50+
Eigen::Ref<Eigen::VectorXd> v_interp);
51+
4652
// Pinocchio model
4753
Model model_;
4854
};

‎src/interpolator.cpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
// All rights reserved.
77
///////////////////////////////////////////////////////////////////////////////
88
#include "simple-mpc/interpolator.hpp"
9+
#include <algorithm>
910

1011
namespace simple_mpc
1112
{
@@ -79,4 +80,18 @@ namespace simple_mpc
7980
}
8081
}
8182

83+
void Interpolator::interpolateDiscrete(
84+
const double delay,
85+
const double timestep,
86+
const std::vector<Eigen::VectorXd> & vs,
87+
Eigen::Ref<Eigen::VectorXd> v_interp)
88+
{
89+
// Compute the time knot corresponding to the current delay
90+
size_t step_nb = static_cast<size_t>(delay / timestep);
91+
step_nb = std::clamp(step_nb, 0UL, vs.size()-1);
92+
93+
// Set the output arg
94+
v_interp = vs[step_nb];
95+
}
96+
8297
} // namespace simple_mpc

‎tests/interpolator.cpp‎

Lines changed: 139 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -20,117 +20,164 @@ BOOST_AUTO_TEST_CASE(interpolate)
2020
Interpolator interpolator = Interpolator(model);
2121

2222
// Test configuration interpolation method
23-
std::vector<Eigen::VectorXd> qs;
24-
for (std::size_t i = 0; i < 4; i++)
2523
{
26-
Eigen::VectorXd q0(model.nq);
27-
Eigen::VectorXd q1(model.nq);
28-
q0 = pinocchio::neutral(model);
24+
std::vector<Eigen::VectorXd> qs;
25+
for (std::size_t i = 0; i < 4; i++)
26+
{
27+
Eigen::VectorXd q0(model.nq);
28+
Eigen::VectorXd q1(model.nq);
29+
q0 = pinocchio::neutral(model);
30+
Eigen::VectorXd dq(model.nv);
31+
dq.setRandom();
32+
pinocchio::integrate(model, q0, dq, q1);
33+
qs.push_back(q1);
34+
}
35+
Eigen::VectorXd q_interp(model.nq);
36+
37+
double delay = 0.02;
38+
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
39+
40+
BOOST_CHECK(qs[2].isApprox(q_interp));
41+
42+
delay = 0.5;
43+
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
44+
BOOST_CHECK(qs.back().isApprox(q_interp));
45+
46+
delay = 0.005;
47+
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
48+
Eigen::VectorXd q_interp2(model.nq);
2949
Eigen::VectorXd dq(model.nv);
30-
dq.setRandom();
31-
pinocchio::integrate(model, q0, dq, q1);
32-
33-
qs.push_back(q1);
34-
}
35-
double delay = 0.02;
36-
37-
Eigen::VectorXd q_interp(model.nq);
38-
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
39-
40-
BOOST_CHECK(qs[2].isApprox(q_interp));
41-
42-
delay = 0.5;
43-
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
44-
BOOST_CHECK(qs.back().isApprox(q_interp));
45-
46-
delay = 0.005;
47-
interpolator.interpolateConfiguration(delay, timestep, qs, q_interp);
48-
Eigen::VectorXd q_interp2(model.nq);
49-
Eigen::VectorXd dq(model.nv);
50-
pinocchio::difference(model, qs[0], qs[1], dq);
51-
pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2);
52-
53-
BOOST_CHECK(q_interp2.isApprox(q_interp));
54-
55-
std::vector<Eigen::VectorXd> qs2;
56-
for (std::size_t i = 0; i < 2; i++)
57-
{
58-
qs2.push_back(qs[0]);
50+
pinocchio::difference(model, qs[0], qs[1], dq);
51+
pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2);
52+
53+
BOOST_CHECK(q_interp2.isApprox(q_interp));
54+
55+
std::vector<Eigen::VectorXd> qs2;
56+
for (std::size_t i = 0; i < 2; i++)
57+
{
58+
qs2.push_back(qs[0]);
59+
}
60+
interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
61+
BOOST_CHECK(qs2[0].isApprox(q_interp));
5962
}
60-
interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp);
61-
BOOST_CHECK(qs2[0].isApprox(q_interp));
6263

6364
// Test state interpolation method
64-
std::vector<Eigen::VectorXd> xs;
65-
for (std::size_t i = 0; i < 4; i++)
6665
{
67-
Eigen::VectorXd x0(model.nq + model.nv);
68-
x0.head(model.nq) = pinocchio::neutral(model);
66+
std::vector<Eigen::VectorXd> xs;
67+
for (std::size_t i = 0; i < 4; i++)
68+
{
69+
Eigen::VectorXd x0(model.nq + model.nv);
70+
x0.head(model.nq) = pinocchio::neutral(model);
71+
Eigen::VectorXd dq(model.nv);
72+
dq.setRandom();
73+
pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq));
74+
x0.tail(model.nv).setRandom();
75+
xs.push_back(x0);
76+
}
77+
Eigen::VectorXd x_interp(model.nq + model.nv);
6978
Eigen::VectorXd dq(model.nv);
70-
dq.setRandom();
71-
pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq));
72-
x0.tail(model.nv).setRandom();
7379

74-
xs.push_back(x0);
80+
double delay = 0.02;
81+
interpolator.interpolateState(delay, timestep, xs, x_interp);
82+
BOOST_CHECK(xs[2].isApprox(x_interp));
83+
84+
delay = 0.5;
85+
interpolator.interpolateState(delay, timestep, xs, x_interp);
86+
BOOST_CHECK(xs.back().isApprox(x_interp));
87+
88+
delay = 0.005;
89+
interpolator.interpolateState(delay, timestep, xs, x_interp);
90+
Eigen::VectorXd x_interp2(model.nq + model.nv);
91+
pinocchio::difference(model, xs[0].head(model.nq), xs[1].head(model.nq), dq);
92+
pinocchio::integrate(model, xs[0].head(model.nq), dq * 0.5, x_interp2.head(model.nq));
93+
x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5;
94+
BOOST_CHECK(x_interp2.isApprox(x_interp));
95+
96+
std::vector<Eigen::VectorXd> xs2;
97+
for (std::size_t i = 0; i < 2; i++)
98+
{
99+
xs2.push_back(xs[0]);
100+
}
101+
interpolator.interpolateState(delay, timestep, xs2, x_interp);
102+
BOOST_CHECK(xs2[0].isApprox(x_interp));
75103
}
76-
delay = 0.02;
77-
78-
Eigen::VectorXd x_interp(model.nq + model.nv);
79-
interpolator.interpolateState(delay, timestep, xs, x_interp);
80-
BOOST_CHECK(xs[2].isApprox(x_interp));
81-
82-
delay = 0.5;
83-
interpolator.interpolateState(delay, timestep, xs, x_interp);
84-
BOOST_CHECK(xs.back().isApprox(x_interp));
85-
86-
delay = 0.005;
87-
interpolator.interpolateState(delay, timestep, xs, x_interp);
88-
Eigen::VectorXd x_interp2(model.nq + model.nv);
89-
pinocchio::difference(model, xs[0].head(model.nq), xs[1].head(model.nq), dq);
90-
pinocchio::integrate(model, xs[0].head(model.nq), dq * 0.5, x_interp2.head(model.nq));
91-
x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5;
92-
BOOST_CHECK(x_interp2.isApprox(x_interp));
93-
94-
std::vector<Eigen::VectorXd> xs2;
95-
for (std::size_t i = 0; i < 2; i++)
104+
105+
// Test linear interpolation method
96106
{
97-
xs2.push_back(xs[0]);
107+
std::vector<Eigen::VectorXd> vs;
108+
for (std::size_t i = 0; i < 4; i++)
109+
{
110+
Eigen::VectorXd v0(model.nv);
111+
v0.setRandom();
112+
vs.push_back(v0);
113+
}
114+
Eigen::VectorXd v_interp(model.nv);
115+
116+
double delay = 0.02;
117+
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
118+
BOOST_CHECK(vs[2].isApprox(v_interp));
119+
120+
delay = 0.5;
121+
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
122+
BOOST_CHECK(vs.back().isApprox(v_interp));
123+
124+
delay = 0.005;
125+
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
126+
Eigen::VectorXd v_interp2(model.nv);
127+
v_interp2 = (vs[0] + vs[1]) * 0.5;
128+
129+
BOOST_CHECK(v_interp2.isApprox(v_interp));
130+
131+
std::vector<Eigen::VectorXd> vs2;
132+
for (std::size_t i = 0; i < 2; i++)
133+
{
134+
vs2.push_back(vs[0]);
135+
}
136+
interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
137+
BOOST_CHECK(vs2[0].isApprox(v_interp));
98138
}
99-
interpolator.interpolateState(delay, timestep, xs2, x_interp);
100-
BOOST_CHECK(xs2[0].isApprox(x_interp));
101139

102-
// Test linear interpolation method
103-
std::vector<Eigen::VectorXd> vs;
104-
for (std::size_t i = 0; i < 4; i++)
140+
// Test discrete interpolation method
105141
{
106-
Eigen::VectorXd v0(model.nv);
107-
v0.setRandom();
142+
std::vector<Eigen::VectorXd> vs;
143+
for (std::size_t i = 0; i < 4; i++)
144+
{
145+
Eigen::VectorXd v0(model.nv);
146+
v0.setRandom();
147+
vs.push_back(v0);
148+
}
149+
Eigen::VectorXd v_interp(model.nv);
108150

109-
vs.push_back(v0);
110-
}
111-
delay = 0.02;
112-
Eigen::VectorXd v_interp(model.nv);
113-
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
114-
BOOST_CHECK(vs[2].isApprox(v_interp));
151+
// First element
152+
interpolator.interpolateDiscrete(-0.001, 0.1, vs, v_interp);
153+
BOOST_CHECK(vs[0].isApprox(v_interp));
115154

116-
delay = 0.5;
117-
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
118-
BOOST_CHECK(vs.back().isApprox(v_interp));
155+
interpolator.interpolateDiscrete(0, 0.1, vs, v_interp);
156+
BOOST_CHECK(vs[0].isApprox(v_interp));
119157

120-
delay = 0.005;
121-
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
122-
Eigen::VectorXd v_interp2(model.nv);
123-
v_interp2 = (vs[0] + vs[1]) * 0.5;
158+
interpolator.interpolateDiscrete(0.001, 0.1, vs, v_interp);
159+
BOOST_CHECK(vs[0].isApprox(v_interp));
124160

125-
BOOST_CHECK(v_interp2.isApprox(v_interp));
161+
// Middle element
162+
interpolator.interpolateDiscrete(0.1, 0.1, vs, v_interp);
163+
BOOST_CHECK(vs[1].isApprox(v_interp));
126164

127-
std::vector<Eigen::VectorXd> vs2;
128-
for (std::size_t i = 0; i < 2; i++)
129-
{
130-
vs2.push_back(vs[0]);
165+
interpolator.interpolateDiscrete(0.15, 0.1, vs, v_interp);
166+
BOOST_CHECK(vs[1].isApprox(v_interp));
167+
168+
interpolator.interpolateDiscrete(0.2, 0.1, vs, v_interp);
169+
BOOST_CHECK(vs[2].isApprox(v_interp));
170+
171+
// Last element
172+
interpolator.interpolateDiscrete(0.39, 0.1, vs, v_interp);
173+
BOOST_CHECK(vs[3].isApprox(v_interp));
174+
175+
interpolator.interpolateDiscrete(0.4, 0.1, vs, v_interp);
176+
BOOST_CHECK(vs[3].isApprox(v_interp));
177+
178+
interpolator.interpolateDiscrete(0.5, 0.1, vs, v_interp);
179+
BOOST_CHECK(vs[3].isApprox(v_interp));
131180
}
132-
interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
133-
BOOST_CHECK(vs2[0].isApprox(v_interp));
134181
}
135182

136183
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)