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

int main() {
    
    // Create the system.
    
    MultibodySystem system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
    Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));

    MobilizedBody lastBody = matter.Ground();
    for (int i = 0; i < 10; ++i) {
        MobilizedBody::Ball pendulum(lastBody,     Transform(Vec3(0)), 
                                     pendulumBody, Transform(Vec3(0, 1, 0)));
        lastBody = pendulum;
    }

    system.addEventReporter(new Visualizer::Reporter(system, 1./30));
    
    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();
    Random::Gaussian random;
    for (int i = 0; i < state.getNQ(); ++i)
        state.updQ()[i] = random.getValue();
    
    // Simulate it.

    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10.0);
}

int main() {
    try { // catch errors if any

    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system; 
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, 9.8);

    // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
    Real mass = 3; Vec3 pos(0,-3,0);
    Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
    bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));

    // Create the tree of mobilized bodies, reusing the above body description.
    MobilizedBody::Pin bodyT  (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
    MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0),    bodyInfo, Vec3(0));
    MobilizedBody::Pin rtArm  (bodyT, Vec3(2, 0, 0),     bodyInfo, Vec3(0));

    // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
    Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0), 
        10000,  // stiffness
        0.5,    // dissipation coefficient
        0*Pi,   // lower stop
        Pi/5);  // upper stop

    // Ask for visualization every 1/30 second.
    system.setUseUniformBackground(true); // turn off floor    
    system.addEventReporter(new Visualizer::Reporter(system, 1./30));
    
    // Initialize the system and state.    
    State state = system.realizeTopology();
    leftArm.setAngle(state, Pi/5);

    // Simulate for 10 seconds.
    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10);

    } catch (const std::exception& e) {
        std::cout << "ERROR: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}

int main() {
  try
  { // Create the system.
    
    MultibodySystem         system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::UniformGravity   gravity(forces, matter, Vec3(0, -9.8, 0));

    Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
    MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)), 
                                pendulumBody,    Transform(Vec3(0, 1, 0)));
    //Motion::Steady(pendulum, 1);

    Visualizer viz(system);
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
   
    // Initialize the system and state.
    
    system.realizeTopology();
    State state = system.getDefaultState();
    pendulum.setOneU(state, 0, 1.0);

    
    // Simulate it.

    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(100.0);

  } catch (const std::exception& e) {
    std::printf("EXCEPTION THROWN: %s\n", e.what());
    exit(1);

  } catch (...) {
    std::printf("UNKNOWN EXCEPTION THROWN\n");
    exit(1);
  }

    return 0;
}

int main() {
  try {   
    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);

    // Hint to Visualizer: don't show ground plane.
    system.setUseUniformBackground(true);

    // Add gravity as a force element.
    Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));

    // Create the body and some artwork for it.
    const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
    const Real mass = 2; // total mass of brick (kg)
    Body::Rigid pendulumBody(MassProperties(mass, Vec3(0), 
                                mass*UnitInertia::brick(halfLengths)));
    pendulumBody.addDecoration(Transform(), 
        DecorativeBrick(halfLengths).setColor(Red));

    // Add an instance of the body to the multibody system by connecting
    // it to Ground via a Ball mobilizer.
    MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)), 
                                  pendulumBody,       Transform(Vec3( 0, 1, 0)));

    // Add a second instance of the pendulum nearby.
    MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)), 
                                  pendulumBody,       Transform(Vec3(0, 1, 0)));

    // Connect the origins of the two pendulum bodies together with our
    // rod-like custom constraint.
    const Real d = 1.5; // desired separation distance
    Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));

    // Visualize with default options.
    Visualizer viz(system);

    // Add a rubber band line connecting the origins of the two bodies to
    // represent the rod constraint.
    viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
        DecorativeLine().setColor(Blue).setLineThickness(3));

    // Ask for a report every 1/30 of a second to match the Visualizer's 
    // default rate of 30 frames per second.
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    // Output total energy to the console once per second.
    system.addEventReporter(new TextDataEventReporter
                                   (system, new MyEvaluateEnergy(), 1.0));
    
    // Initialize the system and state.   
    State state = system.realizeTopology();

    // Orient the two pendulums asymmetrically so they'll do something more 
    // interesting than just hang there.
    pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
    pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
                                                Pi/4, ZAxis, Pi/4, YAxis));

    // Evaluate the system at the new state and draw one frame manually.
    system.realize(state);
    viz.report(state);

    // Simulate it.
    cout << "Hit ENTER to run a short simulation.\n";
    cout << "(Energy should be conserved to about four decimal places.)\n";
    getchar();

    RungeKuttaMersonIntegrator integ(system);
    integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(10.0);

  } catch (const std::exception& e) {
      std::cout << "ERROR: " << e.what() << std::endl;
      return 1;
  } catch (...) {
      std::cout << "UNKNOWN EXCEPTION\n";
      return 1;
  }

    return 0;
}

