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..2794366f --- /dev/null +++ b/bindings/expose-interpolate.cpp @@ -0,0 +1,45 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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; + + Eigen::VectorXd interpolateConfigurationProxy( + Interpolator & self, const double delay, const double timestep, const std::vector & qs) + { + Eigen::VectorXd q_interp(qs[0].size()); + self.interpolateConfiguration(delay, timestep, qs, q_interp); + + return q_interp; + } + + Eigen::VectorXd interpolateLinearProxy( + Interpolator & self, const double delay, const double timestep, const std::vector & vs) + { + Eigen::VectorXd v_interp(vs[0].size()); + self.interpolateLinear(delay, timestep, vs, v_interp); + + return v_interp; + } + + void exposeInterpolator() + { + bp::class_("Interpolator", bp::init(bp::args("self", "model"))) + .def("interpolateConfiguration", &interpolateConfigurationProxy) + .def("interpolateLinear", &interpolateLinearProxy); + } + } // 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/bindings/module.cpp b/bindings/module.cpp index 696757fd..1871c79d 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -17,6 +17,7 @@ namespace simple_mpc::python void exposeMPC(); void exposeIDSolver(); void exposeIKIDSolver(); + void exposeInterpolator(); void exposeFrictionCompensation(); /* PYTHON MODULE */ @@ -36,6 +37,7 @@ namespace simple_mpc::python exposeMPC(); exposeIDSolver(); exposeIKIDSolver(); + exposeInterpolator(); exposeFrictionCompensation(); } diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index 427eb7b2..07847f39 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -1,6 +1,14 @@ 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, + Interpolator, + FrictionCompensation +) import example_robot_data as erd import pinocchio as pin import time @@ -26,6 +34,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 +154,8 @@ """ Friction """ fcompensation = FrictionCompensation(model_handler.getModel(), True) +""" Interpolation """ +interpolator = Interpolator(model_handler.getModel()) """ Initialize simulation""" device = BulletRobot( @@ -155,8 +167,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 +247,12 @@ force_RL.append(RL_f) force_RR.append(RR_f) + forces = [total_forces, total_forces] + ddqs = [a0, a1] + 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) FR_measured.append(mpc.getDataHandler().getFootPose("FR_foot").translation) RL_measured.append(mpc.getDataHandler().getFootPose("RL_foot").translation) @@ -251,10 +267,15 @@ 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] + delay = j / float(N_simu) * dt + + 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]) @@ -269,9 +290,9 @@ mpc.getDataHandler().getData(), contact_states, x_measured[nq:], - a0, + acc_interp, current_torque, - total_forces, + force_interp, 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, ) diff --git a/include/simple-mpc/interpolator.hpp b/include/simple-mpc/interpolator.hpp new file mode 100644 index 00000000..15481796 --- /dev/null +++ b/include/simple-mpc/interpolator.hpp @@ -0,0 +1,50 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 + +#include "simple-mpc/fwd.hpp" +#include "simple-mpc/model-utils.hpp" + +namespace simple_mpc +{ + class Interpolator + { + public: + explicit Interpolator(const Model & model); + + void interpolateConfiguration( + const double delay, + const double timestep, + const std::vector & qs, + Eigen::Ref q_interp); + + void interpolateLinear( + const double delay, + const double timestep, + const std::vector & vs, + Eigen::Ref v_interp); + + // Pinocchio model + Model model_; + }; + +} // 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/interpolator.cpp b/src/interpolator.cpp new file mode 100644 index 00000000..3dfa330d --- /dev/null +++ b/src/interpolator.cpp @@ -0,0 +1,58 @@ +/////////////////////////////////////////////////////////////////////////////// +// 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 Model & model) + { + model_ = model; + } + + void Interpolator::interpolateConfiguration( + const double delay, + const double timestep, + 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 configuration trajectory + if (step_nb >= qs.size() - 1) + q_interp = qs.back(); + else + { + q_interp = pinocchio::interpolate(model_, qs[step_nb], qs[step_nb + 1], step_progress); + } + } + + void Interpolator::interpolateLinear( + const double delay, + const double timestep, + 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 configuration trajectory + if (step_nb >= vs.size() - 1) + v_interp = vs.back(); + else + { + v_interp = vs[step_nb + 1] * step_progress + vs[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..22cf41fe --- /dev/null +++ b/tests/interpolator.cpp @@ -0,0 +1,97 @@ + +#include + +#include "simple-mpc/interpolator.hpp" +#include "test_utils.cpp" + +BOOST_AUTO_TEST_SUITE(interpolator) + +using namespace simple_mpc; + +BOOST_AUTO_TEST_CASE(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; + Interpolator interpolator = Interpolator(model); + + // Test configuration interpolation method + std::vector qs; + for (std::size_t i = 0; i < 4; i++) + { + Eigen::VectorXd q0(model.nq); + Eigen::VectorXd q1(model.nq); + q0 = pinocchio::neutral(model); + Eigen::VectorXd dq(model.nv); + dq.setRandom(); + pinocchio::integrate(model, q0, dq, q1); + + qs.push_back(q1); + } + double delay = 0.02; + + Eigen::VectorXd q_interp(model.nq); + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); + + BOOST_CHECK(qs[2].isApprox(q_interp)); + + delay = 0.5; + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); + BOOST_CHECK(qs.back().isApprox(q_interp)); + + delay = 0.005; + interpolator.interpolateConfiguration(delay, timestep, qs, q_interp); + Eigen::VectorXd q_interp2(model.nq); + Eigen::VectorXd dq(model.nv); + pinocchio::difference(model, qs[0], qs[1], dq); + pinocchio::integrate(model, qs[0], dq * 0.5, q_interp2); + + BOOST_CHECK(q_interp2.isApprox(q_interp)); + + std::vector qs2; + for (std::size_t i = 0; i < 2; i++) + { + qs2.push_back(qs[0]); + } + 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++) + { + Eigen::VectorXd v0(model.nv); + v0.setRandom(); + + vs.push_back(v0); + } + delay = 0.02; + Eigen::VectorXd v_interp(model.nv); + interpolator.interpolateLinear(delay, timestep, vs, v_interp); + BOOST_CHECK(vs[2].isApprox(v_interp)); + + delay = 0.5; + interpolator.interpolateLinear(delay, timestep, vs, v_interp); + BOOST_CHECK(vs.back().isApprox(v_interp)); + + delay = 0.005; + interpolator.interpolateLinear(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)); + + std::vector vs2; + for (std::size_t i = 0; i < 2; i++) + { + vs2.push_back(vs[0]); + } + interpolator.interpolateLinear(delay, timestep, vs2, v_interp); + BOOST_CHECK(vs2[0].isApprox(v_interp)); +} + +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;