Skip to content

Commit 2bb1bc2

Browse files
EtienneArEtienneAr
authored
Merge pull request #39 from Simple-Robotics/topic/interpolation
Add interpolation class to simple_mpc
2 parents d7b4950 + f05351e commit 2bb1bc2

File tree

13 files changed

+313
-31
lines changed

13 files changed

+313
-31
lines changed

‎bindings/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@ set(
99
expose-robot-handler.cpp
1010
expose-problem.cpp
1111
expose-mpc.cpp
12-
expose-lowlevel.cpp
12+
expose-qp.cpp
13+
expose-interpolate.cpp
1314
expose-centroidal.cpp
1415
expose-fulldynamics.cpp
1516
expose-kinodynamics.cpp

‎bindings/expose-interpolate.cpp‎

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
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+
///////////////////////////////////////////////////////////////////////////////
8+
9+
#include <eigenpy/deprecation-policy.hpp>
10+
#include <eigenpy/std-vector.hpp>
11+
12+
#include "simple-mpc/interpolator.hpp"
13+
14+
namespace simple_mpc
15+
{
16+
namespace python
17+
{
18+
namespace bp = boost::python;
19+
20+
Eigen::VectorXd interpolateConfigurationProxy(
21+
Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & qs)
22+
{
23+
Eigen::VectorXd q_interp(qs[0].size());
24+
self.interpolateConfiguration(delay, timestep, qs, q_interp);
25+
26+
return q_interp;
27+
}
28+
29+
Eigen::VectorXd interpolateLinearProxy(
30+
Interpolator & self, const double delay, const double timestep, const std::vector<Eigen::VectorXd> & vs)
31+
{
32+
Eigen::VectorXd v_interp(vs[0].size());
33+
self.interpolateLinear(delay, timestep, vs, v_interp);
34+
35+
return v_interp;
36+
}
37+
38+
void exposeInterpolator()
39+
{
40+
bp::class_<Interpolator>("Interpolator", bp::init<const Model &>(bp::args("self", "model")))
41+
.def("interpolateConfiguration", &interpolateConfigurationProxy)
42+
.def("interpolateLinear", &interpolateLinearProxy);
43+
}
44+
} // namespace python
45+
} // namespace simple_mpc

‎bindings/expose-lowlevel.cpp‎ renamed to ‎bindings/expose-qp.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
#include <eigenpy/deprecation-policy.hpp>
1111
#include <eigenpy/std-vector.hpp>
1212

13-
#include "simple-mpc/lowlevel-control.hpp"
13+
#include "simple-mpc/qp-solvers.hpp"
1414
#include <proxsuite/proxqp/dense/dense.hpp>
1515
#include <proxsuite/proxqp/dense/wrapper.hpp>
1616

‎bindings/module.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ namespace simple_mpc::python
1717
void exposeMPC();
1818
void exposeIDSolver();
1919
void exposeIKIDSolver();
20+
void exposeInterpolator();
2021
void exposeFrictionCompensation();
2122

2223
/* PYTHON MODULE */
@@ -36,6 +37,7 @@ namespace simple_mpc::python
3637
exposeMPC();
3738
exposeIDSolver();
3839
exposeIKIDSolver();
40+
exposeInterpolator();
3941
exposeFrictionCompensation();
4042
}
4143

‎examples/go2_fulldynamics.py‎

Lines changed: 30 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,14 @@
11
import numpy as np
22
from bullet_robot import BulletRobot
3-
from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation
3+
from simple_mpc import (
4+
RobotModelHandler,
5+
RobotDataHandler,
6+
FullDynamicsOCP,
7+
MPC,
8+
IDSolver,
9+
Interpolator,
10+
FrictionCompensation
11+
)
412
import example_robot_data as erd
513
import pinocchio as pin
614
import time
@@ -26,6 +34,8 @@
2634
nu = nv - 6
2735
force_size = 3
2836
nk = len(model_handler.getFeetNames())
37+
nf = force_size
38+
2939
gravity = np.array([0, 0, -9.81])
3040
fref = np.zeros(force_size)
3141
fref[2] = -model_handler.getMass() / nk * gravity[2]
@@ -144,6 +154,8 @@
144154

