C++ MultibodySystem::getMatterSubsystem方法代码示例

//------------------------------------------------------------------------------
//                                 ASSEMBLER
//------------------------------------------------------------------------------
Assembler::Assembler(const MultibodySystem& system)
:   system(system), accuracy(0), tolerance(0), // i.e., 1e-3, 1e-4
    forceNumericalGradient(false), forceNumericalJacobian(false),
    useRMSErrorNorm(false), alreadyInitialized(false),
    asmSys(0), optimizer(0), nAssemblySteps(0), nInitializations(0)
{
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
    matter.convertToEulerAngles(system.getDefaultState(),
                                internalState);
    system.realizeModel(internalState);

    // Make sure the System's Constraints are always present; this sets the
    // weight to Infinity which makes us treat this as an assembly error
    // rather than merely a goal; that can be changed by the user.
    systemConstraints = adoptAssemblyError(new BuiltInConstraints());
}

bool testFitting
   (const MultibodySystem& mbs, State& state, 
    const vector<MobilizedBodyIndex>& bodyIxs, 
    const vector<vector<Vec3> >& stations, 
    const vector<vector<Vec3> >& targetLocations, 
    Real minError, Real maxError, Real endDistance) 
{    
    // Find the best fit.
    
    Real reportedError = ObservedPointFitter::findBestFit(mbs, state, bodyIxs, stations, targetLocations, TOL);
    cout << "[min,max]=" << minError << "," << maxError << " actual=" << reportedError << endl;
    bool result = (reportedError <= maxError && reportedError >= minError);
    
    // Verify that the error was calculated correctly.
    
    Real error = 0.0;
    int numStations = 0;
    mbs.realize(state, Stage::Position);
    const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
    for (int i = 0; i < (int) bodyIxs.size(); ++i) {
        MobilizedBodyIndex id = bodyIxs[i];
        numStations += (int)stations[i].size();
        for (int j = 0; j < (int) stations[i].size(); ++j)
            error += (targetLocations[i][j]-matter.getMobilizedBody(id).getBodyTransform(state)*stations[i][j]).normSqr();
    }
    error = std::sqrt(error/numStations);
    cout << "calc wrms=" << error << endl;
    ASSERT(std::abs(1.0-error/reportedError) < 0.0001); // should match to machine precision
    
    if (endDistance >= 0) {
        // Verify that the ends are the correct distance apart.
        Real distance = (matter.getMobilizedBody(bodyIxs[0]).getBodyOriginLocation(state)-matter.getMobilizedBody(bodyIxs[bodyIxs.size()-1]).getBodyOriginLocation(state)).norm();
        cout << "required dist=" << endDistance << ", actual=" << distance << endl;
        ASSERT(std::abs(1.0-endDistance/distance) < TOL);
    }

    return result;
}

void ObservedPointFitter::
createClonedSystem(const MultibodySystem& original, MultibodySystem& copy, 
                   const Array_<MobilizedBodyIndex>& originalBodyIxs, 
                   Array_<MobilizedBodyIndex>& copyBodyIxs,
                   bool& hasArtificialBaseBody) 
{
    const SimbodyMatterSubsystem& originalMatter = original.getMatterSubsystem();
    SimbodyMatterSubsystem copyMatter(copy);
    Body::Rigid body = Body::Rigid(MassProperties(1, Vec3(0), Inertia(1)));
    body.addDecoration(Transform(), DecorativeSphere(Real(.1)));
    std::map<MobilizedBodyIndex, MobilizedBodyIndex> idMap;
    hasArtificialBaseBody = false;
    for (int i = 0; i < (int)originalBodyIxs.size(); ++i) {
        const MobilizedBody& originalBody = originalMatter.getMobilizedBody(originalBodyIxs[i]);
        MobilizedBody* copyBody;
        if (i == 0) {
            if (originalBody.isGround())
                copyBody = &copyMatter.Ground();
            else {
                hasArtificialBaseBody = true; // not using the original joint here
                MobilizedBody::Free free(copyMatter.Ground(), body);
                copyBody = &copyMatter.updMobilizedBody(free.getMobilizedBodyIndex());
            }
        }
        else {
            MobilizedBody& parent = copyMatter.updMobilizedBody(idMap[originalBody.getParentMobilizedBody().getMobilizedBodyIndex()]);
            copyBody = &originalBody.cloneForNewParent(parent);
        }
        copyBodyIxs.push_back(copyBody->getMobilizedBodyIndex());
        idMap[originalBodyIxs[i]] = copyBody->getMobilizedBodyIndex();
    }
    copy.realizeTopology();
    State& s = copy.updDefaultState();
    copyMatter.setUseEulerAngles(s, true);
    copy.realizeModel(s);
}

