Missing selection matrix in BoundedSpeedConstr

[anastasiabolotnikova]

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

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