Consistency of SurfaceTransformTask in TrajectoryTask

[stephane-caron]

The error vector in TrajectoryTask::update is computed as:

	error_.noalias() = gainPos_*err;
	error_.noalias() += gainVel_*(refVel_ - speed);
	error_.noalias() += refAccel_;

When the underlying task is a SurfaceTransformTask, speed should be the time-derivative of err.

• Note that this is not consistent with other tasks such as the SurfaceOrientationTask where speed is the velocity of the body itself, not of the error vector in the body frame.
• Then that means refVel should be a reference for the time-derivative of the tracking error, which does not make sense.

This makes a difference but we should not see it much in practice because the speed of a SurfaceTransformTask is mostly the surface frame's body velocity.