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