Skip to content

Commit b047e3d

Browse files
edantecedantec
committed
First draft of interpolators
1 parent 93f142b commit b047e3d

File tree

11 files changed

+193
-15
lines changed

11 files changed

+193
-15
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: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
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+
void exposeInterpolator()
21+
{
22+
bp::class_<Interpolator>(
23+
"Interpolator", bp::init<const long, const long, const long, const long, const double>(
24+
bp::args("self", "nx", "nv", "nu", "nu", "MPC_timestep")))
25+
.def("interpolate", &Interpolator::interpolate)
26+
.add_property("MPC_timestep", &Interpolator::MPC_timestep_)
27+
.add_property("x_interpolated", &Interpolator::x_interpolated_)
28+
.add_property("u_interpolated", &Interpolator::u_interpolated_)
29+
.add_property("a_interpolated", &Interpolator::a_interpolated_)
30+
.add_property("forces_interpolated", &Interpolator::forces_interpolated_);
31+
}
32+
33+
} // namespace python
34+
} // 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

‎include/simple-mpc/interpolator.hpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
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 "simple-mpc/fwd.hpp"
17+
18+
namespace simple_mpc
19+
{
20+
class Interpolator
21+
{
22+
public:
23+
explicit Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep);
24+
25+
void interpolate(
26+
const double delay,
27+
std::vector<Eigen::VectorXd> xs,
28+
std::vector<Eigen::VectorXd> us,
29+
std::vector<Eigen::VectorXd> ddqs,
30+
std::vector<Eigen::VectorXd> forces);
31+
32+
Eigen::VectorXd x_interpolated_;
33+
Eigen::VectorXd u_interpolated_;
34+
Eigen::VectorXd a_interpolated_;
35+
Eigen::VectorXd forces_interpolated_;
36+
double MPC_timestep_;
37+
};
38+
} // namespace simple_mpc
39+
40+
/* --- Details -------------------------------------------------------------- */
41+
/* --- Details -------------------------------------------------------------- */
42+
/* --- Details -------------------------------------------------------------- */
43+
44+
#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/fulldynamics.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,7 @@ namespace simple_mpc
195195
space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name),
196196
pinocchio::LOCAL_WORLD_ALIGNED);
197197
FunctionSliceXpr vel_slice = FunctionSliceXpr(velocity_residual, vel_id);
198-
stm.addConstraint(vel_slice, EqualityConstraint());
198+
// stm.addConstraint(vel_slice, EqualityConstraint());
199199

200200
std::vector<int> frame_id = {2};
201201

@@ -205,7 +205,7 @@ namespace simple_mpc
205205

206206
FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id);
207207

208-
stm.addConstraint(frame_slice, EqualityConstraint());
208+
// stm.addConstraint(frame_slice, EqualityConstraint());
209209
}
210210
}
211211
}

‎src/interpolator.cpp‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
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 long nx, const long nv, const long nu, const long nf, const double MPC_timestep)
14+
: MPC_timestep_(MPC_timestep)
15+
{
16+
x_interpolated_.resize(nx);
17+
u_interpolated_.resize(nu);
18+
a_interpolated_.resize(nv);
19+
forces_interpolated_.resize(nf);
20+
}
21+
22+
void Interpolator::interpolate(
23+
const double delay,
24+
std::vector<Eigen::VectorXd> xs,
25+
std::vector<Eigen::VectorXd> us,
26+
std::vector<Eigen::VectorXd> ddqs,
27+
std::vector<Eigen::VectorXd> forces)
28+
{
29+
// Compute the time knot corresponding to the current delay
30+
size_t step_nb = static_cast<size_t>(delay / MPC_timestep_);
31+
double step_progress = (delay - (double)step_nb * MPC_timestep_) / MPC_timestep_;
32+
33+
// Interpolate state and command trajectories
34+
if (step_nb >= xs.size() - 1)
35+
{
36+
step_nb = xs.size() - 1;
37+
step_progress = 0.0;
38+
x_interpolated_ = xs[step_nb];
39+
u_interpolated_ = us[step_nb];
40+
a_interpolated_ = ddqs[step_nb];
41+
forces_interpolated_ = forces[step_nb];
42+
}
43+
else
44+
{
45+
x_interpolated_ = xs[step_nb + 1] * step_progress + xs[step_nb] * (1. - step_progress);
46+
u_interpolated_ = us[step_nb + 1] * step_progress + us[step_nb] * (1. - step_progress);
47+
a_interpolated_ = ddqs[step_nb + 1] * step_progress + ddqs[step_nb] * (1. - step_progress);
48+
forces_interpolated_ = forces[step_nb + 1] * step_progress + forces[step_nb] * (1. - step_progress);
49+
}
50+
}
51+
52+
} // 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_);

‎tests/CMakeLists.txt‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,14 @@ function(add_aligator_test name)
4949
_add_test_prototype(${name} "" ${PROJECT_NAME})
5050
endfunction()
5151

52-
set(TEST_NAMES robot_handler problem mpc lowlevel)
52+
set(
53+
TEST_NAMES
54+
robot_handler
55+
problem
56+
mpc
57+
qpsolvers
58+
interpolator
59+
)
5360

5461
foreach(test_name ${TEST_NAMES})
5562
add_aligator_test(${test_name})

‎tests/interpolator.cpp‎

Lines changed: 41 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,41 @@
1+
2+
#include <boost/test/unit_test.hpp>
3+
4+
#include "simple-mpc/interpolator.hpp"
5+
#include "test_utils.cpp"
6+
7+
BOOST_AUTO_TEST_SUITE(interpolator)
8+
9+
using namespace simple_mpc;
10+
11+
BOOST_AUTO_TEST_CASE(interpolate)
12+
{
13+
long nx = 27;
14+
long nu = 12;
15+
long nv = 18;
16+
long nf = 12;
17+
double MPC_timestep = 0.01;
18+
Interpolator interpolator = Interpolator(nx, nv, nu, nf, MPC_timestep);
19+
20+
std::vector<Eigen::VectorXd> xs;
21+
std::vector<Eigen::VectorXd> us;
22+
std::vector<Eigen::VectorXd> ddqs;
23+
std::vector<Eigen::VectorXd> forces;
24+
for (std::size_t i = 0; i < 4; i++)
25+
{
26+
xs.push_back(Eigen::Vector<double, 27>::Random());
27+
us.push_back(Eigen::Vector<double, 12>::Random());
28+
ddqs.push_back(Eigen::Vector<double, 18>::Random());
29+
forces.push_back(Eigen::Vector<double, 12>::Random());
30+
}
31+
double delay = 0.02;
32+
33+
interpolator.interpolate(delay, xs, us, ddqs, forces);
34+
35+
BOOST_CHECK(xs[2].isApprox(interpolator.x_interpolated_));
36+
BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_));
37+
BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_));
38+
BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_));
39+
}
40+
41+
BOOST_AUTO_TEST_SUITE_END()

0 commit comments

Comments
 (0)