From 6c5e05dfe64b91fb783eda48a82b200ee8ad2ed7 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 26 Feb 2025 15:26:16 +0100 Subject: [PATCH 1/4] Get contact forces from fulldyn mpc --- bindings/expose-mpc.cpp | 4 ++++ examples/go2_fulldynamics.py | 24 ++++++++++++++---------- include/simple-mpc/mpc.hpp | 30 ++++++++++++++++++++++++++++++ tests/mpc.cpp | 2 ++ 4 files changed, 50 insertions(+), 10 deletions(-) diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index 65bcba54..696d2121 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -64,9 +64,12 @@ namespace simple_mpc void exposeMPC() { using StageVec = std::vector>; + using ForceVec = std::vector; using MapBool = std::map; StdVectorPythonVisitor::expose( "StdVec_StageModel", eigenpy::details::overload_base_get_item_for_std_vector()); + StdVectorPythonVisitor::expose( + "StdVec_Force", eigenpy::details::overload_base_get_item_for_std_vector()); StdVectorPythonVisitor, true>::expose("StdVec_MapBool"); @@ -88,6 +91,7 @@ namespace simple_mpc .def("getFootTakeoffCycle", &MPC::getFootTakeoffCycle, ("self"_a, "ee_name")) .def("getFootLandCycle", &MPC::getFootLandCycle, ("self"_a, "ee_name")) .def("getStateDerivative", &MPC::getStateDerivative, ("self"_a, "t")) + .def("getContactForces", &MPC::getContactForces, ("self"_a, "t")) .def("getCyclingContactState", &MPC::getCyclingContactState, ("self"_a, "t", "ee_name")) .def( "getModelHandler", &MPC::getModelHandler, "self"_a, bp::return_internal_reference<>(), diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index c754cea6..fde4354e 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -44,7 +44,7 @@ w_basepos = [0, 0, 0, 0, 0, 0] w_legpos = [10, 10, 10] -w_basevel = [10, 10, 10, 10, 10, 10] +w_basevel = [0, 0, 0, 0, 0, 0] w_legvel = [0.1, 0.1, 0.1] w_x = np.array(w_basepos + w_legpos * 4 + w_basevel + w_legvel * 4) w_cent_lin = np.array([0.0, 0.0, 0]) @@ -201,9 +201,9 @@ L_measured = [] v = np.zeros(6) -v[0] = 0. +v[5] = 0.2 mpc.velocity_base = v -for t in range(500): +for t in range(1000): print("Time " + str(t)) land_LF = mpc.getFootLandCycle("FL_foot") land_RF = mpc.getFootLandCycle("RL_foot") @@ -214,7 +214,10 @@ str(land_RF) + ", takeoff_LF = " + str(takeoff_LF) + ", landing_LF = ", str(land_LF), ) """ - + if t == 400: + v = np.zeros(6) + v[0] = 0.2 + mpc.velocity_base = v """ if t == 200: for s in range(T): device.resetState(mpc.xs[s][:nq]) @@ -238,13 +241,14 @@ a0 = mpc.getStateDerivative(0)[nv:] a1 = mpc.getStateDerivative(1)[nv:] - FL_f, FR_f, RL_f, RR_f = extract_forces(mpc.getTrajOptProblem(), mpc.solver.workspace, 0) + forces_vec = mpc.getContactForces(0) contact_states = mpc.ocp_handler.getContactState(0) - total_forces = np.concatenate((FL_f, FR_f, RL_f, RR_f)) - force_FL.append(FL_f) - force_FR.append(FR_f) - force_RL.append(RL_f) - force_RR.append(RR_f) + total_forces = np.concatenate((forces_vec[0], forces_vec[1], forces_vec[2], forces_vec[3])) + + force_FL.append(forces_vec[0]) + force_FR.append(forces_vec[1]) + force_RL.append(forces_vec[2]) + force_RR.append(forces_vec[3]) forces = [total_forces, total_forces] ddqs = [a0, a1] diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index 03a5e6de..60199088 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include "simple-mpc/deprecated.hpp" @@ -22,6 +23,8 @@ namespace simple_mpc { using ExplicitIntegratorData = dynamics::ExplicitIntegratorDataTpl; + using MultibodyConstraintFwdData = dynamics::MultibodyConstraintFwdDataTpl; + using MultibodyConstraintFwdDynamics = dynamics::MultibodyConstraintFwdDynamicsTpl; struct MPCSettings { @@ -183,6 +186,33 @@ namespace simple_mpc return int_data->continuous_data->xdot_; } + const std::vector getContactForces(const std::size_t t) + { + std::vector contact_forces; + ExplicitIntegratorData * int_data = + dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); + assert(int_data != nullptr); + MultibodyConstraintFwdData * mc_data = dynamic_cast(&*int_data->continuous_data); + assert(mc_data != nullptr); + + std::vector contact_state = ocp_handler_->getContactState(t); + + size_t force_id = 0; + for (size_t i = 0; i < contact_state.size(); i++) + { + if (contact_state[i]) + { + contact_forces.push_back(mc_data->constraint_datas_[force_id].contact_force.linear()); + force_id += 1; + } + else + { + contact_forces.push_back(Eigen::Vector3d::Zero()); + } + } + return contact_forces; + } + void switchToWalk(const Vector6d & velocity_base); void switchToStand(); diff --git a/tests/mpc.cpp b/tests/mpc.cpp index 4143fc1e..1f11a744 100644 --- a/tests/mpc.cpp +++ b/tests/mpc.cpp @@ -90,6 +90,8 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics) BOOST_CHECK_EQUAL(mpc.foot_land_times_.at("right_sole_link")[0], 150); Eigen::VectorXd xdot = mpc.getStateDerivative(0); + + std::vector forces = mpc.getContactForces(0); } BOOST_AUTO_TEST_CASE(mpc_kinodynamics) From 1a35ad51be458d6c2d483aa25b29222129f0546a Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Wed, 26 Feb 2025 16:06:46 +0100 Subject: [PATCH 2/4] Change to Eigen::Vector --- bindings/expose-mpc.cpp | 3 --- examples/go2_fulldynamics.py | 14 +++++++------- include/simple-mpc/mpc.hpp | 10 ++++++---- tests/mpc.cpp | 2 +- 4 files changed, 14 insertions(+), 15 deletions(-) diff --git a/bindings/expose-mpc.cpp b/bindings/expose-mpc.cpp index 696d2121..131409a1 100644 --- a/bindings/expose-mpc.cpp +++ b/bindings/expose-mpc.cpp @@ -64,12 +64,9 @@ namespace simple_mpc void exposeMPC() { using StageVec = std::vector>; - using ForceVec = std::vector; using MapBool = std::map; StdVectorPythonVisitor::expose( "StdVec_StageModel", eigenpy::details::overload_base_get_item_for_std_vector()); - StdVectorPythonVisitor::expose( - "StdVec_Force", eigenpy::details::overload_base_get_item_for_std_vector()); StdVectorPythonVisitor, true>::expose("StdVec_MapBool"); diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index fde4354e..c780f841 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -241,16 +241,16 @@ a0 = mpc.getStateDerivative(0)[nv:] a1 = mpc.getStateDerivative(1)[nv:] - forces_vec = mpc.getContactForces(0) + forces_vec0 = mpc.getContactForces(0) + forces_vec1 = mpc.getContactForces(1) contact_states = mpc.ocp_handler.getContactState(0) - total_forces = np.concatenate((forces_vec[0], forces_vec[1], forces_vec[2], forces_vec[3])) - force_FL.append(forces_vec[0]) - force_FR.append(forces_vec[1]) - force_RL.append(forces_vec[2]) - force_RR.append(forces_vec[3]) + force_FL.append(forces_vec0[:3]) + force_FR.append(forces_vec0[3:6]) + force_RL.append(forces_vec0[6:9]) + force_RR.append(forces_vec0[9:12]) - forces = [total_forces, total_forces] + forces = [forces_vec0, forces_vec1] ddqs = [a0, a1] xss = [mpc.xs[0], mpc.xs[1]] uss = [mpc.us[0], mpc.us[1]] diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index 60199088..8959fb8f 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -186,9 +186,11 @@ namespace simple_mpc return int_data->continuous_data->xdot_; } - const std::vector getContactForces(const std::size_t t) + const Eigen::VectorXd getContactForces(const std::size_t t) { - std::vector contact_forces; + Eigen::VectorXd contact_forces; + contact_forces.resize(3 * (long)ee_names_.size()); + ExplicitIntegratorData * int_data = dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); assert(int_data != nullptr); @@ -202,12 +204,12 @@ namespace simple_mpc { if (contact_state[i]) { - contact_forces.push_back(mc_data->constraint_datas_[force_id].contact_force.linear()); + contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear(); force_id += 1; } else { - contact_forces.push_back(Eigen::Vector3d::Zero()); + contact_forces.segment((long)i * 3, 3).setZero(); } } return contact_forces; diff --git a/tests/mpc.cpp b/tests/mpc.cpp index 1f11a744..e021c009 100644 --- a/tests/mpc.cpp +++ b/tests/mpc.cpp @@ -91,7 +91,7 @@ BOOST_AUTO_TEST_CASE(mpc_fulldynamics) Eigen::VectorXd xdot = mpc.getStateDerivative(0); - std::vector forces = mpc.getContactForces(0); + Eigen::VectorXd forces = mpc.getContactForces(0); } BOOST_AUTO_TEST_CASE(mpc_kinodynamics) From 41de090dd255973853a21f7e09a4404a4d155330 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Thu, 27 Feb 2025 15:58:49 +0100 Subject: [PATCH 3/4] Restore example fulldyn --- examples/go2_fulldynamics.py | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/examples/go2_fulldynamics.py b/examples/go2_fulldynamics.py index c780f841..2ce70547 100644 --- a/examples/go2_fulldynamics.py +++ b/examples/go2_fulldynamics.py @@ -44,7 +44,7 @@ w_basepos = [0, 0, 0, 0, 0, 0] w_legpos = [10, 10, 10] -w_basevel = [0, 0, 0, 0, 0, 0] +w_basevel = [10, 10, 10, 10, 10, 10] w_legvel = [0.1, 0.1, 0.1] w_x = np.array(w_basepos + w_legpos * 4 + w_basevel + w_legvel * 4) w_cent_lin = np.array([0.0, 0.0, 0]) @@ -201,9 +201,9 @@ L_measured = [] v = np.zeros(6) -v[5] = 0.2 +v[0] = 0.2 mpc.velocity_base = v -for t in range(1000): +for t in range(500): print("Time " + str(t)) land_LF = mpc.getFootLandCycle("FL_foot") land_RF = mpc.getFootLandCycle("RL_foot") @@ -214,10 +214,6 @@ str(land_RF) + ", takeoff_LF = " + str(takeoff_LF) + ", landing_LF = ", str(land_LF), ) """ - if t == 400: - v = np.zeros(6) - v[0] = 0.2 - mpc.velocity_base = v """ if t == 200: for s in range(T): device.resetState(mpc.xs[s][:nq]) From 64c0e860e2d7557a0fc0fcc91a5efc42968fa1c1 Mon Sep 17 00:00:00 2001 From: Ewen Dantec Date: Thu, 27 Feb 2025 16:31:43 +0100 Subject: [PATCH 4/4] Shift code to .cpp --- include/simple-mpc/mpc.hpp | 41 ++++++-------------------------------- src/mpc.cpp | 37 ++++++++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+), 35 deletions(-) diff --git a/include/simple-mpc/mpc.hpp b/include/simple-mpc/mpc.hpp index 8959fb8f..a3e09a77 100644 --- a/include/simple-mpc/mpc.hpp +++ b/include/simple-mpc/mpc.hpp @@ -178,42 +178,13 @@ namespace simple_mpc } } - const ConstVectorRef getStateDerivative(const std::size_t t) - { - ExplicitIntegratorData * int_data = - dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); - assert(int_data != nullptr); - return int_data->continuous_data->xdot_; - } - - const Eigen::VectorXd getContactForces(const std::size_t t) - { - Eigen::VectorXd contact_forces; - contact_forces.resize(3 * (long)ee_names_.size()); - - ExplicitIntegratorData * int_data = - dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); - assert(int_data != nullptr); - MultibodyConstraintFwdData * mc_data = dynamic_cast(&*int_data->continuous_data); - assert(mc_data != nullptr); + const ConstVectorRef getStateDerivative(const std::size_t t); - std::vector contact_state = ocp_handler_->getContactState(t); - - size_t force_id = 0; - for (size_t i = 0; i < contact_state.size(); i++) - { - if (contact_state[i]) - { - contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear(); - force_id += 1; - } - else - { - contact_forces.segment((long)i * 3, 3).setZero(); - } - } - return contact_forces; - } + /** + * @brief Return contact forces for a full dynamics MPC problem + * @warning Only work with fulldynamics OCP handler + */ + const Eigen::VectorXd getContactForces(const std::size_t t); void switchToWalk(const Vector6d & velocity_base); diff --git a/src/mpc.cpp b/src/mpc.cpp index 74d36af0..917fcd12 100644 --- a/src/mpc.cpp +++ b/src/mpc.cpp @@ -342,6 +342,43 @@ namespace simple_mpc return ocp_handler_->getProblem(); } + const ConstVectorRef MPC::getStateDerivative(const std::size_t t) + { + ExplicitIntegratorData * int_data = + dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); + assert(int_data != nullptr); + return int_data->continuous_data->xdot_; + } + + const Eigen::VectorXd MPC::getContactForces(const std::size_t t) + { + Eigen::VectorXd contact_forces; + contact_forces.resize(3 * (long)ee_names_.size()); + + ExplicitIntegratorData * int_data = + dynamic_cast(&*solver_->workspace_.problem_data.stage_data[t]->dynamics_data); + assert(int_data != nullptr); + MultibodyConstraintFwdData * mc_data = dynamic_cast(&*int_data->continuous_data); + assert(mc_data != nullptr); + + std::vector contact_state = ocp_handler_->getContactState(t); + + size_t force_id = 0; + for (size_t i = 0; i < contact_state.size(); i++) + { + if (contact_state[i]) + { + contact_forces.segment((long)i * 3, 3) = mc_data->constraint_datas_[force_id].contact_force.linear(); + force_id += 1; + } + else + { + contact_forces.segment((long)i * 3, 3).setZero(); + } + } + return contact_forces; + } + void MPC::switchToWalk(const Vector6d & velocity_base) { now_ = WALKING;