Skip to content

Commit

Permalink
Merge pull request #76 from ndehio/master
Browse files Browse the repository at this point in the history 
introduce joint jerk bounds
  • Loading branch information
@gergondet
gergondet authored Jul 22, 2021
2 parents b713903 + a6892a9 commit d386707
Show file tree
Hide file tree
Showing 4 changed files with 130 additions and 11 deletions.
71 changes: 63 additions & 8 deletions src/QPConstr.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace qp
*/

JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs, int robotIndex, QBound bound, double step)
: JointLimitsConstr(mbs, robotIndex, bound, {}, step)
: JointLimitsConstr(mbs, robotIndex, bound, {}, {}, step)
{
}

Expand All @@ -42,9 +42,20 @@ JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
QBound bound,
const AlphaDBound & aDBound,
double step)
: JointLimitsConstr(mbs, robotIndex, bound, aDBound, {}, step)
{
}

JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
QBound bound,
const AlphaDBound & aDBound,
const AlphaDDBound & aDDBound,
double step)
: robotIndex_(robotIndex), alphaDBegin_(-1),
alphaDOffset_(mbs[robotIndex].joint(0).dof() > 1 ? mbs[robotIndex].joint(0).dof() : 0), step_(step), qMin_(), qMax_(),
qVec_(), alphaVec_(), lower_(), upper_(), alphaDLower_(), alphaDUpper_()
qVec_(), alphaVec_(), lower_(), upper_(), alphaDLower_(), alphaDUpper_(), alphaDDLower_(), alphaDDUpper_(),
prevAlphaD_()
{
assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0);

Expand All @@ -61,6 +72,12 @@ JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
alphaDUpper_.resize(mb.nrDof());
alphaDLower_.setConstant(-std::numeric_limits<double>::infinity());
alphaDUpper_.setConstant(std::numeric_limits<double>::infinity());
alphaDDLower_.resize(mb.nrDof());
alphaDDUpper_.resize(mb.nrDof());
alphaDDLower_.setConstant(-std::numeric_limits<double>::infinity());
alphaDDUpper_.setConstant(std::numeric_limits<double>::infinity());
prevAlphaD_.resize(mb.nrDof());
prevAlphaD_.setZero();

// if first joint is not managed remove it
if(alphaDOffset_ != 0)
Expand All @@ -77,6 +94,11 @@ JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,

rbd::paramToVector(aDBound.lAlphaDBound, alphaDLower_);
rbd::paramToVector(aDBound.uAlphaDBound, alphaDUpper_);

rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_);
rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_);
alphaDDLower_ *= step_;
alphaDDUpper_ *= step_;
}

void JointLimitsConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mbs */, const SolverData & data)
Expand All @@ -96,15 +118,16 @@ void JointLimitsConstr::update(const std::vector<rbd::MultiBody> & /* mbs */,

rbd::paramToVector(mbc.q, qVec_);
rbd::paramToVector(mbc.alpha, alphaVec_);
rbd::paramToVector(mbc.alphaD, prevAlphaD_);

lower_.noalias() = qMin_ - qVec_.tail(vars) - alphaVec_.tail(vars) * step_;
lower_ /= dts;

upper_.noalias() = qMax_ - qVec_.tail(vars) - alphaVec_.tail(vars) * step_;
upper_ /= dts;

lower_ = lower_.cwiseMax(alphaDLower_);
upper_ = upper_.cwiseMin(alphaDUpper_);
lower_ = lower_.cwiseMax(alphaDLower_).cwiseMax(alphaDDLower_ + prevAlphaD_);
upper_ = upper_.cwiseMin(alphaDUpper_).cwiseMin(alphaDDUpper_ + prevAlphaD_);
}

std::string JointLimitsConstr::nameBound() const
Expand Down Expand Up @@ -145,7 +168,29 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector<rbd::MultiBod
double securityPercent,
double damperOffset,
double step)
: DamperJointLimitsConstr(mbs, robotIndex, qBound, aBound, {}, interPercent, securityPercent, damperOffset, step)
: DamperJointLimitsConstr(mbs, robotIndex, qBound, aBound, {}, {}, interPercent, securityPercent, damperOffset, step)
{
}

DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const QBound & qBound,
const AlphaBound & aBound,
const AlphaDBound & aDBound,
double interPercent,
double securityPercent,
double damperOffset,
double step)
: DamperJointLimitsConstr(mbs,
robotIndex,
qBound,
aBound,
aDBound,
{},
interPercent,
securityPercent,
damperOffset,
step)
{
}