145155
""" Friction """
146156
fcompensation = FrictionCompensation(model_handler.getModel(), True)
157+
""" Interpolation """
158+
interpolator = Interpolator(model_handler.getModel())
147159

148160
""" Initialize simulation"""
149161
device = BulletRobot(
@@ -155,8 +167,6 @@
155167
model_handler.getReferenceState()[:3],
156168
)
157169

158-
nq = mpc.getModelHandler().getModel().nq
159-
nv = mpc.getModelHandler().getModel().nv
160170
device.initializeJoints(model_handler.getReferenceState()[:nq])
161171

162172
for i in range(40):
@@ -237,6 +247,12 @@
237247
force_RL.append(RL_f)
238248
force_RR.append(RR_f)
239249

250+
forces = [total_forces, total_forces]
251+
ddqs = [a0, a1]
252+
qss = [mpc.xs[0][:model_handler.getModel().nq], mpc.xs[1][:model_handler.getModel().nq]]
253+
vss = [mpc.xs[0][model_handler.getModel().nq:], mpc.xs[1][model_handler.getModel().nq:]]
254+
uss = [mpc.us[0], mpc.us[1]]
255+
240256
FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation)
241257
FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation)
242258
RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation)
@@ -251,10 +267,15 @@
251267

252268
for j in range(N_simu):
253269
# time.sleep(0.01)
254-
u_interp = (N_simu - j) / N_simu * mpc.us[0] + j / N_simu * mpc.us[1]
255-
a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1
256-
x_interp = (N_simu - j) / N_simu * mpc.xs[0] + j / N_simu * mpc.xs[1]
257-
K_interp = (N_simu - j) / N_simu * mpc.Ks[0] + j / N_simu * mpc.Ks[1]
270+
delay = j / float(N_simu) * dt
271+
272+
q_interp = interpolator.interpolateConfiguration(delay, dt, qss)
273+
v_interp = interpolator.interpolateLinear(delay, dt, vss)
274+
u_interp = interpolator.interpolateLinear(delay, dt, uss)
275+
acc_interp = interpolator.interpolateLinear(delay, dt, ddqs)
276+
force_interp = interpolator.interpolateLinear(delay, dt, forces)
277+
278+
x_interp = np.concatenate((q_interp, v_interp))
258279

259280
q_meas, v_meas = device.measureState()
260281
x_measured = np.concatenate([q_meas, v_meas])
@@ -269,9 +290,9 @@
269290
mpc.getDataHandler().getData(),
270291
contact_states,
271292
x_measured[nq:],
272-
a0,
293+
acc_interp,
273294
current_torque,
274-
total_forces,
295+
force_interp,
275296
mpc.getDataHandler().getData().M,
276297
)
277298

‎examples/go2_kinodynamics.py‎

Lines changed: 17 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
import numpy as np
22
from bullet_robot import BulletRobot
3-
from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver
3+
from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver, Interpolator
44
import example_robot_data as erd
55
import pinocchio as pin
66
import time
@@ -21,6 +21,10 @@
2121
model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1])))
2222
data_handler = RobotDataHandler(model_handler)
2323

24+
nq = model_handler.getModel().nq
25+
nv = model_handler.getModel().nv
26+
nu = nv - 6
27+
nf = 12
2428
force_size = 3
2529
nk = len(model_handler.getFeetNames())
2630
gravity = np.array([0, 0, -9.81])
@@ -144,6 +148,9 @@
144148

145149
qp = IDSolver(id_conf, model_handler.getModel())
146150

