Contact 6D computes spatial acceleration, whereas contact 3D Classical acceleration: is one of them wrong ?

[maximebrunet]

Hello,

Thank you and congratulations for your nice work ! :)

I'm currently trying to implement a Contact5D, and I came across this difference when reading Contact3D and Contact6D implementations.

So, I'm wondering why is there a difference in the computed acceleration ? Are they identical under some assumptions (like contact with a fixed object) ?

Thank you for your help.

P.S.: I'm not certain this is a "bug", but I didn't know where I should post this kind of questions. Please let me know if there is a better place for next time.

[linglluo]

Hello, I also found this issue, and I think there is also an issue in the equation for solving da0_local_dx in contact-3d, has anyone else found this problem?

` pinocchio::getJointAccelerationDerivatives(
*state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);

const std::size_t nv = state_->get_nv();
pinocchio::skew(d->v.linear(), d->vv_skew);
pinocchio::skew(d->v.angular(), d->vw_skew);
d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
d->da0_local_dx.leftCols(nv).noalias() +=
d->vw_skew * d->fXjdv_dq.template topRows<3>();
d->da0_local_dx.leftCols(nv).noalias() -=
d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
d->da0_local_dx.rightCols(nv).noalias() +=
d->vw_skew * d->fJf.template topRows<3>();
d->da0_local_dx.rightCols(nv).noalias() -=
d->vv_skew * d->fJf.template bottomRows<3>();`

[cmastalli]

Hi @maximebrunet!

Both contacts and their derivatives are correct. Our contact models support local, world and world-aligned acceleration constraints. LOCAL stands for spatial acceleration using local coordinates, WORLD for world coordinates, and LOCAL_WORLD_ALIGNED are the so-called classical (Euclidean) acceleration.

Depending on what acceleration constraint is defined, we might need to map the contact accelerations to the classical one. This mapping is similar in spirit, in contrast to what you seem to suspect.

Computing the analytical derivates is tricky and requires a deep understanding of Lie algebra.

I hope this can help.

[linglluo]

Thank you for your reply.

Is there any paper or documentation for calculating the analytical derivative of the contact frame acceleration da0_local_dx?

I'm very confused about this because I can't derive the same equation as the code

[maximebrunet]

Thank you for your quick answer @cmastalli !

I'm no expert in Lie algebra, however, I thought that equation (2.50) from Featherstone's Rigid Body Dynamics Algorithm, revealing the additional term required to compute the classical acceleration from the spatial one was standing no matter the choice of coordinates, and therefore, for LOCAL_WORLD_ALIGNED coordinates as well. Am I mistaken or missing something?