Expand All @@ -154,12 +199,14 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector<rbd::MultiBod
const QBound & qBound,
const AlphaBound & aBound,
const AlphaDBound & aDBound,
const AlphaDDBound & aDDBound,
double interPercent,
double securityPercent,
double damperOffset,
double step)
: robotIndex_(robotIndex), alphaDBegin_(-1), data_(), lower_(mbs[robotIndex].nrDof()), upper_(mbs[robotIndex].nrDof()),
alphaDLower_(mbs[robotIndex].nrDof()), alphaDUpper_(mbs[robotIndex].nrDof()), step_(step), damperOff_(damperOffset)
alphaDLower_(mbs[robotIndex].nrDof()), alphaDUpper_(mbs[robotIndex].nrDof()), alphaDDLower_(mbs[robotIndex].nrDof()),
alphaDDUpper_(mbs[robotIndex].nrDof()), prevAlphaD_(mbs[robotIndex].nrDof()), step_(step), damperOff_(damperOffset)
{
assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0);

Expand All @@ -179,8 +226,15 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector<rbd::MultiBod
rbd::paramToVector(aBound.uAlphaBound, upper_);
alphaDLower_.setConstant(-std::numeric_limits<double>::infinity());
alphaDUpper_.setConstant(std::numeric_limits<double>::infinity());
alphaDDLower_.setConstant(-std::numeric_limits<double>::infinity());
alphaDDUpper_.setConstant(std::numeric_limits<double>::infinity());
rbd::paramToVector(aDBound.lAlphaDBound, alphaDLower_);
rbd::paramToVector(aDBound.uAlphaDBound, alphaDUpper_);
rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_);
rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_);
alphaDDLower_ *= step_;
alphaDDUpper_ *= step_;
prevAlphaD_.setZero();
}

void DamperJointLimitsConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mbs */, const SolverData & data)
Expand All @@ -193,6 +247,7 @@ void DamperJointLimitsConstr::update(const std::vector<rbd::MultiBody> & /* mbs
const SolverData & /* data */)
{
const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_];
rbd::paramToVector(mbc.alphaD, prevAlphaD_);

for(DampData & d : data_)
{
Expand Down Expand Up @@ -236,8 +291,8 @@ void DamperJointLimitsConstr::update(const std::vector<rbd::MultiBody> & /* mbs
d.state = DampData::Free;
}
}
lower_ = lower_.cwiseMax(alphaDLower_);
upper_ = upper_.cwiseMin(alphaDUpper_);
lower_ = lower_.cwiseMax(alphaDLower_).cwiseMax(alphaDDLower_ + prevAlphaD_);
upper_ = upper_.cwiseMin(alphaDUpper_).cwiseMin(alphaDDUpper_ + prevAlphaD_);
}

std::string DamperJointLimitsConstr::nameBound() const
Expand Down
18 changes: 18 additions & 0 deletions src/Tasks/Bounds.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,24 @@ struct AlphaDBound
std::vector<std::vector<double>> uAlphaDBound;
};

/**
* General jerk vector bounds
* \f$ \underline{\ddot{\alpha}} \f$ and \f$ \overline{\ddot{\alpha}} \f$.
*/
struct AlphaDDBound
{
AlphaDDBound() {}
AlphaDDBound(std::vector<std::vector<double>> lADDB, std::vector<std::vector<double>> uADDB)
: lAlphaDDBound(std::move(lADDB)), uAlphaDDBound(std::move(uADDB))
{
}

/// \f$ \underline{\ddot{\alpha}} \f$
std::vector<std::vector<double>> lAlphaDDBound;
/// \f$ \overline{\ddot{\alpha}} \f$
std::vector<std::vector<double>> uAlphaDDBound;
};

/**
* General force vector bounds
* \f$ \underline{\tau} \f$ and \f$ \overline{\tau} \f$.
Expand Down
42 changes: 42 additions & 0 deletions src/Tasks/QPConstr.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ namespace tasks
struct QBound;
struct AlphaBound;
struct AlphaDBound;
struct AlphaDDBound;

namespace qp
{
Expand Down Expand Up @@ -76,6 +77,21 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction<Bound>
const AlphaDBound & aDBound,
double step);

/**
* @param mbs Multi-robot system.
* @param robotIndex Constrained robot Index in mbs.
* @param bound Articular position bounds.
* @param aDBound Articular acceleration bounds.
* @param aDDBound Articular jerk bounds.
* @param step Time step in second.
*/
JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
QBound bound,
const AlphaDBound & aDBound,
const AlphaDDBound & aDDBound,
double step);

// Constraint
virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;

Expand All @@ -99,6 +115,8 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction<Bound>
Eigen::VectorXd qVec_, alphaVec_;
Eigen::VectorXd lower_, upper_;
Eigen::VectorXd alphaDLower_, alphaDUpper_;
Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
Eigen::VectorXd prevAlphaD_;
};