151+
""" Interpolation """
152+
interpolator = Interpolator(nq + nv, nv, nu, nf, 0.01)
153+
147154
""" Initialize simulation"""
148155
device = BulletRobot(
149156
model_handler.getModel().names,
@@ -155,10 +162,7 @@
155162
)
156163

157164
device.initializeJoints(model_handler.getReferenceState()[:model_handler.getModel().nq])
158-
159165
device.changeCamera(1.0, 60, -15, [0.6, -0.2, 0.5])
160-
nq = mpc.getModelHandler().getModel().nq
161-
nv = mpc.getModelHandler().getModel().nv
162166

163167
q_meas, v_meas = device.measureState()
164168
x_measured = np.concatenate([q_meas, v_meas])
@@ -234,6 +238,11 @@
234238
forces1 = mpc.us[1][: nk * force_size]
235239
contact_states = mpc.ocp_handler.getContactState(0)
236240

241+
forces = [forces0, forces1]
242+
ddqs = [a0, a1]
243+
xss = [mpc.xs[0], mpc.xs[1]]
244+
uss = [mpc.us[0], mpc.us[1]]
245+
237246
device.moveQuadrupedFeet(
238247
mpc.getReferencePose(0, "FL_foot").translation,
239248
mpc.getReferencePose(0, "FR_foot").translation,
@@ -243,21 +252,20 @@
243252

244253
for j in range(N_simu):
245254
# time.sleep(0.01)
255+
interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces)
256+
246257
q_meas, v_meas = device.measureState()
247258
x_measured = np.concatenate([q_meas, v_meas])
248259

249260
mpc.getDataHandler().updateInternalData(x_measured, True)
250261

251-
a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1
252-
f_interp = (N_simu - j) / N_simu * forces0 + j / N_simu * forces1
253-
254262
qp.solveQP(
255263
mpc.getDataHandler().getData(),
256264
contact_states,
257265
x_measured[nq:],
258-
a_interp,
266+
interpolator.a_interpolated,
259267
np.zeros(12),
260-
f_interp,
268+
interpolator.forces_interpolated,
261269
mpc.getDataHandler().getData().M,
262270
)
263271

‎include/simple-mpc/interpolator.hpp‎

Lines changed: 50 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,50 @@
1+
///////////////////////////////////////////////////////////////////////////////
2+
// BSD 3-Clause License
3+
//
4+
// Copyright (C) 2025, INRIA
5+
// Copyright note valid unless otherwise stated in individual files.
6+
// All rights reserved.
7+
///////////////////////////////////////////////////////////////////////////////
8+
/**
9+
* @file interpolator.hpp
10+
* @brief Interpolation class for practical control of the robot
11+
*/
12+
13+
#ifndef SIMPLE_MPC_INTERPOLATOR_HPP_
14+
#define SIMPLE_MPC_INTERPOLATOR_HPP_
15+
16+
#include <pinocchio/algorithm/joint-configuration.hpp>
17+
18+
#include "simple-mpc/fwd.hpp"
19+
#include "simple-mpc/model-utils.hpp"
20+
21+
namespace simple_mpc
22+
{
23+
class Interpolator
24+
{
25+
public:
26+
explicit Interpolator(const Model & model);
27+
28+
void interpolateConfiguration(
29+
const double delay,
30+
const double timestep,
31+
const std::vector<Eigen::VectorXd> & qs,
32+
Eigen::Ref<Eigen::VectorXd> q_interp);
33+
34+
void interpolateLinear(
35+
const double delay,
36+
const double timestep,
37+
const std::vector<Eigen::VectorXd> & vs,
38+
Eigen::Ref<Eigen::VectorXd> v_interp);
39+
40+
// Pinocchio model
41+
Model model_;
42+
};
43+
44+
} // namespace simple_mpc
45+
46+
/* --- Details -------------------------------------------------------------- */
47+
/* --- Details -------------------------------------------------------------- */
48+
/* --- Details -------------------------------------------------------------- */
49+
50+
#endif // SIMPLE_MPC_INTERPOLATOR_HPP_

