From 0514b18a65bf43dbec82a15d9f66d5a9bb2c29b1 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Tue, 4 Feb 2025 17:29:27 +0100 Subject: [PATCH 1/9] First draft of interpolators --- bindings/CMakeLists.txt | 3 +- bindings/expose-interpolate.cpp | 34 ++++++++++++ .../{expose-lowlevel.cpp => expose-qp.cpp} | 2 +- include/simple-mpc/interpolator.hpp | 44 ++++++++++++++++ .../{lowlevel-control.hpp => qp-solvers.hpp} | 11 ++-- src/fulldynamics.cpp | 4 +- src/interpolator.cpp | 52 +++++++++++++++++++ src/{lowlevel-control.cpp => qp-solvers.cpp} | 4 +- tests/CMakeLists.txt | 3 +- tests/interpolator.cpp | 41 +++++++++++++++ tests/{lowlevel.cpp => qpsolvers.cpp} | 4 +- 11 files changed, 187 insertions(+), 15 deletions(-) create mode 100644 bindings/expose-interpolate.cpp rename bindings/{expose-lowlevel.cpp => expose-qp.cpp} (99%) create mode 100644 include/simple-mpc/interpolator.hpp rename include/simple-mpc/{lowlevel-control.hpp => qp-solvers.hpp} (96%) create mode 100644 src/interpolator.cpp rename src/{lowlevel-control.cpp => qp-solvers.cpp} (99%) create mode 100644 tests/interpolator.cpp rename tests/{lowlevel.cpp => qpsolvers.cpp} (98%) diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 35c47bbf..137cecfc 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -9,7 +9,8 @@ set( expose-robot-handler.cpp expose-problem.cpp expose-mpc.cpp - expose-lowlevel.cpp + expose-qp.cpp + expose-interpolate.cpp expose-centroidal.cpp expose-fulldynamics.cpp expose-kinodynamics.cpp diff --git a/bindings/expose-interpolate.cpp b/bindings/expose-interpolate.cpp new file mode 100644 index 00000000..58f9f7ac --- /dev/null +++ b/bindings/expose-interpolate.cpp @@ -0,0 +1,34 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 2-Clause License +// +// Copyright (C) 2025, INRIA +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// + +#include +#include + +#include "simple-mpc/interpolator.hpp" + +namespace simple_mpc +{ + namespace python + { + namespace bp = boost::python; + + void exposeInterpolator() + { + bp::class_( + "Interpolator", bp::init( + bp::args("self", "nx", "nv", "nu", "nu", "MPC_timestep"))) + .def("interpolate", &Interpolator::interpolate) + .add_property("MPC_timestep", &Interpolator::MPC_timestep_) + .add_property("x_interpolated", &Interpolator::x_interpolated_) + .add_property("u_interpolated", &Interpolator::u_interpolated_) + .add_property("a_interpolated", &Interpolator::a_interpolated_) + .add_property("forces_interpolated", &Interpolator::forces_interpolated_); + } + + } // namespace python +} // namespace simple_mpc diff --git a/bindings/expose-lowlevel.cpp b/bindings/expose-qp.cpp similarity index 99% rename from bindings/expose-lowlevel.cpp rename to bindings/expose-qp.cpp index 9cf2677b..e3cc3422 100644 --- a/bindings/expose-lowlevel.cpp +++ b/bindings/expose-qp.cpp @@ -10,7 +10,7 @@ #include #include -#include "simple-mpc/lowlevel-control.hpp" +#include "simple-mpc/qp-solvers.hpp" #include #include diff --git a/include/simple-mpc/interpolator.hpp b/include/simple-mpc/interpolator.hpp new file mode 100644 index 00000000..269b00e1 --- /dev/null +++ b/include/simple-mpc/interpolator.hpp @@ -0,0 +1,44 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 3-Clause License +// +// Copyright (C) 2025, INRIA +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// +/** + * @file interpolator.hpp + * @brief Interpolation class for practical control of the robot + */ + +#ifndef SIMPLE_MPC_INTERPOLATOR_HPP_ +#define SIMPLE_MPC_INTERPOLATOR_HPP_ + +#include "simple-mpc/fwd.hpp" + +namespace simple_mpc +{ + class Interpolator + { + public: + explicit Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep); + + void interpolate( + const double delay, + std::vector xs, + std::vector us, + std::vector ddqs, + std::vector forces); + + Eigen::VectorXd x_interpolated_; + Eigen::VectorXd u_interpolated_; + Eigen::VectorXd a_interpolated_; + Eigen::VectorXd forces_interpolated_; + double MPC_timestep_; + }; +} // namespace simple_mpc + +/* --- Details -------------------------------------------------------------- */ +/* --- Details -------------------------------------------------------------- */ +/* --- Details -------------------------------------------------------------- */ + +#endif // SIMPLE_MPC_INTERPOLATOR_HPP_ diff --git a/include/simple-mpc/lowlevel-control.hpp b/include/simple-mpc/qp-solvers.hpp similarity index 96% rename from include/simple-mpc/lowlevel-control.hpp rename to include/simple-mpc/qp-solvers.hpp index 8756960c..e27b1b1e 100644 --- a/include/simple-mpc/lowlevel-control.hpp +++ b/include/simple-mpc/qp-solvers.hpp @@ -6,13 +6,12 @@ // All rights reserved. /////////////////////////////////////////////////////////////////////////////// /** - * @file lowlevel-control.hpp - * @brief Build a low-level control for kinodynamics - * and centroidal MPC schemes + * @file qp-solvers.hpp + * @brief QP inverse dynamics for MPC schemes */ -#ifndef SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_ -#define SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_ +#ifndef SIMPLE_MPC_QP_SOLVERS_HPP_ +#define SIMPLE_MPC_QP_SOLVERS_HPP_ #include "simple-mpc/deprecated.hpp" #include "simple-mpc/fwd.hpp" @@ -241,4 +240,4 @@ namespace simple_mpc /* --- Details -------------------------------------------------------------- */ /* --- Details -------------------------------------------------------------- */ -#endif // SIMPLE_MPC_LOWLEVEL_CONTROL_HPP_ +#endif // SIMPLE_MPC_QP_SOLVERS_HPP_ diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp index 32787f09..8b1c8d77 100644 --- a/src/fulldynamics.cpp +++ b/src/fulldynamics.cpp @@ -195,7 +195,7 @@ namespace simple_mpc space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name), pinocchio::LOCAL_WORLD_ALIGNED); FunctionSliceXpr vel_slice = FunctionSliceXpr(velocity_residual, vel_id); - stm.addConstraint(vel_slice, EqualityConstraint()); + // stm.addConstraint(vel_slice, EqualityConstraint()); std::vector frame_id = {2}; @@ -205,7 +205,7 @@ namespace simple_mpc FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id); - stm.addConstraint(frame_slice, EqualityConstraint()); + // stm.addConstraint(frame_slice, EqualityConstraint()); } } } diff --git a/src/interpolator.cpp b/src/interpolator.cpp new file mode 100644 index 00000000..c257e41c --- /dev/null +++ b/src/interpolator.cpp @@ -0,0 +1,52 @@ +/////////////////////////////////////////////////////////////////////////////// +// BSD 2-Clause License +// +// Copyright (C) 2025, INRIA +// Copyright note valid unless otherwise stated in individual files. +// All rights reserved. +/////////////////////////////////////////////////////////////////////////////// +#include "simple-mpc/interpolator.hpp" + +namespace simple_mpc +{ + + Interpolator::Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep) + : MPC_timestep_(MPC_timestep) + { + x_interpolated_.resize(nx); + u_interpolated_.resize(nu); + a_interpolated_.resize(nv); + forces_interpolated_.resize(nf); + } + + void Interpolator::interpolate( + const double delay, + std::vector xs, + std::vector us, + std::vector ddqs, + std::vector forces) + { + // Compute the time knot corresponding to the current delay + size_t step_nb = static_cast(delay / MPC_timestep_); + double step_progress = (delay - (double)step_nb * MPC_timestep_) / MPC_timestep_; + + // Interpolate state and command trajectories + if (step_nb >= xs.size() - 1) + { + step_nb = xs.size() - 1; + step_progress = 0.0; + x_interpolated_ = xs[step_nb]; + u_interpolated_ = us[step_nb]; + a_interpolated_ = ddqs[step_nb]; + forces_interpolated_ = forces[step_nb]; + } + else + { + x_interpolated_ = xs[step_nb + 1] * step_progress + xs[step_nb] * (1. - step_progress); + u_interpolated_ = us[step_nb + 1] * step_progress + us[step_nb] * (1. - step_progress); + a_interpolated_ = ddqs[step_nb + 1] * step_progress + ddqs[step_nb] * (1. - step_progress); + forces_interpolated_ = forces[step_nb + 1] * step_progress + forces[step_nb] * (1. - step_progress); + } + } + +} // namespace simple_mpc diff --git a/src/lowlevel-control.cpp b/src/qp-solvers.cpp similarity index 99% rename from src/lowlevel-control.cpp rename to src/qp-solvers.cpp index fd87c91f..f5437b2e 100644 --- a/src/lowlevel-control.cpp +++ b/src/qp-solvers.cpp @@ -5,7 +5,7 @@ // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// -#include "simple-mpc/lowlevel-control.hpp" +#include "simple-mpc/qp-solvers.hpp" #include #include #include @@ -75,7 +75,7 @@ namespace simple_mpc } // Set the block matrix for torque limits - C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6); + // C_.bottomRightCorner(model_.nv - 6, model_.nv - 6).diagonal() = Eigen::VectorXd::Ones(model_.nv - 6); // Set size of solutions solved_forces_.resize(force_dim_); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index b6ef9edb..a2f194aa 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -54,8 +54,9 @@ set( robot_handler problem mpc - lowlevel friction + qpsolvers + interpolator ) foreach(test_name ${TEST_NAMES}) diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp new file mode 100644 index 00000000..b5df981d --- /dev/null +++ b/tests/interpolator.cpp @@ -0,0 +1,41 @@ + +#include + +#include "simple-mpc/interpolator.hpp" +#include "test_utils.cpp" + +BOOST_AUTO_TEST_SUITE(interpolator) + +using namespace simple_mpc; + +BOOST_AUTO_TEST_CASE(interpolate) +{ + long nx = 27; + long nu = 12; + long nv = 18; + long nf = 12; + double MPC_timestep = 0.01; + Interpolator interpolator = Interpolator(nx, nv, nu, nf, MPC_timestep); + + std::vector xs; + std::vector us; + std::vector ddqs; + std::vector forces; + for (std::size_t i = 0; i < 4; i++) + { + xs.push_back(Eigen::Vector::Random()); + us.push_back(Eigen::Vector::Random()); + ddqs.push_back(Eigen::Vector::Random()); + forces.push_back(Eigen::Vector::Random()); + } + double delay = 0.02; + + interpolator.interpolate(delay, xs, us, ddqs, forces); + + BOOST_CHECK(xs[2].isApprox(interpolator.x_interpolated_)); + BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_)); + BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_)); + BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_)); +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/tests/lowlevel.cpp b/tests/qpsolvers.cpp similarity index 98% rename from tests/lowlevel.cpp rename to tests/qpsolvers.cpp index 2d604592..72711ece 100644 --- a/tests/lowlevel.cpp +++ b/tests/qpsolvers.cpp @@ -2,11 +2,11 @@ #include #include -#include "simple-mpc/lowlevel-control.hpp" +#include "simple-mpc/qp-solvers.hpp" #include "simple-mpc/robot-handler.hpp" #include "test_utils.cpp" -BOOST_AUTO_TEST_SUITE(lowlevel) +BOOST_AUTO_TEST_SUITE(qpsolvers) using namespace simple_mpc; From 5657e3d85af5b2819df0586e513058eafcbb5541 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 5 Feb 2025 11:02:52 +0100 Subject: [PATCH 2/9] Access to step_nb and step_progress --- bindings/expose-interpolate.cpp | 4 +++- include/simple-mpc/interpolator.hpp | 9 +++++++++ src/interpolator.cpp | 26 +++++++++++++------------- tests/interpolator.cpp | 2 ++ 4 files changed, 27 insertions(+), 14 deletions(-) diff --git a/bindings/expose-interpolate.cpp b/bindings/expose-interpolate.cpp index 58f9f7ac..ca1aa670 100644 --- a/bindings/expose-interpolate.cpp +++ b/bindings/expose-interpolate.cpp @@ -27,7 +27,9 @@ namespace simple_mpc .add_property("x_interpolated", &Interpolator::x_interpolated_) .add_property("u_interpolated", &Interpolator::u_interpolated_) .add_property("a_interpolated", &Interpolator::a_interpolated_) - .add_property("forces_interpolated", &Interpolator::forces_interpolated_); + .add_property("forces_interpolated", &Interpolator::forces_interpolated_) + .add_property("step_nb", &Interpolator::step_nb_) + .add_property("step_progress", &Interpolator::step_progress_); } } // namespace python diff --git a/include/simple-mpc/interpolator.hpp b/include/simple-mpc/interpolator.hpp index 269b00e1..4f47b4de 100644 --- a/include/simple-mpc/interpolator.hpp +++ b/include/simple-mpc/interpolator.hpp @@ -29,11 +29,20 @@ namespace simple_mpc std::vector ddqs, std::vector forces); + // Interpolated trajectories Eigen::VectorXd x_interpolated_; Eigen::VectorXd u_interpolated_; Eigen::VectorXd a_interpolated_; Eigen::VectorXd forces_interpolated_; + + // Timestep of trajectories double MPC_timestep_; + + // Time knot associated with current delay + size_t step_nb_; + + // Interpolation time between two knots + double step_progress_; }; } // namespace simple_mpc diff --git a/src/interpolator.cpp b/src/interpolator.cpp index c257e41c..2ae2643b 100644 --- a/src/interpolator.cpp +++ b/src/interpolator.cpp @@ -27,25 +27,25 @@ namespace simple_mpc std::vector forces) { // Compute the time knot corresponding to the current delay - size_t step_nb = static_cast(delay / MPC_timestep_); - double step_progress = (delay - (double)step_nb * MPC_timestep_) / MPC_timestep_; + step_nb_ = static_cast(delay / MPC_timestep_); + step_progress_ = (delay - (double)step_nb_ * MPC_timestep_) / MPC_timestep_; // Interpolate state and command trajectories - if (step_nb >= xs.size() - 1) + if (step_nb_ >= xs.size() - 1) { - step_nb = xs.size() - 1; - step_progress = 0.0; - x_interpolated_ = xs[step_nb]; - u_interpolated_ = us[step_nb]; - a_interpolated_ = ddqs[step_nb]; - forces_interpolated_ = forces[step_nb]; + step_nb_ = xs.size() - 1; + step_progress_ = 0.0; + x_interpolated_ = xs[step_nb_]; + u_interpolated_ = us[step_nb_]; + a_interpolated_ = ddqs[step_nb_]; + forces_interpolated_ = forces[step_nb_]; } else { - x_interpolated_ = xs[step_nb + 1] * step_progress + xs[step_nb] * (1. - step_progress); - u_interpolated_ = us[step_nb + 1] * step_progress + us[step_nb] * (1. - step_progress); - a_interpolated_ = ddqs[step_nb + 1] * step_progress + ddqs[step_nb] * (1. - step_progress); - forces_interpolated_ = forces[step_nb + 1] * step_progress + forces[step_nb] * (1. - step_progress); + x_interpolated_ = xs[step_nb_ + 1] * step_progress_ + xs[step_nb_] * (1. - step_progress_); + u_interpolated_ = us[step_nb_ + 1] * step_progress_ + us[step_nb_] * (1. - step_progress_); + a_interpolated_ = ddqs[step_nb_ + 1] * step_progress_ + ddqs[step_nb_] * (1. - step_progress_); + forces_interpolated_ = forces[step_nb_ + 1] * step_progress_ + forces[step_nb_] * (1. - step_progress_); } } diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp index b5df981d..7558fa57 100644 --- a/tests/interpolator.cpp +++ b/tests/interpolator.cpp @@ -36,6 +36,8 @@ BOOST_AUTO_TEST_CASE(interpolate) BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_)); BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_)); BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_)); + BOOST_CHECK(interpolator.step_nb_ == 2); + BOOST_CHECK(interpolator.step_progress_ == 0.); } BOOST_AUTO_TEST_SUITE_END() From 82ab1052ef62f640a948840b71dfd8a72b762da0 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 5 Feb 2025 13:13:51 +0100 Subject: [PATCH 3/9] Expose interpolator --- bindings/module.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bindings/module.cpp b/bindings/module.cpp index 696757fd..6ccbee72 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -18,6 +18,7 @@ namespace simple_mpc::python void exposeIDSolver(); void exposeIKIDSolver(); void exposeFrictionCompensation(); + void exposeInterpolator(); /* PYTHON MODULE */ BOOST_PYTHON_MODULE(simple_mpc_pywrap) @@ -37,6 +38,7 @@ namespace simple_mpc::python exposeIDSolver(); exposeIKIDSolver(); exposeFrictionCompensation(); + exposeInterpolator(); } } // namespace simple_mpc::python From 9b5aa57fe8deb9243345dff3cfa33c021286be81 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 5 Feb 2025 14:17:38 +0100 Subject: [PATCH 4/9] Use interpolation in python examples --- examples/go2_fulldynamics.py | 26 +++++++++++++++----------- examples/go2_kinodynamics.py | 26 +++++++++++++++++--------- 2 files changed, 32 insertions(+), 20 deletions(-) diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 427eb7b2..eb8641c5 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -1,6 +1,6 @@ import numpy as np from bullet_robot import BulletRobot -from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation +from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation, Interpolator import example_robot_data as erd import pinocchio as pin import time @@ -26,6 +26,8 @@ nu = nv - 6 force_size = 3 nk = len(model_handler.getFeetNames()) +nf = force_size + gravity = np.array([0, 0, -9.81]) fref = np.zeros(force_size) fref[2] = -model_handler.getMass() / nk * gravity[2] @@ -144,6 +146,8 @@ """ Friction """ fcompensation = FrictionCompensation(model_handler.getModel(), True) +""" Interpolation """ +interpolator = Interpolator(nq + nv, nv, nu, nf, dt) """ Initialize simulation""" device = BulletRobot( @@ -155,8 +159,6 @@ model_handler.getReferenceState()[:3], ) -nq = mpc.getModelHandler().getModel().nq -nv = mpc.getModelHandler().getModel().nv device.initializeJoints(model_handler.getReferenceState()[:nq]) for i in range(40): @@ -237,6 +239,11 @@ force_RL.append(RL_f) force_RR.append(RR_f) + forces = [total_forces, total_forces] + ddqs = [a0, a1] + xss = [mpc.xs[0], mpc.xs[1]] + uss = [mpc.us[0], mpc.us[1]] + FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation) FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation) RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation) @@ -251,27 +258,24 @@ for j in range(N_simu): # time.sleep(0.01) - u_interp = (N_simu - j) / N_simu * mpc.us[0] + j / N_simu * mpc.us[1] - a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1 - x_interp = (N_simu - j) / N_simu * mpc.xs[0] + j / N_simu * mpc.xs[1] - K_interp = (N_simu - j) / N_simu * mpc.Ks[0] + j / N_simu * mpc.Ks[1] + interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) mpc.getDataHandler().updateInternalData(x_measured, True) - current_torque = u_interp - 1. * mpc.Ks[0] @ model_handler.difference( - x_measured, x_interp + current_torque = interpolator.u_interpolated - 1. * mpc.Ks[0] @ model_handler.difference( + x_measured, interpolator.x_interpolated ) qp.solveQP( mpc.getDataHandler().getData(), contact_states, x_measured[nq:], - a0, + interpolator.a_interpolated, current_torque, - total_forces, + interpolator.forces_interpolated, mpc.getDataHandler().getData().M, ) diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index e3a6ea02..6a34b636 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -1,6 +1,6 @@ import numpy as np from bullet_robot import BulletRobot -from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver +from simple_mpc import RobotModelHandler, RobotDataHandler, KinodynamicsOCP, MPC, IDSolver, Interpolator import example_robot_data as erd import pinocchio as pin import time @@ -21,6 +21,10 @@ model_handler.addFoot("RR_foot", base_joint_name, pin.XYZQUATToSE3(np.array([-0.24,-0.15, 0.0, 0,0,0,1]))) data_handler = RobotDataHandler(model_handler) +nq = model_handler.getModel().nq +nv = model_handler.getModel().nv +nu = nv - 6 +nf = 12 force_size = 3 nk = len(model_handler.getFeetNames()) gravity = np.array([0, 0, -9.81]) @@ -144,6 +148,9 @@ qp = IDSolver(id_conf, model_handler.getModel()) +""" Interpolation """ +interpolator = Interpolator(nq + nv, nv, nu, nf, 0.01) + """ Initialize simulation""" device = BulletRobot( model_handler.getModel().names, @@ -155,10 +162,7 @@ ) device.initializeJoints(model_handler.getReferenceState()[:model_handler.getModel().nq]) - device.changeCamera(1.0, 60, -15, [0.6, -0.2, 0.5]) -nq = mpc.getModelHandler().getModel().nq -nv = mpc.getModelHandler().getModel().nv q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) @@ -234,6 +238,11 @@ forces1 = mpc.us[1][: nk * force_size] contact_states = mpc.ocp_handler.getContactState(0) + forces = [forces0, forces1] + ddqs = [a0, a1] + xss = [mpc.xs[0], mpc.xs[1]] + uss = [mpc.us[0], mpc.us[1]] + device.moveQuadrupedFeet( mpc.getReferencePose(0, "FL_foot").translation, mpc.getReferencePose(0, "FR_foot").translation, @@ -243,21 +252,20 @@ for j in range(N_simu): # time.sleep(0.01) + interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces) + q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) mpc.getDataHandler().updateInternalData(x_measured, True) - a_interp = (N_simu - j) / N_simu * a0 + j / N_simu * a1 - f_interp = (N_simu - j) / N_simu * forces0 + j / N_simu * forces1 - qp.solveQP( mpc.getDataHandler().getData(), contact_states, x_measured[nq:], - a_interp, + interpolator.a_interpolated, np.zeros(12), - f_interp, + interpolator.forces_interpolated, mpc.getDataHandler().getData().M, ) From 3001e810274e6a83564d47dc94c487de1e178f00 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 14 Feb 2025 14:07:36 +0100 Subject: [PATCH 5/9] State and linear interpolators --- bindings/expose-interpolate.cpp | 39 +++++++++++------ examples/go2_fulldynamics.py | 30 ++++++++++--- include/simple-mpc/interpolator.hpp | 39 +++++++++-------- src/interpolator.cpp | 65 +++++++++++++++++----------- tests/interpolator.cpp | 66 ++++++++++++++++++++--------- 5 files changed, 159 insertions(+), 80 deletions(-) diff --git a/bindings/expose-interpolate.cpp b/bindings/expose-interpolate.cpp index ca1aa670..1dd3ee65 100644 --- a/bindings/expose-interpolate.cpp +++ b/bindings/expose-interpolate.cpp @@ -17,19 +17,34 @@ namespace simple_mpc { namespace bp = boost::python; - void exposeInterpolator() + Eigen::VectorXd stateInterpolateProxy( + StateInterpolator & self, const double delay, const double timestep, const std::vector xs) { - bp::class_( - "Interpolator", bp::init( - bp::args("self", "nx", "nv", "nu", "nu", "MPC_timestep"))) - .def("interpolate", &Interpolator::interpolate) - .add_property("MPC_timestep", &Interpolator::MPC_timestep_) - .add_property("x_interpolated", &Interpolator::x_interpolated_) - .add_property("u_interpolated", &Interpolator::u_interpolated_) - .add_property("a_interpolated", &Interpolator::a_interpolated_) - .add_property("forces_interpolated", &Interpolator::forces_interpolated_) - .add_property("step_nb", &Interpolator::step_nb_) - .add_property("step_progress", &Interpolator::step_progress_); + Eigen::VectorXd x_interp(xs[0].size()); + self.interpolate(delay, timestep, xs, x_interp); + + return x_interp; + } + + Eigen::VectorXd linearInterpolateProxy( + LinearInterpolator & self, const double delay, const double timestep, const std::vector xs) + { + Eigen::VectorXd x_interp(xs[0].size()); + self.interpolate(delay, timestep, xs, x_interp); + + return x_interp; + } + + void exposeStateInterpolator() + { + bp::class_("StateInterpolator", bp::init(bp::args("self", "model"))) + .def("interpolate", &stateInterpolateProxy); + } + + void exposeLinearInterpolator() + { + bp::class_("LinearInterpolator", bp::init(bp::args("self", "vec_size"))) + .def("interpolate", &linearInterpolateProxy); } } // namespace python diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index eb8641c5..2cf48722 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -1,6 +1,15 @@ import numpy as np from bullet_robot import BulletRobot -from simple_mpc import RobotModelHandler, RobotDataHandler, FullDynamicsOCP, MPC, IDSolver, FrictionCompensation, Interpolator +from simple_mpc import ( + RobotModelHandler, + RobotDataHandler, + FullDynamicsOCP, + MPC, + IDSolver, + StateInterpolator, + LinearInterpolator, + FrictionCompensation +) import example_robot_data as erd import pinocchio as pin import time @@ -147,7 +156,10 @@ """ Friction """ fcompensation = FrictionCompensation(model_handler.getModel(), True) """ Interpolation """ -interpolator = Interpolator(nq + nv, nv, nu, nf, dt) +state_interpolator = StateInterpolator(model_handler.getModel()) +acc_interpolator = LinearInterpolator(model_handler.getModel().nv) +u_interpolator = LinearInterpolator(nu) +force_interpolator = LinearInterpolator(nf) """ Initialize simulation""" device = BulletRobot( @@ -258,24 +270,28 @@ for j in range(N_simu): # time.sleep(0.01) - interpolator.interpolate(j / float(N_simu), xss, uss, ddqs, forces) + delay = j / float(N_simu) + x_interp = state_interpolator.interpolate(delay, dt, xss) + u_interp = u_interpolator.interpolate(delay, dt, uss) + acc_interp = acc_interpolator.interpolate(delay, dt, ddqs) + force_interp = force_interpolator.interpolate(delay, dt, forces) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) mpc.getDataHandler().updateInternalData(x_measured, True) - current_torque = interpolator.u_interpolated - 1. * mpc.Ks[0] @ model_handler.difference( - x_measured, interpolator.x_interpolated + current_torque = u_interp - 1. * mpc.Ks[0] @ model_handler.difference( + x_measured, x_interp ) qp.solveQP( mpc.getDataHandler().getData(), contact_states, x_measured[nq:], - interpolator.a_interpolated, + acc_interp, current_torque, - interpolator.forces_interpolated, + force_interp, mpc.getDataHandler().getData().M, ) diff --git a/include/simple-mpc/interpolator.hpp b/include/simple-mpc/interpolator.hpp index 4f47b4de..1f2a41b1 100644 --- a/include/simple-mpc/interpolator.hpp +++ b/include/simple-mpc/interpolator.hpp @@ -14,35 +14,40 @@ #define SIMPLE_MPC_INTERPOLATOR_HPP_ #include "simple-mpc/fwd.hpp" +#include "simple-mpc/model-utils.hpp" namespace simple_mpc { - class Interpolator + class StateInterpolator { public: - explicit Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep); + explicit StateInterpolator(const Model & model); void interpolate( const double delay, - std::vector xs, - std::vector us, - std::vector ddqs, - std::vector forces); + const double timestep, + const std::vector xs, + Eigen::Ref x_interp); - // Interpolated trajectories - Eigen::VectorXd x_interpolated_; - Eigen::VectorXd u_interpolated_; - Eigen::VectorXd a_interpolated_; - Eigen::VectorXd forces_interpolated_; + // Intermediate differential configuration + Eigen::VectorXd diff_q_; - // Timestep of trajectories - double MPC_timestep_; + // Pinocchio model + Model model_; + }; + + class LinearInterpolator + { + public: + explicit LinearInterpolator(const size_t vec_size); - // Time knot associated with current delay - size_t step_nb_; + void interpolate( + const double delay, + const double timestep, + const std::vector vecs, + Eigen::Ref vec_interp); - // Interpolation time between two knots - double step_progress_; + size_t vec_size_; }; } // namespace simple_mpc diff --git a/src/interpolator.cpp b/src/interpolator.cpp index 2ae2643b..86e0b3a0 100644 --- a/src/interpolator.cpp +++ b/src/interpolator.cpp @@ -10,42 +10,59 @@ namespace simple_mpc { - Interpolator::Interpolator(const long nx, const long nv, const long nu, const long nf, const double MPC_timestep) - : MPC_timestep_(MPC_timestep) + StateInterpolator::StateInterpolator(const Model & model) { - x_interpolated_.resize(nx); - u_interpolated_.resize(nu); - a_interpolated_.resize(nv); - forces_interpolated_.resize(nf); + model_ = model; + diff_q_.resize(model_.nv); } - void Interpolator::interpolate( + void StateInterpolator::interpolate( const double delay, - std::vector xs, - std::vector us, - std::vector ddqs, - std::vector forces) + const double timestep, + const std::vector xs, + Eigen::Ref x_interp) { // Compute the time knot corresponding to the current delay - step_nb_ = static_cast(delay / MPC_timestep_); - step_progress_ = (delay - (double)step_nb_ * MPC_timestep_) / MPC_timestep_; + size_t step_nb = static_cast(delay / timestep); + double step_progress = (delay - (double)step_nb * timestep) / timestep; // Interpolate state and command trajectories - if (step_nb_ >= xs.size() - 1) + if (step_nb >= xs.size() - 1) + x_interp = xs[step_nb]; + else { - step_nb_ = xs.size() - 1; - step_progress_ = 0.0; - x_interpolated_ = xs[step_nb_]; - u_interpolated_ = us[step_nb_]; - a_interpolated_ = ddqs[step_nb_]; - forces_interpolated_ = forces[step_nb_]; + // Compute the differential between configuration + diff_q_ = pinocchio::difference(model_, xs[step_nb].head(model_.nq), xs[step_nb + 1].head(model_.nq)); + + pinocchio::integrate(model_, xs[step_nb].head(model_.nq), diff_q_ * step_progress, x_interp.head(model_.nq)); + + // Compute velocity interpolation + x_interp.tail(model_.nv) = + xs[step_nb + 1].tail(model_.nv) * step_progress + xs[step_nb].tail(model_.nv) * (1. - step_progress); } + } + + LinearInterpolator::LinearInterpolator(const size_t vec_size) + { + vec_size_ = vec_size; + } + + void LinearInterpolator::interpolate( + const double delay, + const double timestep, + const std::vector vecs, + Eigen::Ref vec_interp) + { + // Compute the time knot corresponding to the current delay + size_t step_nb = static_cast(delay / timestep); + double step_progress = (delay - (double)step_nb * timestep) / timestep; + + // Interpolate state and command trajectories + if (step_nb >= vecs.size() - 1) + vec_interp = vecs[step_nb]; else { - x_interpolated_ = xs[step_nb_ + 1] * step_progress_ + xs[step_nb_] * (1. - step_progress_); - u_interpolated_ = us[step_nb_ + 1] * step_progress_ + us[step_nb_] * (1. - step_progress_); - a_interpolated_ = ddqs[step_nb_ + 1] * step_progress_ + ddqs[step_nb_] * (1. - step_progress_); - forces_interpolated_ = forces[step_nb_ + 1] * step_progress_ + forces[step_nb_] * (1. - step_progress_); + vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress); } } diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp index 7558fa57..c1387689 100644 --- a/tests/interpolator.cpp +++ b/tests/interpolator.cpp @@ -10,34 +10,60 @@ using namespace simple_mpc; BOOST_AUTO_TEST_CASE(interpolate) { - long nx = 27; - long nu = 12; - long nv = 18; - long nf = 12; - double MPC_timestep = 0.01; - Interpolator interpolator = Interpolator(nx, nv, nu, nf, MPC_timestep); + Model model; + // Load pinocchio model from example robot data + const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"; + const std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/srdf/solo.srdf"; + + pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); + double timestep = 0.01; + StateInterpolator interpolator = StateInterpolator(model); std::vector xs; - std::vector us; - std::vector ddqs; - std::vector forces; for (std::size_t i = 0; i < 4; i++) { - xs.push_back(Eigen::Vector::Random()); - us.push_back(Eigen::Vector::Random()); - ddqs.push_back(Eigen::Vector::Random()); - forces.push_back(Eigen::Vector::Random()); + Eigen::VectorXd x0(model.nq + model.nv); + x0.tail(model.nv).setRandom(); + x0.head(model.nq) = pinocchio::neutral(model); + Eigen::VectorXd dq(model.nv); + dq.setRandom(); + pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq)); + + xs.push_back(x0); + } + double delay = 0.02; + + Eigen::VectorXd x_interp(model.nq + model.nv); + interpolator.interpolate(delay, timestep, xs, x_interp); + + BOOST_CHECK(xs[2].isApprox(x_interp)); +} + +BOOST_AUTO_TEST_CASE(linear_interpolate) +{ + Model model; + // Load pinocchio model from example robot data + const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"; + const std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/srdf/solo.srdf"; + + pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); + double timestep = 0.01; + LinearInterpolator interpolator = LinearInterpolator((size_t)model.nv); + + std::vector vs; + for (std::size_t i = 0; i < 4; i++) + { + Eigen::VectorXd v0(model.nv); + v0.setRandom(); + + vs.push_back(v0); } double delay = 0.02; - interpolator.interpolate(delay, xs, us, ddqs, forces); + Eigen::VectorXd v_interp(model.nv); + interpolator.interpolate(delay, timestep, vs, v_interp); - BOOST_CHECK(xs[2].isApprox(interpolator.x_interpolated_)); - BOOST_CHECK(us[2].isApprox(interpolator.u_interpolated_)); - BOOST_CHECK(ddqs[2].isApprox(interpolator.a_interpolated_)); - BOOST_CHECK(forces[2].isApprox(interpolator.forces_interpolated_)); - BOOST_CHECK(interpolator.step_nb_ == 2); - BOOST_CHECK(interpolator.step_progress_ == 0.); + BOOST_CHECK(vs[2].isApprox(v_interp)); } BOOST_AUTO_TEST_SUITE_END() From b8a9607eaecd167782d41ec7a372e2d076816919 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 14 Feb 2025 15:04:03 +0100 Subject: [PATCH 6/9] Working tests --- bindings/module.cpp | 6 ++++-- examples/go2_fulldynamics.py | 3 ++- src/fulldynamics.cpp | 4 ++-- src/interpolator.cpp | 4 ++-- tests/interpolator.cpp | 26 +++++++++++++++++++++++++- 5 files changed, 35 insertions(+), 8 deletions(-) diff --git a/bindings/module.cpp b/bindings/module.cpp index 6ccbee72..b40d2aec 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -17,8 +17,9 @@ namespace simple_mpc::python void exposeMPC(); void exposeIDSolver(); void exposeIKIDSolver(); + void exposeStateInterpolator(); + void exposeLinearInterpolator(); void exposeFrictionCompensation(); - void exposeInterpolator(); /* PYTHON MODULE */ BOOST_PYTHON_MODULE(simple_mpc_pywrap) @@ -37,8 +38,9 @@ namespace simple_mpc::python exposeMPC(); exposeIDSolver(); exposeIKIDSolver(); + exposeStateInterpolator(); + exposeLinearInterpolator(); exposeFrictionCompensation(); - exposeInterpolator(); } } // namespace simple_mpc::python diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 2cf48722..93ff2565 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -270,7 +270,8 @@ for j in range(N_simu): # time.sleep(0.01) - delay = j / float(N_simu) + delay = j / float(N_simu) * dt + x_interp = state_interpolator.interpolate(delay, dt, xss) u_interp = u_interpolator.interpolate(delay, dt, uss) acc_interp = acc_interpolator.interpolate(delay, dt, ddqs) diff --git a/src/fulldynamics.cpp b/src/fulldynamics.cpp index 8b1c8d77..32787f09 100644 --- a/src/fulldynamics.cpp +++ b/src/fulldynamics.cpp @@ -195,7 +195,7 @@ namespace simple_mpc space.ndx(), nu_, model_handler_.getModel(), Motion::Zero(), model_handler_.getFootId(name), pinocchio::LOCAL_WORLD_ALIGNED); FunctionSliceXpr vel_slice = FunctionSliceXpr(velocity_residual, vel_id); - // stm.addConstraint(vel_slice, EqualityConstraint()); + stm.addConstraint(vel_slice, EqualityConstraint()); std::vector frame_id = {2}; @@ -205,7 +205,7 @@ namespace simple_mpc FunctionSliceXpr frame_slice = FunctionSliceXpr(frame_residual, frame_id); - // stm.addConstraint(frame_slice, EqualityConstraint()); + stm.addConstraint(frame_slice, EqualityConstraint()); } } } diff --git a/src/interpolator.cpp b/src/interpolator.cpp index 86e0b3a0..da751d3d 100644 --- a/src/interpolator.cpp +++ b/src/interpolator.cpp @@ -28,7 +28,7 @@ namespace simple_mpc // Interpolate state and command trajectories if (step_nb >= xs.size() - 1) - x_interp = xs[step_nb]; + x_interp = xs.back(); else { // Compute the differential between configuration @@ -59,7 +59,7 @@ namespace simple_mpc // Interpolate state and command trajectories if (step_nb >= vecs.size() - 1) - vec_interp = vecs[step_nb]; + vec_interp = vecs.back(); else { vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress); diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp index c1387689..93eb2a5a 100644 --- a/tests/interpolator.cpp +++ b/tests/interpolator.cpp @@ -37,6 +37,20 @@ BOOST_AUTO_TEST_CASE(interpolate) interpolator.interpolate(delay, timestep, xs, x_interp); BOOST_CHECK(xs[2].isApprox(x_interp)); + + delay = 0.5; + interpolator.interpolate(delay, timestep, xs, x_interp); + BOOST_CHECK(xs.back().isApprox(x_interp)); + + delay = 0.005; + interpolator.interpolate(delay, timestep, xs, x_interp); + Eigen::VectorXd x_interp2(model.nq + model.nv); + Eigen::VectorXd dq(model.nv); + pinocchio::difference(model, xs[0].head(model.nq), xs[1].head(model.nq), dq); + pinocchio::integrate(model, xs[0].head(model.nq), dq * 0.5, x_interp2.head(model.nq)); + x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5; + + BOOST_CHECK(x_interp2.isApprox(x_interp)); } BOOST_AUTO_TEST_CASE(linear_interpolate) @@ -62,8 +76,18 @@ BOOST_AUTO_TEST_CASE(linear_interpolate) Eigen::VectorXd v_interp(model.nv); interpolator.interpolate(delay, timestep, vs, v_interp); - BOOST_CHECK(vs[2].isApprox(v_interp)); + + delay = 0.5; + interpolator.interpolate(delay, timestep, vs, v_interp); + BOOST_CHECK(vs.back().isApprox(v_interp)); + + delay = 0.005; + interpolator.interpolate(delay, timestep, vs, v_interp); + Eigen::VectorXd v_interp2(model.nv); + v_interp2 = (vs[0] + vs[1]) * 0.5; + + BOOST_CHECK(v_interp2.isApprox(v_interp)); } BOOST_AUTO_TEST_SUITE_END() From 981473d20c901ef9fcd10a010cbef94507df7208 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 14 Feb 2025 15:07:53 +0100 Subject: [PATCH 7/9] interpolate btwn two equal points --- tests/interpolator.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp index 93eb2a5a..3e59f242 100644 --- a/tests/interpolator.cpp +++ b/tests/interpolator.cpp @@ -51,6 +51,14 @@ BOOST_AUTO_TEST_CASE(interpolate) x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5; BOOST_CHECK(x_interp2.isApprox(x_interp)); + + std::vector xs2; + for (std::size_t i = 0; i < 2; i++) + { + xs2.push_back(xs[0]); + } + interpolator.interpolate(delay, timestep, xs2, x_interp); + BOOST_CHECK(xs2[0].isApprox(x_interp)); } BOOST_AUTO_TEST_CASE(linear_interpolate) @@ -88,6 +96,14 @@ BOOST_AUTO_TEST_CASE(linear_interpolate) v_interp2 = (vs[0] + vs[1]) * 0.5; BOOST_CHECK(v_interp2.isApprox(v_interp)); + + std::vector vs2; + for (std::size_t i = 0; i < 2; i++) + { + vs2.push_back(vs[0]); + } + interpolator.interpolate(delay, timestep, vs2, v_interp); + BOOST_CHECK(vs2[0].isApprox(v_interp)); } BOOST_AUTO_TEST_SUITE_END() From 9f413a1518d90da6121454569e7f8ac2399da984 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Fri, 14 Feb 2025 15:37:52 +0100 Subject: [PATCH 8/9] Add include to pinocchio --- src/interpolator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/interpolator.cpp b/src/interpolator.cpp index da751d3d..bd5f46a7 100644 --- a/src/interpolator.cpp +++ b/src/interpolator.cpp @@ -6,6 +6,7 @@ // All rights reserved. /////////////////////////////////////////////////////////////////////////////// #include "simple-mpc/interpolator.hpp" +#include namespace simple_mpc { From f05351efc6a182cc7ca9cc707eb89b17ea90f64c Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Mon, 17 Feb 2025 19:25:47 +0100 Subject: [PATCH 9/9] One class for all interpolate methods --- bindings/expose-interpolate.cpp | 34 ++++++--------- bindings/module.cpp | 6 +-- examples/go2_fulldynamics.py | 22 +++++----- include/simple-mpc/interpolator.hpp | 32 +++++--------- src/interpolator.cpp | 46 ++++++++----------- tests/interpolator.cpp | 68 ++++++++++++----------------- 6 files changed, 84 insertions(+), 124 deletions(-) diff --git a/bindings/expose-interpolate.cpp b/bindings/expose-interpolate.cpp index 1dd3ee65..2794366f 100644 --- a/bindings/expose-interpolate.cpp +++ b/bindings/expose-interpolate.cpp @@ -17,35 +17,29 @@ namespace simple_mpc { namespace bp = boost::python; - Eigen::VectorXd stateInterpolateProxy( - StateInterpolator & self, const double delay, const double timestep, const std::vector xs) + Eigen::VectorXd interpolateConfigurationProxy( + Interpolator & self, const double delay, const double timestep, const std::vector & qs) { - Eigen::VectorXd x_interp(xs[0].size()); - self.interpolate(delay, timestep, xs, x_interp); + Eigen::VectorXd q_interp(qs[0].size()); + self.interpolateConfiguration(delay, timestep, qs, q_interp); - return x_interp; + return q_interp; } - Eigen::VectorXd linearInterpolateProxy( - LinearInterpolator & self, const double delay, const double timestep, const std::vector xs) + Eigen::VectorXd interpolateLinearProxy( + Interpolator & self, const double delay, const double timestep, const std::vector & vs) { - Eigen::VectorXd x_interp(xs[0].size()); - self.interpolate(delay, timestep, xs, x_interp); + Eigen::VectorXd v_interp(vs[0].size()); + self.interpolateLinear(delay, timestep, vs, v_interp); - return x_interp; + return v_interp; } - void exposeStateInterpolator() + void exposeInterpolator() { - bp::class_("StateInterpolator", bp::init(bp::args("self", "model"))) - .def("interpolate", &stateInterpolateProxy); + bp::class_("Interpolator", bp::init(bp::args("self", "model"))) + .def("interpolateConfiguration", &interpolateConfigurationProxy) + .def("interpolateLinear", &interpolateLinearProxy); } - - void exposeLinearInterpolator() - { - bp::class_("LinearInterpolator", bp::init(bp::args("self", "vec_size"))) - .def("interpolate", &linearInterpolateProxy); - } - } // namespace python } // namespace simple_mpc diff --git a/bindings/module.cpp b/bindings/module.cpp index b40d2aec..1871c79d 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -17,8 +17,7 @@ namespace simple_mpc::python void exposeMPC(); void exposeIDSolver(); void exposeIKIDSolver(); - void exposeStateInterpolator(); - void exposeLinearInterpolator(); + void exposeInterpolator(); void exposeFrictionCompensation(); /* PYTHON MODULE */ @@ -38,8 +37,7 @@ namespace simple_mpc::python exposeMPC(); exposeIDSolver(); exposeIKIDSolver(); - exposeStateInterpolator(); - exposeLinearInterpolator(); + exposeInterpolator(); exposeFrictionCompensation(); } diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 93ff2565..07847f39 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -6,8 +6,7 @@ FullDynamicsOCP, MPC, IDSolver, - StateInterpolator, - LinearInterpolator, + Interpolator, FrictionCompensation ) import example_robot_data as erd @@ -156,10 +155,7 @@ """ Friction """ fcompensation = FrictionCompensation(model_handler.getModel(), True) """ Interpolation """ -state_interpolator = StateInterpolator(model_handler.getModel()) -acc_interpolator = LinearInterpolator(model_handler.getModel().nv) -u_interpolator = LinearInterpolator(nu) -force_interpolator = LinearInterpolator(nf) +interpolator = Interpolator(model_handler.getModel()) """ Initialize simulation""" device = BulletRobot( @@ -253,7 +249,8 @@ forces = [total_forces, total_forces] ddqs = [a0, a1] - xss = [mpc.xs[0], mpc.xs[1]] + qss = [mpc.xs[0][:model_handler.getModel().nq], mpc.xs[1][:model_handler.getModel().nq]] + vss = [mpc.xs[0][model_handler.getModel().nq:], mpc.xs[1][model_handler.getModel().nq:]] uss = [mpc.us[0], mpc.us[1]] FL_measured.append(mpc.getDataHandler().getFootPose("FL_foot").translation) @@ -272,10 +269,13 @@ # time.sleep(0.01) delay = j / float(N_simu) * dt - x_interp = state_interpolator.interpolate(delay, dt, xss) - u_interp = u_interpolator.interpolate(delay, dt, uss) - acc_interp = acc_interpolator.interpolate(delay, dt, ddqs) - force_interp = force_interpolator.interpolate(delay, dt, forces) + q_interp = interpolator.interpolateConfiguration(delay, dt, qss) + v_interp = interpolator.interpolateLinear(delay, dt, vss) + u_interp = interpolator.interpolateLinear(delay, dt, uss) + acc_interp = interpolator.interpolateLinear(delay, dt, ddqs) + force_interp = interpolator.interpolateLinear(delay, dt, forces) + + x_interp = np.concatenate((q_interp, v_interp)) q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) diff --git a/include/simple-mpc/interpolator.hpp b/include/simple-mpc/interpolator.hpp index 1f2a41b1..15481796 100644 --- a/include/simple-mpc/interpolator.hpp +++ b/include/simple-mpc/interpolator.hpp @@ -13,42 +13,34 @@ #ifndef SIMPLE_MPC_INTERPOLATOR_HPP_ #define SIMPLE_MPC_INTERPOLATOR_HPP_ +#include + #include "simple-mpc/fwd.hpp" #include "simple-mpc/model-utils.hpp" namespace simple_mpc { - class StateInterpolator + class Interpolator { public: - explicit StateInterpolator(const Model & model); + explicit Interpolator(const Model & model); - void interpolate( + void interpolateConfiguration( const double delay, const double timestep, - const std::vector xs, - Eigen::Ref x_interp); + const std::vector & qs, + Eigen::Ref q_interp); - // Intermediate differential configuration - Eigen::VectorXd diff_q_; + void interpolateLinear( + const double delay, + const double timestep, + const std::vector & vs, + Eigen::Ref v_interp); // Pinocchio model Model model_; }; - class LinearInterpolator - { - public: - explicit LinearInterpolator(const size_t vec_size); - - void interpolate( - const double delay, - const double timestep, - const std::vector vecs, - Eigen::Ref vec_interp); - - size_t vec_size_; - }; } // namespace simple_mpc /* --- Details -------------------------------------------------------------- */ diff --git a/src/interpolator.cpp b/src/interpolator.cpp index bd5f46a7..3dfa330d 100644 --- a/src/interpolator.cpp +++ b/src/interpolator.cpp @@ -6,64 +6,52 @@ // All rights reserved. /////////////////////////////////////////////////////////////////////////////// #include "simple-mpc/interpolator.hpp" -#include namespace simple_mpc { - StateInterpolator::StateInterpolator(const Model & model) + Interpolator::Interpolator(const Model & model) { model_ = model; - diff_q_.resize(model_.nv); } - void StateInterpolator::interpolate( + void Interpolator::interpolateConfiguration( const double delay, const double timestep, - const std::vector xs, - Eigen::Ref x_interp) + const std::vector & qs, + Eigen::Ref q_interp) { + assert(("Configuration is not of the right size", qs[0].size() == model_.nq)); + // Compute the time knot corresponding to the current delay size_t step_nb = static_cast(delay / timestep); double step_progress = (delay - (double)step_nb * timestep) / timestep; - // Interpolate state and command trajectories - if (step_nb >= xs.size() - 1) - x_interp = xs.back(); + // Interpolate configuration trajectory + if (step_nb >= qs.size() - 1) + q_interp = qs.back(); else { - // Compute the differential between configuration - diff_q_ = pinocchio::difference(model_, xs[step_nb].head(model_.nq), xs[step_nb + 1].head(model_.nq)); - - pinocchio::integrate(model_, xs[step_nb].head(model_.nq), diff_q_ * step_progress, x_interp.head(model_.nq)); - - // Compute velocity interpolation - x_interp.tail(model_.nv) = - xs[step_nb + 1].tail(model_.nv) * step_progress + xs[step_nb].tail(model_.nv) * (1. - step_progress); + q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress); } } - LinearInterpolator::LinearInterpolator(const size_t vec_size) - { - vec_size_ = vec_size; - } - - void LinearInterpolator::interpolate( + void Interpolator::interpolateLinear( const double delay, const double timestep, - const std::vector vecs, - Eigen::Ref vec_interp) + const std::vector & vs, + Eigen::Ref v_interp) { // Compute the time knot corresponding to the current delay size_t step_nb = static_cast(delay / timestep); double step_progress = (delay - (double)step_nb * timestep) / timestep; - // Interpolate state and command trajectories - if (step_nb >= vecs.size() - 1) - vec_interp = vecs.back(); + // Interpolate configuration trajectory + if (step_nb >= vs.size() - 1) + v_interp = vs.back(); else { - vec_interp = vecs[step_nb + 1] * step_progress + vecs[step_nb] * (1. - step_progress); + v_interp = vs[step_nb + 1] * step_progress + vs[step_nb] * (1. - step_progress); } } diff --git a/tests/interpolator.cpp b/tests/interpolator.cpp index 3e59f242..22cf41fe 100644 --- a/tests/interpolator.cpp +++ b/tests/interpolator.cpp @@ -17,61 +17,50 @@ BOOST_AUTO_TEST_CASE(interpolate) pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); double timestep = 0.01; - StateInterpolator interpolator = StateInterpolator(model); + Interpolator interpolator = Interpolator(model); - std::vector xs; + // Test configuration interpolation method + std::vector qs; for (std::size_t i = 0; i < 4; i++) { - Eigen::VectorXd x0(model.nq + model.nv); - x0.tail(model.nv).setRandom(); - x0.head(model.nq) = pinocchio::neutral(model); + Eigen::VectorXd q0(model.nq); + Eigen::VectorXd q1(model.nq); + q0 = pinocchio::neutral(model); Eigen::VectorXd dq(model.nv); dq.setRandom(); - pinocchio::integrate(model, x0.head(model.nq), dq, x0.head(model.nq)); + pinocchio::integrate(model, q0, dq, q1); - xs.push_back(x0); + qs.push_back(q1); } double delay = 0.02; - Eigen::VectorXd x_interp(model.nq + model.nv); - interpolator.interpolate(delay, timestep, xs, x_interp); + Eigen::VectorXd q_interp(model.nq); + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); - BOOST_CHECK(xs[2].isApprox(x_interp)); + BOOST_CHECK(qs[2].isApprox(q_interp)); delay = 0.5; - interpolator.interpolate(delay, timestep, xs, x_interp); - BOOST_CHECK(xs.back().isApprox(x_interp)); + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); + BOOST_CHECK(qs.back().isApprox(q_interp)); delay = 0.005; - interpolator.interpolate(delay, timestep, xs, x_interp); - Eigen::VectorXd x_interp2(model.nq + model.nv); + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); + Eigen::VectorXd q_interp2(model.nq); Eigen::VectorXd dq(model.nv); - pinocchio::difference(model, xs[0].head(model.nq), xs[1].head(model.nq), dq); - pinocchio::integrate(model, xs[0].head(model.nq), dq * 0.5, x_interp2.head(model.nq)); - x_interp2.tail(model.nv) = (xs[0].tail(model.nv) + xs[1].tail(model.nv)) * 0.5; + pinocchio::difference(model, qs[0], qs[1], dq); + pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2); - BOOST_CHECK(x_interp2.isApprox(x_interp)); + BOOST_CHECK(q_interp2.isApprox(q_interp)); - std::vector xs2; + std::vector qs2; for (std::size_t i = 0; i < 2; i++) { - xs2.push_back(xs[0]); + qs2.push_back(qs[0]); } - interpolator.interpolate(delay, timestep, xs2, x_interp); - BOOST_CHECK(xs2[0].isApprox(x_interp)); -} - -BOOST_AUTO_TEST_CASE(linear_interpolate) -{ - Model model; - // Load pinocchio model from example robot data - const std::string urdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/robots/solo12.urdf"; - const std::string srdf_path = EXAMPLE_ROBOT_DATA_MODEL_DIR "/solo_description/srdf/solo.srdf"; - - pinocchio::urdf::buildModel(urdf_path, JointModelFreeFlyer(), model); - double timestep = 0.01; - LinearInterpolator interpolator = LinearInterpolator((size_t)model.nv); + interpolator.interpolateConfiguration(delay, timestep, qs2, q_interp); + BOOST_CHECK(qs2[0].isApprox(q_interp)); + // Test linear interpolation method std::vector vs; for (std::size_t i = 0; i < 4; i++) { @@ -80,18 +69,17 @@ BOOST_AUTO_TEST_CASE(linear_interpolate) vs.push_back(v0); } - double delay = 0.02; - + delay = 0.02; Eigen::VectorXd v_interp(model.nv); - interpolator.interpolate(delay, timestep, vs, v_interp); + interpolator.interpolateLinear(delay, timestep, vs, v_interp); BOOST_CHECK(vs[2].isApprox(v_interp)); delay = 0.5; - interpolator.interpolate(delay, timestep, vs, v_interp); + interpolator.interpolateLinear(delay, timestep, vs, v_interp); BOOST_CHECK(vs.back().isApprox(v_interp)); delay = 0.005; - interpolator.interpolate(delay, timestep, vs, v_interp); + interpolator.interpolateLinear(delay, timestep, vs, v_interp); Eigen::VectorXd v_interp2(model.nv); v_interp2 = (vs[0] + vs[1]) * 0.5; @@ -102,7 +90,7 @@ BOOST_AUTO_TEST_CASE(linear_interpolate) { vs2.push_back(vs[0]); } - interpolator.interpolate(delay, timestep, vs2, v_interp); + interpolator.interpolateLinear(delay, timestep, vs2, v_interp); BOOST_CHECK(vs2[0].isApprox(v_interp)); }