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

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

void testConstraintForces() {
    // Weld one body to ground, push on it, verify that it reacts to match the load.
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));

    MobilizedBody::Weld welded(matter.Ground(), Transform(),
                               body, Transform());

    MobilizedBody::Free loose(matter.Ground(), Transform(),
                               body, Transform());
    Constraint::Weld glue(matter.Ground(), Transform(),
                          loose, Transform());

    // Apply forces to the body welded straight to ground.
    Force::ConstantForce(forces, welded, Vec3(0,0,0), Vec3(1,2,3));
    Force::ConstantTorque(forces, welded, Vec3(20,30,40));

    // Apply the same forces to the "free" body which is welded by constraint.
    Force::ConstantForce(forces, loose, Vec3(0,0,0), Vec3(1,2,3));
    Force::ConstantTorque(forces, loose, Vec3(20,30,40));

    State state = system.realizeTopology();
    system.realize(state, Stage::Acceleration);

    Vector_<SpatialVec> mobilizerReactions;
    matter.calcMobilizerReactionForces(state, mobilizerReactions);

    //NOT IMPLEMENTED YET:
    //cout << "Weld constraint reaction on Ground: " << glue.getWeldReactionOnBody1(state) << endl;
    //cout << "Weld constraint reaction on Body: " << glue.getWeldReactionOnBody2(state) << endl;


    // Note that constraint forces have opposite sign to applied forces, because
    // we calculate the multiplier lambda from M udot + ~G lambda = f_applied. We'll negate
    // the calculated multipliers to turn these into applied forces.
    const Vector mults = -state.getMultipliers();
    Vector_<SpatialVec> constraintForces;
    Vector mobilityForces;
    matter.calcConstraintForcesFromMultipliers(state, mults,
        constraintForces, mobilityForces);

    MACHINE_TEST(constraintForces[loose.getMobilizedBodyIndex()], 
                 mobilizerReactions[welded.getMobilizedBodyIndex()]);

    // This returns just the forces on the weld's two bodies: in this
    // case Ground and "loose", in that order.
    Vector_<SpatialVec> glueForces = 
        glue.getConstrainedBodyForcesAsVector(state);

    MACHINE_TEST(-glueForces[1], // watch the sign!
                 mobilizerReactions[welded.getMobilizedBodyIndex()]);
}

void testCompositeInertia() {
    MultibodySystem         mbs;
    SimbodyMatterSubsystem  pend(mbs);

    Body::Rigid pointMass(MassProperties(3, Vec3(0), Inertia(0)));

    // Point mass at x=1.5 rotating about (0,0,0).
    MobilizedBody::Pin
        body1( pend.Ground(), Transform(), 
               pointMass, Vec3(1.5,0,0));
    const MobilizedBodyIndex body1x = body1.getMobilizedBodyIndex();

    // A second body 2 units further along x, rotating about the
    // first point mass origin.
    MobilizedBody::Pin
        body2( body1, Transform(), 
               pointMass, Vec3(2,0,0));
    const MobilizedBodyIndex body2x = body2.getMobilizedBodyIndex();

    State state = mbs.realizeTopology();
    mbs.realize(state, Stage::Position);

    Array_<SpatialInertia, MobilizedBodyIndex> R(pend.getNumBodies());
    pend.calcCompositeBodyInertias(state, R);

    // Calculate expected inertias about the joint axes.
    Real expInertia2 = body2.getBodyMassProperties(state).getMass()*square(2);
    Real expInertia1 = body1.getBodyMassProperties(state).getMass()*square(1.5)
                           + body2.getBodyMassProperties(state).getMass()*square(3.5);

    // Should be able to recover these inertias by projecting the composite
    // body inertias onto the joint axes using H matrices.
    const SpatialVec H1 = body1.getHCol(state, MobilizerUIndex(0));
    const SpatialVec H2 = body2.getHCol(state, MobilizerUIndex(0));
    SimTK_TEST_EQ(~H2*(R[body2x]*H2), expInertia2);
    SimTK_TEST_EQ(~H1*(R[body1x]*H1), expInertia1);

    // This should force realization of the composite body inertias.
    SpatialInertia cbi = pend.getCompositeBodyInertia(state, body1);

    body2.setAngle(state, Pi/4);
    // This is not allowed until Position stage.
    SimTK_TEST_MUST_THROW(pend.getCompositeBodyInertia(state, body1));
    mbs.realize(state, Stage::Position);
    // Now it should be OK.
    cbi = pend.getCompositeBodyInertia(state, body1);

    mbs.realize(state, Stage::Acceleration);
    //cout << "udots=" << state.getUDot() << endl;

    body1.setRate(state, 27);
    mbs.realize(state, Stage::Acceleration);
    //cout << "udots=" << state.getUDot() << endl;
}

