Skip to content
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

introduce joint jerk bounds #76

Merged
merged 3 commits into from
Jul 22, 2021
Merged

introduce joint jerk bounds #76

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5a3435e
introduce joint jerk bounds
ndehio Jul 19, 2021
c9c79d3
consider current joint accelerations for the jerk limits
ndehio Jul 21, 2021
a6892a9
rename variable curAlphaD_ as prevAlphaD_ and some other fixes
ndehio Jul 21, 2021
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
59 changes: 55 additions & 4 deletions src/QPConstr.cpp
View file
Open in desktop
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_(),
curAlphaD_()
{
assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0);

Expand All @@ -61,6 +72,11 @@ 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());
curAlphaD_.resize(mb.nrDof());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

curAlphaD_.setZero();


// if first joint is not managed remove it
if(alphaDOffset_ != 0)
Expand All @@ -77,6 +93,9 @@ 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_);
gergondet marked this conversation as resolved.
Show resolved Hide resolved
}

void JointLimitsConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mbs */, const SolverData & data)
Expand Down Expand Up @@ -105,6 +124,8 @@ void JointLimitsConstr::update(const std::vector<rbd::MultiBody> & /* mbs */,

lower_ = lower_.cwiseMax(alphaDLower_);
upper_ = upper_.cwiseMin(alphaDUpper_);
lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_);
upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

curAlphaD_ needs to be updated (and I would rename it prevAlphaD_ I think this is clearer)

}

std::string JointLimitsConstr::nameBound() const
Expand Down Expand Up @@ -145,7 +166,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 +197,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()), curAlphaD_(mbs[robotIndex].nrDof()), step_(step), damperOff_(damperOffset)
{
assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0);

Expand All @@ -179,8 +224,12 @@ 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());
gergondet marked this conversation as resolved.
Show resolved Hide resolved
rbd::paramToVector(aDBound.lAlphaDBound, alphaDLower_);
rbd::paramToVector(aDBound.uAlphaDBound, alphaDUpper_);
rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_);
rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_);
gergondet marked this conversation as resolved.
Show resolved Hide resolved
}

void DamperJointLimitsConstr::updateNrVars(const std::vector<rbd::MultiBody> & /* mbs */, const SolverData & data)
Expand Down Expand Up @@ -238,6 +287,8 @@ void DamperJointLimitsConstr::update(const std::vector<rbd::MultiBody> & /* mbs
}
lower_ = lower_.cwiseMax(alphaDLower_);
upper_ = upper_.cwiseMin(alphaDUpper_);
lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_);
upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same remark regarding curAlphaD_ update and renaming to prevAlphaD_

}

std::string DamperJointLimitsConstr::nameBound() const
Expand Down
18 changes: 18 additions & 0 deletions src/Tasks/Bounds.h
View file
Open in desktop
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
Open in desktop
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 curAlphaD_;
};

/**
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 curAlphaD_;
double step_;
double damperOff_;
};
Expand Down
10 changes: 7 additions & 3 deletions tests/QPSolverTest.cpp
View file
Open in desktop
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