diff --git a/src/QPMotionConstr.cpp b/src/QPMotionConstr.cpp index da7d1b37..b849e014 100644 --- a/src/QPMotionConstr.cpp +++ b/src/QPMotionConstr.cpp @@ -109,9 +109,12 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb, } } -MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, int robotIndex) +MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, + int robotIndex, + std::optional 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 @@ -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; } @@ -220,6 +224,11 @@ void MotionConstrCommon::computeMatrix(const std::vector & mbs, // BEq = -C AL_ = -fd_.C(); AU_ = -fd_.C(); + if(extTorque_) + { + AL_ += extTorque_.value(); + AU_ += extTorque_.value(); + } } int MotionConstrCommon::maxGenInEq() const @@ -253,12 +262,22 @@ 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, + std::optional externalTorque) +: MotionConstr(mbs, robotIndex, tb, {}, 0.001, externalTorque) { } @@ -266,9 +285,11 @@ 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_) + double dt, + 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_) { rbd::paramToVector(tb.lTorqueBound, torqueL_); rbd::paramToVector(tb.uTorqueBound, torqueU_); @@ -316,8 +337,9 @@ const rbd::ForwardDynamics MotionConstr::fd() const MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, - const std::vector & springs) -: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs) + const std::vector & springs, + std::optional externalTorque) +: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs, externalTorque) { } @@ -326,8 +348,9 @@ MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, const TorqueBound & tb, const TorqueDBound & tdb, double dt, - const std::vector & springs) -: MotionConstr(mbs, robotIndex, tb, tdb, dt), springs_() + const std::vector & springs, + std::optional externalTorque) +: MotionConstr(mbs, robotIndex, tb, tdb, dt, externalTorque), springs_() { const rbd::MultiBody & mb = mbs[robotIndex_]; for(const SpringJoint & sj : springs) diff --git a/src/Tasks/QPMotionConstr.h b/src/Tasks/QPMotionConstr.h index e585eb52..80e795c8 100644 --- a/src/Tasks/QPMotionConstr.h +++ b/src/Tasks/QPMotionConstr.h @@ -17,6 +17,7 @@ // Tasks #include "QPSolver.h" +#include namespace tasks { @@ -65,7 +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, + std::optional externalTorque = std::nullopt); void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda); const Eigen::VectorXd & torque() const; @@ -87,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 { @@ -114,6 +120,8 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction Eigen::VectorXd curTorque_; + std::optional extTorque_; + Eigen::MatrixXd A_; Eigen::VectorXd AL_, AU_; size_t updateIter_ = 0; @@ -122,13 +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, + std::optional externalTorque = std::nullopt); MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const TorqueDBound & tdb, - double dt); + double dt, + std::optional externalTorque = std::nullopt); // Constraint virtual void update(const std::vector & mbs, @@ -162,14 +174,16 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, - const std::vector & springs); + const std::vector & springs, + std::optional externalTorque = std::nullopt); MotionSpringConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb, const TorqueDBound & tdb, double dt, - const std::vector & springs); + const std::vector & springs, + std::optional externalTorque = std::nullopt); // Constraint virtual void update(const std::vector & mbs,