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 1 commit
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
78 changes: 77 additions & 1 deletion src/QPMotionConstr.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,22 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb,

MotionConstrCommon::MotionConstrCommon(const std::vector<rbd::MultiBody> & 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
Comment thread
mathieu-celerier marked this conversation as resolved.
Outdated
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<rbd::MultiBody> & 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
Expand All @@ -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;
}

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

int MotionConstrCommon::maxGenInEq() const
Expand Down Expand Up @@ -262,6 +283,14 @@ MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & mbs, int robotInd
{
}

MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const Eigen::VectorXd & externalTorque)
: MotionConstr(mbs, robotIndex, tb, {}, 0.001, externalTorque)
{
}

MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
Expand All @@ -280,6 +309,26 @@ MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & mbs,
torqueDtU_ *= dt;
}

MotionConstr::MotionConstr(const std::vector<rbd::MultiBody> & 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<double>::infinity());
torqueDtU_.setConstant(std::numeric_limits<double>::infinity());
rbd::paramToVector(tdb.lTorqueDBound, torqueDtL_);
rbd::paramToVector(tdb.uTorqueDBound, torqueDtU_);
torqueDtL_ *= dt;
torqueDtU_ *= dt;
}

void MotionConstr::update(const std::vector<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
const SolverData & /* data */)
Expand Down Expand Up @@ -321,6 +370,15 @@ MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
{
}

MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const std::vector<SpringJoint> & springs,
const Eigen::VectorXd & externalTorque)
: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs, externalTorque)
{
}

MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
Expand All @@ -338,6 +396,24 @@ MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
}
}

MotionSpringConstr::MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const TorqueDBound & tdb,
double dt,
const std::vector<SpringJoint> & 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<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
const SolverData & /* data */)
Expand Down
29 changes: 29 additions & 0 deletions src/Tasks/QPMotionConstr.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,7 @@ 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, const Eigen::VectorXd & externalTorque);

void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda);
const Eigen::VectorXd & torque() const;
Expand Down Expand Up @@ -114,6 +115,9 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction<GenInequality>

Eigen::VectorXd curTorque_;

bool useExternalTorque_;
const Eigen::VectorXd & extTorque_;
Comment thread
mathieu-celerier marked this conversation as resolved.
Outdated

Eigen::MatrixXd A_;
Eigen::VectorXd AL_, AU_;
size_t updateIter_ = 0;
Expand All @@ -123,13 +127,24 @@ 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,
const Eigen::VectorXd & externalTorque);

MotionConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const TorqueDBound & tdb,
double dt);

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

// Constraint
virtual void update(const std::vector<rbd::MultiBody> & mbs,
const std::vector<rbd::MultiBodyConfig> & mbcs,
Expand Down Expand Up @@ -164,13 +179,27 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr
const TorqueBound & tb,
const std::vector<SpringJoint> & springs);

MotionSpringConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const TorqueBound & tb,
const std::vector<SpringJoint> & springs,
const Eigen::VectorXd & externalTorque);

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

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

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