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

//------------------------------------------------------------------------------
//                                 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());
}

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 testCoordinateCouplerConstraint()
{
    using namespace SimTK;

    cout << endl;
    cout << "=================================================================" << endl;
    cout << " OpenSim CoordinateCouplerConstraint vs. FunctionBasedMobilizer  " << endl;
    cout << "=================================================================" << endl;

    // Define spline data for the custom knee joint
    int npx = 12;
    double angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197, -0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443, 0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
    double kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000, 0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000, -0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
    int npy = 7;
    double angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399, -0.174532925199, 0.159148563428, 2.094395102393};
    double kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000, -0.396600000000, -0.395264000000, -0.396000000000 };


    for(int i = 0; i<npy; i++) {
        // Spline data points from experiment w.r.t. hip location. Change to make it w.r.t knee location
        kneeY[i] += (-kneeInFemur[1]+hipInFemur[1]); 
    }

    SimmSpline tx(npx, angX, kneeX);
    SimmSpline ty(npy, angY, kneeY);;

    // Define the functions that specify the FunctionBased Mobilized Body.
    std::vector<std::vector<int> > coordIndices;
    std::vector<const SimTK::Function*> functions;
    std::vector<bool> isdof(6,false);

    // Set the 1 spatial rotation about Z-axis
    isdof[2] = true;  //rot Z
    int nm = 0;
    for(int i=0; i<6; i++){
        if(isdof[i]) {
            Vector coeff(2);
            coeff[0] = 1;
            coeff[1] = 0;
            std::vector<int> findex(1);
            findex[0] = nm++;
            functions.push_back(new SimTK::Function::Linear(coeff));
            coordIndices.push_back(findex);
        }
        else if(i==3 || i ==4){
            std::vector<int> findex(1,0);
            if(i==3)
                functions.push_back(tx.createSimTKFunction());
            else
                functions.push_back(ty.createSimTKFunction());

            coordIndices.push_back(findex); 
        }
        else{
            std::vector<int> findex(0);
            functions.push_back(new SimTK::Function::Constant(0, 0));
            coordIndices.push_back(findex);
        }
    }

    // Define the Simbody system
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    SimTK::Force::UniformGravity gravity(forces, matter, gravity_vec);
    //system.updDefaultSubsystem().addEventReporter(new VTKEventReporter(system, 0.01));

    // Thigh connected by hip
    MobilizedBody::Pin thigh(matter.Ground(), Transform(hipInGround), 
        SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
    //Function-based knee connects shank
    MobilizedBody::FunctionBased shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia), nm, functions, coordIndices);
    //MobilizedBody::Pin shank(thigh, SimTK::Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), SimTK::Transform(kneeInTibia));

    // Simbody model state setup
    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    //==========================================================================================================
    // Setup OpenSim model
    Model *osimModel = new Model;
    //OpenSim bodies
    const Ground& ground = osimModel->getGround();;
    
    OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);

    // create hip as a pin joint
    PinJoint hip("hip",ground, hipInGround, Vec3(0), osim_thigh, hipInFemur, Vec3(0));

    // Rename hip coordinates for a pin joint
    hip.getCoordinateSet()[0].setName("hip_flex");
    
    // Add the thigh body which now also contains the hip joint to the model
    osimModel->addBody(&osim_thigh);
    osimModel->addJoint(&hip);

    // Add another body via a knee joint
    OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
//.........这里部分代码省略.........