void testRollingOnSurfaceConstraint()
{
    using namespace SimTK;

    cout << endl;
    cout << "=================================================================" << endl;
    cout << " OpenSim RollingOnSurfaceConstraint Simulation " << endl;
    cout << "=================================================================" << endl;

    // angle of the rot w.r.t. vertical
    double theta = -SimTK::Pi / 6; // 30 degs
    double omega = -2.1234567890;
    double halfRodLength = 1.0 / (omega*omega);

    UnitVec3 surfaceNormal(0,1,0);
    double planeHeight = 0.0;
    Vec3 comInRod(0, halfRodLength, 0);
    Vec3 contactPointOnRod(0, 0, 0);

    double mass = 7.0;
    SimTK::Inertia inertiaAboutCom = mass*SimTK::Inertia::cylinderAlongY(0.1, 1.0);

    SimTK::MassProperties rodMass(7.0, comInRod,
        inertiaAboutCom.shiftFromMassCenter(comInRod, mass));

    // Define the Simbody system
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);

    // Create a free joint between the rod and ground
    MobilizedBody::Planar rod(matter.Ground(), Transform(Vec3(0)), 
        SimTK::Body::Rigid(rodMass), Transform());

    // Get underlying mobilized bodies
    SimTK::MobilizedBody surface = matter.getGround();

    // Add a fictitious massless body to be the "Case" reference body coincident with surface for the no-slip constraint
    SimTK::MobilizedBody::Weld  cb(surface, SimTK::Body::Massless());

    // Constrain the rod to move on the ground surface
    SimTK::Constraint::PointInPlane contactY(surface, surfaceNormal, planeHeight, rod, contactPointOnRod);
    SimTK::Constraint::ConstantAngle contactTorqueAboutY(surface, SimTK::UnitVec3(1, 0, 0), rod, SimTK::UnitVec3(0, 0, 1));
    // Constrain the rod to roll on surface and not slide 
    SimTK::Constraint::NoSlip1D contactPointXdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(1, 0, 0), surface, rod);
    SimTK::Constraint::NoSlip1D contactPointZdir(cb, SimTK::Vec3(0), SimTK::UnitVec3(0, 0, 1), surface, rod);
    
    // Simbody model state setup
    system.realizeTopology();
    State state = system.getDefaultState();

    //state = system.realizeTopology();
    state.updQ()[0] = theta;
    state.updQ()[1] = 0;
    state.updQ()[2] = 0;
    state.updU()[0] = omega;

    system.realize(state, Stage::Acceleration);
    state.getUDot().dump("Simbody Accelerations");

    Vec3 pcom = system.getMatterSubsystem().calcSystemMassCenterLocationInGround(state);
    Vec3 vcom = system.getMatterSubsystem().calcSystemMassCenterVelocityInGround(state);
    Vec3 acom = system.getMatterSubsystem().calcSystemMassCenterAccelerationInGround(state);

    //==========================================================================================================
    // Setup OpenSim model
    Model *osimModel = new Model;
    osimModel->setGravity(gravity_vec);

    //OpenSim bodies
    Ground& ground = osimModel->updGround();;
    Mesh arrowGeom("arrow.vtp");
    arrowGeom.setColor(Vec3(1, 0, 0));
    ground.attachGeometry(arrowGeom.clone());

    //OpenSim rod
    auto osim_rod = new OpenSim::Body("rod", mass, comInRod, inertiaAboutCom);
    OpenSim::PhysicalOffsetFrame* cylFrame = new PhysicalOffsetFrame(*osim_rod, Transform(comInRod));
    cylFrame->setName("comInRod");
    osimModel->addFrame(cylFrame);
    Mesh cylGeom("cylinder.vtp");
    cylGeom.set_scale_factors(2 * halfRodLength*Vec3(0.1, 1, 0.1));
    cylFrame->attachGeometry(cylGeom.clone());

    // create rod as a free joint
    auto rodJoint = new PlanarJoint("rodToGround", ground, *osim_rod);

    // Add the thigh body which now also contains the hip joint to the model
    osimModel->addBody(osim_rod);
    osimModel->addJoint(rodJoint);

    // add a point on line constraint
    auto roll = new RollingOnSurfaceConstraint();
    roll->setRollingBodyByName("rod");
    roll->setSurfaceBodyByName("ground");

    /*double h = */roll->get_surface_height();
    
    osimModel->addConstraint(roll);
