Skip to content

Commit 9da8e74

Browse files
edantecedantec
authored
Merge pull request #58 from Simple-Robotics/topic/discrete_interpollator
Add discrete interpollator and fix tests
2 parents dbea314 + 2ac72e8 commit 9da8e74

File tree

6 files changed

+173
-116
lines changed

6 files changed

+173
-116
lines changed

‎.gersemirc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
definitions: [./CMakeLists.txt, ./benchmark, ./bindings, ./cmake, ./tests]
1+
definitions: [./CMakeLists.txt, ./cmake, ./benchmark, ./bindings, ./tests]
22
line_length: 80
33
indent: 2
44
warn_about_unknown_commands: false

‎.pre-commit-config.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,3 +30,4 @@ repos:
3030
rev: 0.19.3
3131
hooks:
3232
- id: gersemi
33+
args: ["--print-config=verbose"]

‎bindings/expose-interpolate.cpp

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -44,12 +44,22 @@ namespace simple_mpc
4444
return v_interp;
4545
}
4646

47+
std::vector<bool> interpolateContactsProxy(
48+
Interpolator & self, const double delay, const double timestep, const std::vector<std::vector<bool>> & cs)
49+
{
50+
std::vector<bool> c_interp(cs[0].size());
51+
self.interpolateContacts(delay, timestep, cs, c_interp);
52+
53+
return c_interp;
54+
}
55+
4756
void exposeInterpolator()
4857
{
4958
bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
5059
.def("interpolateConfiguration", &interpolateConfigurationProxy)
5160
.def("interpolateState", &interpolateStateProxy)
52-
.def("interpolateLinear", &interpolateLinearProxy);
61+
.def("interpolateLinear", &interpolateLinearProxy)
62+
.def("interpolateContacts", &interpolateContactsProxy);
5363
}
5464
} // namespace python
5565
} // namespace simple_mpc

‎include/simple-mpc/interpolator.hpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,7 @@
1010
* @brief Interpolation class for practical control of the robot
1111
*/
1212

13-
#ifndef SIMPLE_MPC_INTERPOLATOR_HPP_
14-
#define SIMPLE_MPC_INTERPOLATOR_HPP_
13+
#pragma once
1514

1615
#include <pinocchio/algorithm/joint-configuration.hpp>
1716

@@ -23,7 +22,8 @@ namespace simple_mpc
2322
class Interpolator
2423
{
2524
public:
26-
explicit Interpolator(const Model & model);
25+
explicit Interpolator(const Model & model)
26+
: model_(model) {};
2727

2828
void interpolateConfiguration(
2929
const double delay,
@@ -43,14 +43,14 @@ namespace simple_mpc
4343
const std::vector<Eigen::VectorXd> & vs,
4444
Eigen::Ref<Eigen::VectorXd> v_interp);
4545

46+
void interpolateContacts(
47+
const double delay,
48+
const double timestep,
49+
const std::vector<std::vector<bool>> & cs,
50+
std::vector<bool> & c_interp);
51+
4652
// Pinocchio model
4753
Model model_;
4854
};
4955

5056
} // namespace simple_mpc
51-
52-
/* --- Details -------------------------------------------------------------- */
53-
/* --- Details -------------------------------------------------------------- */
54-
/* --- Details -------------------------------------------------------------- */
55-
56-
#endif // SIMPLE_MPC_INTERPOLATOR_HPP_

‎src/interpolator.cpp

Lines changed: 10 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,20 +1,7 @@
1-
///////////////////////////////////////////////////////////////////////////////
2-
// BSD 2-Clause License
3-
//
4-
// Copyright (C) 2025, INRIA
5-
// Copyright note valid unless otherwise stated in individual files.
6-
// All rights reserved.
7-
///////////////////////////////////////////////////////////////////////////////
81
#include "simple-mpc/interpolator.hpp"
92

103
namespace simple_mpc
114
{
12-
13-
Interpolator::Interpolator(const Model & model)
14-
{
15-
model_ = model;
16-
}
17-
185
void Interpolator::interpolateConfiguration(
196
const double delay,
207
const double timestep,
@@ -79,4 +66,14 @@ namespace simple_mpc
7966
}
8067
}
8168

69+
void Interpolator::interpolateContacts(
70+
const double delay, const double timestep, const std::vector<std::vector<bool>> & cs, std::vector<bool> & c_interp)
71+
{
72+
// Compute the time knot corresponding to the current delay
73+
size_t step_nb = static_cast<size_t>(delay / timestep);
74+
step_nb = std::clamp(step_nb, 0UL, cs.size() - 1);
75+
76+
// Set the output arg
77+
c_interp = cs[step_nb];
78+
}
8279
} // namespace simple_mpc

‎tests/interpolator.cpp

Lines changed: 141 additions & 92 deletions
Original file line numberDiff line numberDiff line change
@@ -20,117 +20,166 @@ 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 contacts interpolation method
105141
{
106-
Eigen::VectorXd v0(model.nv);
107-
v0.setRandom();
142+
const size_t n_contacts = 4;
108143

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));
144+
std::vector<std::vector<bool>> cs;
145+
for (std::size_t i = 0; i < n_contacts; i++)
146+
{
147+
std::vector<bool> c(n_contacts, false);
148+
c[i] = true;
149+
cs.push_back(c);
150+
}
151+
std::vector<bool> c_interp(n_contacts);
115152

116-
delay = 0.5;
117-
interpolator.interpolateLinear(delay, timestep, vs, v_interp);
118-
BOOST_CHECK(vs.back().isApprox(v_interp));
153+
// First element
154+
interpolator.interpolateContacts(-0.001, 0.1, cs, c_interp);
155+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
119156

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;
157+
interpolator.interpolateContacts(0, 0.1, cs, c_interp);
158+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
124159

125-
BOOST_CHECK(v_interp2.isApprox(v_interp));
160+
interpolator.interpolateContacts(0.001, 0.1, cs, c_interp);
161+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[0].begin()));
126162

127-
std::vector<Eigen::VectorXd> vs2;
128-
for (std::size_t i = 0; i < 2; i++)
129-
{
130-
vs2.push_back(vs[0]);
163+
// Middle element
164+
interpolator.interpolateContacts(0.1, 0.1, cs, c_interp);
165+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
166+
167+
interpolator.interpolateContacts(0.15, 0.1, cs, c_interp);
168+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[1].begin()));
169+
170+
interpolator.interpolateContacts(0.2, 0.1, cs, c_interp);
171+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[2].begin()));
172+
173+
// Last element
174+
interpolator.interpolateContacts(0.39, 0.1, cs, c_interp);
175+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
176+
177+
interpolator.interpolateContacts(0.4, 0.1, cs, c_interp);
178+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
179+
180+
interpolator.interpolateContacts(0.5, 0.1, cs, c_interp);
181+
BOOST_CHECK(std::equal(c_interp.begin(), c_interp.end(), cs[3].begin()));
131182
}
132-
interpolator.interpolateLinear(delay, timestep, vs2, v_interp);
133-
BOOST_CHECK(vs2[0].isApprox(v_interp));
134183
}
135184

136185
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)