From 8f29769718cfb0a43b36c79e3ec56d9661c8e88b Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Mon, 2 Jun 2025 17:32:36 +0900 Subject: [PATCH 1/9] [mc_rbdyn][mc_tvm] Add external and compensation torques member and their equivalent joint accelerations to `Robot` Add the new members `externalTorques_`, `compensationTorques_`, `exteralTorquesAcc_` and `compensationTorquesAcc_` to `mc_rbdyn::Robot`. Allong with their setters and getters `setExternalTorques`, `setCompensationTorques`, `setExternalTorquesAcc`, `setCompensationTorquesAcc`. Note that the compensation torques and associated equivalent accelerations are set as std::optional. This is intended for later use for external forces compensation and explicit compliance. --- include/mc_rbdyn/Robot.h | 45 +++++++++++++++++++++++++++++++ include/mc_tvm/Robot.h | 35 ++++++++++++++++++++++-- src/mc_rbdyn/Robot.cpp | 58 ++++++++++++++++++++++++++++++++++++++++ src/mc_tvm/Robot.cpp | 20 +++++++++++++- 4 files changed, 155 insertions(+), 3 deletions(-) diff --git a/include/mc_rbdyn/Robot.h b/include/mc_rbdyn/Robot.h index f3dd837fe1..b1ae9cec23 100644 --- a/include/mc_rbdyn/Robot.h +++ b/include/mc_rbdyn/Robot.h @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -1114,6 +1115,42 @@ struct MC_RBDYN_DLLAPI Robot */ mc_tvm::Convex & tvmConvex(const std::string & name) const; + /** @name External Torques + * + * These functions are used to get or set: + * - The estimation of external torques being applied on the robot and its 'equivalent' acceleration + * - The additional feedforward compensation torque and its 'equivalent' acceleration + * + * @{ + */ + + /** Set the external torques */ + void setExternalTorques(const Eigen::VectorXd & torques); + + /** Get the external torques */ + const Eigen::VectorXd & externalTorques(void) const; + + /** Set the external torques equivalent accelerations */ + void setExternalTorquesAcc(const Eigen::VectorXd & accelerations); + + /** Get the external torques equivalent accelerations */ + const Eigen::VectorXd & externalTorquesAcc(void) const; + + /** Set the compensation torques */ + void setCompensationTorques(const Eigen::VectorXd & torques); + + /** Get the compensation torques */ + const std::optional & compensationTorques(void) const; + + /** Set the compensation torques equivalent accelerations */ + void setCompensationTorquesAcc(const Eigen::VectorXd & accelerations); + + /** Get the compensation torques equivalent accelerations */ + const std::optional & compensationTorquesAcc(void) const; + + /** @} */ + /* End of External Forces group */ + private: Robots * robots_; unsigned int robots_idx_; @@ -1149,6 +1186,14 @@ struct MC_RBDYN_DLLAPI Robot std::unordered_map frames_; /** Mass of this robot */ double mass_ = 0.0; + /** Non modeled external forces acting on the robot **/ + Eigen::VectorXd externalTorques_; + /** Joint accelerations from external forces acting on the robot **/ + Eigen::VectorXd externalTorquesEquivalentAcc_; + /** External forces to be compensated in the commanded torque **/ + std::optional externalTorqueCompensation_ = std::nullopt; + /** Joint accelerations from the compensation in the commanded torque **/ + std::optional compensationEquivalentAcc_ = std::nullopt; protected: struct NewRobotToken diff --git a/include/mc_tvm/Robot.h b/include/mc_tvm/Robot.h index bc238e355d..4f4893e448 100644 --- a/include/mc_tvm/Robot.h +++ b/include/mc_tvm/Robot.h @@ -16,6 +16,8 @@ #include +#include + namespace mc_tvm { @@ -47,8 +49,8 @@ namespace mc_tvm */ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node { - SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C) - SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C) + SET_OUTPUTS(Robot, FK, FV, FA, NormalAcceleration, tau, H, C, ExternalForces) + SET_UPDATES(Robot, FK, FV, FA, NormalAcceleration, H, C, ExternalForces) friend struct mc_rbdyn::Robot; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -96,6 +98,11 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node /** Access q second derivative (joint acceleration) */ inline tvm::VariablePtr & alphaD() noexcept { return ddq_; } + /** Access joint acceleration from external forces (const) */ + inline const Eigen::VectorXd & alphaDExternal() const noexcept { return ddq_ext_; } + /** Access joint acceleration from external forces */ + inline Eigen::VectorXd & alphaDExternal() noexcept { return ddq_ext_; } + /** Access floating-base variable (const) */ inline const tvm::VariablePtr & qFloatingBase() const noexcept { return q_fb_; } /** Access free-flyer variable */ @@ -134,6 +141,21 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node /** Access tau variable */ inline tvm::VariablePtr & tau() { return tau_; } + /** Access tau external variable (const) */ + inline const Eigen::VectorXd & tauExternal() const noexcept { return tau_ext_; } + /** Access tau external variable */ + inline Eigen::VectorXd & tauExternal() { return tau_ext_; } + + /** Access tau compensation (const) */ + inline const std::optional & tauCompensation() const noexcept { return tau_comp_; } + /** Access tau compensation */ + inline std::optional & tauCompensation() noexcept { return tau_comp_; } + + /** Access joint acceleration from compensation torques (const) */ + inline const std::optional & alphaDCompensation() const noexcept { return ddq_comp_; } + /** Access joint acceleration from compensation torques */ + inline std::optional & alphaDCompensation() noexcept { return ddq_comp_; } + /** Returns the CoM algorithm associated to this robot (const) */ inline const CoM & comAlgo() const noexcept { return *com_; } @@ -203,8 +225,16 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node tvm::VariablePtr dq_; /** Double derivative of q */ tvm::VariablePtr ddq_; + /** Joint acceleration from external forces */ + Eigen::VectorXd ddq_ext_; + /** Joint acceleration from compensation torques */ + std::optional ddq_comp_; /** Tau variable */ tvm::VariablePtr tau_; + /** Tau external variable */ + Eigen::VectorXd tau_ext_; + /** Tau compensation variable */ + std::optional tau_comp_; /** Normal accelerations of the bodies */ std::vector normalAccB_; /** Forward dynamics algorithm associated to this robot */ @@ -225,6 +255,7 @@ struct MC_TVM_DLLAPI Robot : public tvm::graph::abstract::Node void updateNormalAcceleration(); void updateH(); void updateC(); + void updateExternalForces(); }; } // namespace mc_tvm diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 04ae4cdf8c..da260e79f0 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -18,10 +18,13 @@ #include #include +#include #include #include #include +#include +#include #include #include @@ -510,6 +513,9 @@ Robot::Robot(NewRobotToken, flexibility_ = module_.flexibility(); zmp_ = Eigen::Vector3d::Zero(); + + externalTorques_ = Eigen::VectorXd::Zero(mb().nrDof()); + externalTorquesEquivalentAcc_ = Eigen::VectorXd::Zero(mb().nrDof()); } Robot::~Robot() @@ -1567,4 +1573,56 @@ mc_tvm::Convex & Robot::tvmConvex(const std::string & name) const return *it->second; } +void Robot::setExternalTorques(const Eigen::VectorXd & torques) +{ + externalTorques_.noalias() = torques; +} + +const Eigen::VectorXd & Robot::externalTorques(void) const +{ + return externalTorques_; +} + +void Robot::setExternalTorquesAcc(const Eigen::VectorXd & accelerations) +{ + externalTorquesEquivalentAcc_.noalias() = accelerations; +} + +const Eigen::VectorXd & Robot::externalTorquesAcc(void) const +{ + return externalTorquesEquivalentAcc_; +} + +void Robot::setCompensationTorques(const Eigen::VectorXd & torques) +{ + if(!externalTorqueCompensation_) { externalTorqueCompensation_.emplace(torques.size()); } + else if(externalTorqueCompensation_->size() == torques.size()) + { + externalTorqueCompensation_->resize(torques.size()); + } + + externalTorqueCompensation_->noalias() = torques; +} + +const std::optional & Robot::compensationTorques(void) const +{ + return externalTorqueCompensation_; +} + +void Robot::setCompensationTorquesAcc(const Eigen::VectorXd & accelerations) +{ + if(!externalTorqueCompensation_) { compensationEquivalentAcc_.emplace(accelerations.size()); } + else if(externalTorqueCompensation_->size() == accelerations.size()) + { + + compensationEquivalentAcc_->resize(accelerations.size()); + } + compensationEquivalentAcc_->noalias() = accelerations; +} + +const std::optional & Robot::compensationTorquesAcc(void) const +{ + return compensationEquivalentAcc_; +} + } // namespace mc_rbdyn diff --git a/src/mc_tvm/Robot.cpp b/src/mc_tvm/Robot.cpp index 7b4d4f9aa5..01f1afe95b 100644 --- a/src/mc_tvm/Robot.cpp +++ b/src/mc_tvm/Robot.cpp @@ -136,6 +136,8 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) dq_->setZero(); ddq_->setZero(); tau_->setZero(); + tau_ext_ = Eigen::VectorXd::Zero(robot.mb().nrDof()); + ddq_ext_ = Eigen::VectorXd::Zero(robot.mb().nrDof()); const auto & rjo = robot.refJointOrder(); refJointIndexToQIndex_.resize(rjo.size()); @@ -159,7 +161,7 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) /** Signal setup */ registerUpdates(Update::FK, &Robot::updateFK, Update::FV, &Robot::updateFV, Update::FA, &Robot::updateFA, Update::NormalAcceleration, &Robot::updateNormalAcceleration, Update::H, &Robot::updateH, Update::C, - &Robot::updateC); + &Robot::updateC, Update::ExternalForces, &Robot::updateExternalForces); /** Output dependencies setup */ addOutputDependency(Output::FK, Update::FK); addOutputDependency(Output::FV, Update::FV); @@ -168,12 +170,15 @@ Robot::Robot(NewRobotToken, const mc_rbdyn::Robot & robot) addOutputDependency(Output::H, Update::H); addOutputDependency(Output::C, Update::C); addOutputDependency(Output::FV, Update::FV); + addOutputDependency(Output::ExternalForces, Update::ExternalForces); /** Internal dependencies setup */ addInternalDependency(Update::FV, Update::FK); addInternalDependency(Update::H, Update::FV); addInternalDependency(Update::C, Update::FV); addInternalDependency(Update::FA, Update::FV); addInternalDependency(Update::NormalAcceleration, Update::FV); + addInternalDependency(Update::ExternalForces, Update::H); + addInternalDependency(Update::ExternalForces, Update::FA); } void Robot::updateFK() @@ -238,4 +243,17 @@ tvm::VariablePtr Robot::qJoint(size_t jIdx) tvm::Space(offsetDof, offsetParam, offsetDof)); } +void Robot::updateExternalForces() +{ + tau_ext_ = robot_.externalTorques(); + tau_comp_ = robot_.compensationTorques(); + auto H_inv = H().ldlt(); + ddq_ext_ = H_inv.solve(tau_ext_); + if(tau_comp_) + { + if(ddq_comp_->size() != tau_comp_->size()) { ddq_comp_->resize(tau_comp_->size()); } + ddq_comp_ = H_inv.solve(tau_comp_.value()); + } +} + } // namespace mc_tvm From 3364753c6ada3cb1b6d2cbcdebd1f5f42dd9ac23 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Wed, 4 Feb 2026 13:37:56 +0900 Subject: [PATCH 2/9] [mc_solver] Manage computation of equivalent accelerations once for Tasks backend and closedLoop feedback type in both TVM and Tasks Since Tasks backend doesn't benefit from a computation graph, the joint acceleration equivalent to the external/compensation torques should be computed once before hand for later use by tasks in the solver. For both Tasks and TVM, the information of external torques in robot are updated from realRobot. --- src/mc_solver/TVMQPSolver.cpp | 9 +++++++++ src/mc_solver/TasksQPSolver.cpp | 23 +++++++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/src/mc_solver/TVMQPSolver.cpp b/src/mc_solver/TVMQPSolver.cpp index 7d0cf4236f..ac133c9859 100644 --- a/src/mc_solver/TVMQPSolver.cpp +++ b/src/mc_solver/TVMQPSolver.cpp @@ -221,6 +221,15 @@ bool TVMQPSolver::runClosedLoop(bool integrateControlState) robot.forwardKinematics(); robot.forwardVelocity(); robot.forwardAcceleration(); + + // Update robot with realRobot's external/compenstation torques informations + robot.setExternalTorques(realRobot.externalTorques()); + robot.setExternalTorquesAcc(realRobot.externalTorquesAcc()); + if(realRobot.compensationTorques()) + { + robot.setCompensationTorques(realRobot.compensationTorques().value()); + robot.setCompensationTorquesAcc(realRobot.compensationTorquesAcc().value()); + } } // Solve QP and integrate diff --git a/src/mc_solver/TasksQPSolver.cpp b/src/mc_solver/TasksQPSolver.cpp index 1360529f5c..6e2a711f16 100644 --- a/src/mc_solver/TasksQPSolver.cpp +++ b/src/mc_solver/TasksQPSolver.cpp @@ -155,6 +155,21 @@ const sva::ForceVecd TasksQPSolver::desiredContactForce(const mc_rbdyn::Contact bool TasksQPSolver::run_impl(FeedbackType fType) { bool success = false; + + for(size_t i = 0; i < robots().size(); ++i) + { + auto & realRobot = realRobots().robot(i); + if(realRobot.externalTorques().size() == 0) continue; + auto fd = rbd::ForwardDynamics(realRobot.mb()); + fd.computeH(realRobot.mb(), realRobot.mbc()); + auto Hinv = fd.H().ldlt(); + realRobot.setExternalTorquesAcc(Hinv.solve(realRobot.externalTorques())); + if(realRobot.compensationTorques() && !realRobot.compensationTorquesAcc()) + { + realRobot.setCompensationTorquesAcc(Hinv.solve(realRobot.compensationTorques().value())); + } + } + switch(fType) { case FeedbackType::None: @@ -315,6 +330,14 @@ bool TasksQPSolver::runClosedLoop(bool integrateControlState) robot.forwardKinematics(); robot.forwardVelocity(); robot.forwardAcceleration(); + // Update robot with realRobot's external/compenstation torques informations + robot.setExternalTorques(realRobot.externalTorques()); + robot.setExternalTorquesAcc(realRobot.externalTorquesAcc()); + if(realRobot.compensationTorques()) + { + robot.setCompensationTorques(realRobot.compensationTorques().value()); + robot.setCompensationTorquesAcc(realRobot.compensationTorquesAcc().value()); + } } // Update tasks and constraints from estimated robots From f0660567c83967fa16006b63525f220d564a11da Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 5 Feb 2026 09:16:40 +0000 Subject: [PATCH 3/9] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/mc_rtc/Configuration.h | 10 +++++----- include/mc_solver/QPSolver.h | 10 +++++----- src/mc_rbdyn/configuration_io.cpp | 10 +++++----- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/include/mc_rtc/Configuration.h b/include/mc_rtc/Configuration.h index 6904f6b5de..05ea70e2db 100644 --- a/include/mc_rtc/Configuration.h +++ b/include/mc_rtc/Configuration.h @@ -1807,11 +1807,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const mc_rtc::Configuration & c, FormatContext & ctx) - #else - auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const mc_rtc::Configuration & c, FormatContext & ctx) +#else + auto format(const mc_rtc::Configuration & c, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { return formatter::format(static_cast(c), ctx); } diff --git a/include/mc_solver/QPSolver.h b/include/mc_solver/QPSolver.h index 68709b927b..9fa96de448 100644 --- a/include/mc_solver/QPSolver.h +++ b/include/mc_solver/QPSolver.h @@ -344,11 +344,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) - #else - auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) +#else + auto format(const mc_solver::QPSolver::Backend & backend, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { using Backend = mc_solver::QPSolver::Backend; switch(backend) diff --git a/src/mc_rbdyn/configuration_io.cpp b/src/mc_rbdyn/configuration_io.cpp index 1a33959791..bead2b37ab 100644 --- a/src/mc_rbdyn/configuration_io.cpp +++ b/src/mc_rbdyn/configuration_io.cpp @@ -29,11 +29,11 @@ template<> struct formatter : public formatter { template - #if FMT_VERSION <= 9 * 10000 - auto format(const rbd::Joint::Type & t, FormatContext & ctx) - #else - auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out()) - #endif +#if FMT_VERSION <= 9 * 10000 + auto format(const rbd::Joint::Type & t, FormatContext & ctx) +#else + auto format(const rbd::Joint::Type & t, FormatContext & ctx) const -> decltype(ctx.out()) +#endif { switch(t) { From 2304eb142ebef9cd94ff2144dfbeac27257702ec Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Tue, 3 Jun 2025 12:59:50 +0900 Subject: [PATCH 4/9] [mc_solver] Add feedforward compensation torques to the `DynamicConstraint` in the Tasks backend Add an additional boolean argument to `DynamicConstraint` to enable compensation of external torques disturbance estimation. The boolean is defaulted to false to prevent breaking change. The feedforward compensation torques are passed as an `Eigen::VectorXd` to Tasks' `MotionConstr` and `MotionSpringConstr`. The compensation torques are obtained from `mc_rbdyn::Robot::compensationTorques()` if available or `mc_rbdyn::Robot::externalTorques()` otherwise. --- include/mc_solver/DynamicsConstraint.h | 15 ++++++- src/mc_solver/DynamicsConstraint.cpp | 62 +++++++++++++++++++++----- 2 files changed, 64 insertions(+), 13 deletions(-) diff --git a/include/mc_solver/DynamicsConstraint.h b/include/mc_solver/DynamicsConstraint.h index 55c194b28f..9ff2a294d0 100644 --- a/include/mc_solver/DynamicsConstraint.h +++ b/include/mc_solver/DynamicsConstraint.h @@ -28,8 +28,15 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint * \param robotIndex The index of the robot affected by this constraint * \param timeStep Solver timestep * \param infTorque If true, ignore the torque limits set in the robot model + * \param compensateExtTorques If true, compensates external disturbances using a feedworward torque signal. The + * constraint will search for the compensation value in robot by calling `compensationTorques()` method, if not an + * estimation of external torques acting on the robot will be used by calling `externalTorques()` method. */ - DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, bool infTorque = false); + DynamicsConstraint(const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double timeStep, + bool infTorque = false, + bool compensateExtTorques = false); /** Constructor * Builds a damped joint limits constraint and a motion constr depending on @@ -42,13 +49,17 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint * offset} * \param velocityPercent Maximum joint velocity percentage, 0.5 is advised * \param infTorque If true, ignore the torque limits set in the robot model + * \param compensateExtTorques If true, compensates external disturbances using a feedworward torque signal. The + * constraint will search for the compensation value in robot by calling `compensationTorques()` method, if not an + * estimation of external torques acting on the robot will be used by calling `externalTorques()` method. */ DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, const std::array & damper, double velocityPercent = 1.0, - bool infTorque = false); + bool infTorque = false, + bool compensateExtTorques = false); /** Returns the tasks::qp::MotionConstr * diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index 09b82a5845..6f64ea5881 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -19,7 +19,8 @@ namespace mc_solver static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExtTorques) { const auto & robot = robots.robot(robotIndex); std::vector> tl = robot.tl(); @@ -54,13 +55,47 @@ static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, { sjList.push_back(tasks::qp::SpringJoint(flex.jointName, flex.K, flex.C, flex.O)); } - return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, - tDBound, timeStep, sjList); + if(compensateExtTorques) + { + if(robot.compensationTorques()) + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList, + robot.compensationTorques().value()); + } + else + { + + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList, robot.externalTorques()); + } + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, sjList); + } } else { - return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, tDBound, - timeStep); + if(compensateExtTorques) + { + if(robot.compensationTorques()) + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, robot.compensationTorques().value()); + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, + tDBound, timeStep, robot.externalTorques()); + } + } + else + { + return mc_rtc::make_void_ptr(robots.mbs(), static_cast(robotIndex), tBound, tDBound, + timeStep); + } } } @@ -73,12 +108,13 @@ static mc_rtc::void_ptr initialize(QPSolver::Backend backend, const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExtTorques) { switch(backend) { case QPSolver::Backend::Tasks: - return initialize_tasks(robots, robotIndex, timeStep, infTorque); + return initialize_tasks(robots, robotIndex, timeStep, infTorque, compensateExtTorques); case QPSolver::Backend::TVM: return initialize_tvm(robots.robot(robotIndex)); default: @@ -89,9 +125,11 @@ static mc_rtc::void_ptr initialize(QPSolver::Backend backend, DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, unsigned int robotIndex, double timeStep, - bool infTorque) + bool infTorque, + bool compensateExtTorques) : KinematicsConstraint(robots, robotIndex, timeStep), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExtTorques)), + robotIndex_(robotIndex) { } @@ -100,9 +138,11 @@ DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, double timeStep, const std::array & damper, double velocityPercent, - bool infTorque) + bool infTorque, + bool compensateExtTorques) : KinematicsConstraint(robots, robotIndex, timeStep, damper, velocityPercent), - motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque)), robotIndex_(robotIndex) + motion_constr_(initialize(backend_, robots, robotIndex, timeStep, infTorque, compensateExtTorques)), + robotIndex_(robotIndex) { } From 497205cee28d7dd1474f475c8ea12a74bf4686b7 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Wed, 4 Feb 2026 14:26:23 +0900 Subject: [PATCH 5/9] [mc_solver] Add feedforward compensation torques to the `DynamicConstraint` in the TVM backend Add an additional boolean argument to `mc_tvm::DynamicFunction` to enable compensation of external torques disturbance estimation. The boolean is defaulted to false to prevent breaking change. The feedforward compensation torques are obtained directly in the `DynamicFunction` using `Robot`. The compensation torques are obtained from `mc_tvm::Robot::tauCompensation()` if available or `mc_rbdyn::Robot::tauExternal()` otherwise. --- include/mc_tvm/DynamicFunction.h | 3 ++- src/mc_solver/DynamicsConstraint.cpp | 7 ++++--- src/mc_tvm/DynamicFunction.cpp | 17 +++++++++++++++-- 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/include/mc_tvm/DynamicFunction.h b/include/mc_tvm/DynamicFunction.h index 2e2156a0ca..d13682f838 100644 --- a/include/mc_tvm/DynamicFunction.h +++ b/include/mc_tvm/DynamicFunction.h @@ -36,7 +36,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun SET_UPDATES(DynamicFunction, Jacobian, B) /** Construct the equation of motion for a given robot */ - DynamicFunction(const mc_rbdyn::Robot & robot); + DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces = false); /** Add a contact to the function * @@ -73,6 +73,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun void updateb(); const mc_rbdyn::Robot & robot_; + const bool compensateExternalForces_; /** Holds data for the force part of the motion equation */ struct ForceContact diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index 6f64ea5881..ea404abb70 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -99,9 +99,10 @@ static mc_rtc::void_ptr initialize_tasks(const mc_rbdyn::Robots & robots, } } -mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot) +mc_rtc::void_ptr initialize_tvm(const mc_rbdyn::Robot & robot, bool compensateExtTorques) { - return mc_rtc::make_void_ptr(std::make_shared(robot)); + return mc_rtc::make_void_ptr( + std::make_shared(robot, compensateExtTorques)); } static mc_rtc::void_ptr initialize(QPSolver::Backend backend, @@ -116,7 +117,7 @@ static mc_rtc::void_ptr initialize(QPSolver::Backend backend, case QPSolver::Backend::Tasks: return initialize_tasks(robots, robotIndex, timeStep, infTorque, compensateExtTorques); case QPSolver::Backend::TVM: - return initialize_tvm(robots.robot(robotIndex)); + return initialize_tvm(robots.robot(robotIndex), compensateExtTorques); default: mc_rtc::log::error_and_throw("[DynamicsConstraint] Not implemented for solver backend: {}", backend); } diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 9023bd0922..b1fe6b1ece 100644 --- a/src/mc_tvm/DynamicFunction.cpp +++ b/src/mc_tvm/DynamicFunction.cpp @@ -10,8 +10,9 @@ namespace mc_tvm { -DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot) -: tvm::function::abstract::LinearFunction(robot.mb().nrDof()), robot_(robot) +DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot, bool compensateExternalForces) +: tvm::function::abstract::LinearFunction(robot.mb().nrDof()), robot_(robot), + compensateExternalForces_(compensateExternalForces) { registerUpdates(Update::B, &DynamicFunction::updateb); registerUpdates(Update::Jacobian, &DynamicFunction::updateJacobian); @@ -20,6 +21,10 @@ DynamicFunction::DynamicFunction(const mc_rbdyn::Robot & robot) auto & tvm_robot = robot.tvmRobot(); addInputDependency(Update::Jacobian, tvm_robot, Robot::Output::H); addInputDependency(Update::B, tvm_robot, Robot::Output::C); + if(compensateExternalForces_) + { + addInputDependency(Update::B, tvm_robot, Robot::Output::ExternalForces); + } addVariable(tvm::dot(tvm_robot.q(), 2), true); addVariable(tvm_robot.tau(), true); jacobian_[tvm_robot.tau().get()] = -Eigen::MatrixXd::Identity(robot_.mb().nrDof(), robot_.mb().nrDof()); @@ -103,6 +108,14 @@ sva::ForceVecd DynamicFunction::contactForce(const mc_rbdyn::RobotFrame & frame) void DynamicFunction::updateb() { b_ = robot_.tvmRobot().C(); + if(compensateExternalForces_) + { + if(robot_.tvmRobot().tauCompensation()) { b_ -= robot_.tvmRobot().tauCompensation().value(); } + else + { + b_ -= robot_.tvmRobot().tauExternal(); + } + } } void DynamicFunction::updateJacobian() From 50e5f8986d7eaba23646163af80833ba13470f45 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Mon, 23 Mar 2026 19:03:04 +0900 Subject: [PATCH 6/9] fix(mc_solver): Update the dynamic constraint to make use of the new setExternalTorques funtion of Tasks MotionConstr --- include/mc_solver/DynamicsConstraint.h | 8 ++++++++ src/mc_solver/DynamicsConstraint.cpp | 17 +++++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/include/mc_solver/DynamicsConstraint.h b/include/mc_solver/DynamicsConstraint.h index 9ff2a294d0..4872df1054 100644 --- a/include/mc_solver/DynamicsConstraint.h +++ b/include/mc_solver/DynamicsConstraint.h @@ -61,6 +61,14 @@ struct MC_SOLVER_DLLAPI DynamicsConstraint : public KinematicsConstraint bool infTorque = false, bool compensateExtTorques = false); + /** \brief Update the constraint + * + * This is called at every iteration of the controller once the constraint has been added to a solver + * + * \param solver Solver in which the constraint has been inserted + */ + void update(QPSolver & solver) override; + /** Returns the tasks::qp::MotionConstr * * This assumes the backend was Tasks diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index ea404abb70..7aa697c7ed 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -147,6 +147,23 @@ DynamicsConstraint::DynamicsConstraint(const mc_rbdyn::Robots & robots, { } +void DynamicsConstraint::update(QPSolver & solver) +{ + if(backend_ == QPSolver::Backend::Tasks) + { + auto & robot = solver.robot(robotIndex_); + if(robot.compensationTorques()) + { + static_cast(motion_constr_.get()) + ->setExternalTorques(robot.compensationTorques().value()); + } + else + { + static_cast(motion_constr_.get())->setExternalTorques(robot.externalTorques()); + } + } +} + void DynamicsConstraint::addToSolverImpl(QPSolver & solver) { KinematicsConstraint::addToSolverImpl(solver); From 11cc526e1020d257b5e71bcde4e75dddf8491097 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Fri, 6 Jun 2025 10:51:49 +0900 Subject: [PATCH 7/9] [mc_tasks] Add compliant version of several common tasks for Tasks backend New compliant alternative for tasks are: - CompliantPostureTask - CompliantPositionTask - CompliantOrientationTask - CompliantEndEffectorTask --- include/mc_tasks/CompliantEndEffectorTask.h | 80 ++++++++++ include/mc_tasks/CompliantOrientationTask.h | 58 +++++++ include/mc_tasks/CompliantPositionTask.h | 58 +++++++ include/mc_tasks/CompliantPostureTask.h | 41 +++++ src/CMakeLists.txt | 8 + src/mc_tasks/CompliantEndEffectorTask.cpp | 163 ++++++++++++++++++++ src/mc_tasks/CompliantOrientationTask.cpp | 100 ++++++++++++ src/mc_tasks/CompliantPositionTask.cpp | 100 ++++++++++++ src/mc_tasks/CompliantPostureTask.cpp | 88 +++++++++++ 9 files changed, 696 insertions(+) create mode 100644 include/mc_tasks/CompliantEndEffectorTask.h create mode 100644 include/mc_tasks/CompliantOrientationTask.h create mode 100644 include/mc_tasks/CompliantPositionTask.h create mode 100644 include/mc_tasks/CompliantPostureTask.h create mode 100644 src/mc_tasks/CompliantEndEffectorTask.cpp create mode 100644 src/mc_tasks/CompliantOrientationTask.cpp create mode 100644 src/mc_tasks/CompliantPositionTask.cpp create mode 100644 src/mc_tasks/CompliantPostureTask.cpp diff --git a/include/mc_tasks/CompliantEndEffectorTask.h b/include/mc_tasks/CompliantEndEffectorTask.h new file mode 100644 index 0000000000..8d2ae3b8ca --- /dev/null +++ b/include/mc_tasks/CompliantEndEffectorTask.h @@ -0,0 +1,80 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include +#include + +namespace mc_tasks +{ + +/*! \brief Controls an end-effector + * + * This task is a thin wrapper around the appropriate tasks in Tasks. + * The task objective is given in the world frame. For relative control + * see mc_tasks::RelativeCompliantEndEffectorTask + */ +struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask +{ +public: + /*! \brief Constructor + * + * \param bodyName Name of the body to control + * + * \param robots Robots controlled by this task + * + * \param robotIndex Index of the robot controlled by this task + * + * \param stiffness Task stiffness + * + * \param weight Task weight + * + */ + CompliantEndEffectorTask(const std::string & bodyName, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight); + + /** Change acceleration + * + * \p refAccel Should be of size 6 + */ + void refAccel(const Eigen::Vector6d & refAccel) noexcept; + + // Set the compliant behavior of the task + void makeCompliant(bool compliance); + void setComplianceVector(Eigen::Vector6d gamma); + + // Get compliance state of the task + bool isCompliant(void); + Eigen::Vector6d getComplianceVector(void); + + void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override; + +protected: + void addToSolver(mc_solver::QPSolver & solver) override; + + void update(mc_solver::QPSolver & solver) override; + + void addToGUI(mc_rtc::gui::StateBuilder & gui) override; + + Eigen::Matrix6d compliant_matrix_; + + mc_tvm::Robot * tvm_robot_; + const mc_rbdyn::Robot * robot_; + + unsigned int rIdx_; + + std::string bodyName_; + const mc_rbdyn::RobotFrame & frame_; + + rbd::Jacobian * jac_; + + Eigen::Vector6d refAccel_; +}; + +} // namespace mc_tasks diff --git a/include/mc_tasks/CompliantOrientationTask.h b/include/mc_tasks/CompliantOrientationTask.h new file mode 100644 index 0000000000..eb428bd8bf --- /dev/null +++ b/include/mc_tasks/CompliantOrientationTask.h @@ -0,0 +1,58 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include + +namespace mc_tasks +{ + +struct MC_TASKS_DLLAPI CompliantOrientationTask : public OrientationTask +{ +public: + CompliantOrientationTask(const std::string & bodyName_, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight); + + /** Change reference acceleration + * + * \p refAccel Should be of size nrDof + */ + void refAccel(const Eigen::Vector3d & refAccel) noexcept; + + // Set task to be compliant or not + void makeCompliant(bool compliance); + void setComplianceVector(Eigen::Vector3d gamma); + // Get compliance state of the task + bool isCompliant(void); + Eigen::Vector3d getComplianceVector(void); + + // void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override; + +protected: + void addToSolver(mc_solver::QPSolver & solver) override; + + void update(mc_solver::QPSolver & solver) override; + + void addToGUI(mc_rtc::gui::StateBuilder & gui) override; + + Eigen::Matrix3d Gamma_; + + mc_tvm::Robot & tvm_robot_; + + unsigned int rIdx_; + + std::string bodyName_; + const mc_rbdyn::RobotFrame & frame_; + + rbd::Jacobian * jac_; + + Eigen::Vector3d refAccel_; +}; + +} // namespace mc_tasks diff --git a/include/mc_tasks/CompliantPositionTask.h b/include/mc_tasks/CompliantPositionTask.h new file mode 100644 index 0000000000..7685afe7b0 --- /dev/null +++ b/include/mc_tasks/CompliantPositionTask.h @@ -0,0 +1,58 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include + +namespace mc_tasks +{ + +struct MC_TASKS_DLLAPI CompliantPositionTask : public PositionTask +{ +public: + CompliantPositionTask(const std::string & bodyName_, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight); + + /** Change reference acceleration + * + * \p refAccel Should be of size nrDof + */ + void refAccel(const Eigen::Vector3d & refAccel) noexcept; + + // Set task to be compliant or not + void makeCompliant(bool compliance); + void setComplianceVector(Eigen::Vector3d gamma); + // Get compliance state of the task + bool isCompliant(void); + Eigen::Vector3d getComplianceVector(void); + + // void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override; + +protected: + void addToSolver(mc_solver::QPSolver & solver) override; + + void update(mc_solver::QPSolver & solver) override; + + void addToGUI(mc_rtc::gui::StateBuilder & gui) override; + + Eigen::Matrix3d Gamma_; + + mc_tvm::Robot & tvm_robot_; + + unsigned int rIdx_; + + std::string bodyName_; + const mc_rbdyn::RobotFrame & frame_; + + rbd::Jacobian * jac_; + + Eigen::Vector3d refAccel_; +}; + +} // namespace mc_tasks diff --git a/include/mc_tasks/CompliantPostureTask.h b/include/mc_tasks/CompliantPostureTask.h new file mode 100644 index 0000000000..e387295246 --- /dev/null +++ b/include/mc_tasks/CompliantPostureTask.h @@ -0,0 +1,41 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +namespace mc_tasks +{ + +struct MC_TASKS_DLLAPI CompliantPostureTask : public PostureTask +{ +public: + CompliantPostureTask(const mc_solver::QPSolver & solver, unsigned int rIndex, double stiffness, double weight); + + /** Change reference acceleration + * + * \p refAccel Should be of size nrDof + */ + void refAccel(const Eigen::VectorXd & refAccel) noexcept; + + // Set task to be compliant or not + void makeCompliant(bool compliance); + void makeCompliant(Eigen::VectorXd gamma); + // Get compliance state of the task + bool isCompliant(void); + +protected: + void update(mc_solver::QPSolver & solver); + + void addToGUI(mc_rtc::gui::StateBuilder & gui); + + Eigen::VectorXd gamma_; + + mc_tvm::Robot & tvm_robot_; + + Eigen::VectorXd refAccel_; +}; + +} // namespace mc_tasks diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 57a2fc1be1..74a710c33a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -528,6 +528,10 @@ set(mc_tasks_SRC mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp mc_tasks/lipm_stabilizer/ZMPCC.cpp mc_tasks/lipm_stabilizer/Contact.cpp + mc_tasks/CompliantPostureTask.cpp + mc_tasks/CompliantPositionTask.cpp + mc_tasks/CompliantOrientationTask.cpp + mc_tasks/CompliantEndEffectorTask.cpp ) set(mc_tasks_HDR @@ -564,6 +568,10 @@ set(mc_tasks_HDR ../include/mc_tasks/lipm_stabilizer/StabilizerTask.h ../include/mc_tasks/lipm_stabilizer/Contact.h ../include/mc_tasks/lipm_stabilizer/ZMPCC.h + ../include/mc_tasks/CompliantPostureTask.h + ../include/mc_tasks/CompliantPositionTask.h + ../include/mc_tasks/CompliantOrientationTask.h + ../include/mc_tasks/CompliantEndEffectorTask.h ) add_library(mc_tasks SHARED ${mc_tasks_SRC} ${mc_tasks_HDR}) diff --git a/src/mc_tasks/CompliantEndEffectorTask.cpp b/src/mc_tasks/CompliantEndEffectorTask.cpp new file mode 100644 index 0000000000..79375718cd --- /dev/null +++ b/src/mc_tasks/CompliantEndEffectorTask.cpp @@ -0,0 +1,163 @@ +/* + * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +#include +#include +#include +#include +#include + +namespace mc_tasks +{ + +CompliantEndEffectorTask::CompliantEndEffectorTask(const std::string & bodyName, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight) +: EndEffectorTask(robots.robot(robotIndex).frame(bodyName), stiffness, weight), + compliant_matrix_(Eigen::Matrix6d::Zero()), tvm_robot_(nullptr), robot_(&robots.robot(robotIndex)), rIdx_(robotIndex), + bodyName_(bodyName), frame_(robots.robot(robotIndex).frame(bodyName)), refAccel_(Eigen::Vector6d::Zero()) +{ + if(backend_ != Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use TVM or TVMHierarchical backend", + backend_); + + type_ = "compliant_body6d"; + name_ = "compliant_body6d_" + frame_.robot().name() + "_" + frame_.name(); + EndEffectorTask::name(name_); +} + +void CompliantEndEffectorTask::refAccel(const Eigen::Vector6d & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantEndEffectorTask::makeCompliant(bool compliance) +{ + if(compliance) { compliant_matrix_.diagonal().setOnes(); } + else + { + compliant_matrix_.diagonal().setZero(); + } +} + +void CompliantEndEffectorTask::setComplianceVector(Eigen::Vector6d gamma) +{ + compliant_matrix_.diagonal() = gamma; +} + +bool CompliantEndEffectorTask::isCompliant(void) +{ + return compliant_matrix_.diagonal().norm() > 0; +} + +Eigen::Vector6d CompliantEndEffectorTask::getComplianceVector(void) +{ + return compliant_matrix_.diagonal(); +} + +void CompliantEndEffectorTask::addToSolver(mc_solver::QPSolver & solver) +{ + EndEffectorTask::addToSolver(solver); + tvm_robot_ = &solver.robots().robot(rIdx_).tvmRobot(); + jac_ = new rbd::Jacobian(tvm_robot_->robot().mb(), frame_.body()); +} + +void CompliantEndEffectorTask::update(mc_solver::QPSolver & solver) +{ + Eigen::MatrixXd J = jac_->jacobian(solver.robot(rIdx_).mb(), solver.robot(rIdx_).mbc()); + Eigen::Vector6d disturbance; + Eigen::VectorXd acc; + + if(backend_ == Backend::Tasks) + { + if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + else + { + acc = solver.robot().externalTorquesAcc(); + } + mc_rtc::log::info("Task sensor acc = {}", acc.transpose()); + disturbance = J * acc; + } + else + { + disturbance.setZero(); + } + + Eigen::Vector6d disturbedAccel = refAccel_ + compliant_matrix_ * disturbance; + + EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); + EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); + + EndEffectorTask::update(solver); +} + +void CompliantEndEffectorTask::load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) +{ + MetaTask::load(solver, config); + if(config.has("stiffness")) + { + auto s = config("stiffness"); + if(s.size()) + { + Eigen::VectorXd stiff = s; + positionTask->stiffness(stiff); + orientationTask->stiffness(stiff); + } + else + { + double stiff = s; + positionTask->stiffness(stiff); + orientationTask->stiffness(stiff); + } + } + if(config.has("damping")) + { + auto d = config("damping"); + if(d.size()) + { + positionTask->setGains(positionTask->dimStiffness(), d); + orientationTask->setGains(orientationTask->dimStiffness(), d); + } + else + { + positionTask->setGains(positionTask->stiffness(), d); + orientationTask->setGains(orientationTask->stiffness(), d); + } + } + if(config.has("compliance")) + { + auto g = config("compliance"); + if(g.size()) { setComplianceVector(g); } + else + { + makeCompliant((double)g != 0); + } + } + if(config.has("weight")) + { + double w = config("weight"); + positionTask->weight(w); + orientationTask->weight(w); + } +} + +void CompliantEndEffectorTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + gui.addElement({"Tasks", name_, "Compliance"}, mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant(); }, + [this]() { makeCompliant(!isCompliant()); })); + gui.addElement({"Tasks", name_, "Compliance"}, + mc_rtc::gui::ArrayInput( + "Compliance parameters", {"rx", "ry", "rz", "x", "y", "z"}, [this]() + { return getComplianceVector(); }, [this](Eigen::Vector6d v) { setComplianceVector(v); })); + + EndEffectorTask::addToGUI(gui); +} + +} // namespace mc_tasks diff --git a/src/mc_tasks/CompliantOrientationTask.cpp b/src/mc_tasks/CompliantOrientationTask.cpp new file mode 100644 index 0000000000..d1948cb06b --- /dev/null +++ b/src/mc_tasks/CompliantOrientationTask.cpp @@ -0,0 +1,100 @@ +#include + +#include +#include +#include +#include +#include + +namespace mc_tasks +{ + +CompliantOrientationTask::CompliantOrientationTask(const std::string & bodyName_, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight) +: OrientationTask(robots.robot(robotIndex).frame(bodyName_), stiffness, weight), Gamma_(Eigen::Matrix3d::Zero()), + tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), frame_(robots.robot(robotIndex).frame(bodyName_)), + refAccel_(Eigen::Vector3d::Zero()) +{ + if(backend_ != Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + + type_ = "compliant_position"; + name_ = std::string("compliant_position_") + frame_.robot().name() + "_" + frame_.name(); + OrientationTask::name(name_); +} + +void CompliantOrientationTask::refAccel(const Eigen::Vector3d & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantOrientationTask::update(mc_solver::QPSolver & solver) +{ + auto J = jac_->jacobian(robots.robot(rIndex).mb(), robots.robot(rIndex).mbc()); + Eigen::Vector3d disturbance; + Eigen::VectorXd acc; + if(backend_ == Backend::Tasks) + { + if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + else + { + acc = solver.robot().externalTorquesAcc(); + } + Eigen::Vector3d frame_acc = (J * acc).head(3); + disturbance = Gamma_ * frame_acc; + } + else + { + disturbance.setZero(); + } + // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); + Eigen::Vector3d disturbedAccel = refAccel_ + disturbance; + OrientationTask::refAccel(disturbedAccel); + OrientationTask::update(solver); +} + +void CompliantOrientationTask::makeCompliant(bool compliance) +{ + if(compliance) { Gamma_.diagonal().setOnes(); } + else + { + Gamma_.diagonal().setZero(); + } +} + +void CompliantOrientationTask::setComplianceVector(Eigen::Vector3d Gamma) +{ + Gamma_.diagonal() = Gamma; +} + +bool CompliantOrientationTask::isCompliant(void) +{ + return Gamma_.diagonal().norm() > 0; +} + +Eigen::Vector3d CompliantOrientationTask::getComplianceVector(void) +{ + return Gamma_.diagonal(); +} + +void CompliantOrientationTask::addToSolver(mc_solver::QPSolver & solver) +{ + OrientationTask::addToSolver(solver); + jac_ = new rbd::Jacobian(robots.robot(rIdx_).mb(), frame_.body()); +} + +void CompliantOrientationTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + OrientationTask::addToGUI(gui); + gui.addElement( + {"Tasks", name_, "Compliance"}, + mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant(); }, [this]() { makeCompliant(!isCompliant()); }), + mc_rtc::gui::ArrayInput("Gamma", {"x", "y", "z"}, Gamma_)); +} + +} // namespace mc_tasks diff --git a/src/mc_tasks/CompliantPositionTask.cpp b/src/mc_tasks/CompliantPositionTask.cpp new file mode 100644 index 0000000000..fa23e31bdd --- /dev/null +++ b/src/mc_tasks/CompliantPositionTask.cpp @@ -0,0 +1,100 @@ +#include + +#include +#include +#include +#include +#include + +namespace mc_tasks +{ + +CompliantPositionTask::CompliantPositionTask(const std::string & bodyName_, + const mc_rbdyn::Robots & robots, + unsigned int robotIndex, + double stiffness, + double weight) +: PositionTask(robots.robot(robotIndex).frame(bodyName_), stiffness, weight), Gamma_(Eigen::Matrix3d::Zero()), + tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), frame_(robots.robot(robotIndex).frame(bodyName_)), + refAccel_(Eigen::Vector3d::Zero()) +{ + if(backend_ != Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + + type_ = "compliant_position"; + name_ = std::string("compliant_position_") + frame_.robot().name() + "_" + frame_.name(); + PositionTask::name(name_); +} + +void CompliantPositionTask::refAccel(const Eigen::Vector3d & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantPositionTask::update(mc_solver::QPSolver & solver) +{ + auto J = jac_->jacobian(robots.robot(rIndex).mb(), robots.robot(rIndex).mbc()); + Eigen::Vector3d disturbance; + Eigen::VectorXd acc; + if(backend_ == Backend::Tasks) + { + if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + else + { + acc = solver.robot().externalTorquesAcc(); + } + Eigen::Vector3d frame_acc = (J * acc).tail(3); + disturbance = Gamma_ * frame_acc; + } + else + { + disturbance.setZero(); + } + // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); + Eigen::Vector3d disturbedAccel = refAccel_ + disturbance; + PositionTask::refAccel(disturbedAccel); + PositionTask::update(solver); +} + +void CompliantPositionTask::makeCompliant(bool compliance) +{ + if(compliance) { Gamma_.diagonal().setOnes(); } + else + { + Gamma_.diagonal().setZero(); + } +} + +void CompliantPositionTask::setComplianceVector(Eigen::Vector3d Gamma) +{ + Gamma_.diagonal() = Gamma; +} + +bool CompliantPositionTask::isCompliant(void) +{ + return Gamma_.diagonal().norm() > 0; +} + +Eigen::Vector3d CompliantPositionTask::getComplianceVector(void) +{ + return Gamma_.diagonal(); +} + +void CompliantPositionTask::addToSolver(mc_solver::QPSolver & solver) +{ + PositionTask::addToSolver(solver); + jac_ = new rbd::Jacobian(robots.robot(rIdx_).mb(), frame_.body()); +} + +void CompliantPositionTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + PositionTask::addToGUI(gui); + gui.addElement( + {"Tasks", name_, "Compliance"}, + mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant(); }, [this]() { makeCompliant(!isCompliant()); }), + mc_rtc::gui::ArrayInput("Gamma", {"x", "y", "z"}, Gamma_)); +} + +} // namespace mc_tasks diff --git a/src/mc_tasks/CompliantPostureTask.cpp b/src/mc_tasks/CompliantPostureTask.cpp new file mode 100644 index 0000000000..e46e25d851 --- /dev/null +++ b/src/mc_tasks/CompliantPostureTask.cpp @@ -0,0 +1,88 @@ +#include + +#include +#include +#include "mc_rtc/gui/ArrayInput.h" +#include "mc_rtc/logging.h" +#include + +namespace mc_tasks +{ + +CompliantPostureTask::CompliantPostureTask(const mc_solver::QPSolver & solver, + unsigned int rIndex, + double stiffness, + double weight) +: PostureTask(solver, rIndex, stiffness, weight), + gamma_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())), + tvm_robot_(solver.robots().robot(rIndex).tvmRobot()), + refAccel_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())) +{ + if(backend_ != Backend::Tasks) + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + name_ = std::string("compliant_posture_") + solver.robots().robot(rIndex).name(); + type_ = "compliant_posture"; +} + +void CompliantPostureTask::refAccel(const Eigen::VectorXd & refAccel) noexcept +{ + refAccel_ = refAccel; +} + +void CompliantPostureTask::update(mc_solver::QPSolver & solver) +{ + Eigen::VectorXd disturbance; + Eigen::VectorXd acc; + if(backend_ == Backend::Tasks) + { + if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + else + { + acc = solver.robot().externalTorquesAcc(); + } + disturbance = gamma_.asDiagonal() * acc; + } + else + { + disturbance.setZero(); + } + + // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); + // mc_rtc::log::info("Ref accel : {}", refAccel_.transpose()); + Eigen::VectorXd disturbedAccel = refAccel_ + disturbance; + PostureTask::refAccel(disturbedAccel); + PostureTask::update(solver); +} + +void CompliantPostureTask::makeCompliant(bool compliance) +{ + if(compliance) { gamma_.setOnes(); } + else + { + gamma_.setZero(); + } +} + +void CompliantPostureTask::makeCompliant(Eigen::VectorXd gamma) +{ + gamma_ = gamma; +} + +bool CompliantPostureTask::isCompliant(void) +{ + return gamma_.norm() > 0; +} + +void CompliantPostureTask::addToGUI(mc_rtc::gui::StateBuilder & gui) +{ + PostureTask::addToGUI(gui); + gui.addElement( + {"Tasks", name_, "Compliance"}, + mc_rtc::gui::Checkbox( + "Compliance is active", [this]() { return isCompliant(); }, [this]() { makeCompliant(!isCompliant()); }), + mc_rtc::gui::ArrayInput("Gamma", {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}, + gamma_)); +} + +} // namespace mc_tasks From 3cc4e67c5b54ae4e570bd7afe309c38446144503 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Wed, 4 Feb 2026 15:11:56 +0900 Subject: [PATCH 8/9] [mc_tasks] Add TVM backend support for compliant tasks Impacted tasks: - CompliantPostureTask - CompliantPositionTask - CompliantOrientationTask - CompliantEndEffectorTask --- include/mc_tasks/CompliantEndEffectorTask.h | 4 +- include/mc_tasks/CompliantOrientationTask.h | 1 + include/mc_tasks/CompliantPositionTask.h | 1 + include/mc_tasks/CompliantPostureTask.h | 1 + src/mc_tasks/CompliantEndEffectorTask.cpp | 41 +++++++++++++-------- src/mc_tasks/CompliantOrientationTask.cpp | 28 +++++++++----- src/mc_tasks/CompliantPositionTask.cpp | 28 +++++++++----- src/mc_tasks/CompliantPostureTask.cpp | 27 ++++++++++---- 8 files changed, 88 insertions(+), 43 deletions(-) diff --git a/include/mc_tasks/CompliantEndEffectorTask.h b/include/mc_tasks/CompliantEndEffectorTask.h index 8d2ae3b8ca..278f451a5e 100644 --- a/include/mc_tasks/CompliantEndEffectorTask.h +++ b/include/mc_tasks/CompliantEndEffectorTask.h @@ -64,8 +64,8 @@ struct MC_TASKS_DLLAPI CompliantEndEffectorTask : public EndEffectorTask Eigen::Matrix6d compliant_matrix_; - mc_tvm::Robot * tvm_robot_; - const mc_rbdyn::Robot * robot_; + const mc_rbdyn::Robot & robot_; + mc_tvm::Robot & tvm_robot_; unsigned int rIdx_; diff --git a/include/mc_tasks/CompliantOrientationTask.h b/include/mc_tasks/CompliantOrientationTask.h index eb428bd8bf..924741df92 100644 --- a/include/mc_tasks/CompliantOrientationTask.h +++ b/include/mc_tasks/CompliantOrientationTask.h @@ -43,6 +43,7 @@ struct MC_TASKS_DLLAPI CompliantOrientationTask : public OrientationTask Eigen::Matrix3d Gamma_; + const mc_rbdyn::Robot & robot_; mc_tvm::Robot & tvm_robot_; unsigned int rIdx_; diff --git a/include/mc_tasks/CompliantPositionTask.h b/include/mc_tasks/CompliantPositionTask.h index 7685afe7b0..6f170fcf89 100644 --- a/include/mc_tasks/CompliantPositionTask.h +++ b/include/mc_tasks/CompliantPositionTask.h @@ -43,6 +43,7 @@ struct MC_TASKS_DLLAPI CompliantPositionTask : public PositionTask Eigen::Matrix3d Gamma_; + const mc_rbdyn::Robot & robot_; mc_tvm::Robot & tvm_robot_; unsigned int rIdx_; diff --git a/include/mc_tasks/CompliantPostureTask.h b/include/mc_tasks/CompliantPostureTask.h index e387295246..c2985037b4 100644 --- a/include/mc_tasks/CompliantPostureTask.h +++ b/include/mc_tasks/CompliantPostureTask.h @@ -33,6 +33,7 @@ struct MC_TASKS_DLLAPI CompliantPostureTask : public PostureTask Eigen::VectorXd gamma_; + const mc_rbdyn::Robot & robot_; mc_tvm::Robot & tvm_robot_; Eigen::VectorXd refAccel_; diff --git a/src/mc_tasks/CompliantEndEffectorTask.cpp b/src/mc_tasks/CompliantEndEffectorTask.cpp index 79375718cd..5ddd949944 100644 --- a/src/mc_tasks/CompliantEndEffectorTask.cpp +++ b/src/mc_tasks/CompliantEndEffectorTask.cpp @@ -19,13 +19,20 @@ CompliantEndEffectorTask::CompliantEndEffectorTask(const std::string & bodyName, double stiffness, double weight) : EndEffectorTask(robots.robot(robotIndex).frame(bodyName), stiffness, weight), - compliant_matrix_(Eigen::Matrix6d::Zero()), tvm_robot_(nullptr), robot_(&robots.robot(robotIndex)), rIdx_(robotIndex), - bodyName_(bodyName), frame_(robots.robot(robotIndex).frame(bodyName)), refAccel_(Eigen::Vector6d::Zero()) + compliant_matrix_(Eigen::Matrix6d::Zero()), robot_(robots.robot(robotIndex)), + tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), bodyName_(bodyName), + frame_(robots.robot(robotIndex).frame(bodyName)), refAccel_(Eigen::Vector6d::Zero()) { - if(backend_ != Backend::Tasks) - mc_rtc::log::error_and_throw( - "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use TVM or TVMHierarchical backend", - backend_); + switch(backend_) + { + case Backend::Tasks: + case Backend::TVM: + break; + default: + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks or TVM backend", backend_); + break; + } type_ = "compliant_body6d"; name_ = "compliant_body6d_" + frame_.robot().name() + "_" + frame_.name(); @@ -64,37 +71,41 @@ Eigen::Vector6d CompliantEndEffectorTask::getComplianceVector(void) void CompliantEndEffectorTask::addToSolver(mc_solver::QPSolver & solver) { EndEffectorTask::addToSolver(solver); - tvm_robot_ = &solver.robots().robot(rIdx_).tvmRobot(); - jac_ = new rbd::Jacobian(tvm_robot_->robot().mb(), frame_.body()); + jac_ = new rbd::Jacobian(robot_.mb(), frame_.body()); } void CompliantEndEffectorTask::update(mc_solver::QPSolver & solver) { Eigen::MatrixXd J = jac_->jacobian(solver.robot(rIdx_).mb(), solver.robot(rIdx_).mbc()); - Eigen::Vector6d disturbance; Eigen::VectorXd acc; if(backend_ == Backend::Tasks) { - if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + if(robot_.compensationTorquesAcc()) { acc = robot_.compensationTorquesAcc().value(); } else { - acc = solver.robot().externalTorquesAcc(); + acc = robot_.externalTorquesAcc(); } - mc_rtc::log::info("Task sensor acc = {}", acc.transpose()); - disturbance = J * acc; } else { - disturbance.setZero(); + if(tvm_robot_.alphaDCompensation()) { acc = tvm_robot_.alphaDCompensation().value(); } + else + { + acc = tvm_robot_.alphaDExternal(); + } } + Eigen::Vector6d disturbance = J * acc; + mc_rtc::log::info("Task sensor acc = {}", acc.transpose()); + mc_rtc::log::info("Task equivalent acc = {}", disturbance.transpose()); Eigen::Vector6d disturbedAccel = refAccel_ + compliant_matrix_ * disturbance; + mc_rtc::log::info("Task disturbed ref acc = {}", disturbedAccel.transpose()); EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); - EndEffectorTask::update(solver); + // EndEffectorTask::update(solver); } void CompliantEndEffectorTask::load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) diff --git a/src/mc_tasks/CompliantOrientationTask.cpp b/src/mc_tasks/CompliantOrientationTask.cpp index d1948cb06b..57b0152e64 100644 --- a/src/mc_tasks/CompliantOrientationTask.cpp +++ b/src/mc_tasks/CompliantOrientationTask.cpp @@ -15,12 +15,19 @@ CompliantOrientationTask::CompliantOrientationTask(const std::string & bodyName_ double stiffness, double weight) : OrientationTask(robots.robot(robotIndex).frame(bodyName_), stiffness, weight), Gamma_(Eigen::Matrix3d::Zero()), - tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), frame_(robots.robot(robotIndex).frame(bodyName_)), - refAccel_(Eigen::Vector3d::Zero()) + robot_(robots.robot(robotIndex)), tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), + frame_(robots.robot(robotIndex).frame(bodyName_)), refAccel_(Eigen::Vector3d::Zero()) { - if(backend_ != Backend::Tasks) - mc_rtc::log::error_and_throw( - "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + switch(backend_) + { + case Backend::Tasks: + case Backend::TVM: + break; + default: + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantOrientationTask with {} backend, please use Tasks or TVM backend", backend_); + break; + } type_ = "compliant_position"; name_ = std::string("compliant_position_") + frame_.robot().name() + "_" + frame_.name(); @@ -35,7 +42,6 @@ void CompliantOrientationTask::refAccel(const Eigen::Vector3d & refAccel) noexce void CompliantOrientationTask::update(mc_solver::QPSolver & solver) { auto J = jac_->jacobian(robots.robot(rIndex).mb(), robots.robot(rIndex).mbc()); - Eigen::Vector3d disturbance; Eigen::VectorXd acc; if(backend_ == Backend::Tasks) { @@ -44,13 +50,17 @@ void CompliantOrientationTask::update(mc_solver::QPSolver & solver) { acc = solver.robot().externalTorquesAcc(); } - Eigen::Vector3d frame_acc = (J * acc).head(3); - disturbance = Gamma_ * frame_acc; } else { - disturbance.setZero(); + if(tvm_robot_.alphaDCompensation()) { acc = tvm_robot_.alphaDCompensation().value(); } + else + { + acc = tvm_robot_.alphaDExternal(); + } } + Eigen::Vector3d frame_acc = (J * acc).head(3); + Eigen::Vector3d disturbance = Gamma_ * frame_acc; // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); Eigen::Vector3d disturbedAccel = refAccel_ + disturbance; OrientationTask::refAccel(disturbedAccel); diff --git a/src/mc_tasks/CompliantPositionTask.cpp b/src/mc_tasks/CompliantPositionTask.cpp index fa23e31bdd..6c44fc07b1 100644 --- a/src/mc_tasks/CompliantPositionTask.cpp +++ b/src/mc_tasks/CompliantPositionTask.cpp @@ -15,12 +15,19 @@ CompliantPositionTask::CompliantPositionTask(const std::string & bodyName_, double stiffness, double weight) : PositionTask(robots.robot(robotIndex).frame(bodyName_), stiffness, weight), Gamma_(Eigen::Matrix3d::Zero()), - tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), frame_(robots.robot(robotIndex).frame(bodyName_)), - refAccel_(Eigen::Vector3d::Zero()) + robot_(robots.robot(robotIndex)), tvm_robot_(robots.robot(robotIndex).tvmRobot()), rIdx_(robotIndex), + frame_(robots.robot(robotIndex).frame(bodyName_)), refAccel_(Eigen::Vector3d::Zero()) { - if(backend_ != Backend::Tasks) - mc_rtc::log::error_and_throw( - "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + switch(backend_) + { + case Backend::Tasks: + case Backend::TVM: + break; + default: + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantPositionTask with {} backend, please use Tasks or TVM backend", backend_); + break; + } type_ = "compliant_position"; name_ = std::string("compliant_position_") + frame_.robot().name() + "_" + frame_.name(); @@ -35,7 +42,6 @@ void CompliantPositionTask::refAccel(const Eigen::Vector3d & refAccel) noexcept void CompliantPositionTask::update(mc_solver::QPSolver & solver) { auto J = jac_->jacobian(robots.robot(rIndex).mb(), robots.robot(rIndex).mbc()); - Eigen::Vector3d disturbance; Eigen::VectorXd acc; if(backend_ == Backend::Tasks) { @@ -44,13 +50,17 @@ void CompliantPositionTask::update(mc_solver::QPSolver & solver) { acc = solver.robot().externalTorquesAcc(); } - Eigen::Vector3d frame_acc = (J * acc).tail(3); - disturbance = Gamma_ * frame_acc; } else { - disturbance.setZero(); + if(tvm_robot_.alphaDCompensation()) { acc = tvm_robot_.alphaDCompensation().value(); } + else + { + acc = tvm_robot_.alphaDExternal(); + } } + Eigen::Vector3d frame_acc = (J * acc).tail(3); + Eigen::Vector3d disturbance = Gamma_ * frame_acc; // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); Eigen::Vector3d disturbedAccel = refAccel_ + disturbance; PositionTask::refAccel(disturbedAccel); diff --git a/src/mc_tasks/CompliantPostureTask.cpp b/src/mc_tasks/CompliantPostureTask.cpp index e46e25d851..b3a7a3d9b7 100644 --- a/src/mc_tasks/CompliantPostureTask.cpp +++ b/src/mc_tasks/CompliantPostureTask.cpp @@ -14,13 +14,20 @@ CompliantPostureTask::CompliantPostureTask(const mc_solver::QPSolver & solver, double stiffness, double weight) : PostureTask(solver, rIndex, stiffness, weight), - gamma_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())), + gamma_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())), robot_(solver.robots().robot(rIndex)), tvm_robot_(solver.robots().robot(rIndex).tvmRobot()), refAccel_(Eigen::VectorXd::Zero(solver.robots().robot(rIndex).mb().nrDof())) { - if(backend_ != Backend::Tasks) - mc_rtc::log::error_and_throw( - "[mc_tasks] Can't use CompliantEndEffectorTask with {} backend, please use Tasks backend", backend_); + switch(backend_) + { + case Backend::Tasks: + case Backend::TVM: + break; + default: + mc_rtc::log::error_and_throw( + "[mc_tasks] Can't use CompliantPostureTask with {} backend, please use Tasks or TVM backend", backend_); + break; + } name_ = std::string("compliant_posture_") + solver.robots().robot(rIndex).name(); type_ = "compliant_posture"; } @@ -36,17 +43,21 @@ void CompliantPostureTask::update(mc_solver::QPSolver & solver) Eigen::VectorXd acc; if(backend_ == Backend::Tasks) { - if(solver.robot().compensationTorquesAcc()) { acc = solver.robot().compensationTorquesAcc().value(); } + if(robot_.compensationTorquesAcc()) { acc = robot_.compensationTorquesAcc().value(); } else { - acc = solver.robot().externalTorquesAcc(); + acc = robot_.externalTorquesAcc(); } - disturbance = gamma_.asDiagonal() * acc; } else { - disturbance.setZero(); + if(tvm_robot_.alphaDCompensation()) { acc = tvm_robot_.alphaDCompensation().value(); } + else + { + acc = tvm_robot_.alphaDExternal(); + } } + disturbance = gamma_.asDiagonal() * acc; // mc_rtc::log::info("Ref accel from disturbance : {}", disturbance.transpose()); // mc_rtc::log::info("Ref accel : {}", refAccel_.transpose()); From e8cd4b0d7634c5bc979c120302c17cf36988c243 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Tue, 10 Mar 2026 10:54:14 +0900 Subject: [PATCH 9/9] fix: Remove logs --- src/mc_tasks/CompliantEndEffectorTask.cpp | 3 --- src/mc_tasks/CompliantPostureTask.cpp | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/mc_tasks/CompliantEndEffectorTask.cpp b/src/mc_tasks/CompliantEndEffectorTask.cpp index 5ddd949944..168d7bee95 100644 --- a/src/mc_tasks/CompliantEndEffectorTask.cpp +++ b/src/mc_tasks/CompliantEndEffectorTask.cpp @@ -96,11 +96,8 @@ void CompliantEndEffectorTask::update(mc_solver::QPSolver & solver) } } Eigen::Vector6d disturbance = J * acc; - mc_rtc::log::info("Task sensor acc = {}", acc.transpose()); - mc_rtc::log::info("Task equivalent acc = {}", disturbance.transpose()); Eigen::Vector6d disturbedAccel = refAccel_ + compliant_matrix_ * disturbance; - mc_rtc::log::info("Task disturbed ref acc = {}", disturbedAccel.transpose()); EndEffectorTask::positionTask->refAccel(disturbedAccel.tail(3)); EndEffectorTask::orientationTask->refAccel(disturbedAccel.head(3)); diff --git a/src/mc_tasks/CompliantPostureTask.cpp b/src/mc_tasks/CompliantPostureTask.cpp index b3a7a3d9b7..ed5c449614 100644 --- a/src/mc_tasks/CompliantPostureTask.cpp +++ b/src/mc_tasks/CompliantPostureTask.cpp @@ -63,7 +63,7 @@ void CompliantPostureTask::update(mc_solver::QPSolver & solver) // mc_rtc::log::info("Ref accel : {}", refAccel_.transpose()); Eigen::VectorXd disturbedAccel = refAccel_ + disturbance; PostureTask::refAccel(disturbedAccel); - PostureTask::update(solver); + // PostureTask::update(solver); } void CompliantPostureTask::makeCompliant(bool compliance)