Skip to content

Add the possibility to compensate an external disturbance torques #112

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mathieu-celerier wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Add the possibility to compensate an external disturbance torques #112

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d5659b7
Add the possibility to compensate an external disturbance torques
mathieu-celerier Jun 3, 2025
a150ade
Change external torque to std::optional, remove the reference and add…
mathieu-celerier Mar 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 34 additions & 11 deletions src/QPMotionConstr.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,9 +109,12 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb,
}
}

MotionConstrCommon::MotionConstrCommon(const std::vector<rbd::MultiBody> & mbs, int robotIndex)
MotionConstrCommon::MotionConstrCommon(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
std::optional<Eigen::VectorXd> externalTorque)
: 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_), 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
Expand All @@ -122,6 +125,7 @@ void MotionConstrCommon::computeTorque(const Eigen::VectorXd & alphaD, const Eig
{
curTorque_ = fd_.H() * alphaD.segment(alphaDBegin_, nrDof_);
curTorque_ += fd_.C();
if(extTorque_) { curTorque_ -= extTorque_.value(); }
curTorque_ += A_.block(0, lambdaBegin_, nrDof_, A_.cols() - lambdaBegin_) * lambda;
}

Expand Down Expand Up @@ -220,6 +224,11 @@ void MotionConstrCommon::computeMatrix(const std::vector<rbd::MultiBody> & mbs,
// BEq = -C
AL_ = -fd_.C();
AU_ = -fd_.C();
if(extTorque_)
{
AL_ += extTorque_.value();
AU_ += extTorque_.value();
}
}

int MotionConstrCommon::maxGenInEq() const
Expand Down Expand Up @@ -253,22 +262,34 @@ std::string MotionConstrCommon::descGenInEq(const std::vector<rbd::MultiBody> &
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<rbd::MultiBody> & mbs, int robotIndex, const TorqueBound & tb)
: MotionConstr(mbs, robotIndex, tb, {}, 0.001)
MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
std::optional<Eigen::VectorXd> externalTorque)
: MotionConstr(mbs, robotIndex, tb, {}, 0.001, externalTorque)
{
}

MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & 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_)
double dt,
std::optional<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_);
Expand Down Expand Up @@ -316,8 +337,9 @@ const rbd::ForwardDynamics MotionConstr::fd() const
MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const std::vector<SpringJoint> & springs)
: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs)
const std::vector<SpringJoint> & springs,
std::optional<Eigen::VectorXd> externalTorque)
: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs, externalTorque)
{
}

Expand All @@ -326,8 +348,9 @@ MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
const TorqueBound & tb,
const TorqueDBound & tdb,
double dt,
const std::vector<SpringJoint> & springs)
: MotionConstr(mbs, robotIndex, tb, tdb, dt), springs_()
const std::vector<SpringJoint> & springs,
std::optional<Eigen::VectorXd> externalTorque)
: MotionConstr(mbs, robotIndex, tb, tdb, dt, externalTorque), springs_()
{
const rbd::MultiBody & mb = mbs[robotIndex_];
for(const SpringJoint & sj : springs)
Expand Down
24 changes: 19 additions & 5 deletions src/Tasks/QPMotionConstr.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

// Tasks
#include "QPSolver.h"
#include <optional>

namespace tasks
{
Expand Down Expand Up @@ -65,7 +66,9 @@ class TASKS_DLLAPI PositiveLambda : public ConstraintFunction<Bound>
class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction<GenInequality>
{
public:
MotionConstrCommon(const std::vector<rbd::MultiBody> & mbs, int robotIndex);
MotionConstrCommon(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
std::optional<Eigen::VectorXd> externalTorque = std::nullopt);

void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda);
const Eigen::VectorXd & torque() const;
Expand All @@ -87,6 +90,9 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction<GenInequality>
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
{
Expand Down Expand Up @@ -114,6 +120,8 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction<GenInequality>

Eigen::VectorXd curTorque_;

std::optional<Eigen::VectorXd> extTorque_;

Eigen::MatrixXd A_;
Eigen::VectorXd AL_, AU_;
size_t updateIter_ = 0;
Expand All @@ -122,13 +130,17 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction<GenInequality>
class TASKS_DLLAPI MotionConstr : public MotionConstrCommon
{
public:
MotionConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, const TorqueBound & tb);
MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
std::optional<Eigen::VectorXd> externalTorque = std::nullopt);

MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const TorqueDBound & tdb,
double dt);
double dt,
std::optional<Eigen::VectorXd> externalTorque = std::nullopt);

// Constraint
virtual void update(const std::vector<rbd::MultiBody> & mbs,
Expand Down Expand Up @@ -162,14 +174,16 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr
MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const std::vector<SpringJoint> & springs);
const std::vector<SpringJoint> & springs,
std::optional<Eigen::VectorXd> externalTorque = std::nullopt);

MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const TorqueDBound & tdb,
double dt,
const std::vector<SpringJoint> & springs);
const std::vector<SpringJoint> & springs,
std::optional<Eigen::VectorXd> externalTorque = std::nullopt);

// Constraint
virtual void update(const std::vector<rbd::MultiBody> & mbs,
Expand Down