search

jrl-umi3218 / Tasks

The Tasks library is designed to make real-time control for kinematics tree and list of kinematics tree.
BSD 2-Clause "Simplified" License
90 stars 29 forks source link

Missing selection matrix in BoundedSpeedConstr #58

Open anastasiabolotnikova opened 4 years ago

anastasiabolotnikova commented 4 years ago

There seems to be a missing multiplication by a selection matrix in the BoundedSpeedConstr::update.

Starting from the constraint documentation in QPConstr.h we have the following missing_selectin_matrix

Multiplication by a selection matrix indicated in red above seems to be missing in the implementation QPConstr.cpp

lower_.segment(index, rows).noalias() = cont_[i].dof * (-normalAcc - (speed / timeStep_));
upper_.segment(index, rows).noalias() = lower_.segment(index, rows);

lower_.segment(index, rows).noalias() += (cont_[i].lSpeed / timeStep_);
upper_.segment(index, rows).noalias() += (cont_[i].uSpeed / timeStep_);

This should give undesired results in case when selection matrix cont_[i].dof zeros some DoFs for which the lower and/or upper speed limits are specified as non-zero. I suppose, this usually won't happen, but to have correct constraint implementation and to make it foolproof against such situation, it should be the following:

lower_.segment(index, rows).noalias() = cont_[i].dof * (-normalAcc - (speed / timeStep_));
upper_.segment(index, rows).noalias() = lower_.segment(index, rows);

lower_.segment(index, rows).noalias() += cont_[i].dof * (cont_[i].lSpeed / timeStep_);
upper_.segment(index, rows).noalias() += cont_[i].dof * (cont_[i].uSpeed / timeStep_);