From d5659b73ead60631d9096c2c8a326d6a984222d7 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Tue, 3 Jun 2025 12:57:38 +0900 Subject: [PATCH 1/2] Add the possibility to compensate an external disturbance torques Add a copy of each constructors of `MotionConstrCommon`, `MotionConstr` and `MotionSpringConstr` that includes an additional reference to an `Eigen::VectorXd` that contains an estimation or the exact value of the external disturbance torques to be compensated for. `MotionConstrCommon` set a `bool` depending on which constructor is called to know if external torques should be accounted for. - This is required to know if the reference is properly initialized. This value is later used in the `computeMatrix` and `computeTorque` function of `MotionConstrCommon` if external torques needs to be compensated. --- src/QPMotionConstr.cpp | 78 +++++++++++++++++++++++++++++++++++++- src/Tasks/QPMotionConstr.h | 29 ++++++++++++++ 2 files changed, 106 insertions(+), 1 deletion(-) diff --git a/src/QPMotionConstr.cpp b/src/QPMotionConstr.cpp index da7d1b37..9b88a073 100644 --- a/src/QPMotionConstr.cpp +++ b/src/QPMotionConstr.cpp @@ -111,7 +111,22 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb, MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, int robotIndex) : robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(mbs[robotIndex_]), - fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), A_(), AL_(nrDof_), AU_(nrDof_) + fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), useExternalTorque_(false), + extTorque_(curTorque_), A_(), AL_(nrDof_), AU_(nrDof_) +{ + // We set the extTorque_ reference to curTorque_ as it needs to be initialized in the struct but we are not going to + // use it + assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); + // This is technically incorrect but practically not a huge deal, see #66 + curTorque_.setZero(); +} + +MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, + int robotIndex, + const Eigen::VectorXd & externalTorque) +: robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(mbs[robotIndex_]), + fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), useExternalTorque_(true), + extTorque_(externalTorque), A_(), AL_(nrDof_), AU_(nrDof_) { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); // This is technically incorrect but practically not a huge deal, see #66 @@ -122,6 +137,7 @@ void MotionConstrCommon::computeTorque(const Eigen::VectorXd & alphaD, const Eig { curTorque_ = fd_.H() * alphaD.segment(alphaDBegin_, nrDof_); curTorque_ += fd_.C(); + if(useExternalTorque_) { curTorque_ -= extTorque_; } curTorque_ += A_.block(0, lambdaBegin_, nrDof_, A_.cols() - lambdaBegin_) * lambda; } @@ -220,6 +236,11 @@ void MotionConstrCommon::computeMatrix(const std::vector & mbs, // BEq = -C AL_ = -fd_.C(); AU_ = -fd_.C(); + if(useExternalTorque_) + { + AL_ += extTorque_; + AU_ += extTorque_; + } } int MotionConstrCommon::maxGenInEq() const @@ -262,6 +283,14 @@ MotionConstr::MotionConstr(const std::vector & mbs, int robotInd { } +MotionConstr::MotionConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const Eigen::VectorXd & externalTorque) +: MotionConstr(mbs, robotIndex, tb, {}, 0.001, externalTorque) +{ +} + MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, @@ -280,6 +309,26 @@ MotionConstr::MotionConstr(const std::vector & mbs, torqueDtU_ *= dt; } +MotionConstr::MotionConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const TorqueDBound & tdb, + double dt, + const Eigen::VectorXd & externalTorque) +: MotionConstrCommon(mbs, robotIndex, externalTorque), torqueL_(mbs[robotIndex].nrDof()), + torqueU_(mbs[robotIndex].nrDof()), torqueDtL_(mbs[robotIndex].nrDof()), torqueDtU_(mbs[robotIndex].nrDof()), + tmpL_(nrDof_), tmpU_(nrDof_) +{ + rbd::paramToVector(tb.lTorqueBound, torqueL_); + rbd::paramToVector(tb.uTorqueBound, torqueU_); + torqueDtL_.setConstant(-std::numeric_limits::infinity()); + torqueDtU_.setConstant(std::numeric_limits::infinity()); + rbd::paramToVector(tdb.lTorqueDBound, torqueDtL_); + rbd::paramToVector(tdb.uTorqueDBound, torqueDtU_); + torqueDtL_ *= dt; + torqueDtU_ *= dt; +} + void MotionConstr::update(const std::vector & mbs, const std::vector & mbcs, const SolverData & /* data */) @@ -321,6 +370,15 @@ MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, { } +MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const std::vector & springs, + const Eigen::VectorXd & externalTorque) +: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs, externalTorque) +{ +} + MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, @@ -338,6 +396,24 @@ MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, } } +MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const TorqueDBound & tdb, + double dt, + const std::vector & springs, + const Eigen::VectorXd & externalTorque) +: MotionConstr(mbs, robotIndex, tb, tdb, dt, externalTorque), springs_() +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + for(const SpringJoint & sj : springs) + { + int index = mb.jointIndexByName(sj.jointName); + int posInDof = mb.jointPosInDof(index); + springs_.push_back({index, posInDof, sj.K, sj.C, sj.O}); + } +} + void MotionSpringConstr::update(const std::vector & mbs, const std::vector & mbcs, const SolverData & /* data */) diff --git a/src/Tasks/QPMotionConstr.h b/src/Tasks/QPMotionConstr.h index e585eb52..131808e0 100644 --- a/src/Tasks/QPMotionConstr.h +++ b/src/Tasks/QPMotionConstr.h @@ -66,6 +66,7 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction { public: MotionConstrCommon(const std::vector & mbs, int robotIndex); + MotionConstrCommon(const std::vector & mbs, int robotIndex, const Eigen::VectorXd & externalTorque); void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda); const Eigen::VectorXd & torque() const; @@ -114,6 +115,9 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction Eigen::VectorXd curTorque_; + bool useExternalTorque_; + const Eigen::VectorXd & extTorque_; + Eigen::MatrixXd A_; Eigen::VectorXd AL_, AU_; size_t updateIter_ = 0; @@ -123,6 +127,10 @@ class TASKS_DLLAPI MotionConstr : public MotionConstrCommon { public: MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb); + MotionConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const Eigen::VectorXd & externalTorque); MotionConstr(const std::vector & mbs, int robotIndex, @@ -130,6 +138,13 @@ class TASKS_DLLAPI MotionConstr : public MotionConstrCommon const TorqueDBound & tdb, double dt); + MotionConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const TorqueDBound & tdb, + double dt, + const Eigen::VectorXd & externalTorque); + // Constraint virtual void update(const std::vector & mbs, const std::vector & mbcs, @@ -164,6 +179,12 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr const TorqueBound & tb, const std::vector & springs); + MotionSpringConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const std::vector & springs, + const Eigen::VectorXd & externalTorque); + MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, @@ -171,6 +192,14 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr double dt, const std::vector & springs); + MotionSpringConstr(const std::vector & mbs, + int robotIndex, + const TorqueBound & tb, + const TorqueDBound & tdb, + double dt, + const std::vector & springs, + const Eigen::VectorXd & externalTorque); + // Constraint virtual void update(const std::vector & mbs, const std::vector & mbc, From a150ade7359f6d71d686ffdb422e638c83c80122 Mon Sep 17 00:00:00 2001 From: Mathieu Celerier Date: Mon, 23 Mar 2026 16:52:33 +0900 Subject: [PATCH 2/2] Change external torque to std::optional, remove the reference and add external torques update function --- src/QPMotionConstr.cpp | 89 ++++++++------------------------------ src/Tasks/QPMotionConstr.h | 39 +++++------------ 2 files changed, 30 insertions(+), 98 deletions(-) diff --git a/src/QPMotionConstr.cpp b/src/QPMotionConstr.cpp index 9b88a073..b849e014 100644 --- a/src/QPMotionConstr.cpp +++ b/src/QPMotionConstr.cpp @@ -109,24 +109,12 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb, } } -MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, int robotIndex) -: robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(mbs[robotIndex_]), - fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), useExternalTorque_(false), - extTorque_(curTorque_), A_(), AL_(nrDof_), AU_(nrDof_) -{ - // We set the extTorque_ reference to curTorque_ as it needs to be initialized in the struct but we are not going to - // use it - assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); - // This is technically incorrect but practically not a huge deal, see #66 - curTorque_.setZero(); -} - MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, int robotIndex, - const Eigen::VectorXd & externalTorque) + std::optional externalTorque) : robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(mbs[robotIndex_]), - fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), useExternalTorque_(true), - extTorque_(externalTorque), A_(), AL_(nrDof_), AU_(nrDof_) + fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), extTorque_(externalTorque), A_(), + AL_(nrDof_), AU_(nrDof_) { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); // This is technically incorrect but practically not a huge deal, see #66 @@ -137,7 +125,7 @@ void MotionConstrCommon::computeTorque(const Eigen::VectorXd & alphaD, const Eig { curTorque_ = fd_.H() * alphaD.segment(alphaDBegin_, nrDof_); curTorque_ += fd_.C(); - if(useExternalTorque_) { curTorque_ -= extTorque_; } + if(extTorque_) { curTorque_ -= extTorque_.value(); } curTorque_ += A_.block(0, lambdaBegin_, nrDof_, A_.cols() - lambdaBegin_) * lambda; } @@ -236,10 +224,10 @@ void MotionConstrCommon::computeMatrix(const std::vector & mbs, // BEq = -C AL_ = -fd_.C(); AU_ = -fd_.C(); - if(useExternalTorque_) + if(extTorque_) { - AL_ += extTorque_; - AU_ += extTorque_; + AL_ += extTorque_.value(); + AU_ += extTorque_.value(); } } @@ -274,47 +262,31 @@ std::string MotionConstrCommon::descGenInEq(const std::vector & return std::string("Joint: ") + mbs[robotIndex_].joint(jIndex).name(); } +void MotionConstrCommon::setExternalTorques(const Eigen::VectorXd & torques) +{ + if(!extTorque_) { extTorque_.emplace(torques.size()); } + else if(extTorque_->size() == torques.size()) { extTorque_->resize(torques.size()); } + extTorque_->noalias() = torques; +} + /** * MotionConstr */ -MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb) -: MotionConstr(mbs, robotIndex, tb, {}, 0.001) -{ -} - MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, - const Eigen::VectorXd & externalTorque) + std::optional externalTorque) : MotionConstr(mbs, robotIndex, tb, {}, 0.001, externalTorque) { } -MotionConstr::MotionConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const TorqueDBound & tdb, - double dt) -: MotionConstrCommon(mbs, robotIndex), torqueL_(mbs[robotIndex].nrDof()), torqueU_(mbs[robotIndex].nrDof()), - torqueDtL_(mbs[robotIndex].nrDof()), torqueDtU_(mbs[robotIndex].nrDof()), tmpL_(nrDof_), tmpU_(nrDof_) -{ - rbd::paramToVector(tb.lTorqueBound, torqueL_); - rbd::paramToVector(tb.uTorqueBound, torqueU_); - torqueDtL_.setConstant(-std::numeric_limits::infinity()); - torqueDtU_.setConstant(std::numeric_limits::infinity()); - rbd::paramToVector(tdb.lTorqueDBound, torqueDtL_); - rbd::paramToVector(tdb.uTorqueDBound, torqueDtU_); - torqueDtL_ *= dt; - torqueDtU_ *= dt; -} - MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const TorqueDBound & tdb, double dt, - const Eigen::VectorXd & externalTorque) + std::optional externalTorque) : MotionConstrCommon(mbs, robotIndex, externalTorque), torqueL_(mbs[robotIndex].nrDof()), torqueU_(mbs[robotIndex].nrDof()), torqueDtL_(mbs[robotIndex].nrDof()), torqueDtU_(mbs[robotIndex].nrDof()), tmpL_(nrDof_), tmpU_(nrDof_) @@ -362,47 +334,22 @@ const rbd::ForwardDynamics MotionConstr::fd() const * MotionSpringConstr */ -MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const std::vector & springs) -: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs) -{ -} - MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const std::vector & springs, - const Eigen::VectorXd & externalTorque) + std::optional externalTorque) : MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs, externalTorque) { } -MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const TorqueDBound & tdb, - double dt, - const std::vector & springs) -: MotionConstr(mbs, robotIndex, tb, tdb, dt), springs_() -{ - const rbd::MultiBody & mb = mbs[robotIndex_]; - for(const SpringJoint & sj : springs) - { - int index = mb.jointIndexByName(sj.jointName); - int posInDof = mb.jointPosInDof(index); - springs_.push_back({index, posInDof, sj.K, sj.C, sj.O}); - } -} - MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const TorqueDBound & tdb, double dt, const std::vector & springs, - const Eigen::VectorXd & externalTorque) + std::optional externalTorque) : MotionConstr(mbs, robotIndex, tb, tdb, dt, externalTorque), springs_() { const rbd::MultiBody & mb = mbs[robotIndex_]; diff --git a/src/Tasks/QPMotionConstr.h b/src/Tasks/QPMotionConstr.h index 131808e0..80e795c8 100644 --- a/src/Tasks/QPMotionConstr.h +++ b/src/Tasks/QPMotionConstr.h @@ -17,6 +17,7 @@ // Tasks #include "QPSolver.h" +#include namespace tasks { @@ -65,8 +66,9 @@ class TASKS_DLLAPI PositiveLambda : public ConstraintFunction class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction { public: - MotionConstrCommon(const std::vector & mbs, int robotIndex); - MotionConstrCommon(const std::vector & mbs, int robotIndex, const Eigen::VectorXd & externalTorque); + MotionConstrCommon(const std::vector & mbs, + int robotIndex, + std::optional externalTorque = std::nullopt); void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda); const Eigen::VectorXd & torque() const; @@ -88,6 +90,9 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction virtual const Eigen::VectorXd & LowerGenInEq() const override; virtual const Eigen::VectorXd & UpperGenInEq() const override; + // Update external torques value + void setExternalTorques(const Eigen::VectorXd & torques); + protected: struct ContactData { @@ -115,8 +120,7 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction Eigen::VectorXd curTorque_; - bool useExternalTorque_; - const Eigen::VectorXd & extTorque_; + std::optional extTorque_; Eigen::MatrixXd A_; Eigen::VectorXd AL_, AU_; @@ -126,24 +130,17 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction class TASKS_DLLAPI MotionConstr : public MotionConstrCommon { public: - MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb); MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, - const Eigen::VectorXd & externalTorque); - - MotionConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const TorqueDBound & tdb, - double dt); + std::optional externalTorque = std::nullopt); MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const TorqueDBound & tdb, double dt, - const Eigen::VectorXd & externalTorque); + std::optional externalTorque = std::nullopt); // Constraint virtual void update(const std::vector & mbs, @@ -174,23 +171,11 @@ struct SpringJoint class TASKS_DLLAPI MotionSpringConstr : public MotionConstr { public: - MotionSpringConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const std::vector & springs); - MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const std::vector & springs, - const Eigen::VectorXd & externalTorque); - - MotionSpringConstr(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const TorqueDBound & tdb, - double dt, - const std::vector & springs); + std::optional externalTorque = std::nullopt); MotionSpringConstr(const std::vector & mbs, int robotIndex, @@ -198,7 +183,7 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr const TorqueDBound & tdb, double dt, const std::vector & springs, - const Eigen::VectorXd & externalTorque); + std::optional externalTorque = std::nullopt); // Constraint virtual void update(const std::vector & mbs,