void testPointOnLineConstraint()
{
    using namespace SimTK;

    cout << endl;
    cout << "==================================================================" << endl;
    cout << "OpenSim PointOnLineConstraint vs. Simbody Constraint::PointOnLine " << endl;
    cout << "==================================================================" << endl;

    Random::Uniform randomDirection(-1, 1);
    Vec3 lineDirection(randomDirection.getValue(), randomDirection.getValue(),
        randomDirection.getValue());
    UnitVec3 normLineDirection(lineDirection.normalize());
    Vec3 pointOnLine(0,0,0);
    Vec3 pointOnFollower(0,0,0);

    // 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 foot and ground
    MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), 
        SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
    
    // Constrain foot to line on ground
    SimTK::Constraint::PointOnLine simtkPointOnLine(matter.Ground(), 
        normLineDirection, pointOnLine, foot, pointOnFollower);

    // Simbody model state setup
    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    //=========================================================================
    // Setup OpenSim model
    Model *osimModel = new Model;
    //OpenSim bodies
    const Ground& ground = osimModel->getGround();;

    //OpenSim foot
    OpenSim::Body osim_foot("foot", footMass.getMass(),
        footMass.getMassCenter(), footMass.getInertia());

    // create foot as a free joint
    FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0),
                                     osim_foot, Vec3(0), Vec3(0));
    
    // Add the thigh body which now also contains the hip joint to the model
    osimModel->addBody(&osim_foot);
    osimModel->addJoint(&footJoint);

    // add a point on line constraint
    PointOnLineConstraint lineConstraint(ground, normLineDirection, pointOnLine,
        osim_foot, pointOnFollower);
    osimModel->addConstraint(&lineConstraint);

    // BAD: have to set memoryOwner to false or program will crash when this test is complete.
    osimModel->disownAllComponents();

    osimModel->setGravity(gravity_vec);

    //Add analyses before setting up the model for simulation
    Kinematics *kinAnalysis = new Kinematics(osimModel);
    kinAnalysis->setInDegrees(false);
    osimModel->addAnalysis(kinAnalysis);

    // Need to setup model before adding an analysis since it creates the AnalysisSet
    // for the model if it does not exist.
    State& osim_state = osimModel->initSystem();

    //==========================================================================================================
    // Compare Simbody system and OpenSim model simulations
    compareSimulations(system, state, osimModel, osim_state, "testPointOnLineConstraint FAILED\n");
} // end testPointOnLineConstraint

