コード例 #1
0
int main() {
  try
  { // Create the system.
    
    MultibodySystem         system;
    system.setUpDirection(ZAxis);
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity          gravity(forces, matter, -ZAxis, 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-3);

    const Vec3 hdim(.2,.3,.4); // Brick half dimensions
    const Real rad = .1;    // Contact sphere radius
    const Real brickMass = 2;

    #ifdef USE_SHERM_PARAMETERS
        const Real mu_d =.3; // dynamic friction
        const Real mu_s =.3; // static friction
        const Real mu_v = 0;  // viscous friction (1/v)
        const Real dissipation = .1;
        const Real fK = 1e6; // stiffness in pascals
        const Real simDuration = 5.;
    #endif
    #ifdef USE_TOM_PARAMETERS
        const Real mu_d =.3; // dynamic friction
        const Real mu_s =.3; // static friction
        const Real mu_v = 0;  // viscous friction (1/v)
        const Real dissipation = .1756;  //Second impact at 0.685 s.
        const Real fK = 1e6; // stiffness in pascals
        const Real simDuration = 0.5; //3.0; //0.8;
    #endif

    const ContactMaterial material(fK,dissipation,mu_s,mu_d,mu_v);

    // Halfspace floor
    const Rotation R_xdown(Pi/2,YAxis);
    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(), material));

    Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), 
                            UnitInertia::brick(hdim)));
    brickBody.addDecoration(Transform(), 
        DecorativeBrick(hdim).setColor(BrickColor).setOpacity(.7));

    for (int i=-1; i<=1; i+=2)
    for (int j=-1; j<=1; j+=2)
    for (int k=-1; k<=1; k+=2) {
        const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(hdim);
        brickBody.addContactSurface(pt,
            ContactSurface(ContactGeometry::Sphere(rad), material));
        brickBody.addDecoration(pt,
            DecorativeSphere(rad).setColor(SphereColor));
    }

    MobilizedBody::Free brick(matter.Ground(), Transform(),
                              brickBody, Transform());

    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces,
                                                       brick));
    //viz.addFrameController(new BodyWatcher(brick));
    viz.addFrameController(new BodyWatcher(matter.Ground()));
    //viz.setShowSimTime(true);
    //viz.setShowFrameNumber(true);
    viz.setDesiredFrameRate(FrameRate);
    //viz.setShowFrameRate(true);

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

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

    // SET INITIAL CONDITIONS
    #ifdef USE_SHERM_PARAMETERS
        brick.setQToFitTranslation(state, Vec3(0,2,.8));
        brick.setQToFitRotation(state, Rotation(BodyRotationSequence, 
                                                Pi/4, XAxis, Pi/6, YAxis));
        brick.setUToFitLinearVelocity(state, Vec3(-5,0,0));
    #endif
    #ifdef USE_TOM_PARAMETERS
        Vector initQ = Vector(Vec7(1,0,0,0, 0,1,0.8));
        initQ(0,4) = Vector(Quaternion(Rotation(SimTK::Pi/4, XAxis) *
                                       Rotation(SimTK::Pi/6, YAxis))
                            .asVec4());
        Vector initU = Vector(Vec6(0,0,0, 0,0,6));
        initQ[6] = 1.5;
        initU[5] = -3.96;  //First impact at 0.181 s.
        initU[3] = -5.0;
        state.setQ(initQ);
        state.setU(initU);
    #endif

    saveEm.reserve(10000);

    viz.report(state);
    printf("Default state\n");
    cout << "t=" << state.getTime() 
         << " q=" << brick.getQAsVector(state)  
         << " u=" << brick.getUAsVector(state)  
         << endl;

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem) 
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);

   
    // Simulate it.

    // The system as parameterized is very stiff (mostly due to friction)
    // and thus runs best with CPodes which is extremely stable for
    // stiff problems. To get reasonable performance out of the explicit
    // integrators (like the RKs) you'll have to run at a very loose
    // accuracy like 0.1, or reduce the friction coefficients and
    // maybe the stiffnesses.

    //SemiExplicitEuler2Integrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    RungeKuttaMersonIntegrator integ(system);
    integ.setReturnEveryInternalStep(true);
    integ.setAllowInterpolation(false);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    //integ.setAccuracy(1e-3); // minimum for CPodes
    integ.setAccuracy(1e-5);
    //integ.setAccuracy(.01);

    integ.initialize(state);
    double cpuStart = cpuTime();
    double realStart = realTime();
    Real lastReport = -Infinity;
    while (integ.getTime() < simDuration) {
        // Advance time by no more than ReportInterval. Might require multiple 
        // internal steps.
        integ.stepBy(ReportInterval);

        if (integ.getTime() >= lastReport + VizReportInterval) {
            // The state being used by the integrator.
            const State& s = integ.getState();
            viz.report(s);
            saveEm.push_back(s); // save state for playback
            lastReport = s.getTime();
        }
    }

    const double timeInSec = realTime() - realStart;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << integ.getTime() << "s sim (avg step=" 
        << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms) " 
        << (1000*integ.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n", 
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    viz.dumpStats(std::cout);

    // Add as slider to control playback speed.
    viz.addSlider("Speed", 1, 0, 2, 1);
    viz.setMode(Visualizer::PassThrough);

    silo->clear(); // forget earlier input
    double speed = 1; // will change if slider moves
    while(true) {
        cout << "Choose Run/Replay to see that again ...\n";

        int menuId, item;
        silo->waitForMenuPick(menuId, item);


        if (menuId != RunMenuId) {
            cout << "\aUse the Run menu!\n";
            continue;
        }

        if (item == QuitItem)
            break;
        if (item != ReplayItem) {
            cout << "\aHuh? Try again.\n";
            continue;
        }

        for (double i=0; i < (int)saveEm.size(); i += speed ) {
            int slider; Real newValue;
            if (silo->takeSliderMove(slider,newValue)) {
                speed = newValue;
            }
            viz.report(saveEm[(int)i]);
        }
    }

  } 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;
}
コード例 #2
0
ファイル: PassThrough.cpp プロジェクト: BrianZ1/simbody
int main() {
    try
    { // Create the system.

        MultibodySystem         system;
        SimbodyMatterSubsystem  matter(system);
        GeneralForceSubsystem   forces(system);

        /// uncoment gravity to get some sort of collision interaction
        /// for cylinder mesh
        // Force::UniformGravity gravity(forces, matter,Vec3(0,0.001,0), 2);

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

        for(SubsystemIndex i(0); i<system.getNumSubsystems(); ++i)
        {
            fprintf(stderr,"subsytem name %d %s\n", (int)i,
                system.getSubsystem((SubsystemIndex)i).getName().c_str());
        }

        const Real rad = .4;
        PolygonalMesh pyramidMesh1,pyramidMesh2;

        /// load cylinder forces drawn, but interaction depends on gravity???


        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

        Body::Rigid pendulumBody3(MassProperties(100.0, Vec3(0), 100*Inertia(1)));
        PolygonalMesh body3contact = PolygonalMesh::createSphereMesh(rad, 2);
        ContactGeometry::TriangleMesh geo3(body3contact);

        const DecorativeMesh mesh3(geo3.createPolygonalMesh());
        pendulumBody3.addDecoration(Transform(),
                                    DecorativeMesh(mesh3).setOpacity(.2));
        pendulumBody3.addDecoration(Transform(),
                                    DecorativeMesh(mesh3).setColor(Gray)
                                    .setRepresentation(DecorativeGeometry::DrawWireframe)
                                    .setOpacity(.1));
        ContactSurface s1(geo3,
                          ContactMaterial(fK*.1,fDis*.9,fFac*.8,fFac*.7,fVis*10));
        s1.setThickness(1);
        s1.setShape(geo3);
        //ContactGeometry::Sphere geo3(rad);
        pendulumBody3.addContactSurface(Transform(),s1);
        /*
                std::ifstream meshFile1,meshFile2;
                meshFile1.open("cyl3.obj");
                pyramidMesh1.loadObjFile(meshFile1); meshFile1.close();
*/
        pyramidMesh1 = PolygonalMesh::createSphereMesh(rad, 2);
        ContactGeometry::TriangleMesh pyramid1(pyramidMesh1);

        DecorativeMesh showPyramid1(pyramid1.createPolygonalMesh());
        const Real ballMass = 200;
        Body::Rigid ballBody(MassProperties(ballMass, Vec3(0),
                                            ballMass*UnitInertia::sphere(1)));

        ballBody.addDecoration(Transform(),
                               showPyramid1.setColor(Cyan).setOpacity(.2));
        ballBody.addDecoration(Transform(),
                               showPyramid1.setColor(Gray)
                               .setRepresentation(DecorativeGeometry::DrawWireframe));

        ContactSurface s2(pyramid1,
                          ContactMaterial(fK*.1,fDis*.9,
                                          .1*fFac*.8,.1*fFac*.7,fVis*1));
        s2.setThickness(1);
        s2.setShape(pyramid1);
        ballBody.addContactSurface(Transform(),/*ContactSurface(ContactGeometry::Sphere(rad),ContactMaterial(fK*.1,fDis*.9,
                                                              .1*fFac*.8,.1*fFac*.7,fVis*1))*/  s2/*.joinClique(clique1)*/);

        /*   Body::Rigid d(MassProperties(1.0, Vec3(0),Inertia(1)));

        MobilizedBody::Pin dud(matter.Ground(),Transform(),d,Transform());
*/
        MobilizedBody::Free ball(matter.Ground(), Transform(Vec3(-2,-2,0)),
                                 ballBody, Transform(Vec3(0)));



        MobilizedBody::Free ball1(matter.Ground(), Transform(Vec3(0,0,0)),
                                  ballBody, Transform(Vec3(0)));
        /*
        MobilizedBody::Free ball2(matter.Ground(), Transform(Vec3(-4,0,0)),
                                 ballBody, Transform(Vec3(0)));
*/

        MobilizedBody::Free ball3(matter.Ground(), Transform(Vec3(-1,-2,0)),
                                  ballBody, Transform(Vec3(0)));


        MobilizedBody::Pin pendulum3(matter.Ground(), Transform(Vec3(-2,0,0)),
                                     pendulumBody3, Transform(Vec3(0, 2, 0)));

        ball.updBody();
        ball1.updBody();

        Visualizer viz(system);
        viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
        viz.setMode(Visualizer::RealTime);
        viz.setDesiredBufferLengthInSec(1);
        viz.setDesiredFrameRate(FrameRate);
        viz.setGroundHeight(-3);
        viz.setShowShadows(true);
        viz.setBackgroundType(Visualizer::SolidColor);
        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));
        //  system.addEventHandler(new TriggeredEventHandler(Stage::Model));
        // Initialize the system and state.

        system.realizeTopology();

        State state = system.getDefaultState();
        /*
        ball.setQToFitTransform(state, Transform(Rotation(Pi/2,XAxis),
                                                 Vec3(0,-1.8,0)));
*/
        //pendulum.setOneQ(state, 0, -Pi/12);
        pendulum3.setOneQ(state, 0, -Pi/2);
        pendulum3.setOneU(state, 0, Pi/4);
        // ball.setOneU(state, 1, 0.1);
        viz.report(state);
        matter.updAllParticleVelocities(state);
        printf("Default state\n");
        /* cout << "t=" << state.getTime()
         << " q=" << pendulum.getQAsVector(state) << pendulum2.getQAsVector(state)
         << " u=" << pendulum.getUAsVector(state) << pendulum2.getUAsVector(state)
         << endl;
*/
        cout << "\nChoose 'Go' from Run menu to simulate:\n";
        int menuId, item;
        do { silo->waitForMenuPick(menuId, item);
            if (menuId != RunMenuId || item != GoItem)
                cout << "\aDude ... follow instructions!\n";
        } while (menuId != RunMenuId || item != GoItem);

        // Simulate it.

        // The system as parameterized is very stiff (mostly due to friction)
        // and thus runs best with CPodes which is extremely stable for
        // stiff problems. To get reasonable performance out of the explicit
        // integrators (like the RKs) you'll have to run at a very loose
        // accuracy like 0.1, or reduce the friction coefficients and
        // maybe the stiffnesses.

        //ExplicitEulerIntegrator integ(system);
        CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
        //RungeKuttaFeldbergIntegrator integ(system);
        //RungeKuttaMersonIntegrator integ(system);
        //RungeKutta3Integrator integ(system);
        //VerletIntegrator integ(system);
        //integ.setMaximumStepSize(1e-1);
        //integ.setAllowInterpolation(false);
        integ.setAccuracy(1e-3); // minimum for CPodes
        //integ.setAccuracy(.1);
        TimeStepper ts(system, integ);


        ts.initialize(state);
        double cpuStart = cpuTime();
        double realStart = realTime();

        ts.stepTo(2000.0);

        const double timeInSec = realTime() - realStart;
        const int evals = integ.getNumRealizations();
        /*  cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
        << (1000*ts.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n",
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
*/
        viz.dumpStats(std::cout);

        // Add as slider to control playback speed.
        viz.addSlider("Speed", 1, 0, 4, 1);
        viz.setMode(Visualizer::PassThrough);

        silo->clear(); // forget earlier input
        double speed = 1; // will change if slider moves
        while(true) {
            cout << "Choose Run/Replay to see that again ...\n";

            int menuId, item;
            silo->waitForMenuPick(menuId, item);


            if (menuId != RunMenuId) {
                cout << "\aUse the Run menu!\n";
                continue;
            }

            if (item == QuitItem)
                break;
            if (item != ReplayItem) {
                cout << "\aHuh? Try again.\n";
                continue;
            }

            for (double i=0; i < (int)saveEm.size(); i += speed ) {
                int slider; Real newValue;
                if (silo->takeSliderMove(slider,newValue)) {
                    speed = newValue;
                }
                viz.report(saveEm[(int)i]);
            }
        }

    } 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;
}
コード例 #3
0
ファイル: Rattleback.cpp プロジェクト: thomasklau/simbody
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);
         if (menuId != RunMenuId || item != GoItem)
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);



    // Simulate it.

    //ExplicitEulerIntegrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    //RungeKuttaFeldbergIntegrator integ(system);
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    integ.setAccuracy(1e-4); // minimum for CPodes
    //integ.setAccuracy(.01);
    TimeStepper ts(system, integ);


    ts.initialize(state);
    double cpuStart = cpuTime();
    double realStart = realTime();

    ts.stepTo(10.0);

    const double timeInSec = realTime() - realStart;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step="
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
        << (1000*ts.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n",
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    viz.dumpStats(std::cout);

    // Add as slider to control playback speed.
    viz.addSlider("Speed", 1, 0, 4, 1);
    viz.setMode(Visualizer::PassThrough);

    silo->clear(); // forget earlier input
    double speed = 1; // will change if slider moves
    while(true) {
        cout << "Choose Run/Replay to see that again ...\n";

        int menuId, item;
        silo->waitForMenuPick(menuId, item);


        if (menuId != RunMenuId) {
            cout << "\aUse the Run menu!\n";
            continue;
        }

        if (item == QuitItem)
            break;
        if (item != ReplayItem) {
            cout << "\aHuh? Try again.\n";
            continue;
        }

        for (double i=0; i < (int)saveEm.size(); i += speed ) {
            int slider; Real newValue;
            if (silo->takeSliderMove(slider,newValue)) {
                speed = newValue;
            }
            viz.report(saveEm[(int)i]);
        }
    }

  } 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;
}
コード例 #4
0
//==============================================================================
//                                  MAIN
//==============================================================================
int main(int argc, char **argv) {
  try {
    cout << "This is Simbody example '" 
         << SimbodyExampleHelper::getExampleName() << "'\n";
    cout << "Working dir=" << Pathname::getCurrentWorkingDirectory() << endl;

    const std::string auxDir = 
        SimbodyExampleHelper::findAuxiliaryDirectoryContaining
        ("models/atlas_v4_free_pelvis.urdf");
    std::cout << "Getting geometry and models from '" << auxDir << "'\n";

    // Set some options.
    const double duration = Infinity; // seconds.

    // Create the "real" robot (the one that is being simulated).
    //Atlas realRobot("atlas_v4_locked_pelvis.urdf");
    Atlas realRobot(auxDir, "atlas_v4_free_pelvis.urdf");

    // Weld the feet to the floor.
    Constraint::Weld(realRobot.updMatterSubsystem().Ground(),Vec3(-.1,.1,0),
                     realRobot.updBody("l_foot"), Vec3(0,0,-.1));
    Constraint::Weld(realRobot.updMatterSubsystem().Ground(),Vec3(.1,-.1,0),
                     realRobot.updBody("r_foot"), Vec3(0,0,-.1));

    // Add a sinusoidal prescribed motion to the pelvis.
    MobilizedBody pelvis = realRobot.updBody("pelvis");
    Motion::Sinusoid(pelvis, Motion::Position,
                     .1, .5, 0);


    // Add the controller.
    ReachingAndGravityCompensation* controller =
        new ReachingAndGravityCompensation(auxDir, realRobot);
    // Force::Custom takes ownership over controller.
    Force::Custom control(realRobot.updForceSubsystem(), controller);

    // Set up visualizer and event handlers.
    Visualizer viz(realRobot);
    viz.setShowFrameRate(true);
    viz.setShowSimTime(true);

    viz.addSlider("Rate sensor noise", UNoise, 0, 1, 0); 
    viz.addSlider("Angle sensor noise", QNoise, 0, 1, 0); 

    Visualizer::InputSilo* userInput = new Visualizer::InputSilo();
    viz.addInputListener(userInput);
    realRobot.addEventHandler(
            new UserInputHandler(*userInput, realRobot, *controller, 0.05));
    realRobot.addEventReporter(
            new Visualizer::Reporter(viz, 1./30));

    // Display message on the screen about how to start simulation.
    DecorativeText help("Any input to start; ESC to quit.");
    help.setIsScreenText(true);
    viz.addDecoration(MobilizedBodyIndex(0), Vec3(0), help);
    help.setText("Move target: Arrows, PageUp/Down");
    viz.addDecoration(MobilizedBodyIndex(0), Vec3(0), help);

    // Initialize the real robot and other related classes.
    State s;
    realRobot.initialize(s);
    printf("Real robot has %d dofs.\n", s.getNU());
    controller->mapModelToRealRobot(s);

    // Bend knees and hips so assembly will come out reasonable.
    realRobot.getBody("l_uleg").setOneQ(s,0,-.3);   // hips
    realRobot.getBody("r_uleg").setOneQ(s,0,-.3);
    realRobot.getBody("l_lleg").setOneQ(s,0,1);     // knees
    realRobot.getBody("r_lleg").setOneQ(s,0,1);
    realRobot.realize(s);

    //RungeKuttaMersonIntegrator integ(realRobot);
    SemiExplicitEuler2Integrator integ(realRobot);
    integ.setAccuracy(0.001);
    TimeStepper ts(realRobot, integ);
    ts.initialize(s);
    viz.report(ts.getState());

    userInput->waitForAnyUserInput();
    userInput->clear();

    const double startCPU  = cpuTime(), startTime = realTime();

    // Simulate.
    ts.stepTo(duration);

    std::cout << "CPU time: " << cpuTime() - startCPU << " seconds. "
                << "Real time: " << realTime() - startTime << " seconds."
                << std::endl;

  } catch (const std::exception& e) {
    std::cout << "ERROR: " << e.what() << std::endl;
    return 1;
  }
  return 0;
}
コード例 #5
0
//==============================================================================
//                                   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());

    viz.report(state);
    printf("Hit any key to assemble ...");
    silo->waitForAnyUserInput(); silo->clear();
    Assembler(system).assemble(state);
    printf("ASSEMBLED\n");

    // Simulate.
    SemiExplicitEuler2Integrator integ(system);
    integ.setAccuracy(0.1);
    integ.setConstraintTolerance(.001);
    integ.setMaximumStepSize(1./60);
    TimeStepper ts(system, integ);
    ts.initialize(state);
    viz.report(ts.getState());
    printf("Hit ENTER to simulate ... (ESC to quit)\n");
    silo->waitForAnyUserInput(); silo->clear();

    const double startCPU  = cpuTime(), startTime = realTime();
    ts.stepTo(Infinity); // RUN
    dumpIntegratorStats(startCPU, startTime, integ);

    } catch (const std::exception& e) {
        std::cout << "ERROR: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}
コード例 #6
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);
    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

            stepsSinceViz = 0;
        }

        // Advance time by MaxStepSize (might take multiple steps to get there).
        integ.stepBy(MaxStepSize);

        // Now poll for user input.
        int whichSlider, whichMenu, whichItem; Real newValue;

        // Did a slider move?
        if (userInput->takeSliderMove(whichSlider, newValue)) {
            State& state = integ.updAdvancedState();
            switch(whichSlider) {
            case SliderIdMotorSpeed:
                // TODO: momentum balance?
                //motor.setRate(state, newValue);
                motor.setSpeed(state, newValue);
                system.realize(state, Stage::Position);
                system.prescribeU(state);
                system.realize(state, Stage::Velocity);
                system.projectU(state);
                break;
            case SliderIdDissipation:
                stop.setMaterialProperties(state, StopStiffness, newValue);
                system.realize(state, Stage::Position);
                break;
            }
        }

        // Was there a menu pick?
        if (userInput->takeMenuPick(whichMenu, whichItem)) {
            if (whichItem == QuitItem) 
                break; // done

            // If Reset, stop the motor and restore default dissipation. 
            // Tell visualizer to update the sliders to match.
            // Zero out all the q's and u's.
            if (whichItem == ResetItem) {
                State& state = integ.updAdvancedState();
                //motor.setRate(state, 0);
                motor.setSpeed(state, 0);
                viz.setSliderValue(SliderIdMotorSpeed, 0);

                stop.setMaterialProperties(state, StopStiffness, InitialDissipation);
                viz.setSliderValue(SliderIdDissipation, InitialDissipation);

                state.updQ() = 0; // all positions to zero
                state.updU() = 0; // all velocities to zero
                system.realize(state, Stage::Position);
                system.prescribeU(state);
                system.realize(state, Stage::Velocity);
                system.projectU(state);
            }
        }

    }
    const int evals = integ.getNumRealizations();
    std::cout << "Done -- simulated " << integ.getTime() << "s with " 
            << integ.getNumStepsTaken() << " steps, avg step=" 
        << (1000*integ.getTime())/integ.getNumStepsTaken() << "ms " 
        << (1000*integ.getTime())/evals << "ms/eval\n";

    printf("Used Integrator %s at accuracy %g:\n", 
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    } catch (const std::exception& e) {
        std::cout << "ERROR: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}
コード例 #7
0
ファイル: ContactBrickTest.cpp プロジェクト: Edwin2K/simbody
int main() {
  try
  { // Create the system.
    
    MultibodySystem         system;
    SimbodyMatterSubsystem  matter(system);
    GeneralForceSubsystem   forces(system);
    Force::Gravity          gravity(forces, matter, UnitVec3(.1,-1,0), 9.81);

    ContactTrackerSubsystem  tracker(system);
    CompliantContactSubsystem contactForces(system, tracker);
    contactForces.setTrackDissipatedEnergy(true);
    contactForces.setTransitionVelocity(1e-3);

    const Vec3 hdim(1,2,3);

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

    // Halfspace floor
    const Rotation R_xdown(-Pi/2,ZAxis);
    matter.Ground().updBody().addDecoration(
        Transform(Vec3(0,-.5,0)),
        DecorativeBrick(Vec3(10,.5,20)).setColor(Green).setOpacity(.1));
    matter.Ground().updBody().addContactSurface(
        Transform(R_xdown, Vec3(0,0,0)),
        ContactSurface(ContactGeometry::HalfSpace(),
                       ContactMaterial(fK*.1,fDis*.9,
                           fFac*.8,fFac*.7,fVis*10)));

    const Real brickMass = 10;
    Body::Rigid brickBody(MassProperties(brickMass, Vec3(0), 
                            UnitInertia::brick(hdim)));
    brickBody.addDecoration(Transform(), 
                            DecorativeBrick(hdim).setColor(Cyan).setOpacity(.3));
    const int surfx = brickBody.addContactSurface(Transform(),
        ContactSurface(ContactGeometry::Brick(hdim),
                       ContactMaterial(fK,fDis,
                                       fFac*.8,fFac*.7,fVis))
                       );
    //brickBody.addContactSurface(Transform(),
    //    ContactSurface(ContactGeometry::Ellipsoid(hdim),
    //                   ContactMaterial(fK*.1,fDis*.9,
    //                                   .1*fFac*.8,.1*fFac*.7,fVis*1))
    //                   );
    const ContactSurface& surf = brickBody.getContactSurface(surfx);
    const ContactGeometry& cg = surf.getShape();
    const ContactGeometry::Brick& cgbrick = ContactGeometry::Brick::getAs(cg);
    cout << "cgbrick.hdim=" << cgbrick.getHalfLengths() << endl;
    const Geo::Box& box = cgbrick.getGeoBox();
    cout << "box.hdim=" << box.getHalfLengths() << endl;

    // Vertices
    for (int i=0; i<8; ++i) {
        const Vec3 vpos = box.getVertexPos(i);
        const UnitVec3 vn = box.getVertexNormal(i);
        brickBody.addDecoration
            (DecorativePoint(vpos).setColor(Orange));
        brickBody.addDecoration
            (DecorativeText(String(i)).setTransform(vpos).setColor(White)
             .setScale(.5));
        brickBody.addDecoration
           (DecorativeLine(vpos, vpos + 0.5*vn).setColor(Orange));

        printf("vertex %d:\n", i);
        int e[3],ew[3],f[3],fw[3];
        box.getVertexEdges(i,e,ew);
        box.getVertexFaces(i,f,fw);
        for (int ex=0; ex<3; ++ex) {
            int ev[2]; box.getEdgeVertices(e[ex], ev);
            printf("  e%2d(%d) ev=%d\n", e[ex], ew[ex], ev[ew[ex]]);
        }
        for (int fx=0; fx<3; ++fx) {
            int fv[4]; box.getFaceVertices(f[fx], fv);
            printf("  f%2d(%d) fv=%d\n", f[fx], fw[fx], fv[fw[fx]]);
        }
    }

    // Edges
    for (int i=0; i<12; ++i) {
        const UnitVec3 n = box.getEdgeNormal(i);
        const UnitVec3 d = box.getEdgeDirection(i);
        const Vec3 ctr = box.getEdgeCenter(i);
        const Real len = .75;
        brickBody.addDecoration
           (DecorativePoint(ctr).setColor(Green).setScale(2));
        brickBody.addDecoration
            (DecorativeText(String(i)).setTransform(ctr+len*n)
             .setColor(Green).setScale(.3));
        brickBody.addDecoration
           (DecorativeLine(ctr, ctr + len*n).setColor(Green));
        brickBody.addDecoration
           (DecorativeLine(ctr, ctr + len*d).setColor(Green));

        printf("edge %d:\n", i);
        int f[2],fw[2];
        box.getEdgeFaces(i,f,fw);
        for (int fx=0; fx<2; ++fx) {
            int fe[4]; box.getFaceEdges(f[fx], fe);
            printf("  f%2d(%d) fe=%d\n", f[fx], fw[fx], fe[fw[fx]]);
        }
    }

    // Faces
    for (int i=0; i<6; ++i) {
        int vertices[4]; box.getFaceVertices(i,vertices);
        const UnitVec3 n = box.getFaceNormal(i);
        const Vec3 ctr = box.getFaceCenter(i);
        brickBody.addDecoration
           (DecorativePoint(ctr).setColor(Magenta).setScale(3));
        brickBody.addDecoration
           (Transform(Rotation(n,ZAxis,Vec3(0,1,0),YAxis),ctr),
            DecorativeText(String(i)).setColor(Magenta)
             .setScale(.75).setFaceCamera(false));
        brickBody.addDecoration
           (DecorativeLine(ctr, ctr + 1.*n).setColor(Magenta));
    }

    MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0,3,0)),
        brickBody, Transform(Vec3(0)));

    Visualizer viz(system);
    viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
    viz.setShowShadows(true);
    viz.setShowSimTime(true);
    viz.setDesiredFrameRate(FrameRate);
    viz.setShowFrameRate(true);
    viz.setBackgroundType(Visualizer::SolidColor);
    viz.setBackgroundColor(White*.9);

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

    brick.setQToFitRotation(state, Rotation(SpaceRotationSequence, 
                                            .1, ZAxis, .05, XAxis));
    brick.setUToFitLinearVelocity(state, Vec3(2,0,0));

    saveEm.reserve(10000);

    viz.report(state);
    printf("Default state\n");
    cout << "t=" << state.getTime() 
         << " q=" << brick.getQAsVector(state)  
         << " u=" << brick.getUAsVector(state)  
         << endl;

    cout << "\nChoose 'Go' from Run menu to simulate:\n";
    int menuId, item;
    do { silo->waitForMenuPick(menuId, item);
         if (menuId != RunMenuId || item != GoItem) 
             cout << "\aDude ... follow instructions!\n";
    } while (menuId != RunMenuId || item != GoItem);

   
    // Simulate it.

    // The system as parameterized is very stiff (mostly due to friction)
    // and thus runs best with CPodes which is extremely stable for
    // stiff problems. To get reasonable performance out of the explicit
    // integrators (like the RKs) you'll have to run at a very loose
    // accuracy like 0.1, or reduce the friction coefficients and
    // maybe the stiffnesses.

    //SemiExplicitEuler2Integrator integ(system);
    //CPodesIntegrator integ(system,CPodes::BDF,CPodes::Newton);
    RungeKuttaMersonIntegrator integ(system);
    //RungeKutta3Integrator integ(system);
    //VerletIntegrator integ(system);
    //integ.setMaximumStepSize(1e-0001);
    //integ.setAccuracy(1e-3); // minimum for CPodes
    integ.setAccuracy(1e-5);
    //integ.setAccuracy(.01);
    TimeStepper ts(system, integ);


    ts.initialize(state);
    double cpuStart = cpuTime();
    double realStart = realTime();

    ts.stepTo(20.0);

    const double timeInSec = realTime() - realStart;
    const int evals = integ.getNumRealizations();
    cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
        timeInSec << "s elapsed for " << ts.getTime() << "s sim (avg step=" 
        << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) " 
        << (1000*ts.getTime())/evals << "ms/eval\n";
    cout << "  CPU time was " << cpuTime() - cpuStart << "s\n";

    printf("Using Integrator %s at accuracy %g:\n", 
        integ.getMethodName(), integ.getAccuracyInUse());
    printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
    printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
    printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());

    viz.dumpStats(std::cout);

    // Add as slider to control playback speed.
    viz.addSlider("Speed", 1, 0, 4, 1);
    viz.setMode(Visualizer::PassThrough);

    silo->clear(); // forget earlier input
    double speed = 1; // will change if slider moves
    while(true) {
        cout << "Choose Run/Replay to see that again ...\n";

        int menuId, item;
        silo->waitForMenuPick(menuId, item);


        if (menuId != RunMenuId) {
            cout << "\aUse the Run menu!\n";
            continue;
        }

        if (item == QuitItem)
            break;
        if (item != ReplayItem) {
            cout << "\aHuh? Try again.\n";
            continue;
        }

        for (double i=0; i < (int)saveEm.size(); i += speed ) {
            int slider; Real newValue;
            if (silo->takeSliderMove(slider,newValue)) {
                speed = newValue;
            }
            viz.report(saveEm[(int)i]);
        }
    }

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