void testCalculationMethods() {

    // Create a system with two bodies.

    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
    MobilizedBody::Free b1(matter.Ground(), body);
    MobilizedBody::Free b2(matter.Ground(), body);

    // Set all the state variables to random values.

    system.realizeTopology();
    State state = system.getDefaultState();
    Random::Gaussian random;

    for (int i = 0; i < state.getNY(); ++i)
        state.updY()[i] = random.getValue();

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

    // Test the low level methods for transforming points and vectors.

    const Vec3 point(0.5, 1, -1.5);
    SimTK_TEST_EQ(b1.findStationLocationInGround(state, Vec3(0)), b1.getBodyOriginLocation(state));
    SimTK_TEST_EQ(b1.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), point);
    SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, point)), b1.findStationLocationInAnotherBody(state, point, b2));
    SimTK_TEST_EQ(b2.findStationAtGroundPoint(state, b1.findStationLocationInGround(state, Vec3(0))).norm(), (b1.getBodyOriginLocation(state)-b2.getBodyOriginLocation(state)).norm());
    SimTK_TEST_EQ(b2.findMassCenterLocationInGround(state), b2.findStationLocationInGround(state, b2.getBodyMassCenterStation(state)));
    SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, Vec3(0)), Vec3(0));
    SimTK_TEST_EQ(b1.expressVectorInGroundFrame(state, point), b1.getBodyRotation(state)*point);
    SimTK_TEST_EQ(b1.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), point);
    SimTK_TEST_EQ(b2.expressGroundVectorInBodyFrame(state, b1.expressVectorInGroundFrame(state, point)), b1.expressVectorInAnotherBodyFrame(state, point, b2));

    // Test the routines for mapping locations, velocities, and accelerations.

    Vec3 r, v, a;
    b1.findStationLocationVelocityAndAccelerationInGround(state, point, r, v, a);
    SimTK_TEST_EQ(v, b1.findStationVelocityInGround(state, point));
    SimTK_TEST_EQ(a, b1.findStationAccelerationInGround(state, point));
    {
        Vec3 r2, v2;
        b1.findStationLocationAndVelocityInGround(state, point, r2, v2);
        SimTK_TEST_EQ(r, r2);
        SimTK_TEST_EQ(v, v2);
    }
    SimTK_TEST_EQ(b1.findStationVelocityInGround(state, Vec3(0)), b1.getBodyOriginVelocity(state));
    SimTK_TEST_EQ(b1.findStationAccelerationInGround(state, Vec3(0)), b1.getBodyOriginAcceleration(state));
    SimTK_TEST_EQ(b1.findStationVelocityInGround(state, point), b1.findStationVelocityInAnotherBody(state, point, matter.Ground()));
}

void createState(MultibodySystem& system, State& state, const Vector& qOverride=Vector()) {
    system.realizeTopology();
    state = system.getDefaultState();
    Random::Uniform random;
    for (int i = 0; i < state.getNY(); ++i)
        state.updY()[i] = random.getValue();
    if (qOverride.size())
        state.updQ() = qOverride;
    system.realize(state, Stage::Velocity);

    Vector dummy;
    system.project(state, ConstraintTol);
    system.realize(state, Stage::Acceleration);
}

void createState(MultibodySystem& system, State& state, const Vector& y=Vector()) {
    system.realizeTopology();
    state = system.getDefaultState();
    if (y.size() > 0)
        state.updY() = y;
    else {
        Random::Uniform random;
        for (int i = 0; i < state.getNY(); ++i)
            state.updY()[i] = random.getValue();
    }
    system.realize(state, Stage::Velocity);

    // Solve to tight tolerance here
    system.project(state, 1e-12);
    system.realize(state, Stage::Acceleration);
}

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;
}

