From e5a7679a401092eefa1ba0dc319621c842acb7d4 Mon Sep 17 00:00:00 2001 From: earlaud Date: Sat, 25 Oct 2025 15:38:33 +0200 Subject: [PATCH 1/4] Remove deleted binding expose functions --- bindings/module.cpp | 2 -- include/simple-mpc/fwd.hpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/bindings/module.cpp b/bindings/module.cpp index c1c7aaa1..e2fe0609 100644 --- a/bindings/module.cpp +++ b/bindings/module.cpp @@ -15,8 +15,6 @@ namespace simple_mpc::python void exposeCentroidalOcp(); void exposeKinodynamicsOcp(); void exposeMPC(); - void exposeIDSolver(); - void exposeIKIDSolver(); void exposeInterpolator(); void exposeFrictionCompensation(); void exposeInverseDynamics(); diff --git a/include/simple-mpc/fwd.hpp b/include/simple-mpc/fwd.hpp index f6425f5a..457a7d20 100644 --- a/include/simple-mpc/fwd.hpp +++ b/include/simple-mpc/fwd.hpp @@ -36,8 +36,6 @@ namespace simple_mpc class KinodynamicsOCP; class CentroidalOCP; class OCPHandler; - class IDSolver; - class IKIDSolver; class FrictionCompensation; /// EIGEN TYPEDEFS From 0497429019e8736bbfafcdd077c6a71d3da0f6db Mon Sep 17 00:00:00 2001 From: earlaud Date: Sat, 25 Oct 2025 15:39:04 +0200 Subject: [PATCH 2/4] Remove useless compute in the example --- examples/go2_kinodynamics.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/examples/go2_kinodynamics.py b/examples/go2_kinodynamics.py index eefd608a..0f96df54 100644 --- a/examples/go2_kinodynamics.py +++ b/examples/go2_kinodynamics.py @@ -289,8 +289,6 @@ q_meas, v_meas = device.measureState() x_measured = np.concatenate([q_meas, v_meas]) - mpc.getDataHandler().updateInternalData(x_measured, True) - kino_ID.setTarget(q_interp, v_interp, acc_interp, contact_states, force_interp) tau_cmd = kino_ID.solve(t, q_meas, v_meas) From 0ea2e03ee7ecdc49742423e4ee8de5d903cc2a70 Mon Sep 17 00:00:00 2001 From: earlaud Date: Sat, 25 Oct 2025 15:43:54 +0200 Subject: [PATCH 3/4] Fix velocity in world aligned frame --- src/inverse-dynamics/kinodynamics-id.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/inverse-dynamics/kinodynamics-id.cpp b/src/inverse-dynamics/kinodynamics-id.cpp index 27ae9786..a453c461 100644 --- a/src/inverse-dynamics/kinodynamics-id.cpp +++ b/src/inverse-dynamics/kinodynamics-id.cpp @@ -133,10 +133,11 @@ void KinodynamicsID::setTarget( postureTask_->setReference(samplePosture_); // Base task - tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos); - sampleBase_.setDerivative(v_target.head<6>()); - sampleBase_.setSecondDerivative(a_target.head<6>()); - baseTask_->setReference(sampleBase_); + const pinocchio::SE3 oMb{data_handler_.getBaseFramePose()}; + const pinocchio::SE3 oMb_rotation(oMb.rotation(), Eigen::Vector3d::Zero()); + const pinocchio::Motion v_world_aligned{oMb_rotation.act(pinocchio::Motion(v_target.head<6>()))}; + const pinocchio::Motion a_world_aligned{oMb_rotation.act(pinocchio::Motion(a_target.head<6>()))}; + baseTask_->setReference(oMb, v_world_aligned, a_world_aligned); // Foot contacts for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) From 9905c041c1866d99cc265fad106c7126fea43a7a Mon Sep 17 00:00:00 2001 From: earlaud Date: Sat, 25 Oct 2025 21:26:29 +0200 Subject: [PATCH 4/4] World algined base vel and acc computed properly at solve --- .../inverse-dynamics/kinodynamics-id.hpp | 2 ++ src/inverse-dynamics/kinodynamics-id.cpp | 20 ++++++++++++++----- 2 files changed, 17 insertions(+), 5 deletions(-) diff --git a/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp b/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp index c36f9586..78475c7b 100644 --- a/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp +++ b/include/simple-mpc/inverse-dynamics/kinodynamics-id.hpp @@ -85,6 +85,8 @@ namespace simple_mpc tsid::solvers::HQPOutput last_solution_; tsid::trajectories::TrajectorySample samplePosture_; tsid::trajectories::TrajectorySample sampleBase_; + pinocchio::Motion targetVelBase_; + pinocchio::Motion targetAccBase_; }; } // namespace simple_mpc diff --git a/src/inverse-dynamics/kinodynamics-id.cpp b/src/inverse-dynamics/kinodynamics-id.cpp index a453c461..02735139 100644 --- a/src/inverse-dynamics/kinodynamics-id.cpp +++ b/src/inverse-dynamics/kinodynamics-id.cpp @@ -133,11 +133,12 @@ void KinodynamicsID::setTarget( postureTask_->setReference(samplePosture_); // Base task - const pinocchio::SE3 oMb{data_handler_.getBaseFramePose()}; - const pinocchio::SE3 oMb_rotation(oMb.rotation(), Eigen::Vector3d::Zero()); - const pinocchio::Motion v_world_aligned{oMb_rotation.act(pinocchio::Motion(v_target.head<6>()))}; - const pinocchio::Motion a_world_aligned{oMb_rotation.act(pinocchio::Motion(a_target.head<6>()))}; - baseTask_->setReference(oMb, v_world_aligned, a_world_aligned); + tsid::math::SE3ToVector(data_handler_.getBaseFramePose(), sampleBase_.pos); + /* Velocity and acceleration not set here since the actual position of the robot is needed to transform from local to + * world-aligned frame. Will be done in solve() + */ + targetVelBase_ = v_target.head<6>(); + targetAccBase_ = a_target.head<6>(); // Foot contacts for (std::size_t foot_nb = 0; foot_nb < model_handler_.getFeetNb(); foot_nb++) @@ -215,6 +216,15 @@ void KinodynamicsID::solve( } } + // Convert robot base vel/acc from local to world-aligned frame using actual robot pose + const pinocchio::SE3 oMb_rotation(data_handler_.getBaseFramePose().rotation(), Eigen::Vector3d::Zero()); + const pinocchio::Motion v_world_aligned{oMb_rotation.act(pinocchio::Motion(targetVelBase_))}; + const pinocchio::Motion a_world_aligned{oMb_rotation.act(pinocchio::Motion(targetAccBase_))}; + sampleBase_.setDerivative(v_world_aligned.toVector()); + sampleBase_.setDerivative(a_world_aligned.toVector()); + baseTask_->setReference(sampleBase_); + + // Solve QP const tsid::solvers::HQPData & solver_data = formulation_.computeProblemData(t, q_meas, v_meas); last_solution_ = solver_.solve(solver_data); assert(last_solution_.status == tsid::solvers::HQPStatus::HQP_STATUS_OPTIMAL);