‎include/simple-mpc/lowlevel-control.hpp‎ renamed to ‎include/simple-mpc/qp-solvers.hpp‎

Lines changed: 5 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,13 +6,12 @@
66
// All rights reserved.
77
///////////////////////////////////////////////////////////////////////////////
88
/**
9-
* @file lowlevel-control.hpp
10-
* @brief Build a low-level control for kinodynamics
11-
* and centroidal MPC schemes
9+
* @file qp-solvers.hpp
10+
* @brief QP inverse dynamics for MPC schemes
1211
*/
1312

14-
#ifndef SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
15-
#define SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
13+
#ifndef SIMPLE_MPC_QP_SOLVERS_HPP_
14+
#define SIMPLE_MPC_QP_SOLVERS_HPP_
1615

1716
#include "simple-mpc/deprecated.hpp"
1817
#include "simple-mpc/fwd.hpp"
@@ -241,4 +240,4 @@ namespace simple_mpc
241240
/* --- Details -------------------------------------------------------------- */
242241
/* --- Details -------------------------------------------------------------- */
243242

244-
#endif // SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_
243+
#endif // SIMPLE_MPC_QP_SOLVERS_HPP_

‎src/interpolator.cpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
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+
///////////////////////////////////////////////////////////////////////////////
8+
#include "simple-mpc/interpolator.hpp"
9+
10+
namespace simple_mpc
11+
{
12+
13+
Interpolator::Interpolator(const Model & model)
14+
{
15+
model_ = model;
16+
}
17+
18+
void Interpolator::interpolateConfiguration(
19+
const double delay,
20+
const double timestep,
21+
const std::vector<Eigen::VectorXd> & qs,
22+
Eigen::Ref<Eigen::VectorXd> q_interp)
23+
{
24+
assert(("Configuration is not of the right size", qs[0].size() == model_.nq));
25+
26+
// Compute the time knot corresponding to the current delay
27+
size_t step_nb = static_cast<size_t>(delay / timestep);
28+
double step_progress = (delay - (double)step_nb * timestep) / timestep;
29+
30+
// Interpolate configuration trajectory
31+
if (step_nb >= qs.size() - 1)
32+
q_interp = qs.back();
33+
else
34+
{
35+
q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress);
36+
}
37+
}
38+
39+
void Interpolator::interpolateLinear(
40+
const double delay,
41+
const double timestep,
42+
const std::vector<Eigen::VectorXd> & vs,
43+
Eigen::Ref<Eigen::VectorXd> v_interp)
44+
{
45+
// Compute the time knot corresponding to the current delay
46+
size_t step_nb = static_cast<size_t>(delay / timestep);
47+
double step_progress = (delay - (double)step_nb * timestep) / timestep;
48+
49+
// Interpolate configuration trajectory
50+
if (step_nb >= vs.size() - 1)
51+
v_interp = vs.back();
52+
else
53+
{
54+
v_interp = vs[step_nb + 1] * step_progress + vs[step_nb] * (1. - step_progress);
55+
}
56+
}
57+
58+
} // namespace simple_mpc

‎src/lowlevel-control.cpp‎ renamed to ‎src/qp-solvers.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
// Copyright note valid unless otherwise stated in individual files.
66
// All rights reserved.
77
///////////////////////////////////////////////////////////////////////////////
8-
#include "simple-mpc/lowlevel-control.hpp"
8+
#include "simple-mpc/qp-solvers.hpp"
99
#include <pinocchio/algorithm/frames.hpp>
1010
#include <pinocchio/algorithm/joint-configuration.hpp>
1111
#include <proxsuite/proxqp/settings.hpp>
@@ -75,7 +75,7 @@ namespace simple_mpc
7575
}
7676

7777
// Set the block matrix for torque limits
78-
C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6);
78+
// C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6);
7979

8080
// Set size of solutions
8181
solved_forces_.resize(force_dim_);

0 commit comments

Comments
 (0)