void testWeld() {
    MultibodySystem system;
    SimbodyMatterSubsystem matter(system);
    GeneralForceSubsystem forces(system);
    Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0));
    Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));

    // Create two pendulums, each with two welded bodies.  One uses a Weld MobilizedBody,
    // and the other uses a Weld constraint.

    Transform inboard(Vec3(0.1, 0.5, -1));
    Transform outboard(Vec3(0.2, -0.2, 0));
    MobilizedBody::Ball p1(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
    MobilizedBody::Ball p2(matter.updGround(), Vec3(0), body, Vec3(0, 1, 0));
    MobilizedBody::Weld c1(p1, inboard, body, outboard);
    MobilizedBody::Free c2(p2, inboard, body, outboard);
    Constraint::Weld constraint(p2, inboard, c2, outboard);

    // It is not a general test unless the Weld mobilizer has children!
    MobilizedBody::Pin wchild1(c1, inboard, body, outboard);
    MobilizedBody::Pin wchild2(c2, inboard, body, outboard);
    Force::MobilityLinearSpring(forces, wchild1, 0, 1000, 0);
    Force::MobilityLinearSpring(forces, wchild2, 0, 1000, 0);

    State state = system.realizeTopology();
    p1.setU(state, Vec3(1, 2, 3));
    p2.setU(state, Vec3(1, 2, 3));
    system.realize(state, Stage::Velocity);
    system.project(state, 1e-10);

    SimTK_TEST_EQ(c1.getBodyTransform(state), c2.getBodyTransform(state));
    SimTK_TEST_EQ(c1.getBodyVelocity(state), c2.getBodyVelocity(state));

    // Simulate it and see if both pendulums behave identically.

    RungeKuttaMersonIntegrator integ(system);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    ts.stepTo(5);
    system.realize(integ.getState(), Stage::Velocity);
    SimTK_TEST_EQ_TOL(c1.getBodyTransform(integ.getState()),
                      c2.getBodyTransform(integ.getState()), 1e-10);
    SimTK_TEST_EQ_TOL(c1.getBodyVelocity(integ.getState()),
                      c2.getBodyVelocity(integ.getState()), 1e-10);
}

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;
}

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 testConservationOfEnergy() {
    // Create the system.
    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::UniformGravity   gravity(forces, matter, Vec3(0, -9.8, 0));

    const Real Mass = 1;
    const Vec3 HalfShape = Vec3(1,.5,.25)/2;
    const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
    Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
                                Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
    //Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
    //                        Mass*UnitInertia::ellipsoid(HalfShape)));
    brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
                                            .setOpacity(0.25)
                                            .setColor(Blue));
    brickBody.addDecoration(BodyAttach,
                DecorativeFrame(0.5).setColor(Red));

    const int NBod=50;
    MobilizedBody::Free brick1(matter.Ground(), Transform(),
                               brickBody,       BodyAttach);
    MobilizedBody::Free brick2(brick1, Transform(),
                               brickBody,       BodyAttach);
    MobilizedBody prev=brick2;
    MobilizedBody body25;
    for (int i=0; i<NBod; ++i) {
        MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
                                 brickBody, BodyAttach);
        if (i==25) body25=next;
        //Force::TwoPointLinearSpring(forces,
        //    prev, Vec3(0), next, Vec3(0), 1000, 1);
        prev=next;
    }

    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
        prev, BodyAttach.p());
    Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
        body25, BodyAttach.p());


    Vec6 k1(1,100,1,100,100,100), c1(0);
    Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
                 prev, BodyAttach.p(), k1, c1);
    matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));

    Force::Thermostat thermo(forces, matter,
        SimTK_BOLTZMANN_CONSTANT_MD,
        5000,
        .1);

    Vec6 k(1,100,1,100,100,100), c(0);
    Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
        brick1, BodyAttach, k, c);
    Force::LinearBushing bushing2(forces, brick1, Transform(),
        brick2, BodyAttach, k, c);
    matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
        DecorativeFrame().setColor(Green));


    Visualizer viz(system);
    Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
    viz.setBackgroundType(Visualizer::SolidColor);
    system.addEventReporter(reporter);

    ThermoReporter* thermoReport = new ThermoReporter
        (system, thermo, bushing1, bushing2, 1./10);
    system.addEventReporter(thermoReport);

    // Initialize the system and state.

    system.realizeTopology();
    State state = system.getDefaultState();

    viz.report(state);
    printf("Default state -- hit ENTER\n");
    cout << "t=" << state.getTime()
         << " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
         << " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
         << "\nnChains=" << thermo.getNumChains(state)
         << " T="        << thermo.getBathTemperature(state)
         << "\nt_relax=" << thermo.getRelaxationTime(state)
         << " kB="       << thermo.getBoltzmannsConstant()
         << endl;
    getchar();

    state.setTime(0);
    system.realize(state, Stage::Acceleration);
    Vector initU(state.getNU());
    initU = Test::randVector(state.getNU());
    state.updU()=initU;


    RungeKuttaMersonIntegrator integ(system);
    //integ.setMinimumStepSize(1e-1);
    integ.setAccuracy(1e-2);
    TimeStepper ts(system, integ);
    ts.initialize(state);