void testWeldConstraint()
{
    using namespace SimTK;

    cout << endl;
    cout << "==================================================================" << endl;
    cout << " OpenSim WeldConstraint vs. Simbody Constraint::Weld " << endl;
    cout << "==================================================================" << endl;

    Random::Uniform randomValue(-0.05, 0.1);
    Vec3 weldInGround(randomValue.getValue(), randomValue.getValue(), 0);
    Vec3 weldInFoot(0.1*randomValue.getValue(), 0.1*randomValue.getValue(), 0);

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

    // Thigh connected by hip
    MobilizedBody::Pin thigh(matter.Ground(), SimTK::Transform(hipInGround), 
        SimTK::Body::Rigid(MassProperties(femurMass, femurCOM, femurInertiaAboutCOM.shiftFromMassCenter(femurCOM, femurMass))), Transform(hipInFemur));
    // Pin knee connects shank
    MobilizedBody::Pin shank(thigh, Transform(kneeInFemur), SimTK::Body::Rigid(tibiaMass), Transform(kneeInTibia));
    // Pin ankle connects foot
    MobilizedBody::Pin foot(shank, Transform(ankleInTibia), SimTK::Body::Rigid(footMass), Transform(ankleInFoot));

    SimTK::Constraint::Weld weld(matter.Ground(), Transform(weldInGround), foot, Transform(weldInFoot));

    // Simbody model state setup
    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    //==========================================================================================================
    // Setup OpenSim model
    Model *osimModel = new Model;
    //OpenSim bodies
    const Ground& ground = osimModel->getGround();;

    //OpenSim thigh
    OpenSim::Body osim_thigh("thigh", femurMass, femurCOM, femurInertiaAboutCOM);

    // create Pin hip joint
    PinJoint hip("hip", ground, hipInGround, Vec3(0), 
                        osim_thigh, hipInFemur, Vec3(0));

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

    //OpenSim shank
    OpenSim::Body osim_shank("shank", tibiaMass.getMass(),
        tibiaMass.getMassCenter(), tibiaMass.getInertia());

    // create Pin knee joint
    PinJoint knee("knee", osim_thigh, kneeInFemur, Vec3(0), 
                          osim_shank, kneeInTibia, Vec3(0));

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

    //OpenSim foot
    OpenSim::Body osim_foot("foot", footMass.getMass(), 
        footMass.getMassCenter(), footMass.getInertia());

    // create Pin ankle joint
    PinJoint ankle("ankle", osim_shank, ankleInTibia, Vec3(0),
                             osim_foot, ankleInFoot, Vec3(0));

    // Add the foot body which now also contains the hip joint to the model
    osimModel->addBody(&osim_foot);
    osimModel->addJoint(&ankle);

    // add a point on line constraint
    WeldConstraint footConstraint( "footConstraint", 
                                   ground, SimTK::Transform(weldInGround), 
                                   osim_foot, SimTK::Transform(weldInFoot) );
    osimModel->addConstraint(&footConstraint);

    // BAD: but if model maintains ownership, it will attempt to delete stack allocated objects
    osimModel->disownAllComponents();

    osimModel->setGravity(gravity_vec);

    //Add analyses before setting up the model for simulation
    Kinematics *kinAnalysis = new Kinematics(osimModel);
    kinAnalysis->setInDegrees(false);
    osimModel->addAnalysis(kinAnalysis);

    osimModel->setup();
    osimModel->print("testWeldConstraint.osim");

    // Need to setup model before adding an analysis since it creates the AnalysisSet
    // for the model if it does not exist.
    State& osim_state = osimModel->initSystem();

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

void testConstantDistanceConstraint()
{
    using namespace SimTK;

    cout << endl;
    cout << "==================================================================" << endl;
    cout << " OpenSim ConstantDistanceConstraint vs. Simbody Constraint::Rod " << endl;
    cout << "==================================================================" << endl;

    Random::Uniform randomLocation(-1, 1);
    Vec3 pointOnFoot(randomLocation.getValue(), randomLocation.getValue(), randomLocation.getValue());
    Vec3 pointOnGround(0,0,0);
    /** for some reason, adding another Random::Uniform causes testWeldConstraint to fail.  
    Why doesn't it cause this test to fail???? */
    //Random::Uniform randomLength(0.01, 0.2);
    //randomLength.setSeed(1024);
    //double rodLength = randomLength.getValue(); 
    double rodLength = 0.05;

    //std::cout << "Random Length = " << rodLength2 << ", used length = " << rodLength << std::endl;

    // 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 foot and ground
    MobilizedBody::Free foot(matter.Ground(), Transform(Vec3(0)), 
        SimTK::Body::Rigid(footMass), Transform(Vec3(0)));
    
    // Constrain foot to point on ground
    SimTK::Constraint::Rod simtkRod(matter.Ground(), pointOnGround, foot, pointOnFoot, rodLength);


    // Simbody model state setup
    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    //==========================================================================================================
    // Setup OpenSim model
    Model *osimModel = new Model;
    //OpenSim bodies
    const Ground& ground = osimModel->getGround();;

    //OpenSim foot
    OpenSim::Body osim_foot("foot", footMass.getMass(), footMass.getMassCenter(), footMass.getInertia());

    // create foot as a free joint
    FreeJoint footJoint("footToGround", ground, Vec3(0), Vec3(0), osim_foot, Vec3(0), Vec3(0));
    
    // Add the thigh body which now also contains the hip joint to the model
    osimModel->addBody(&osim_foot);
    osimModel->addJoint(&footJoint);

    // add a constant distance constraint
    ConstantDistanceConstraint rodConstraint(ground, pointOnGround, osim_foot, pointOnFoot,rodLength);
    osimModel->addConstraint(&rodConstraint);

    // BAD: have to set memoryOwner to false or program will crash when this test is complete.
    osimModel->disownAllComponents();

    osimModel->setGravity(gravity_vec);

    //Add analyses before setting up the model for simulation
    Kinematics *kinAnalysis = new Kinematics(osimModel);
    kinAnalysis->setInDegrees(false);
    osimModel->addAnalysis(kinAnalysis);

    // Need to setup model before adding an analysis since it creates the AnalysisSet
    // for the model if it does not exist.
    State& osim_state = osimModel->initSystem();

    //==========================================================================================================
    // Compare Simbody system and OpenSim model simulations
    compareSimulations(system, state, osimModel, osim_state, "testConstantDistanceConstraint FAILED\n");
}

int main(int argc, char** argv) {
  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem         mbs; mbs.setUseUniformBackground(true);

    SimbodyMatterSubsystem  crankRocker(mbs);
    GeneralForceSubsystem   forces(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity   gravity(forces, crankRocker, Vec3(0, -g, 0));

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid crankBody  = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
    crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
    sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
                                .setColor(Blue)
                                .setResolution(10)
                                .setOpacity(.2));
    Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
    longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));

    MobilizedBody::Pin
        crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
    MobilizedBody::Pin
        slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));

    MobilizedBody::Universal
        rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
                  longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));

    Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
    ball.setDefaultRadius(0.01);

    //Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
    //Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
    //Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
    //Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
    //Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);

    // Add visualization line (orange=spring, black=constraint)
    //viz.addRubberBandLine(crank, Vec3(0,-3,0),
     //                     slider, Vec3(0,-5,0),
       //                   DecorativeLine().setColor(Black).setLineThickness(4));

    //forces.addMobilityConstantForce(leftPendulum, 0, 20);
    //forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
    //forces.addGlobalEnergyDrain(1);

    Force::MobilityConstantForce(forces, crank, 0, 1);
    Force::MobilityLinearDamper(forces, crank, 0, 1.0);


    //Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
    //Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
    //lmot.setAxis(1, Vec3(d,e,f));
    //Motion::Orientation(someBall, orientFuncOfT);
    //someBall.prescribeOrientation(orientFunc);
    //Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver

    // Like this, or should this be an Instance-stage mode of every mobilizer?
    //Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
    //Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
    //Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
    // or ...
    //crank.lock(state);
    //crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);

    // need a way to register a mobilizer with a particular relaxation solver,
    // switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
    // what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?


    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System

    //crankRate.setRate(s, 3);
    crank.setAngle(s, 5); //q
    crank.setRate(s, 3);  //u

    Visualizer display(mbs);

    mbs.realize(s, Stage::Position);
    display.report(s);
    cout << "q=" << s.getQ() << endl;
    cout << "qErr=" << s.getQErr() << endl;

    crank.setAngle(s, -30*Deg2Rad);
    //crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));


    //TODO
    //rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));

    //crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));

    mbs.realize(s, Stage::Velocity);