/**
Expand Down Expand Up @@ -151,13 +169,34 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>
double securityPercent,
double damperOffset,
double step);
/**
* @param mbs Multi-robot system.
* @param robotIndex Constrained robot Index in mbs.
* @param qBound Articular position bounds.
* @param aBound Articular velocity bounds.
* @param aDBound Articular acceleration bounds.
* @param interPercent \f$ interPercent (\overline{q} - \underline{q}) \f$
* @param securityPercent \f$ securityPercent (\overline{q} - \underline{q}) \f$
* @param damperOffset \f$ \xi_{\text{off}} \f$
* @param step Time step in second.
*/
DamperJointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
int robotIndex,
const QBound & qBound,
const AlphaBound & aBound,
const AlphaDBound & aDBound,
double interPercent,
double securityPercent,
double damperOffset,
double step);

/**
* @param mbs Multi-robot system.
* @param robotIndex Constrained robot Index in mbs.
* @param qBound Articular position bounds.
* @param aBound Articular velocity bounds.
* @param aDBound Articular acceleration bounds.
* @param aDDBound Articular jerk bounds.
* @param interPercent \f$ interPercent (\overline{q} - \underline{q}) \f$
* @param securityPercent \f$ securityPercent (\overline{q} - \underline{q}) \f$
* @param damperOffset \f$ \xi_{\text{off}} \f$
Expand All @@ -168,6 +207,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>
const QBound & qBound,
const AlphaBound & aBound,
const AlphaDBound & aDBound,
const AlphaDDBound & aDDBound,
double interPercent,
double securityPercent,
double damperOffset,
Expand Down Expand Up @@ -224,6 +264,8 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction<Bound>

Eigen::VectorXd lower_, upper_;
Eigen::VectorXd alphaDLower_, alphaDUpper_;
Eigen::VectorXd alphaDDLower_, alphaDDUpper_;
Eigen::VectorXd prevAlphaD_;
double step_;
double damperOff_;
};
Expand Down
10 changes: 7 additions & 3 deletions tests/QPSolverTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -464,8 +464,10 @@ BOOST_AUTO_TEST_CASE(QPJointLimitsTest)
std::vector<std::vector<double>> uBound = {{}, {cst::pi<double>() / 4.}, {inf}, {inf}};
std::vector<std::vector<double>> lDDBound = {{}, {-inf}, {-inf}, {-inf}};
std::vector<std::vector<double>> uDDBound = {{}, {inf}, {inf}, {inf}};
std::vector<std::vector<double>> lDDDBound = {{}, {-inf}, {-inf}, {-inf}};
std::vector<std::vector<double>> uDDDBound = {{}, {inf}, {inf}, {inf}};

qp::JointLimitsConstr jointConstr(mbs, 0, {lBound, uBound}, {lDDBound, uDDBound}, 0.001);
qp::JointLimitsConstr jointConstr(mbs, 0, {lBound, uBound}, {lDDBound, uDDBound}, {lDDDBound, uDDDBound}, 0.001);

// Test add*Constraint
solver.addBoundConstraint(&jointConstr);
Expand Down Expand Up @@ -545,9 +547,11 @@ BOOST_AUTO_TEST_CASE(QPDamperJointLimitsTest)
std::vector<std::vector<double>> uVel = {{}, {inf}, {inf}, {inf}};
std::vector<std::vector<double>> lAcc = {{}, {-inf}, {-inf}, {-inf}};
std::vector<std::vector<double>> uAcc = {{}, {inf}, {inf}, {inf}};
std::vector<std::vector<double>> lJer = {{}, {-inf}, {-inf}, {-inf}};
std::vector<std::vector<double>> uJer = {{}, {inf}, {inf}, {inf}};

qp::DamperJointLimitsConstr dampJointConstr(mbs, 0, {lBound, uBound}, {lVel, uVel}, {lAcc, uAcc}, 0.125, 0.025, 1.,
0.001);
qp::DamperJointLimitsConstr dampJointConstr(mbs, 0, {lBound, uBound}, {lVel, uVel}, {lAcc, uAcc}, {lJer, uJer}, 0.125,
0.025, 1., 0.001);

// Test add*Constraint
dampJointConstr.addToSolver(solver);
Expand Down

0 comments on commit d386707

Please sign in to comment.