introduce joint jerk bounds #76

ndehio · 2021-07-19T10:49:08Z

Analogously to the existing joint acceleration bounds, this PR introduces joint jerk bounds.
For some robots these limits are defined by the manufacturer, see for example the panda manipulator.

gergondet · 2021-07-21T02:29:48Z

I don't think this implementation is correct. This is bounding the acceleration to be lower than (jerk bound)*dt but that's not the same as bounding the jerk. This imposes more restrictive acceleration bounds without preventing jerk limits violations.

For example with the Panda jerk bounds it limits the acceleration to half of the actual acceleration bounds but if the acceleration goes from the minimum bound (-jerk_max * dt) to the minimum bound (jerk_max * dt) in one iteration this would generate a 2*jerk_max jerk (it's unlikely to happen but that would generate the maximum jerk allowed by the constraint).

ndehio · 2021-07-21T09:20:19Z

Oh right, similar to the torque-derivative constraint that considers the current torques, we here need to involve the current joint accelerations for the jerk constraint.
I added a member for that purpose, but it is not clear to me how to update it. Can we do it like this?

rbd::paramToVector(mbc.alphaD, curAlphaD_); //line 118 of QPConstr.cpp

and

curAlphaD_[d.alphaDBegin] = mbc.alphaD[d.jointIndex][0]; //line 252 of QPConstr.cpp

gergondet

My main complaint would be to rename curAlphaD_ to prevAlphaD_ in both case and make sure to update it

gergondet · 2021-07-21T09:38:00Z

src/QPConstr.cpp

@@ -75,6 +76,7 @@ JointLimitsConstr::JointLimitsConstr(const std::vector<rbd::MultiBody> & mbs,
  alphaDDUpper_.resize(mb.nrDof());
  alphaDDLower_.setConstant(-std::numeric_limits<double>::infinity());
  alphaDDUpper_.setConstant(std::numeric_limits<double>::infinity());
+  curAlphaD_.resize(mb.nrDof());


curAlphaD_.setZero();

src/QPConstr.cpp

gergondet · 2021-07-21T09:42:16Z

src/QPConstr.cpp

@@ -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_);


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

src/QPConstr.cpp

gergondet · 2021-07-21T09:43:14Z

src/QPConstr.cpp

@@ -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_);


Same remark regarding curAlphaD_ update and renaming to prevAlphaD_

gergondet · 2021-07-21T09:48:33Z

I added a member for that purpose, but it is not clear to me how to update it. Can we do it like this?
rbd::paramToVector(mbc.alphaD, curAlphaD_); //line 118 of QPConstr.cpp

Yes. This works to update the full curAlphaD_ (or rather prevAlphaD_)

and

curAlphaD_[d.alphaDBegin] = mbc.alphaD[d.jointIndex][0]; //line 252 of QPConstr.cpp

No. This would be wrong. You only need the first line to update prevAlphaD_ after the update in both JointLimitsConstr and DamperJointLimitsConstr

ndehio · 2021-07-21T10:45:55Z

Thanks @gergondet for your comments, I applied them all.

- Add joint jerk bounds support (#76) - Add tasks::qp::PostureTask::dimWeight (#77)

introduce joint jerk bounds

5a3435e

consider current joint accelerations for the jerk limits

c9c79d3

gergondet approved these changes Jul 21, 2021

View reviewed changes

rename variable curAlphaD_ as prevAlphaD_ and some other fixes

a6892a9

gergondet approved these changes Jul 22, 2021

View reviewed changes

gergondet merged commit d386707 into jrl-umi3218:master Jul 22, 2021

gergondet added a commit that referenced this pull request Jul 30, 2021

Release v1.5.0

cb38fb5

- Add joint jerk bounds support (#76) - Add tasks::qp::PostureTask::dimWeight (#77)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

introduce joint jerk bounds #76

introduce joint jerk bounds #76

ndehio commented Jul 19, 2021

gergondet commented Jul 21, 2021

ndehio commented Jul 21, 2021

gergondet left a comment

gergondet Jul 21, 2021

gergondet Jul 21, 2021

gergondet Jul 21, 2021

gergondet commented Jul 21, 2021

ndehio commented Jul 21, 2021

introduce joint jerk bounds #76

introduce joint jerk bounds #76

Conversation

ndehio commented Jul 19, 2021

gergondet commented Jul 21, 2021

ndehio commented Jul 21, 2021

gergondet left a comment

Choose a reason for hiding this comment

gergondet Jul 21, 2021

Choose a reason for hiding this comment

gergondet Jul 21, 2021

Choose a reason for hiding this comment

gergondet Jul 21, 2021

Choose a reason for hiding this comment

gergondet commented Jul 21, 2021

ndehio commented Jul 21, 2021