//.........这里部分代码省略.........

void main_simulation()
#endif
{
    // inputs
    double fitness;

    #ifdef OPTI
    double *optiParams;
    #endif

    Loop_inputs *loop_inputs;

    // initialization
	loop_inputs = init_simulation();

    // optimization init
    #ifdef OPTI

    optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));

    get_real_params_to_opt(optiNorms, optiParams);

    erase_for_opti(optiParams, loop_inputs->MBSdata);

    free(optiParams);

    #endif
    
    // -- Simbody -- //

    #ifdef SIMBODY

    // / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
    MultibodySystem           system;
    SimbodyMatterSubsystem    matter(system);
    ContactTrackerSubsystem   tracker(system); 
    CompliantContactSubsystem contactForces(system, tracker);
    system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X


    SimbodyVariables simbodyVariables;

    // set all the mechanical parameters of the contact
    
    simbodyVariables.p_system        = &system;
    simbodyVariables.p_matter        = &matter;
    simbodyVariables.p_tracker       = &tracker;
    simbodyVariables.p_contactForces = &contactForces;

    //  cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
    
    //init_Simbody(&simbodyVariables);

    init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
    
    //it is "system" commands. We cannot avoid them.    
    system.realizeTopology();
    State state = system.getDefaultState();

    simbodyVariables.p_state = &state;

    //it is "system" command. We cannot avoid them. 
    system.realizeModel(state);

    p_simbodyVariables = &simbodyVariables;

    #endif

	// loop
    fitness = loop_simulation(loop_inputs);

    // end of the simulation
    finish_simulation(loop_inputs);

    #ifdef OPTI

    return fitness;

    #else

    #if defined(PRINT_REPORT)
    printf("fitness: %f\n", fitness);
    #endif

    #endif
}

