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