//.........这里部分代码省略.........

//==============================================================================
//                                   MAIN
//==============================================================================
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);
    ContactTrackerSubsystem   tracker(system);
    CompliantContactSubsystem contact(system, tracker);
    contact.setTransitionVelocity(transitionVelocity);
    Force::Gravity(forces, matter, -YAxis, 9.81);

    // Set up visualization and ask for a frame every 1/30 second.
    Visualizer viz(system);
    viz.setShowSimTime(true); viz.setShowFrameRate(true);
    viz.addSlider("Speed", SpeedControlSlider, -10, 10, 0);
    Visualizer::InputSilo* silo = new Visualizer::InputSilo();
    viz.addInputListener(silo);
    #ifdef ANIMATE
    system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
    #endif
    DecorativeText help("Any input to start; ESC to quit");
    help.setIsScreenText(true);
    viz.addDecoration(MobilizedBodyIndex(0),Vec3(0),help);
    matter.setShowDefaultGeometry(false);

    // Add the Ground contact geometry. Contact half space has -XAxis normal
    // (right hand wall) so we have to rotate.
    MobilizedBody& Ground = matter.updGround(); // Nicer name for Ground.
    Ground.updBody().addContactSurface(Transform(YtoX,Vec3(0)),
        ContactSurface(ContactGeometry::HalfSpace(),concrete));

    // Add some speed bumps.
    const int NBumps = 2; const Vec3 BumpShape(.8,0.2,2);
    for (int i=0; i < NBumps; ++i) {
        const Real x = -2*(i+1);
        Ground.updBody().addContactSurface(Vec3(x,0,0),
            ContactSurface(ContactGeometry::Ellipsoid(BumpShape), rubber));
        Ground.updBody().addDecoration(Vec3(x,0,0),
            DecorativeEllipsoid(BumpShape).setColor(Gray).setResolution(3));
    }

    // TORSO
    const Real TorsoHeight = 1.1;
    const Vec3 torsoHDims(1,.08,.8);
    const Real torsoVolume = 8*torsoHDims[0]*torsoHDims[1]*torsoHDims[2];
    const Real torsoMass = torsoVolume*rubber_density/10;
    const Vec3 torsoCOM(0,-.75,0); // put it low for stability
    Body::Rigid torsoInfo(MassProperties(torsoMass,torsoCOM,
        UnitInertia::brick(torsoHDims).shiftFromCentroidInPlace(-torsoCOM)));
    torsoInfo.addDecoration(Vec3(0),
        DecorativeBrick(torsoHDims).setColor(Cyan));

    // CRANK
    const Vec3 crankCenter(0,0,0); // in torso frame
    const Vec3 crankOffset(0,0,torsoHDims[2]+LinkDepth); // left/right offset
    const Real MLen=15/100.; // crank radius
    Body::Rigid crankInfo(MassProperties(.1,Vec3(0),
                            UnitInertia::cylinderAlongZ(MLen, LinkDepth)));
    crankInfo.addDecoration(Vec3(0),
        DecorativeBrick(Vec3(LinkWidth,LinkWidth,torsoHDims[2]))
        .setColor(Black));
    const Vec3 CrankConnect(MLen,0,0); // in crank frame

    // Add the torso and crank mobilized bodies.
    MobilizedBody::Free torso(Ground,Vec3(0,TorsoHeight,0), torsoInfo,Vec3(0));
    MobilizedBody::Pin crank(torso,crankCenter, crankInfo, Vec3(0));

    // Add the legs.
    for (int i=-1; i<=1; ++i) {
        const Vec3 offset = crankCenter + i*crankOffset;
        const Vec3 linkSpace(0,0,2*LinkDepth);
        const Rotation R_CP(i*2*Pi/3,ZAxis);
        // Add crank bars for looks.
        crank.addBodyDecoration(
            Transform(R_CP, offset+1.5*MLen/2*R_CP.x()+(i==0?linkSpace:Vec3(0))),
            DecorativeBrick(Vec3(1.5*MLen/2,LinkWidth,LinkDepth))
                        .setColor(Yellow));

        addOneLeg(viz, torso, offset + i*linkSpace,
                  crank, R_CP*CrankConnect);
        addOneLeg(viz, torso, Transform(Rotation(Pi,YAxis), offset + i*linkSpace),
                  crank, R_CP*CrankConnect);
    }

    // Add speed control.
    Motion::Steady motor(crank, 0); // User controls speed.
    system.addEventHandler
       (new UserInputHandler(*silo, motor, Real(0.1))); //check input every 100ms

    // Initialize the system and state.
    State state = system.realizeTopology();
    system.realize(state);
    printf("Theo Jansen Strandbeest in 3D:\n");
    printf("%d bodies, %d mobilities, -%d constraint equations -%d motions\n",
        matter.getNumBodies(), state.getNU(), state.getNMultipliers(),
        matter.getMotionMultipliers(state).size());

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