int main() {
  try
  { std::cout << "Current working directory: " 
              << Pathname::getCurrentWorkingDirectory() << std::endl;

    // Create the system.
    
    MultibodySystem         system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::UniformGravity   gravity(forces, matter, 0*Vec3(2, -9.8, 0));

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);

    GeneralContactSubsystem OLDcontact(system);
    const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();

    contactForces.setTransitionVelocity(1e-3);

    std::ifstream meshFile1, meshFile2;
    PolygonalMesh femurMesh; 
    meshFile1.open("ContactBigMeshes_Femur.obj"); 
    femurMesh.loadObjFile(meshFile1); meshFile1.close();
    PolygonalMesh patellaMesh; 
    meshFile2.open("ContactBigMeshes_Patella.obj"); 
    patellaMesh.loadObjFile(meshFile2); meshFile2.close();

    ContactGeometry::TriangleMesh femurTri(femurMesh);
    ContactGeometry::TriangleMesh patellaTri(patellaMesh);

    DecorativeMesh showFemur(femurTri.createPolygonalMesh());
    Array_<DecorativeLine> femurNormals;
    const Real NormalLength = .02;
    //for (int fx=0; fx < femurTri.getNumFaces(); ++fx)
    //    femurNormals.push_back(
    //    DecorativeLine(femurTri.findCentroid(fx),
    //                   femurTri.findCentroid(fx)
    //                       + NormalLength*femurTri.getFaceNormal(fx)));

    DecorativeMesh showPatella(patellaTri.createPolygonalMesh());
    Array_<DecorativeLine> patellaNormals;
    //for (int fx=0; fx < patellaTri.getNumFaces(); ++fx)
    //    patellaNormals.push_back(
    //    DecorativeLine(patellaTri.findCentroid(fx),
    //                   patellaTri.findCentroid(fx)
    //                       + NormalLength*patellaTri.getFaceNormal(fx)));

    // This transform has the meshes close enough that their OBBs overlap
    // but in the end none of the faces are touching.
    const Transform X_FP(
        Rotation(Mat33( 0.97107625831404454, 0.23876955530133021, 0,
                       -0.23876955530133021, 0.97107625831404454, 0,
                        0,                   0,                   1), true),
        Vec3(0.057400580865008571, 0.43859170879135373, 
             -0.00016506240185135300)
        );


    const Real fFac =1; // to turn off friction
    const Real fDis = .5*0.2; // to turn off dissipation
    const Real fVis =  .1*.1; // to turn off viscous friction
    const Real fK = 100*1e6; // pascals

    // Put femur on ground at origin
    matter.Ground().updBody().addDecoration(Vec3(0,0,0),
        showFemur.setColor(Cyan).setOpacity(.2));
    matter.Ground().updBody().addContactSurface(Vec3(0,0,0),
        ContactSurface(femurTri,
            ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10),
            .01 /*thickness*/));


    Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
    patellaBody.addDecoration(Transform(), 
        showPatella.setColor(Red).setOpacity(.2));
    patellaBody.addContactSurface(Transform(),
        ContactSurface(patellaTri,
            ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
            .01 /*thickness*/));

    MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)), 
                                patellaBody,    Transform(Vec3(0)));


    //// The old way ...
    //OLDcontact.addBody(OLDcontactSet, ball,
    //    pyramid, Transform());

    //OLDcontact.addBody(OLDcontactSet, matter.updGround(),
    //    ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
    //ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
    //Real stiffness = 1e6, dissipation = 0.01, us = 0.1, 
    //    ud = 0.05, uv = 0.01, vt = 0.01;
    ////Real stiffness = 1e6, dissipation = 0.1, us = 0.8, 
    ////    ud = 0.7, uv = 0.01, vt = 0.01;

    //ef.setBodyParameters(ContactSurfaceIndex(0), 
    //    stiffness, dissipation, us, ud, uv);
//.........这里部分代码省略.........

