From 5a3435ea69d4da2210b1cae8028c94cc661f00cc Mon Sep 17 00:00:00 2001 From: Niels Dehio Date: Mon, 19 Jul 2021 12:44:25 +0200 Subject: [PATCH 1/3] introduce joint jerk bounds --- src/QPConstr.cpp | 57 +++++++++++++++++++++++++++++++++++++++--- src/Tasks/Bounds.h | 18 +++++++++++++ src/Tasks/QPConstr.h | 40 +++++++++++++++++++++++++++++ tests/QPSolverTest.cpp | 10 +++++--- 4 files changed, 118 insertions(+), 7 deletions(-) diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index bf36863a4..1f5c24028 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -33,7 +33,7 @@ namespace qp */ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, int robotIndex, QBound bound, double step) -: JointLimitsConstr(mbs, robotIndex, bound, {}, step) +: JointLimitsConstr(mbs, robotIndex, bound, {}, {}, step) { } @@ -42,9 +42,19 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, QBound bound, const AlphaDBound & aDBound, double step) +: JointLimitsConstr(mbs, robotIndex, bound, aDBound, {}, step) +{ +} + +JointLimitsConstr::JointLimitsConstr(const std::vector & 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_() { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); @@ -61,6 +71,10 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, alphaDUpper_.resize(mb.nrDof()); alphaDLower_.setConstant(-std::numeric_limits::infinity()); alphaDUpper_.setConstant(std::numeric_limits::infinity()); + alphaDDLower_.resize(mb.nrDof()); + alphaDDUpper_.resize(mb.nrDof()); + alphaDDLower_.setConstant(-std::numeric_limits::infinity()); + alphaDDUpper_.setConstant(std::numeric_limits::infinity()); // if first joint is not managed remove it if(alphaDOffset_ != 0) @@ -77,6 +91,9 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, rbd::paramToVector(aDBound.lAlphaDBound, alphaDLower_); rbd::paramToVector(aDBound.uAlphaDBound, alphaDUpper_); + + rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_); + rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_); } void JointLimitsConstr::updateNrVars(const std::vector & /* mbs */, const SolverData & data) @@ -105,6 +122,8 @@ void JointLimitsConstr::update(const std::vector & /* mbs */, lower_ = lower_.cwiseMax(alphaDLower_); upper_ = upper_.cwiseMin(alphaDUpper_); + lower_ = lower_.cwiseMax((alphaDDLower_ * step_)); + upper_ = upper_.cwiseMin((alphaDDUpper_ * step_)); } std::string JointLimitsConstr::nameBound() const @@ -145,7 +164,29 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector & 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) { } @@ -154,12 +195,14 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector= 0); @@ -179,8 +222,12 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector::infinity()); alphaDUpper_.setConstant(std::numeric_limits::infinity()); + alphaDDLower_.setConstant(-std::numeric_limits::infinity()); + alphaDDUpper_.setConstant(std::numeric_limits::infinity()); rbd::paramToVector(aDBound.lAlphaDBound, alphaDLower_); rbd::paramToVector(aDBound.uAlphaDBound, alphaDUpper_); + rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_); + rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_); } void DamperJointLimitsConstr::updateNrVars(const std::vector & /* mbs */, const SolverData & data) @@ -238,6 +285,8 @@ void DamperJointLimitsConstr::update(const std::vector & /* mbs } lower_ = lower_.cwiseMax(alphaDLower_); upper_ = upper_.cwiseMin(alphaDUpper_); + lower_ = lower_.cwiseMax((alphaDDLower_ * step_)); + upper_ = upper_.cwiseMin((alphaDDUpper_ * step_)); } std::string DamperJointLimitsConstr::nameBound() const diff --git a/src/Tasks/Bounds.h b/src/Tasks/Bounds.h index 78e777b2d..23af392c4 100644 --- a/src/Tasks/Bounds.h +++ b/src/Tasks/Bounds.h @@ -69,6 +69,24 @@ struct AlphaDBound std::vector> uAlphaDBound; }; +/** + * General jerk vector bounds + * \f$ \underline{\ddot{\alpha}} \f$ and \f$ \overline{\ddot{\alpha}} \f$. + */ +struct AlphaDDBound +{ + AlphaDDBound() {} + AlphaDDBound(std::vector> lADDB, std::vector> uADDB) + : lAlphaDDBound(std::move(lADDB)), uAlphaDDBound(std::move(uADDB)) + { + } + + /// \f$ \underline{\ddot{\alpha}} \f$ + std::vector> lAlphaDDBound; + /// \f$ \overline{\ddot{\alpha}} \f$ + std::vector> uAlphaDDBound; +}; + /** * General force vector bounds * \f$ \underline{\tau} \f$ and \f$ \overline{\tau} \f$. diff --git a/src/Tasks/QPConstr.h b/src/Tasks/QPConstr.h index f462beb52..05a8fae0b 100644 --- a/src/Tasks/QPConstr.h +++ b/src/Tasks/QPConstr.h @@ -35,6 +35,7 @@ namespace tasks struct QBound; struct AlphaBound; struct AlphaDBound; +struct AlphaDDBound; namespace qp { @@ -76,6 +77,21 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction 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 & mbs, + int robotIndex, + QBound bound, + const AlphaDBound & aDBound, + const AlphaDDBound & aDDBound, + double step); + // Constraint virtual void updateNrVars(const std::vector & mbs, const SolverData & data) override; @@ -99,6 +115,7 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction Eigen::VectorXd qVec_, alphaVec_; Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; + Eigen::VectorXd alphaDDLower_, alphaDDUpper_; }; /** @@ -151,6 +168,26 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction 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 & 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. @@ -158,6 +195,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction * @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$ @@ -168,6 +206,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction const QBound & qBound, const AlphaBound & aBound, const AlphaDBound & aDBound, + const AlphaDDBound & aDDBound, double interPercent, double securityPercent, double damperOffset, @@ -224,6 +263,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; + Eigen::VectorXd alphaDDLower_, alphaDDUpper_; double step_; double damperOff_; }; diff --git a/tests/QPSolverTest.cpp b/tests/QPSolverTest.cpp index 3a025ad98..c481e81be 100644 --- a/tests/QPSolverTest.cpp +++ b/tests/QPSolverTest.cpp @@ -464,8 +464,10 @@ BOOST_AUTO_TEST_CASE(QPJointLimitsTest) std::vector> uBound = {{}, {cst::pi() / 4.}, {inf}, {inf}}; std::vector> lDDBound = {{}, {-inf}, {-inf}, {-inf}}; std::vector> uDDBound = {{}, {inf}, {inf}, {inf}}; + std::vector> lDDDBound = {{}, {-inf}, {-inf}, {-inf}}; + std::vector> 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); @@ -545,9 +547,11 @@ BOOST_AUTO_TEST_CASE(QPDamperJointLimitsTest) std::vector> uVel = {{}, {inf}, {inf}, {inf}}; std::vector> lAcc = {{}, {-inf}, {-inf}, {-inf}}; std::vector> uAcc = {{}, {inf}, {inf}, {inf}}; + std::vector> lJer = {{}, {-inf}, {-inf}, {-inf}}; + std::vector> 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); From c9c79d3ae1792cf8285c7f210e26f3bc149784b8 Mon Sep 17 00:00:00 2001 From: Niels Dehio Date: Wed, 21 Jul 2021 11:10:08 +0200 Subject: [PATCH 2/3] consider current joint accelerations for the jerk limits --- src/QPConstr.cpp | 14 ++++++++------ src/Tasks/QPConstr.h | 2 ++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index 1f5c24028..23496642c 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -54,7 +54,8 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, 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_(), alphaDDLower_(), alphaDDUpper_() + qVec_(), alphaVec_(), lower_(), upper_(), alphaDLower_(), alphaDUpper_(), alphaDDLower_(), alphaDDUpper_(), + curAlphaD_() { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); @@ -75,6 +76,7 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, alphaDDUpper_.resize(mb.nrDof()); alphaDDLower_.setConstant(-std::numeric_limits::infinity()); alphaDDUpper_.setConstant(std::numeric_limits::infinity()); + curAlphaD_.resize(mb.nrDof()); // if first joint is not managed remove it if(alphaDOffset_ != 0) @@ -122,8 +124,8 @@ void JointLimitsConstr::update(const std::vector & /* mbs */, lower_ = lower_.cwiseMax(alphaDLower_); upper_ = upper_.cwiseMin(alphaDUpper_); - lower_ = lower_.cwiseMax((alphaDDLower_ * step_)); - upper_ = upper_.cwiseMin((alphaDDUpper_ * step_)); + lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_); + upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_); } std::string JointLimitsConstr::nameBound() const @@ -202,7 +204,7 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector= 0); @@ -285,8 +287,8 @@ void DamperJointLimitsConstr::update(const std::vector & /* mbs } lower_ = lower_.cwiseMax(alphaDLower_); upper_ = upper_.cwiseMin(alphaDUpper_); - lower_ = lower_.cwiseMax((alphaDDLower_ * step_)); - upper_ = upper_.cwiseMin((alphaDDUpper_ * step_)); + lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_); + upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_); } std::string DamperJointLimitsConstr::nameBound() const diff --git a/src/Tasks/QPConstr.h b/src/Tasks/QPConstr.h index 05a8fae0b..f3e44587c 100644 --- a/src/Tasks/QPConstr.h +++ b/src/Tasks/QPConstr.h @@ -116,6 +116,7 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; Eigen::VectorXd alphaDDLower_, alphaDDUpper_; + Eigen::VectorXd curAlphaD_; }; /** @@ -264,6 +265,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; Eigen::VectorXd alphaDDLower_, alphaDDUpper_; + Eigen::VectorXd curAlphaD_; double step_; double damperOff_; }; From a6892a9c846600b532b191837f96f9c24b582d23 Mon Sep 17 00:00:00 2001 From: Niels Dehio Date: Wed, 21 Jul 2021 12:44:39 +0200 Subject: [PATCH 3/3] rename variable curAlphaD_ as prevAlphaD_ and some other fixes --- src/QPConstr.cpp | 26 +++++++++++++++----------- src/Tasks/QPConstr.h | 4 ++-- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/src/QPConstr.cpp b/src/QPConstr.cpp index 23496642c..7df536469 100644 --- a/src/QPConstr.cpp +++ b/src/QPConstr.cpp @@ -55,7 +55,7 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, : 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_(), alphaDDLower_(), alphaDDUpper_(), - curAlphaD_() + prevAlphaD_() { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); @@ -76,7 +76,8 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, alphaDDUpper_.resize(mb.nrDof()); alphaDDLower_.setConstant(-std::numeric_limits::infinity()); alphaDDUpper_.setConstant(std::numeric_limits::infinity()); - curAlphaD_.resize(mb.nrDof()); + prevAlphaD_.resize(mb.nrDof()); + prevAlphaD_.setZero(); // if first joint is not managed remove it if(alphaDOffset_ != 0) @@ -96,6 +97,8 @@ JointLimitsConstr::JointLimitsConstr(const std::vector & mbs, rbd::paramToVector(aDDBound.lAlphaDDBound, alphaDDLower_); rbd::paramToVector(aDDBound.uAlphaDDBound, alphaDDUpper_); + alphaDDLower_ *= step_; + alphaDDUpper_ *= step_; } void JointLimitsConstr::updateNrVars(const std::vector & /* mbs */, const SolverData & data) @@ -115,6 +118,7 @@ void JointLimitsConstr::update(const std::vector & /* 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; @@ -122,10 +126,8 @@ void JointLimitsConstr::update(const std::vector & /* mbs */, upper_.noalias() = qMax_ - qVec_.tail(vars) - alphaVec_.tail(vars) * step_; upper_ /= dts; - lower_ = lower_.cwiseMax(alphaDLower_); - upper_ = upper_.cwiseMin(alphaDUpper_); - lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_); - upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_); + lower_ = lower_.cwiseMax(alphaDLower_).cwiseMax(alphaDDLower_ + prevAlphaD_); + upper_ = upper_.cwiseMin(alphaDUpper_).cwiseMin(alphaDDUpper_ + prevAlphaD_); } std::string JointLimitsConstr::nameBound() const @@ -204,7 +206,7 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector= 0); @@ -230,6 +232,9 @@ DamperJointLimitsConstr::DamperJointLimitsConstr(const std::vector & /* mbs */, const SolverData & data) @@ -242,6 +247,7 @@ void DamperJointLimitsConstr::update(const std::vector & /* mbs const SolverData & /* data */) { const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + rbd::paramToVector(mbc.alphaD, prevAlphaD_); for(DampData & d : data_) { @@ -285,10 +291,8 @@ void DamperJointLimitsConstr::update(const std::vector & /* mbs d.state = DampData::Free; } } - lower_ = lower_.cwiseMax(alphaDLower_); - upper_ = upper_.cwiseMin(alphaDUpper_); - lower_ = lower_.cwiseMax((alphaDDLower_ * step_) + curAlphaD_); - upper_ = upper_.cwiseMin((alphaDDUpper_ * step_) + curAlphaD_); + lower_ = lower_.cwiseMax(alphaDLower_).cwiseMax(alphaDDLower_ + prevAlphaD_); + upper_ = upper_.cwiseMin(alphaDUpper_).cwiseMin(alphaDDUpper_ + prevAlphaD_); } std::string DamperJointLimitsConstr::nameBound() const diff --git a/src/Tasks/QPConstr.h b/src/Tasks/QPConstr.h index f3e44587c..7437411f3 100644 --- a/src/Tasks/QPConstr.h +++ b/src/Tasks/QPConstr.h @@ -116,7 +116,7 @@ class TASKS_DLLAPI JointLimitsConstr : public ConstraintFunction Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; Eigen::VectorXd alphaDDLower_, alphaDDUpper_; - Eigen::VectorXd curAlphaD_; + Eigen::VectorXd prevAlphaD_; }; /** @@ -265,7 +265,7 @@ class TASKS_DLLAPI DamperJointLimitsConstr : public ConstraintFunction Eigen::VectorXd lower_, upper_; Eigen::VectorXd alphaDLower_, alphaDUpper_; Eigen::VectorXd alphaDDLower_, alphaDDUpper_; - Eigen::VectorXd curAlphaD_; + Eigen::VectorXd prevAlphaD_; double step_; double damperOff_; };