//.........这里部分代码省略.........
    Geodesic geod;

    // Create a dummy mb system for visualization
    MultibodySystem dummySystem;
    SimbodyMatterSubsystem matter(dummySystem);


//    matter.updGround().addBodyDecoration(Transform(), DecorativeEllipsoid(radii)
    matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
            .setColor(Gray)
            .setOpacity(0.5)
            .setResolution(5));

    matter.updGround().addBodyDecoration(Transform(),
        DecorativeLine(Vec3(newP), Vec3(newP)+.5*tP).setColor(Green));
    matter.updGround().addBodyDecoration(Transform(),
        DecorativeLine(Vec3(newQ), Vec3(newQ)+.5*tQ).setColor(Red));

    // Visualize with default options; ask for a report every 1/30 of a second
    // to match the Visualizer's default 30 frames per second rate.
    Visualizer viz(dummySystem);
    viz.setBackgroundType(Visualizer::SolidColor);

    // add vizualization callbacks for geodesics, contact points, etc.
    Vector tmp(6); // tmp = ~[P Q]
    tmp[0]=P[0]; tmp[1]=P[1]; tmp[2]=P[2]; tmp[3]=Q[0]; tmp[4]=Q[1]; tmp[5]=Q[2];
    viz.addDecorationGenerator(new PathDecorator(tmp, O, I, Green));
    //viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
    viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
    viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
    viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
    //ExtremePointDecorator* expd = new ExtremePointDecorator(geom, P);
    //viz.addDecorationGenerator(expd);
    dummySystem.realizeTopology();
    State dummyState = dummySystem.getDefaultState();

    /* Sherm playing with separation tracking ...
    expd->setStartPoint(Vec3(1,0,0));
    for (int outer=0; ; ++outer) {
    for (int i=0; i <10; ++i) {
        Real x = i*.2;
        expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,1));
        if (outer) expd->setStartPoint(expd->getStartPoint()-Vec3(.1,0,0));
        expd->setShowStartFrameOnly(true);
        viz.report(dummyState);
        if (outer) getchar();
        viz.report(dummyState);
        if (outer) getchar(); else sleepInSec(.25);
        //sleepInSec(.5);
    }
    for (int i=0; i <10; ++i) {
        Real z = 1+i*.2;
        Real x = 2-i*.2;
        expd->moveLine(Vec3(x,-3,-2), Vec3(0,3,z));
        expd->setShowStartFrameOnly(true);
        viz.report(dummyState);
        viz.report(dummyState); sleepInSec(.25);
        //sleepInSec(.5);
    }
    for (int i=0; i <10; ++i) {
        Real z = 3-i*.5;
        expd->moveLine(Vec3(0,-3,-2), Vec3(0,3,z));
        expd->setShowStartFrameOnly(true);
        viz.report(dummyState);
        viz.report(dummyState); sleepInSec(.25);
        //sleepInSec(.5);

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

int main(int argc, char** argv) {
    static const Transform GroundFrame;
    static const Rotation ZUp(UnitVec3(XAxis), XAxis, UnitVec3(YAxis), ZAxis);
    static const Vec3 TestLoc(1,0,0);

  try { // If anything goes wrong, an exception will be thrown.

        // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
    MultibodySystem             mbs;

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

        // ADD BODIES AND THEIR MOBILIZERS
    Body::Rigid particle = Body::Rigid(MassProperties(m, Vec3(0), Inertia(0)));
    particle.addDecoration(DecorativeSphere(.1).setColor(Red).setOpacity(.3));

    MobilizedBody::SphericalCoords
        anAtom(matter.Ground(), Transform(ZUp, TestLoc),
               particle, Transform(),
               0*Deg2Rad,  false,   // azimuth offset, negated
               0,          false,   // zenith offset, negated
               ZAxis,      true);  // translation axis, negated

    anAtom.setDefaultRadius(.1);
    anAtom.setDefaultAngles(Vec2(0, 30*Deg2Rad));

    viz.addRubberBandLine(matter.Ground(), TestLoc,
                          anAtom, Vec3(0),
                          DecorativeLine().setColor(Orange).setLineThickness(4));

    Force::MobilityLinearSpring(forces, anAtom, 0, 2, -30*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 1, 2, 45*Deg2Rad); // harmonic bend
    Force::MobilityLinearSpring(forces, anAtom, 2, 20, .5); // harmonic stretch

    Force::MobilityLinearDamper(forces, anAtom, 0, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 1, .1); // harmonic bend
    Force::MobilityLinearDamper(forces, anAtom, 2, .1); // harmonic stretch

    
    State s = mbs.realizeTopology(); // returns a reference to the the default state
    mbs.realizeModel(s); // define appropriate states for this System
    mbs.realize(s, Stage::Instance); // instantiate constraints if any

    Visualizer display(mbs);
    display.setBackgroundType(Visualizer::SolidColor);

    mbs.realize(s, Stage::Velocity);
    display.report(s);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;

    char c;
    cout << "Default configuration shown. 1234 to move on.\n";

    //anAtom.setQToFitRotation(s, Rotation(-.9*Pi/2,YAxis));

    while (true) {
        Real x;
        cout << "Torsion (deg)? "; cin >> x; if (x==1234) break;
        Vec2 a = anAtom.getAngles(s); a[0]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Bend (deg)? "; cin >> x; if (x==1234) break;
        a = anAtom.getAngles(s); a[1]=x*Deg2Rad; anAtom.setAngles(s, a);
        display.report(s);
        cout << "Radius? "; cin >> x; if (x==1234) break;
        anAtom.setRadius(s, x);
        display.report(s);
    }
    anAtom.setUToFitAngularVelocity(s, Vec3(.1,.2,.3));

    //anAtom.setAngle(s, 45*Deg2Rad);
    //anAtom.setTranslation(s, Vec2(.4, .1));

    mbs.realize(s, Stage::Dynamics);
    mbs.realize(s, Stage::Acceleration);

    cout << "q=" << s.getQ() << endl;
    cout << "u=" << s.getU() << endl;
    cout << "qdot=" << s.getQDot() << endl;
    cout << "udot=" << s.getUDot() << endl;
    cout << "qdotdot=" << s.getQDotDot() << endl;
    display.report(s);

    cout << "Initialized configuration shown. Ready? ";
    cin >> c;

    RungeKuttaMersonIntegrator myStudy(mbs);
    myStudy.setAccuracy(1e-4);

    const Real dt = .02; // output intervals
    const Real finalTime = 20;

    myStudy.setFinalTime(finalTime);

    // Peforms assembly if constraints are violated.
    myStudy.initialize(s);
//.........这里部分代码省略.........