Velocity error of SurfaceTransformTask

[Stéphane Caron]

I think there is a problem with the formula of the velocity error (maybe acceleration as well), i.e. the time-derivative of the error vector, in the SurfaceTransformTask:

	sva::MotionVecd err_p = sva::transformVelocity(X_p_t, 1e-7);
	sva::MotionVecd V_p_p = jac_.velocity(mb, mbc, X_b_p_);
	sva::MotionVecd w_p_p = sva::MotionVecd(V_p_p.angular(), Eigen::Vector3d::Zero());
	sva::MotionVecd V_err_p = err_p.cross(w_p_p) - V_p_p;
	speed_ = -V_err_p.vector();

What @jorisv is trying to do here (section 4.2.6 of his PhD manuscript) is to derive the expression Err(X_p_t) (transformVelocity) over time. From spatial vector algebra:

(d(err) / dt)_A = d(err_A) / dt + v_A_A x err_A

where A is a frame that is mobile but in rotation only (why?), with the same rotational velocity as the p frame, and coincides with p in translation. Then, v_A_A corresponds to w_p_p.

The argument is then that d (err_A) / dt = 0 - V_p_p, but I think there's a problem here: if A has no translational velocity, the linear part of d (err_A) / dt should be zero.

What do we see?

I think what we see is mostly speed_ = the body velocity of the surface frame. After a quick check, err_p.cross(w_p_p) was between 1e-8 and 1e-14 in my current scenario. Also, seeing the body velocity is consistent with other tasks.