示例#1
0
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;
}
示例#2
0
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;
}
示例#3
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;
}
示例#4
0
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;
}