int main() {
    try { // catch errors if any

    // Create the system, with subsystems for the bodies and some forces.
    MultibodySystem system; 
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, 9.8);
    system.setUseUniformBackground(true); // request no ground & sky    

    // Describe a body with a point mass at (0, -3, 0) and draw a sphere there.
    Real mass = 3; Vec3 pos(0,-3,0);
    Body::Rigid bodyInfo(MassProperties(mass, pos, UnitInertia::pointMassAt(pos)));
    bodyInfo.addDecoration(pos, DecorativeSphere(.2).setOpacity(.5));

    // Create the tree of mobilized bodies, reusing the above body description.
    MobilizedBody::Pin bodyT  (matter.Ground(), Vec3(0), bodyInfo, Vec3(0));
    MobilizedBody::Pin leftArm(bodyT, Vec3(-2, 0, 0),    bodyInfo, Vec3(0));
    MobilizedBody::Pin rtArm  (bodyT, Vec3(2, 0, 0),     bodyInfo, Vec3(0,-1,0));

    // Add some damping.
    Force::MobilityLinearDamper damper1(forces, bodyT, MobilizerUIndex(0), 10);
    Force::MobilityLinearDamper damper2(forces, leftArm, MobilizerUIndex(0), 30);
    Force::MobilityLinearDamper damper3(forces, rtArm, MobilizerUIndex(0), 10);

#ifdef USE_TORQUE_LIMITED_MOTOR
    MyTorqueLimitedMotor* motorp = 
        new MyTorqueLimitedMotor(rtArm, MobilizerUIndex(0), TorqueGain, MaxTorque);
    const MyTorqueLimitedMotor& motor = *motorp;
    Force::Custom(forces, motorp); // takes over ownership
#else
    // Use built-in Steady Motion as a low-budget motor model.
    //Motion::Steady motor(rtArm, InitialMotorRate);
    // Use built-in ConstantSpeed constraint as a low-budget motor model.
    Constraint::ConstantSpeed motor(rtArm, InitialMotorRate);
#endif

    // Add a joint stop to the left arm restricting it to q in [0,Pi/5].
    Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0), 
        StopStiffness,
        InitialDissipation,
        -Pi/8,   // lower stop
         Pi/8);  // upper stop

    Visualizer viz(system);

    // Add sliders.
    viz.addSlider("Motor speed", SliderIdMotorSpeed, -10, 10, InitialMotorRate);
    viz.addSlider("Dissipation", SliderIdDissipation, 0, 10, InitialDissipation);
    viz.addSlider("Tach", SliderIdTach, -20, 20, 0);
    viz.addSlider("Torque", SliderIdTorque, -MaxTorque, MaxTorque, 0);

    // Add Run menu.
    Array_<std::pair<String,int> > runMenuItems;
    runMenuItems.push_back(std::make_pair("Reset", ResetItem));
    runMenuItems.push_back(std::make_pair("Quit", QuitItem));
    viz.addMenu("Run", MenuIdRun, runMenuItems);

    Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
    viz.addInputListener(userInput);
    
    // Initialize the system and state.    
    State initState = system.realizeTopology();

    // Simulate forever with a small max step size. Check for user input
    // in between steps. Note: an alternate way to do this is to let the
    // integrator take whatever steps it wants but use a TimeStepper to 
    // manage a periodic event handler to poll for user input. Here we're 
    // treating completion of a step as an event.
    const Real MaxStepSize = 0.01*3; // 10ms
    const int  DrawEveryN = 3/3;     // 3 steps = 30ms
    //RungeKuttaMersonIntegrator integ(system);
    //RungeKutta2Integrator integ(system);
    SemiExplicitEuler2Integrator integ(system);
    //SemiExplicitEulerIntegrator integ(system, .001);

    integ.setAccuracy(1e-1);
    //integ.setAccuracy(1e-3);

    // Don't permit interpolation because we want the state returned after
    // a step to be modifiable.
    integ.setAllowInterpolation(false);

    integ.initialize(initState);

    int stepsSinceViz = DrawEveryN-1;
    while (true) {
        if (++stepsSinceViz % DrawEveryN == 0) {
            const State& s = integ.getState();
            viz.report(s);
            const Real uActual = rtArm.getOneU(s, MobilizerUIndex(0));
            viz.setSliderValue(SliderIdTach, uActual);
#ifdef USE_TORQUE_LIMITED_MOTOR
            viz.setSliderValue(SliderIdTorque, motor.getTorque(s));
#else
            system.realize(s); // taus are acceleration stage
            //viz.setSliderValue(SliderIdTorque, 
            //                   rtArm.getOneTau(s, MobilizerUIndex(0)));
            viz.setSliderValue(SliderIdTorque, motor.getMultiplier(s));
#endif
//.........这里部分代码省略.........

int main() {
  try {    
    // Create the system.   
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);

    matter.setShowDefaultGeometry(false);

    CableTrackerSubsystem cables(system);
    GeneralForceSubsystem forces(system);

    Force::Gravity gravity(forces, matter, -YAxis, 9.81);
    // Force::GlobalDamper(forces, matter, 5);

    system.setUseUniformBackground(true);    // no ground plane in display
    MobilizedBody Ground = matter.Ground(); // convenient abbreviation

    // Read in some bones.
    PolygonalMesh femur;
    PolygonalMesh tibia;

    femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
    tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
    femur.scaleMesh(30);
    tibia.scaleMesh(30);

    // Build a pendulum
    Body::Rigid pendulumBodyFemur(    MassProperties(1.0, Vec3(0, -5, 0), 
                                    UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));

    pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8)));

    Body::Rigid pendulumBodyTibia(    MassProperties(1.0, Vec3(0, -5, 0), 
                                    UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));

    pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8)));

    Rotation z180(Pi, YAxis);

    MobilizedBody::Pin pendulumFemur(    matter.updGround(),
                                        Transform(Vec3(0, 0, 0)),
                                        pendulumBodyFemur,
                                        Transform(Vec3(0, 0, 0)) );

    Rotation rotZ45(-Pi/4, ZAxis);

    MobilizedBody::Pin pendulumTibia(   pendulumFemur,
                                        Transform(rotZ45, Vec3(0, -12, 0)),
                                        pendulumBodyTibia,
                                        Transform(Vec3(0, 0, 0)) );

    Real initialPendulumOffset = -0.25*Pi;

    Constraint::PrescribedMotion pres(matter, 
       new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0));
               
    // Build a wrapping cable path
    CablePath path2(cables, Ground, Vec3(1, 3, 1),             // origin
                            pendulumTibia, Vec3(1, -4, 0));  // termination
    
    // Create a bicubic surface
    Vec3 patchOffset(0, -5, -1);
    Rotation rotZ90(0.5*Pi, ZAxis);
    Rotation rotX90(0.2*Pi, XAxis);

    Rotation patchRotation = rotZ90 * rotX90 * rotZ90;
    Transform patchTransform(patchRotation, patchOffset);

    Real patchScaleX = 2.0;
    Real patchScaleY = 2.0;
    Real patchScaleF = 0.75;

    const int Nx = 4, Ny = 4;
  
    const Real xData[Nx] = {  -2, -1, 1, 2 };
    const Real yData[Ny] = {  -2, -1, 1, 2 };

    const Real fData[Nx*Ny] = { 2,        3,        3,        1,
                                0,         1.5,  1.5,        0,
                                0,        1.5,  1.5,        0,
                                2,        3,        3,        1    };

    const Vector x_(Nx,        xData);
    const Vector y_(Ny,     yData);
    const Matrix f_(Nx, Ny, fData);

    Vector x = patchScaleX*x_;
    Vector y = patchScaleY*y_;
    Matrix f = patchScaleF*f_; 

    BicubicSurface patch(x, y, f, 0);

    Real highRes = 30;
    Real lowRes  = 1;

    PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
    PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);

   
    pendulumFemur.addBodyDecoration(patchTransform,
//.........这里部分代码省略.........

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

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

//.........这里部分代码省略.........
    // spatial rotation about Z.
    Vector coeff(2);
    // Linear function x3 = coeff[0]*q + coeff[1]
    coeff[0] = 1;  coeff[1] = 0;
    // |--------------------------------|
    // | 3rd function: rotation about Z |
    // |--------------------------------|
    functions[2] = new Function::Linear(coeff);
    // function of coordinate 0 (knee extension angle)
    std::vector<int> qIndex(1,0);
    coordIndices[2] = qIndex;

    // Set the spatial translations as a function (splines) along the parent X and Y axes
    // |-----------------------------------|
    // | 4th function: translation about X |
    // |-----------------------------------|
    functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
    coordIndices[3] =(qIndex);

    // |-----------------------------------|
    // | 5th function: translation about Y |
    // |-----------------------------------|
    functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
    coordIndices[4] =(qIndex);

    // |-----------------------------------|
    // | 6th function: translation about Z |
    // |-----------------------------------|
    functions[5] = (new Function::Constant(0, 0));
    coordIndices[5] = (noIndex);

    // Construct the multibody system
    const Real grav = 9.80665;
    MultibodySystem system; system.setUseUniformBackground(true);
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::Gravity gravity(forces, matter, -YAxis, grav);
    matter.setShowDefaultGeometry(true);

    //--------------------------------------------------------------------------
    // Define the model's physical (body) properties
    //--------------------------------------------------------------------------
    //Thigh
    Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
    femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)), 
        DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));

    //Shank
    Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
    tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)), 
        DecorativeCylinder(0.035, 0.235).setColor(Red));

    //--------------------------------------------------------------------------
    // Build the multibody system by adding mobilized bodies to the matter subsystem
    //--------------------------------------------------------------------------
    // Add the thigh via hip joint
    MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));

    // This is how you might model the knee if you could only use a pin joint.
    //MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)), 
    //                         tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
    
    // NOTE: function of Y-translation data was defined int the femur frame 
    // according to Yamaguchi and Zajac, which had the orgin at the hip joint 
    // center and the Y along the long-axis of the femur and Z out of the page. 
    MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)),