simtk.org

Code: Select all

Vec3::updAs(&pverr[0]) = v_AS - v_AC;

Code: Select all

Vec3::updAs(&pverr[0]) = v_AS - v_AP;

Code: Select all

// We have a ball joint between base body B and follower body F, located at a 
// point P fixed to B and point S fixed on F. All forces will be applied at 
// point S and the coincident material point C on B which is instantaneously at
// the same spatial location as S. We will work in the A frame.
//
//  First, find the material point C of B that is coincident
//  in space with point S of F: p_BC = p_AS-p_AB. This vector
//  is *constant* in the B frame because it is a material point,
//  despite the fact that its definition involves a point which
//  moves with respect to B. The velocity constraint is then
//  very simple: the spatial velocity of point C of B should be
//  the same as the spatial velocity of point S of F:
//      verr = v_AS - v_AC = v_AS - (v_AB + w_AB X p_BC) = 0
//  Integrating to get perr, we get
//      perr = p_AS - p_AC + constant = 0
//  But p_AC=p_AS by construction, so perr=constant=0.
//  The constant is defined by the fact that we want material point
//  C of B to be in the same spatial location as point P of B,
//  so constant=p_BC-p_BP=0. Writing in the A frame we have:
//      perr = p_AS-(p_AB+R_AB*p_BP) = 0 (a constant)
//      verr = v_AS - (v_AB + w_AB X R_AB*p_BC)
//      aerr = a_AS - (a_AB + b_AB X R_AB*p_BC + w_AB X (w_AB X R_AB*p_BC))
//  apply +lambda to S of F, -lambda to C of B.

Code: Select all

Matrix Constraint::calcPositionConstraintMatrixP(const State& s) const {
    int mp,mv,ma;
    getNumConstraintEquationsInUse(s, mp, mv, ma);

    const SimbodyMatterSubsystem& matter = getMatterSubsystem();
    const System&                 system = matter.getSystem();

    const int nu = matter.getNU(s);

    Matrix P(mp, nu);
    if (mp && nu) {
        Vector  pverr0(mp), pverr(mp); // we're interested in the first mp of these
        State   tmp = s;      // don't change s

        matter.updU(tmp) = 0;   // first calculate the bias term -c(t,q)
        system.realize(tmp, Stage::Velocity);
        pverr0 = getVelocityErrorsAsVector(tmp)(0,mp);

        // Now calculate sensitivity of d(perr)/dt=Pu-c(t,q) to each u in turn.
        for (int j=0; j<nu; ++j) {
            matter.updU(tmp)[j] = 1;
            system.realize(tmp, Stage::Velocity);
            pverr = getVelocityErrorsAsVector(tmp)(0,mp);
            matter.updU(tmp)[j] = 0;
            P(j) = pverr - pverr0;
        }
    }
    return P;
}