Ejemplo n.º 1
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;
}
Ejemplo n.º 2
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;
}