//.........这里部分代码省略.........

Real ObservedPointFitter::findBestFit
   (const MultibodySystem& system, State& state, 
    const Array_<MobilizedBodyIndex>&  bodyIxs, 
    const Array_<Array_<Vec3> >&       stations, 
    const Array_<Array_<Vec3> >&       targetLocations, 
    const Array_<Array_<Real> >&       weights, 
    Real tolerance) 
{    
    // Verify the inputs.
    
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();
    SimTK_APIARGCHECK(bodyIxs.size() == stations.size() && stations.size() == targetLocations.size(), "ObservedPointFitter", "findBestFit", "bodyIxs, stations, and targetLocations must all be the same length");
    int numBodies = matter.getNumBodies();
    for (int i = 0; i < (int)stations.size(); ++i) {
        SimTK_APIARGCHECK(bodyIxs[i] >= 0 && bodyIxs[i] < numBodies, "ObservedPointFitter", "findBestFit", "Illegal body ID");
        SimTK_APIARGCHECK(stations[i].size() == targetLocations[i].size(), "ObservedPointFitter", "findBestFit", "Different number of stations and target locations for body");
    }
    
    // Build a list of children for each body.
    
    Array_<Array_<MobilizedBodyIndex> > children(matter.getNumBodies());
    for (int i = 0; i < matter.getNumBodies(); ++i) {
        const MobilizedBody& body = matter.getMobilizedBody(MobilizedBodyIndex(i));
        if (!body.isGround())
            children[body.getParentMobilizedBody().getMobilizedBodyIndex()].push_back(body.getMobilizedBodyIndex());
    }

    // Build a mapping of body IDs to indices.
    
    Array_<int> bodyIndex(matter.getNumBodies());
    for (int i = 0; i < (int) bodyIndex.size(); ++i)
        bodyIndex[i] = -1;
    for (int i = 0; i < (int)bodyIxs.size(); ++i)
        bodyIndex[bodyIxs[i]] = i;
    
    // Find the number of stations on each body with a nonzero weight.
    
    Array_<int> numStations(matter.getNumBodies());
    for (int i = 0; i < (int) numStations.size(); ++i)
        numStations[i] = 0;
    for (int i = 0; i < (int)weights.size(); ++i) {
        for (int j = 0; j < (int)weights[i].size(); ++j)
            if (weights[i][j] != 0)
                numStations[bodyIxs[i]]++;
    }

    // Perform the initial estimation of Q for each mobilizer.
    // Our first guess is the passed-in q's, with quaternions converted
    // to Euler angles if necessary. As we solve a subproblem for each
    // of the bodies in ascending order, we'll update tempState's q's
    // for that body to their solved values.
    State tempState;
    if (!matter.getUseEulerAngles(state))
        matter.convertToEulerAngles(state, tempState);
    else tempState = state;
    system.realizeModel(tempState);
    system.realize(tempState, Stage::Position);

    // This will accumulate best-guess spatial poses for the bodies as
    // they are processed. This is useful for when a body is used as
    // an artificial base body; our first guess will to be to place it
    // wherever it was the last time it was used in a subproblem.
    Array_<Transform> guessX_GB(matter.getNumBodies());
    for (MobilizedBodyIndex mbx(1); mbx < guessX_GB.size(); ++mbx)
        guessX_GB[mbx] = matter.getMobilizedBody(mbx).getBodyTransform(tempState);

    for (int i = 0; i < matter.getNumBodies(); ++i) {
        MobilizedBodyIndex id(i);
        const MobilizedBody& body = matter.getMobilizedBody(id);
        if (body.getNumQ(tempState) == 0)
            continue; // No degrees of freedom to determine.
        if (children[id].size() == 0 && numStations[id] == 0)
            continue; // There are no stations whose positions are affected by this.
        Array_<MobilizedBodyIndex> originalBodyIxs;
        int currentBodyIndex = findBodiesForClonedSystem(body.getMobilizedBodyIndex(), numStations, matter, children, originalBodyIxs);
        if (currentBodyIndex == (int)originalBodyIxs.size()-1 
            && (bodyIndex[id] == -1 || stations[bodyIndex[id]].size() == 0))
            continue; // There are no stations whose positions are affected by this.
        MultibodySystem copy;
        Array_<MobilizedBodyIndex> copyBodyIxs;
        bool hasArtificialBaseBody;
        createClonedSystem(system, copy, originalBodyIxs, copyBodyIxs, hasArtificialBaseBody);
        const SimbodyMatterSubsystem& copyMatter = copy.getMatterSubsystem();
        // Construct an initial state.
        State copyState = copy.getDefaultState();
        assert(copyBodyIxs.size() == originalBodyIxs.size());
        for (int ob=0; ob < (int)originalBodyIxs.size(); ++ob) {
            const MobilizedBody& copyMobod = copyMatter.getMobilizedBody(copyBodyIxs[ob]);
            const MobilizedBody& origMobod = matter.getMobilizedBody(originalBodyIxs[ob]);
            if (ob==0 && hasArtificialBaseBody)
                copyMobod.setQToFitTransform(copyState, guessX_GB[origMobod.getMobilizedBodyIndex()]);
            else
                copyMobod.setQFromVector(copyState, origMobod.getQAsVector(tempState));
        }

        Array_<Array_<Vec3> > copyStations(copyMatter.getNumBodies());
        Array_<Array_<Vec3> > copyTargetLocations(copyMatter.getNumBodies());
        Array_<Array_<Real> > copyWeights(copyMatter.getNumBodies());
        for (int j = 0; j < (int)originalBodyIxs.size(); ++j) {
            int index = bodyIndex[originalBodyIxs[j]];
//.........这里部分代码省略.........

void testConstrainedSystem() {
    MultibodySystem mbs;
    MyForceImpl* frcp;
    makeSystem(true, mbs, frcp);
    const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();

    State state = mbs.realizeTopology();
    mbs.realize(state, Stage::Instance); // allocate multipliers, etc.

    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int m  = state.getNMultipliers();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    mbs.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    Vector randMobFrc = 100*Test::randVector(nu);
    Vector_<SpatialVec> randBodyFrc(nb);
    for (int i=0; i < nb; ++i)
        randBodyFrc[i] = Test::randSpatialVec();

    // Apply random mobility forces
    frcp->setMobilityForces(randMobFrc);

    mbs.realize(state); // calculate accelerations and multipliers
    Vector udot = state.getUDot();
    Vector lambda = state.getMultipliers();
    Vector residual;
    matter.calcResidualForce(state,randMobFrc,Vector_<SpatialVec>(),
                             udot, lambda, residual);

    // Residual should be zero since we accounted for everything.
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    Vector abias, mgbias;
    // These are the acceleration error bias terms.
    matter.calcBiasForAccelerationConstraints(state, abias);
    // These use pverr (velocity-level errors) for holonomic constraints.
    matter.calcBiasForMultiplyByG(state, mgbias);

    Vector mgGudot; matter.multiplyByG(state, udot, mgbias, mgGudot);
    Matrix G; matter.calcG(state, G);
    Vector Gudot = G*udot;
    SimTK_TEST_EQ_TOL(mgGudot, Gudot, Slop);
    Vector aerr = state.getUDotErr(); // won't be zero because bad constraints
    Vector GudotPlusBias = Gudot + abias;
    SimTK_TEST_EQ_TOL(GudotPlusBias, aerr, Slop);

    // Add in some body forces
    state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
    frcp->setBodyForces(randBodyFrc);
    mbs.realize(state);
    udot = state.getUDot();
    lambda = state.getMultipliers();
    matter.calcResidualForce(state,randMobFrc,randBodyFrc,
                             udot, lambda, residual);
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    // Try body forces only.
    state.invalidateAllCacheAtOrAbove(Stage::Dynamics);
    frcp->setMobilityForces(0*randMobFrc);
    mbs.realize(state);
    udot = state.getUDot();
    lambda = state.getMultipliers();
    matter.calcResidualForce(state,Vector(),randBodyFrc,
                             udot, lambda, residual);
    SimTK_TEST_EQ_TOL(residual, 0*randMobFrc, Slop);

    // Put vectors in noncontiguous storage.
    Matrix udotmat(3,nu); // rows are noncontig
    Matrix mobFrcMat(11,nu);
    Matrix lambdamat(5,m);
    Matrix_<SpatialRow> bodyFrcMat(3,nb);
    udotmat[2]    = ~udot;
    lambdamat[3]  = ~lambda;
    mobFrcMat[8] = ~randMobFrc;
    bodyFrcMat[2] = ~randBodyFrc;
    Matrix residmat(4,nu);

    // We last computed udot,lambda with no mobility forces. This time
    // will throw some in and then make sure the residual tries to cancel them.
    matter.calcResidualForce(state,~mobFrcMat[8],~bodyFrcMat[2],
        ~udotmat[2],~lambdamat[3],~residmat[2]);
    SimTK_TEST_EQ_TOL(residmat[2], -1*mobFrcMat[8], Slop);
}

void testUnconstrainedSystem() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    Vector randVec = 100*Test::randVector(nu);
    Vector result1, result2;

    // result1 = M*v
    system.realize(state, Stage::Position);
    matter.multiplyByM(state, randVec, result1);
    SimTK_TEST_EQ(result1.size(), nu);

    // result2 = M^-1 * result1 == M^-1 * M * v == v
    system.realize(state, Stage::Dynamics);
    matter.multiplyByMInv(state, result1, result2);
    SimTK_TEST_EQ(result2.size(), nu);

    SimTK_TEST_EQ_TOL(result2, randVec, Slop);

    Matrix M(nu,nu), MInv(nu,nu);

    Vector v(nu, Real(0));
    for (int j=0; j < nu; ++j) {
        v[j] = 1;
        matter.multiplyByM(state, v, M(j));
        matter.multiplyByMInv(state, v, MInv(j));
        v[j] = 0;
    }

    Matrix MInvCalc(M);
    MInvCalc.invertInPlace();
    SimTK_TEST_EQ_SIZE(MInv, MInvCalc, nu);

    Matrix identity(nu,nu); identity=1;
    SimTK_TEST_EQ_SIZE(M*MInv, identity, nu);
    SimTK_TEST_EQ_SIZE(MInv*M, identity, nu);

    // Compare above-calculated values with values returned by the
    // calcM() and calcMInv() methods.
    Matrix MM, MMInv;
    matter.calcM(state,MM); matter.calcMInv(state,MMInv);
    SimTK_TEST_EQ_SIZE(MM, M, nu);
    SimTK_TEST_EQ_SIZE(MMInv, MInv, nu);

    //assertIsIdentity(eye);
    //assertIsIdentity(MInv*M);

    frcp->setMobilityForces(randVec);
    //cout << "f=" << randVec << endl;
    system.realize(state, Stage::Acceleration);
    Vector accel = state.getUDot();
    //cout << "v!=0, accel=" << accel << endl;

    matter.multiplyByMInv(state, randVec, result1);
    //cout << "With velocities, |a - M^-1*f|=" << (accel-result1).norm() << endl;

    SimTK_TEST_NOTEQ(accel, result1); // because of the velocities
    //SimTK_TEST((accel-result1).norm() > SignificantReal); // because of velocities

    // With no velocities M^-1*f should match calculated acceleration.
    state.updU() = 0;
    system.realize(state, Stage::Acceleration);
    accel = state.getUDot();
    //cout << "v=0, accel=" << accel << endl;

    //cout << "With v=0, |a - M^-1*f|=" << (accel-result1).norm() << endl;

    SimTK_TEST_EQ(accel, result1); // because no velocities

    // And then M*a should = f.
    matter.multiplyByM(state, accel, result2);
    //cout << "v=0, M*accel=" << result2 << endl;
    //cout << "v=0, |M*accel-f|=" << (result2-randVec).norm() << endl;


    // Test forward and inverse dynamics operators.
    // Apply random forces and a random prescribed acceleration to
    // get back the residual generalized forces. Then applying those
    // should result in zero residual, and applying them. 

    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);


//.........这里部分代码省略.........

// Test calculations of Jacobian "bias" terms, where bias=JDot*u.
// We can estimate JDot using a numerical directional derivative
// since JDot = (DJ/Dq)*qdot ~= (J(q+h*qdot)-J(q-h*qdot))/2h.
// Then we multiply JDot*u and compare with the bias calculations.
// Or, we can estimate JDot*u directly with
//       JDotu ~= (J(q+h*qdot)*u - J(q-h*qdot)*u)/2h
// using the fast "multiply by Jacobian" methods.
// We use both methods below.
void testJacobianBiasTerms() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);


    const MobilizedBodyIndex whichBod(8);
    const Vec3 whichPt(1,2,3);
    system.realize(state, Stage::Velocity);
    const Vector& q = state.getQ();
    const Vector& u = state.getU();
    const Vector& qdot = state.getQDot();

    // sbias, fbias, sysbias are the JDot*u quantities we want to check.
    const Vec3 sbias =
        matter.calcBiasForStationJacobian(state, whichBod, whichPt);
    const SpatialVec fbias = 
        matter.calcBiasForFrameJacobian(state, whichBod, whichPt);
    Vector_<SpatialVec> sysbias;
    matter.calcBiasForSystemJacobian(state, sysbias);

    // These are for computing JDot first.
    RowVector_<Vec3> JS_P, JS1_P, JS2_P, JSDot_P;
    RowVector_<SpatialVec> JF_P, JF1_P, JF2_P, JFDot_P;
    Matrix_<SpatialVec> J, J1, J2, JDot;

    // These are for computing JDot*u directly.
    Vec3 JS_Pu, JS1_Pu, JS2_Pu, JSDot_Pu;
    SpatialVec JF_Pu, JF1_Pu, JF2_Pu, JFDot_Pu;
    Vector_<SpatialVec> Ju, J1u, J2u, JDotu;

    // Unperturbed:
    matter.calcStationJacobian(state,whichBod,whichPt, JS_P);
    matter.calcFrameJacobian(state,whichBod,whichPt, JF_P);
    matter.calcSystemJacobian(state, J);

    JS_Pu = matter.multiplyByStationJacobian(state,whichBod,whichPt,u);
    JF_Pu = matter.multiplyByFrameJacobian(state,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(state, u, Ju);

    const Real Delta = 5e-6; // we'll use central difference
    State perturbq = state;
    // Perturbed +:
    perturbq.updQ() = q + Delta*qdot;
    system.realize(perturbq, Stage::Position);
    matter.calcStationJacobian(perturbq,whichBod,whichPt, JS2_P);
    matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF2_P);
    matter.calcSystemJacobian(perturbq, J2);
    JS2_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
    JF2_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(perturbq,u, J2u);

    // Perturbed -:
    perturbq.updQ() = q - Delta*qdot;
    system.realize(perturbq, Stage::Position);
    matter.calcStationJacobian(perturbq,whichBod,whichPt, JS1_P);
    matter.calcFrameJacobian(perturbq,whichBod,whichPt, JF1_P);
    matter.calcSystemJacobian(perturbq, J1);
    JS1_Pu = matter.multiplyByStationJacobian(perturbq,whichBod,whichPt,u);
    JF1_Pu = matter.multiplyByFrameJacobian(perturbq,whichBod,whichPt,u);
    matter.multiplyBySystemJacobian(perturbq,u, J1u);

    // Estimate JDots:
    JSDot_P = (JS2_P-JS1_P)/Delta/2;
    JFDot_P = (JF2_P-JF1_P)/Delta/2;
    JDot    = (J2-J1)/Delta/2;

    // Estimate JDotus:
    JSDot_Pu = (JS2_Pu-JS1_Pu)/Delta/2;
    JFDot_Pu = (JF2_Pu-JF1_Pu)/Delta/2;
    JDotu    = (J2u-J1u)/Delta/2;

    // Calculate errors in JDot*u:
    SimTK_TEST_EQ_TOL((JSDot_P*u-sbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JFDot_P*u-fbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JDot*u-sysbias).norm(), 0, SqrtEps);

    // Calculate errors in JDotu:
    SimTK_TEST_EQ_TOL((JSDot_Pu-sbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JFDot_Pu-fbias).norm(), 0, SqrtEps);
    SimTK_TEST_EQ_TOL((JDotu-sysbias).norm(), 0, SqrtEps);
//.........这里部分代码省略.........

void testTaskJacobians() {
    MultibodySystem system;
    MyForceImpl* frcp;
    makeSystem(false, system, frcp);
    const SimbodyMatterSubsystem& matter = system.getMatterSubsystem();

    State state = system.realizeTopology();
    const int nq = state.getNQ();
    const int nu = state.getNU();
    const int nb = matter.getNumBodies();

    // Attainable accuracy drops with problem size.
    const Real Slop = nu*SignificantReal;

    system.realizeModel(state);
    // Randomize state.
    state.updQ() = Test::randVector(nq);
    state.updU() = Test::randVector(nu);

    system.realize(state, Stage::Position);

    Matrix_<SpatialVec> J;
    Matrix Jmat, Jmat2;
    matter.calcSystemJacobian(state, J);
    SimTK_TEST_EQ(J.nrow(), nb); SimTK_TEST_EQ(J.ncol(), nu);
    matter.calcSystemJacobian(state, Jmat);
    SimTK_TEST_EQ(Jmat.nrow(), 6*nb); SimTK_TEST_EQ(Jmat.ncol(), nu);

    // Unpack J into Jmat2 and compare with Jmat.
    Jmat2.resize(6*nb, nu);
    for (int row=0; row < nb; ++row) {
        const int nxtr = 6*row; // row index into scalar matrix
        for (int col=0; col < nu; ++col) {
            for (int k=0; k<3; ++k) {
                Jmat2(nxtr+k, col) = J(row,col)[0][k];
                Jmat2(nxtr+3+k, col) = J(row,col)[1][k];
            }
        }
    }
    // These should be exactly the same.
    SimTK_TEST_EQ_TOL(Jmat2, Jmat, SignificantReal);

    Vector randU = 100.*Test::randVector(nu), resultU1, resultU2;
    Vector_<SpatialVec> randF(nb), resultF1, resultF2;
    for (int i=0; i<nb; ++i) randF[i] = 100.*Test::randSpatialVec();

    matter.multiplyBySystemJacobian(state, randU, resultF1);
    resultF2 = J*randU;
    SimTK_TEST_EQ_TOL(resultF1, resultF2, Slop);

    matter.multiplyBySystemJacobianTranspose(state, randF, resultU1);
    resultU2 = ~J*randF;
    SimTK_TEST_EQ_TOL(resultU1, resultU2, Slop);

    // See if Station Jacobian can be used to duplicate the translation
    // rows of the System Jacobian, and if Frame Jacobian can be used to
    // duplicate the whole thing.
    Array_<MobilizedBodyIndex> allBodies(nb);
    for (int i=0; i<nb; ++i) allBodies[i]=MobilizedBodyIndex(i);
    Array_<Vec3> allOrigins(nb, Vec3(0));

    Matrix_<Vec3> JS, JS2, JSbyrow;
    Matrix_<SpatialVec> JF, JF2, JFbyrow;

    matter.calcStationJacobian(state, allBodies, allOrigins, JS);
    matter.calcFrameJacobian(state, allBodies, allOrigins, JF);
    for (int i=0; i<nb; ++i) {
        for (int j=0; j<nu; ++j) {
            SimTK_TEST_EQ(JS(i,j), J(i,j)[1]);
            SimTK_TEST_EQ(JF(i,j), J(i,j));
        }
    }

    // Now use random stations to calculate JS & JF.
    Array_<Vec3> randS(nb);
    for (int i=0; i<nb; ++i) randS[i] = 10.*Test::randVec3();
    matter.calcStationJacobian(state, allBodies, randS, JS);
    matter.calcFrameJacobian(state, allBodies, randS, JF);

    // Recalculate one row at a time to test non-contiguous memory handling.
    // Do it backwards just to show off.
    JSbyrow.resize(nb, nu); JFbyrow.resize(nb, nu);
    for (int i=nb-1; i >= 0; --i) {
        matter.calcStationJacobian(state, allBodies[i], randS[i], JSbyrow[i]);
        matter.calcFrameJacobian(state, allBodies[i], randS[i], JFbyrow[i]);
    }
    SimTK_TEST_EQ(JS, JSbyrow);
    SimTK_TEST_EQ(JF, JFbyrow);

    // Calculate JS2=JS and JF2=JF again using multiplication by mobility-space 
    // unit vectors.
    JS2.resize(nb, nu); JF2.resize(nb, nu);
    Vector zeroU(nu, 0.);
    for (int i=0; i < nu; ++i) {
        zeroU[i] = 1;
        matter.multiplyByStationJacobian(state, allBodies, randS, zeroU, JS2(i));
        matter.multiplyByFrameJacobian(state, allBodies, randS, zeroU, JF2(i));
        zeroU[i] = 0;
    }
    SimTK_TEST_EQ_TOL(JS2, JS, Slop);
//.........这里部分代码省略.........