Implemented the feed-forward velocity in the cartesian task in a safe way.

Now the Cartesian task accepts velocity references via the method:
setReference(desiredPose,desiredVelocity)
the old method setReference(desiredPose) sets the desired velocity to 0.
Also a getter method for the new desired velocity has been implemented
getReference(desiredPose,desiredVelocity)
Notice how the _update(x) now also resets the desired velocity for safety reasons.
TO use the feedforward term it is necessary to call a setReference
for each call to _update(x).
A possible bug has been corrected where calling setLambda caused the
desired velocity term to be miscomputed when called after setReference.