Question about the constraint Juestion about the constraint Jacobian matrix of "ChLinkMate.cpp"

[wanduanying]

Hi, this is a question about the constraint Juestion about the constraint Jacobian matrix of "ChLinkMate.cpp";
Recently, I learned this part of "ChLinkMate.cpp"，it is related how to calculate the constraint Jacobian matrix about two rigid body. I wonder what is the generalized coordinates for the constraint equation?

In my opinion, there are the three translational degree, like x, y, z, but for rotational degree, it is not clear, because it is very critical for calculate Jr1 and Jr2. I will very appreciate that if you can offer some guidelance, or offer some related referrences to this part.

void ChLinkMateGeneric::Update(double mytime, bool update_assets) {
// Inherit time changes of parent class (ChLink), basically doing nothing :)
ChLink::Update(mytime, update_assets);

if (this->m_body1 && this->m_body2) {
    this->mask.SetTwoBodiesVariables(&m_body1->Variables(), &m_body2->Variables());

    ChFrame<> F1_W = this->frame1 >> (*this->m_body1);
    ChFrame<> F2_W = this->frame2 >> (*this->m_body2);
    ChFrame<> F1_wrt_F2 = F2_W.TransformParentToLocal(F1_W);
    // Now 'F1_wrt_F2' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.

    ChMatrix33<> Jx1 = F2_W.GetRotMat().transpose();
    ChMatrix33<> Jx2 = -F2_W.GetRotMat().transpose();

    ChMatrix33<> Jr1 = -F2_W.GetRotMat().transpose() * m_body1->GetRotMat() * ChStarMatrix33<>(frame1.GetPos());
    ChVector3d r12_B2 = m_body2->GetRotMat().transpose() * (F1_W.GetPos() - F2_W.GetPos());
    ChMatrix33<> Jr2 = this->frame2.GetRotMat().transpose() * ChStarMatrix33<>(frame2.GetPos() + r12_B2);

    // Premultiply by Jw1 and Jw2 by P = 0.5 * [Fp(q_resid^*)]'.bottomRow(3) to get residual as imaginary part of a
    // quaternion. For small misalignment this effect is almost insignificant because P ~= [I33], but otherwise it
    // is needed (if you want to use the stabilization term - if not, you can live without).
    this->P = 0.5 * (ChMatrix33<>(F1_wrt_F2.GetRot().e0()) + ChStarMatrix33<>(F1_wrt_F2.GetRot().GetVector()));

    ChMatrix33<> Jw1 = this->P.transpose() * F2_W.GetRotMat().transpose() * m_body1->GetRotMat();
    ChMatrix33<> Jw2 = -this->P.transpose() * F2_W.GetRotMat().transpose() * m_body2->GetRotMat();

    // Another equivalent expression:
    // ChMatrix33<> Jw1 = this->P * F1_W.GetRotMat().transpose() * m_body1->GetRotMat();
    // ChMatrix33<> Jw2 = -this->P * F1_W.GetRotMat().transpose() * m_body2->GetRotMat();

    // The Jacobian matrix of constraint is:
    // Cq = [ Jx1,  Jr1,  Jx2,  Jr2 ]
    //      [   0,  Jw1,    0,  Jw2 ]

[dariomangoni]

We made a paper exactly on this!
A unified analytical expression of the tangent stiffness matrix of holonomic constraints

[wanduanying]

Mr dariomangoni, thank you very much