int main() {
  try
  { // Create the system.

    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity   gravity(forces, matter, UnitVec3(-1,0,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-2); // m/s

    // Ground's normal is +x for this model
    system.setUpDirection(+XAxis);

    // Uncomment this if you want a more elegant movie.
    //matter.setShowDefaultGeometry(false);

    const Real ud = .3; // dynamic
    const Real us = .6; // static
    const Real uv = 0;  // viscous (force/velocity)
    const Real k = 1e8; // pascals
    const Real c = 0.01; // dissipation (1/v)


    // Halfspace default is +x, this one occupies -x instead, so flip.
    const Rotation R_xdown(Pi,ZAxis);

    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(k,c,us,ud,uv)));


    const Real ellipsoidMass = 1; // kg
    const Vec3 halfDims(2*Cm2m, 20*Cm2m, 3*Cm2m); // m (read in cm)
    const Vec3 comLoc(-1*Cm2m, 0, 0);
    const Inertia centralInertia(Vec3(17,2,16)*CmSq2mSq, Vec3(0,0,.2)*CmSq2mSq); // now kg-m^2
    const Inertia inertia(centralInertia.shiftFromMassCenter(-comLoc, ellipsoidMass)); // in S
    Body::Rigid ellipsoidBody(MassProperties(ellipsoidMass, comLoc, inertia));

    ellipsoidBody.addDecoration(Transform(),
        DecorativeEllipsoid(halfDims).setColor(Cyan)
         //.setOpacity(.5)
         .setResolution(3));
    ellipsoidBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Ellipsoid(halfDims),
                       ContactMaterial(k,c,us,ud,uv))
                       );
    MobilizedBody::Free ellipsoid(matter.Ground(), Transform(Vec3(0,0,0)),
        ellipsoidBody, Transform(Vec3(0)));


    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setMode(Visualizer::RealTime);
    viz.setDesiredFrameRate(FrameRate);
    viz.setCameraClippingPlanes(0.1, 10);

    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Go", GoItem));
    runMenuItems.push_back(std::make_pair("Replay", ReplayItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", RunMenuId, runMenuItems);

    Array_<std::pair<String,int> > helpMenuItems;
    helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1));
    viz.addMenu("Help", HelpMenuId, helpMenuItems);

    system.addEventReporter(new MyReporter(system,contactForces,ReportInterval));
    system.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));

    // Check for a Run->Quit menu pick every 1/4 second.
    system.addEventHandler(new UserInputHandler(*silo, .25));

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();
    matter.setUseEulerAngles(state, true);
    system.realizeModel(state);

    ellipsoid.setQToFitTransform(state, Transform(
        Rotation(BodyRotationSequence,  0  *Deg2Rad, XAxis,
                                        0.5*Deg2Rad, YAxis,
                                       -0.5*Deg2Rad, ZAxis),
        Vec3(2.1*Cm2m, 0, 0)));

    ellipsoid.setUToFitAngularVelocity(state, 2*Vec3(5,0,0)); // rad/s

    viz.report(state);
    printf("Default state\n");

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.

    MultibodySystem         mbs; mbs.setUseUniformBackground(true);
    GeneralForceSubsystem    forces(mbs);
    SimbodyMatterSubsystem  pend(mbs);
    DecorationSubsystem     viz(mbs);
    Force::UniformGravity gravity(forces, pend, Vec3(0, -g, 0));

    MobilizedBody::Ball connector(pend.Ground(), 
                                    Transform(1*Vec3(0, 0, 0)),
                                  MassProperties(1, Vec3(0,0,0), Inertia(10,20,30)),
                                    Transform(1*Vec3(0, .5, 0)));

    connector.setDefaultRadius(0.05); // for the artwork

    //connector.setDefaultRotation( Rotation(Pi/4, Vec3(0,0,1) );
 
    const Real m1 = 5;
    const Real m2 = 1;
    const Real radiusRatio = std::pow(m2/m1, 1./3.);
    const Vec3 weight1Location(0, 0, -d/2); // in local frame of swinging body
    const Vec3 weight2Location(0, 0,  d/2); // in local frame of swinging body
    const Vec3 COM = (m1*weight1Location+m2*weight2Location)/(m1+m2);

    const MassProperties swingerMassProps
        (m1+m2, COM, 1*Inertia(1,1,1) + m1*UnitInertia::pointMassAt(weight1Location)
                                      + m2*UnitInertia::pointMassAt(weight2Location));
    MobilizedBody::Screw swinger(connector, 
                                    Transform( Rotation( 0*.7, Vec3(9,8,7) ),
                                              1*Vec3(0,-.5,0)),
                                 swingerMassProps,
                                    Transform( Rotation(0*1.3, Vec3(0,0,1) ),
                                              COM+0*Vec3(0,0,3)),    // inboard joint location
                                 0.3); // pitch

    // Add a blue sphere around the weight.
    viz.addBodyFixedDecoration(swinger, weight1Location, 
          DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
    viz.addBodyFixedDecoration(swinger, weight2Location, 
          DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
    viz.addRubberBandLine(GroundIndex, Vec3(0),
                          swinger, Vec3(0),
                          DecorativeLine().setColor(Blue).setLineThickness(10)
                                          .setRepresentation(DecorativeGeometry::DrawPoints));

    //forces.addMobilityConstantForce(swinger, 0, 10);
    Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
    //forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
    //forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
    //forces.addMobilityConstantForce(swinger, 1, 10);
   // forces.addMobilityConstantForce(swinger, 2, 60-1.2);

    State s = mbs.realizeTopology(); // define appropriate states for this System


    pend.setUseEulerAngles(s, true);
    mbs.realizeModel(s);

    mbs.realize(s);

    // Create a study using the Runge Kutta Merson integrator
    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-6);

    // This will pick up decorative geometry from
    // each subsystem that generates any, including of course the 
    // DecorationSubsystem, but not limited to it.
    Visualizer display(mbs);


    const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
    printf("Expected period: %g seconds\n", expectedPeriod);

    const Real dt = 1./30; // output intervals
    const Real finalTime = 1*expectedPeriod;

    for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
        s = mbs.getDefaultState();
        connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );

        printf("time  theta      energy           *************\n");

        swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
        swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));

        s.updTime() = 0;
        myStudy.initialize(s);

        cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
        cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
        cout << "Spatial Inertia    =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());

        int stepNum = 0;
        for (;;) {
            // Should check for errors and other interesting status returns.
            myStudy.stepTo(myStudy.getTime() + dt);
            const State& s = myStudy.getState(); // s is now the integrator's internal state

            if ((stepNum++%10)==0) {
//.........这里部分代码省略.........

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]];
//.........这里部分代码省略.........