int main() {
// Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
MobilizedBody lastBody = matter.Ground();
for (int i = 0; i < 10; ++i) {
MobilizedBody::Ball pendulum(lastBody, Transform(Vec3(0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
lastBody = pendulum;
}
system.addEventReporter(new Visualizer::Reporter(system, 1./30));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
Random::Gaussian random;
for (int i = 0; i < state.getNQ(); ++i)
state.updQ()[i] = random.getValue();
// Simulate it.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10.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);
// 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));
// Add a joint stop to the left arm restricting it to q in [0,Pi/5].
Force::MobilityLinearStop stop(forces, leftArm, MobilizerQIndex(0),
10000, // stiffness
0.5, // dissipation coefficient
0*Pi, // lower stop
Pi/5); // upper stop
// Ask for visualization every 1/30 second.
system.setUseUniformBackground(true); // turn off floor
system.addEventReporter(new Visualizer::Reporter(system, 1./30));
// Initialize the system and state.
State state = system.realizeTopology();
leftArm.setAngle(state, Pi/5);
// Simulate for 10 seconds.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
}
return 0;
}
int main() {
try
{ // Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
pendulumBody.addDecoration(Transform(), DecorativeSphere(0.1));
MobilizedBody::Pin pendulum(matter.Ground(), Transform(Vec3(0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
//Motion::Steady(pendulum, 1);
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
pendulum.setOneU(state, 0, 1.0);
// Simulate it.
RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(100.0);
} 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, with subsystems for the bodies and some forces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
// Hint to Visualizer: don't show ground plane.
system.setUseUniformBackground(true);
// Add gravity as a force element.
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.81, 0));
// Create the body and some artwork for it.
const Vec3 halfLengths(.5, .1, .25); // half-size of brick (m)
const Real mass = 2; // total mass of brick (kg)
Body::Rigid pendulumBody(MassProperties(mass, Vec3(0),
mass*UnitInertia::brick(halfLengths)));
pendulumBody.addDecoration(Transform(),
DecorativeBrick(halfLengths).setColor(Red));
// Add an instance of the body to the multibody system by connecting
// it to Ground via a Ball mobilizer.
MobilizedBody::Ball pendulum1(matter.updGround(), Transform(Vec3(-1,-1, 0)),
pendulumBody, Transform(Vec3( 0, 1, 0)));
// Add a second instance of the pendulum nearby.
MobilizedBody::Ball pendulum2(matter.updGround(), Transform(Vec3(1,-1, 0)),
pendulumBody, Transform(Vec3(0, 1, 0)));
// Connect the origins of the two pendulum bodies together with our
// rod-like custom constraint.
const Real d = 1.5; // desired separation distance
Constraint::Custom rod(new ExampleConstraint(pendulum1, pendulum2, d));
// Visualize with default options.
Visualizer viz(system);
// Add a rubber band line connecting the origins of the two bodies to
// represent the rod constraint.
viz.addRubberBandLine(pendulum1, Vec3(0), pendulum2, Vec3(0),
DecorativeLine().setColor(Blue).setLineThickness(3));
// Ask for a report every 1/30 of a second to match the Visualizer's
// default rate of 30 frames per second.
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Output total energy to the console once per second.
system.addEventReporter(new TextDataEventReporter
(system, new MyEvaluateEnergy(), 1.0));
// Initialize the system and state.
State state = system.realizeTopology();
// Orient the two pendulums asymmetrically so they'll do something more
// interesting than just hang there.
pendulum1.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
pendulum2.setQToFitRotation(state, Rotation(BodyRotationSequence,
Pi/4, ZAxis, Pi/4, YAxis));
// Evaluate the system at the new state and draw one frame manually.
system.realize(state);
viz.report(state);
// Simulate it.
cout << "Hit ENTER to run a short simulation.\n";
cout << "(Energy should be conserved to about four decimal places.)\n";
getchar();
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-4); // ask for about 4 decimal places (default is 3)
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(10.0);
} catch (const std::exception& e) {
std::cout << "ERROR: " << e.what() << std::endl;
return 1;
} catch (...) {
std::cout << "UNKNOWN EXCEPTION\n";
return 1;
}
return 0;
}
int main() {
try
{ std::cout << "Current working directory: "
<< Pathname::getCurrentWorkingDirectory() << std::endl;
// Create the system.
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, 0*Vec3(2, -9.8, 0));
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
contactForces.setTrackDissipatedEnergy(true);
GeneralContactSubsystem OLDcontact(system);
const ContactSetIndex OLDcontactSet = OLDcontact.createContactSet();
contactForces.setTransitionVelocity(1e-3);
std::ifstream meshFile1, meshFile2;
PolygonalMesh femurMesh;
meshFile1.open("ContactBigMeshes_Femur.obj");
femurMesh.loadObjFile(meshFile1); meshFile1.close();
PolygonalMesh patellaMesh;
meshFile2.open("ContactBigMeshes_Patella.obj");
patellaMesh.loadObjFile(meshFile2); meshFile2.close();
ContactGeometry::TriangleMesh femurTri(femurMesh);
ContactGeometry::TriangleMesh patellaTri(patellaMesh);
DecorativeMesh showFemur(femurTri.createPolygonalMesh());
Array_<DecorativeLine> femurNormals;
const Real NormalLength = .02;
//for (int fx=0; fx < femurTri.getNumFaces(); ++fx)
// femurNormals.push_back(
// DecorativeLine(femurTri.findCentroid(fx),
// femurTri.findCentroid(fx)
// + NormalLength*femurTri.getFaceNormal(fx)));
DecorativeMesh showPatella(patellaTri.createPolygonalMesh());
Array_<DecorativeLine> patellaNormals;
//for (int fx=0; fx < patellaTri.getNumFaces(); ++fx)
// patellaNormals.push_back(
// DecorativeLine(patellaTri.findCentroid(fx),
// patellaTri.findCentroid(fx)
// + NormalLength*patellaTri.getFaceNormal(fx)));
// This transform has the meshes close enough that their OBBs overlap
// but in the end none of the faces are touching.
const Transform X_FP(
Rotation(Mat33( 0.97107625831404454, 0.23876955530133021, 0,
-0.23876955530133021, 0.97107625831404454, 0,
0, 0, 1), true),
Vec3(0.057400580865008571, 0.43859170879135373,
-0.00016506240185135300)
);
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
// Put femur on ground at origin
matter.Ground().updBody().addDecoration(Vec3(0,0,0),
showFemur.setColor(Cyan).setOpacity(.2));
matter.Ground().updBody().addContactSurface(Vec3(0,0,0),
ContactSurface(femurTri,
ContactMaterial(fK*.01,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
Body::Rigid patellaBody(MassProperties(1.0, Vec3(0), Inertia(1)));
patellaBody.addDecoration(Transform(),
showPatella.setColor(Red).setOpacity(.2));
patellaBody.addContactSurface(Transform(),
ContactSurface(patellaTri,
ContactMaterial(fK*.001,fDis*.9,fFac*.8,fFac*.7,fVis*10),
.01 /*thickness*/));
MobilizedBody::Free patella(matter.Ground(), Transform(Vec3(0)),
patellaBody, Transform(Vec3(0)));
//// The old way ...
//OLDcontact.addBody(OLDcontactSet, ball,
// pyramid, Transform());
//OLDcontact.addBody(OLDcontactSet, matter.updGround(),
// ContactGeometry::HalfSpace(), Transform(R_xdown, Vec3(0,-3,0)));
//ElasticFoundationForce ef(forces, OLDcontact, OLDcontactSet);
//Real stiffness = 1e6, dissipation = 0.01, us = 0.1,
// ud = 0.05, uv = 0.01, vt = 0.01;
////Real stiffness = 1e6, dissipation = 0.1, us = 0.8,
//// ud = 0.7, uv = 0.01, vt = 0.01;
//ef.setBodyParameters(ContactSurfaceIndex(0),
// stiffness, dissipation, us, ud, uv);
//ef.setTransitionVelocity(vt);
//// end of old way.
Visualizer viz(system);
Visualizer::Reporter& reporter = *new Visualizer::Reporter(viz, ReportInterval);
viz.addDecorationGenerator(new ForceArrowGenerator(system,contactForces));
MyReporter& myRep = *new MyReporter(system,contactForces,ReportInterval);
system.addEventReporter(&myRep);
system.addEventReporter(&reporter);
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
viz.report(state);
printf("Reference state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
char c=getchar();
patella.setQToFitTransform(state, ~X_FP);
viz.report(state);
printf("Initial state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << patella.getQAsVector(state)
<< " u=" << patella.getUAsVector(state)
<< endl;
c=getchar();
// Simulate it.
const clock_t start = clock();
RungeKutta3Integrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(2.0);
const double timeInSec = (double)(clock()-start)/CLOCKS_PER_SEC;
const int evals = integ.getNumRealizations();
cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
timeInSec << "s for " << ts.getTime() << "s sim (avg step="
<< (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
<< (1000*ts.getTime())/evals << "ms/eval\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());
while(true) {
for (int i=0; i < (int)saveEm.size(); ++i) {
viz.report(saveEm[i]);
}
getchar();
}
} 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);
matter.setShowDefaultGeometry(false);
CableTrackerSubsystem cables(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.81);
// Force::GlobalDamper(forces, matter, 5);
system.setUseUniformBackground(true); // no ground plane in display
MobilizedBody Ground = matter.Ground(); // convenient abbreviation
// Read in some bones.
PolygonalMesh femur;
PolygonalMesh tibia;
femur.loadVtpFile("CableOverBicubicSurfaces-femur.vtp");
tibia.loadVtpFile("CableOverBicubicSurfaces-tibia.vtp");
femur.scaleMesh(30);
tibia.scaleMesh(30);
// Build a pendulum
Body::Rigid pendulumBodyFemur( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyFemur.addDecoration(Transform(), DecorativeMesh(femur).setColor(Vec3(0.8, 0.8, 0.8)));
Body::Rigid pendulumBodyTibia( MassProperties(1.0, Vec3(0, -5, 0),
UnitInertia(1).shiftFromCentroid(Vec3(0, 5, 0))));
pendulumBodyTibia.addDecoration(Transform(), DecorativeMesh(tibia).setColor(Vec3(0.8, 0.8, 0.8)));
Rotation z180(Pi, YAxis);
MobilizedBody::Pin pendulumFemur( matter.updGround(),
Transform(Vec3(0, 0, 0)),
pendulumBodyFemur,
Transform(Vec3(0, 0, 0)) );
Rotation rotZ45(-Pi/4, ZAxis);
MobilizedBody::Pin pendulumTibia( pendulumFemur,
Transform(rotZ45, Vec3(0, -12, 0)),
pendulumBodyTibia,
Transform(Vec3(0, 0, 0)) );
Real initialPendulumOffset = -0.25*Pi;
Constraint::PrescribedMotion pres(matter,
new Function::Sinusoid(0.25*Pi, 0.2*Pi, 0*initialPendulumOffset), pendulumTibia, MobilizerQIndex(0));
// Build a wrapping cable path
CablePath path2(cables, Ground, Vec3(1, 3, 1), // origin
pendulumTibia, Vec3(1, -4, 0)); // termination
// Create a bicubic surface
Vec3 patchOffset(0, -5, -1);
Rotation rotZ90(0.5*Pi, ZAxis);
Rotation rotX90(0.2*Pi, XAxis);
Rotation patchRotation = rotZ90 * rotX90 * rotZ90;
Transform patchTransform(patchRotation, patchOffset);
Real patchScaleX = 2.0;
Real patchScaleY = 2.0;
Real patchScaleF = 0.75;
const int Nx = 4, Ny = 4;
const Real xData[Nx] = { -2, -1, 1, 2 };
const Real yData[Ny] = { -2, -1, 1, 2 };
const Real fData[Nx*Ny] = { 2, 3, 3, 1,
0, 1.5, 1.5, 0,
0, 1.5, 1.5, 0,
2, 3, 3, 1 };
const Vector x_(Nx, xData);
const Vector y_(Ny, yData);
const Matrix f_(Nx, Ny, fData);
Vector x = patchScaleX*x_;
Vector y = patchScaleY*y_;
Matrix f = patchScaleF*f_;
BicubicSurface patch(x, y, f, 0);
Real highRes = 30;
Real lowRes = 1;
PolygonalMesh highResPatchMesh = patch.createPolygonalMesh(highRes);
PolygonalMesh lowResPatchMesh = patch.createPolygonalMesh(lowRes);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(highResPatchMesh).setColor(Cyan).setOpacity(.75));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativeMesh(lowResPatchMesh).setRepresentation(DecorativeGeometry::DrawWireframe));
Vec3 patchP(-0.5,-1,2), patchQ(-0.5,1,2);
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchP).setColor(Green).setScale(2));
pendulumFemur.addBodyDecoration(patchTransform,
DecorativePoint(patchQ).setColor(Red).setScale(2));
CableObstacle::Surface patchObstacle(path2, pendulumFemur, patchTransform,
ContactGeometry::SmoothHeightMap(patch));
patchObstacle.setContactPointHints(patchP, patchQ);
patchObstacle.setDisabledByDefault(true);
// Sphere
Real sphRadius = 1.5;
Vec3 sphOffset(0, -0.5, 0);
Rotation sphRotation(0*Pi, YAxis);
Transform sphTransform(sphRotation, sphOffset);
CableObstacle::Surface tibiaSphere(path2, pendulumTibia, sphTransform,
ContactGeometry::Sphere(sphRadius));
Vec3 sphP(1.5,-0.5,0), sphQ(1.5,0.5,0);
tibiaSphere.setContactPointHints(sphP, sphQ);
pendulumTibia.addBodyDecoration(sphTransform,
DecorativeSphere(sphRadius).setColor(Red).setOpacity(0.5));
// Make cable a spring
CableSpring cable2(forces, path2, 50., 18., 0.1);
Visualizer viz(system);
viz.setShowFrameNumber(true);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
system.addEventReporter(new ShowStuff(system, cable2, 0.02));
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
system.realize(state, Stage::Position);
viz.report(state);
cout << "path2 init length=" << path2.getCableLength(state) << endl;
cout << "Hit ENTER ...";
getchar();
// path1.setIntegratedCableLengthDot(state, path1.getCableLength(state));
// Simulate it.
saveStates.clear(); saveStates.reserve(2000);
// RungeKutta3Integrator integ(system);
RungeKuttaMersonIntegrator integ(system);
// CPodesIntegrator integ(system);
// integ.setAllowInterpolation(false);
integ.setAccuracy(1e-5);
TimeStepper ts(system, integ);
ts.initialize(state);
ShowStuff::showHeading(cout);
const Real finalTime = 10;
const double startTime = realTime();
ts.stepTo(finalTime);
cout << "DONE with " << finalTime
<< "s simulated in " << realTime()-startTime
<< "s elapsed.\n";
while (true) {
cout << "Hit ENTER FOR REPLAY, Q to quit ...";
const char ch = getchar();
if (ch=='q' || ch=='Q') break;
for (unsigned i=0; i < saveStates.size(); ++i)
viz.report(saveStates[i]);
}
} catch (const std::exception& e) {
cout << "EXCEPTION: " << e.what() << "\n";
}
}
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
MultibodySystem mbs; mbs.setUseUniformBackground(true);
GeneralForceSubsystem forces(mbs);
SimbodyMatterSubsystem pend(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, pend, Vec3(0, -g, 0));
MobilizedBody::Ball connector(pend.Ground(),
Transform(1*Vec3(0, 0, 0)),
MassProperties(1, Vec3(0,0,0), Inertia(10,20,30)),
Transform(1*Vec3(0, .5, 0)));
connector.setDefaultRadius(0.05); // for the artwork
//connector.setDefaultRotation( Rotation(Pi/4, Vec3(0,0,1) );
const Real m1 = 5;
const Real m2 = 1;
const Real radiusRatio = std::pow(m2/m1, 1./3.);
const Vec3 weight1Location(0, 0, -d/2); // in local frame of swinging body
const Vec3 weight2Location(0, 0, d/2); // in local frame of swinging body
const Vec3 COM = (m1*weight1Location+m2*weight2Location)/(m1+m2);
const MassProperties swingerMassProps
(m1+m2, COM, 1*Inertia(1,1,1) + m1*UnitInertia::pointMassAt(weight1Location)
+ m2*UnitInertia::pointMassAt(weight2Location));
MobilizedBody::Screw swinger(connector,
Transform( Rotation( 0*.7, Vec3(9,8,7) ),
1*Vec3(0,-.5,0)),
swingerMassProps,
Transform( Rotation(0*1.3, Vec3(0,0,1) ),
COM+0*Vec3(0,0,3)), // inboard joint location
0.3); // pitch
// Add a blue sphere around the weight.
viz.addBodyFixedDecoration(swinger, weight1Location,
DecorativeSphere(d/8).setColor(Blue).setOpacity(.2));
viz.addBodyFixedDecoration(swinger, weight2Location,
DecorativeSphere(radiusRatio*d/8).setColor(Green).setOpacity(.2));
viz.addRubberBandLine(GroundIndex, Vec3(0),
swinger, Vec3(0),
DecorativeLine().setColor(Blue).setLineThickness(10)
.setRepresentation(DecorativeGeometry::DrawPoints));
//forces.addMobilityConstantForce(swinger, 0, 10);
Force::ConstantTorque(forces, swinger, Vec3(0,0,10));
//forces.addConstantForce(swinger, Vec3(0), Vec3(0,10,0));
//forces.addConstantForce(swinger, Vec3(0,0,0), Vec3(10,10,0)); // z should do nothing
//forces.addMobilityConstantForce(swinger, 1, 10);
// forces.addMobilityConstantForce(swinger, 2, 60-1.2);
State s = mbs.realizeTopology(); // define appropriate states for this System
pend.setUseEulerAngles(s, true);
mbs.realizeModel(s);
mbs.realize(s);
// Create a study using the Runge Kutta Merson integrator
RungeKuttaMersonIntegrator myStudy(mbs);
myStudy.setAccuracy(1e-6);
// This will pick up decorative geometry from
// each subsystem that generates any, including of course the
// DecorationSubsystem, but not limited to it.
Visualizer display(mbs);
const Real expectedPeriod = 2*Pi*std::sqrt(d/g);
printf("Expected period: %g seconds\n", expectedPeriod);
const Real dt = 1./30; // output intervals
const Real finalTime = 1*expectedPeriod;
for (Real startAngle = 10; startAngle <= 90; startAngle += 10) {
s = mbs.getDefaultState();
connector.setQToFitRotation(s, Rotation(Pi/4, Vec3(1,1,1)) );
printf("time theta energy *************\n");
swinger.setQToFitTransform(s, Transform( Rotation( BodyRotationSequence, 0*Pi/2, XAxis, 0*Pi/2, YAxis ), Vec3(0,0,0) ));
swinger.setUToFitVelocity(s, SpatialVec(0*Vec3(1.1,1.2,1.3),Vec3(0,0,-1)));
s.updTime() = 0;
myStudy.initialize(s);
cout << "MassProperties in B=" << swinger.expressMassPropertiesInAnotherBodyFrame(myStudy.getState(),swinger);
cout << "MassProperties in G=" << swinger.expressMassPropertiesInGroundFrame(myStudy.getState());
cout << "Spatial Inertia =" << swinger.calcBodySpatialInertiaMatrixInGround(myStudy.getState());
int stepNum = 0;
for (;;) {
// Should check for errors and other interesting status returns.
myStudy.stepTo(myStudy.getTime() + dt);
const State& s = myStudy.getState(); // s is now the integrator's internal state
if ((stepNum++%10)==0) {
// This is so we can calculate potential energy (although logically
// one should be able to do that at Stage::Position).
mbs.realize(s, Stage::Dynamics);
cout << s.getTime() << ": E=" << mbs.calcEnergy(s)
<< " connector q=" << connector.getQ(s)
<< ": swinger q=" << swinger.getQ(s) << endl;
// This is so we can look at the UDots.
mbs.realize(s, Stage::Acceleration);
cout << "q =" << pend.getQ(s) << endl;
cout << "u =" << pend.getU(s) << endl;
cout << "ud=" << pend.getUDot(s) << endl;
cout << "Connector V=" << connector.getMobilizerVelocity(s) << endl;
cout << "Swinger V=" << swinger.getMobilizerVelocity(s) << endl;
const Rotation& R_MbM = swinger.getMobilizerTransform(s).R();
Vec4 aaMb = R_MbM.convertRotationToAngleAxis();
cout << "angle=" << aaMb[0] << endl;
cout << "axisMb=" << aaMb.drop1(0) << endl;
cout << "axisMb=" << ~R_MbM*aaMb.drop1(0) << endl;
}
display.report(s);
if (s.getTime() >= finalTime)
break;
}
}
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
}
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
// CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
MultibodySystem mbs; mbs.setUseUniformBackground(true);
SimbodyMatterSubsystem crankRocker(mbs);
GeneralForceSubsystem forces(mbs);
DecorationSubsystem viz(mbs);
Force::UniformGravity gravity(forces, crankRocker, Vec3(0, -g, 0));
// ADD BODIES AND THEIR MOBILIZERS
Body::Rigid crankBody = Body::Rigid(MassProperties(.1, Vec3(0), 0.1*UnitInertia::brick(1,3,.5)));
crankBody.addDecoration(DecorativeEllipsoid(0.1*Vec3(1,3,.4))
.setResolution(10)
.setOpacity(.2));
Body::Rigid sliderBody = Body::Rigid(MassProperties(.2, Vec3(0), 0.2*UnitInertia::brick(1,5,.5)));
sliderBody.addDecoration(DecorativeEllipsoid(0.2*Vec3(1,5,.4))
.setColor(Blue)
.setResolution(10)
.setOpacity(.2));
Body::Rigid longBar = Body::Rigid(MassProperties(0.01, Vec3(0), 0.01*UnitInertia::cylinderAlongX(.1, 5)));
longBar.addDecoration(Rotation(Pi/2,ZAxis), DecorativeCylinder(.01, 1));
MobilizedBody::Pin
crank(crankRocker.Ground(), Transform(), crankBody, Transform(Vec3(0, .15, 0)));
MobilizedBody::Pin
slider(crankRocker.Ground(), Transform(Vec3(2.5,0,0)), sliderBody, Transform(Vec3(0, 1, 0)));
MobilizedBody::Universal
rightConn(crank, Transform(Rotation(-Pi/2,YAxis),Vec3(0,-.3,0)),
longBar, Transform(Rotation(-Pi/2,YAxis),Vec3(-1,0,0)));
Constraint::Ball ball(slider, Vec3(0,-1, 0), rightConn, Vec3(1,0,0));
ball.setDefaultRadius(0.01);
//Constraint::Rod rodl(leftConn, Vec3(0), rightConn, Vec3(-2.5,0,0), 2.5);
//Constraint::Rod rodr(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-1.5,0,0), std::sqrt(2.));
//Constraint::Rod rodo(leftConn, Vec3(1.5,0,0), rightConn, Vec3(-2.5,1,0), std::sqrt(2.));
//Constraint::Rod rodl(leftConn, Vec3(-2.5,0,0), rightConn, Vec3(-2.5,0,0), 5);
//Constraint::Rod rodr(leftConn, Vec3(2.5,0,0), rightConn, Vec3(2.5,0,0), 5);
//Constraint::Rod rodo(leftConn, Vec3(2.5,-1,0), rightConn, Vec3(-2.5,1,0), 2);
// Add visualization line (orange=spring, black=constraint)
//viz.addRubberBandLine(crank, Vec3(0,-3,0),
// slider, Vec3(0,-5,0),
// DecorativeLine().setColor(Black).setLineThickness(4));
//forces.addMobilityConstantForce(leftPendulum, 0, 20);
//forces.addCustomForce(ShermsForce(leftPendulum,rightPendulum));
//forces.addGlobalEnergyDrain(1);
Force::MobilityConstantForce(forces, crank, 0, 1);
Force::MobilityLinearDamper(forces, crank, 0, 1.0);
//Motion::Linear(crank, Vec3(a,b,c)); // crank(t)=at^2+bt+c
//Motion::Linear lmot(rightConn, Vec3(a,b,c)); // both axes follow
//lmot.setAxis(1, Vec3(d,e,f));
//Motion::Orientation(someBall, orientFuncOfT);
//someBall.prescribeOrientation(orientFunc);
//Motion::Relax(crank); // acc=vel=0, pos determined by some default relaxation solver
// Like this, or should this be an Instance-stage mode of every mobilizer?
//Motion::Lock(crank); // acc=vel=0; pos is discrete or fast
//Motion::Steady(crank, Vec3(1,2,3)); // acc=0; vel=constant; pos integrated
//Motion::Steady crankRate(crank, 1); // acc=0; vel=constant, same for each axis; pos integrated
// or ...
//crank.lock(state);
//crank.setMotionType(state, Regular|Discrete|Fast|Prescribed, stage);
// need a way to register a mobilizer with a particular relaxation solver,
// switch between dynamic, continuous relaxed, end-of-step relaxed, discrete.
// what about a "local" (explicit) relaxation, like q=(q1+q2)/2 ?
State s = mbs.realizeTopology(); // returns a reference to the the default state
mbs.realizeModel(s); // define appropriate states for this System
//crankRate.setRate(s, 3);
crank.setAngle(s, 5); //q
crank.setRate(s, 3); //u
Visualizer display(mbs);
mbs.realize(s, Stage::Position);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "qErr=" << s.getQErr() << endl;
crank.setAngle(s, -30*Deg2Rad);
//crank.setQToFitRotation (s, Rotation::aboutZ(-.9*Pi/2));
//TODO
//rightPendulum.setUToFitLinearVelocity(s, Vec3(1.1,0,1.2));
//crank.setUToFitAngularVelocity(s, 10*Vec3(.1,.2,.3));
mbs.realize(s, Stage::Velocity);
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "qErr=" << s.getQErr() << endl;
// These are the SimTK Simmath integrators:
RungeKuttaMersonIntegrator myStudy(mbs);
//CPodesIntegrator myStudy(mbs, CPodes::BDF, CPodes::Newton);
//myStudy.setMaximumStepSize(0.001);
myStudy.setAccuracy(1e-3);
//myStudy.setProjectEveryStep(true);
//myStudy.setAllowInterpolation(false);
//myStudy.setMaximumStepSize(.1);
const Real dt = 1./30; // output intervals
const Real finalTime = 10;
myStudy.setFinalTime(finalTime);
cout << "Hit ENTER in console to continue ...\n";
getchar();
display.report(s);
cout << "Hit ENTER in console to continue ...\n";
getchar();
// Peforms assembly if constraints are violated.
myStudy.initialize(s);
myStudy.setProjectEveryStep(false);
myStudy.setConstraintTolerance(.001);
myStudy.initialize(myStudy.getState());
cout << "Using Integrator " << std::string(myStudy.getMethodName()) << ":\n";
cout << "ACCURACY IN USE=" << myStudy.getAccuracyInUse() << endl;
cout << "CTOL IN USE=" << myStudy.getConstraintToleranceInUse() << endl;
cout << "TIMESCALE=" << mbs.getDefaultTimeScale() << endl;
cout << "U WEIGHTS=" << s.getUWeights() << endl;
cout << "Z WEIGHTS=" << s.getZWeights() << endl;
cout << "1/QTOLS=" << s.getQErrWeights() << endl;
cout << "1/UTOLS=" << s.getUErrWeights() << endl;
{
const State& s = myStudy.getState();
display.report(s);
cout << "q=" << s.getQ() << endl;
cout << "qErr=" << s.getQErr() << endl;
}
Integrator::SuccessfulStepStatus status;
int nextReport = 0;
while ((status=myStudy.stepTo(nextReport*dt, Infinity))
!= Integrator::EndOfSimulation)
{
const State& s = myStudy.getState();
mbs.realize(s);
const Real crankAngle = crank.getBodyRotation(s).convertRotationToAngleAxis()[0] * Rad2Deg;
printf("%5g %10.4g E=%10.8g h%3d=%g %s%s\n", s.getTime(),
crankAngle,
mbs.calcEnergy(s), myStudy.getNumStepsTaken(),
myStudy.getPreviousStepSizeTaken(),
Integrator::getSuccessfulStepStatusString(status).c_str(),
myStudy.isStateInterpolated()?" (INTERP)":"");
printf(" qerr=%10.8g uerr=%10.8g uderr=%10.8g\n",
crankRocker.getQErr(s).normRMS(),
crankRocker.getUErr(s).normRMS(),
crankRocker.getUDotErr(s).normRMS());
display.report(s);
if (status == Integrator::ReachedReportTime)
++nextReport;
}
for (int i=0; i<100; ++i)
display.report(myStudy.getState());
printf("Using Integrator %s:\n", myStudy.getMethodName());
printf("# STEPS/ATTEMPTS = %d/%d\n", myStudy.getNumStepsTaken(), myStudy.getNumStepsAttempted());
printf("# ERR TEST FAILS = %d\n", myStudy.getNumErrorTestFailures());
printf("# REALIZE/PROJECT = %d/%d\n", myStudy.getNumRealizations(), myStudy.getNumProjections());
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
catch (...) {
printf("UNKNOWN EXCEPTION THROWN\n");
exit(1);
}
}
//------------------------------------------------------------------------------
// main program
//------------------------------------------------------------------------------
int main(int argc, char** argv) {
try { // If anything goes wrong, an exception will be thrown.
int i = 0;
//--------------------------------------------------------------------------
// Experimental data points (x,y) of tibia origin (tibial plateau) measured
// w.r.t. to origin of the femur (hip joint center) in the femur frame as a
// function of knee joint angle. From Yamaguchi and Zajac, 1989.
//--------------------------------------------------------------------------
// Tibia X:
int npx = 12;
Real angX[] = {-2.094395102393, -1.745329251994, -1.396263401595, -1.047197551197,
-0.698131700798, -0.349065850399, -0.174532925199, 0.197344221443,
0.337394955864, 0.490177570472, 1.521460267071, 2.094395102393};
Real kneeX[] = {-0.003200000000, 0.001790000000, 0.004110000000, 0.004100000000,
0.002120000000, -0.001000000000, -0.003100000000, -0.005227000000,
-0.005435000000, -0.005574000000, -0.005435000000, -0.005250000000};
// Tibia Y; note that Y data is offset by -0.4 due to body frame placement.
int npy = 7;
Real angY[] = {-2.094395102393, -1.221730476396, -0.523598775598, -0.349065850399,
-0.174532925199, 0.159148563428, 2.094395102393};
Real kneeY[] = {-0.422600000000, -0.408200000000, -0.399000000000, -0.397600000000,
-0.396600000000, -0.395264000000, -0.396000000000 };
// Create SimTK Vectors to hold data points, and initialize from above arrays.
Vector ka_x (npx, angX); // measured knee angles when X data was collected
Vector ka_y (npy, angY); // measured knee angles when Y data was collected
Vector tib_x(npx, kneeX);
Vector tib_y(npy, kneeY);
#ifdef SHOULD_EXAGGERATE
// See above.
tib_x *= ExaggerateX;
tib_y = (tib_y+0.4)*ExaggerateY - 0.4; // exaggerate deviation only, not offset
#endif
// Generate splines from vectors of data points.
const int Degree = 3; // use cubics
SplineFitter<Real> fitterX = SplineFitter<Real>::fitFromGCV(Degree, ka_x, tib_x);
SplineFitter<Real> fitterY = SplineFitter<Real>::fitFromGCV(Degree, ka_y, tib_y);
Spline fx = fitterX.getSpline();
Spline fy = fitterY.getSpline();
//--------------------------------------------------------------------------
// Define the 6-spatial functions that specify the motion of the tibia as a
// a FunctionBased MobilizedBody (shank) w.r.t. to its parent (the thigh).
//--------------------------------------------------------------------------
// Each function has to return 1 Real value
std::vector<const Function*> functions(6);
// as a function of coordIndices (more than one per function)
std::vector< std::vector<int> > coordIndices(6);
// about a body-fixed axis for rotations or in parent translations
std::vector<Vec3> axes(6);
// Set the 1st and 2nd spatial rotation about the orthogonal (X then Y) axes as
// constant values. That is they don't contribute to motion nor do they have
// any coordinates in the equations of motion.
// |--------------------------------|
// | 1st function: rotation about X |
// |--------------------------------|
functions[0] = (new Function::Constant(0, 0));
std::vector<int> noIndex(0);
coordIndices[0] =(noIndex);
// |--------------------------------|
// | 2nd function: rotation about Y |
// |--------------------------------|
functions[1] = (new Function::Constant(0, 0));
coordIndices[1] = (noIndex);
// Set the spatial rotation about third axis to be the knee-extension
// angle (the one q) about the Z-axis of the tibia at the femoral condyles
// Define the coefficients of the linear function of the knee-angle with the
// spatial rotation about Z.
Vector coeff(2);
// Linear function x3 = coeff[0]*q + coeff[1]
coeff[0] = 1; coeff[1] = 0;
// |--------------------------------|
// | 3rd function: rotation about Z |
// |--------------------------------|
functions[2] = new Function::Linear(coeff);
// function of coordinate 0 (knee extension angle)
std::vector<int> qIndex(1,0);
coordIndices[2] = qIndex;
// Set the spatial translations as a function (splines) along the parent X and Y axes
// |-----------------------------------|
// | 4th function: translation about X |
// |-----------------------------------|
functions[3] = new Spline(fx); // Give the mobilizer a copy it can own.
coordIndices[3] =(qIndex);
// |-----------------------------------|
// | 5th function: translation about Y |
// |-----------------------------------|
functions[4] = new Spline(fy); // Give the mobilizer a copy it can own.
coordIndices[4] =(qIndex);
// |-----------------------------------|
// | 6th function: translation about Z |
// |-----------------------------------|
functions[5] = (new Function::Constant(0, 0));
coordIndices[5] = (noIndex);
// Construct the multibody system
const Real grav = 9.80665;
MultibodySystem system; system.setUseUniformBackground(true);
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, grav);
matter.setShowDefaultGeometry(true);
//--------------------------------------------------------------------------
// Define the model's physical (body) properties
//--------------------------------------------------------------------------
//Thigh
Body::Rigid femur(MassProperties(8.806, Vec3(0), Inertia(Vec3(0.1268, 0.0332, 0.1337))));
femur.addDecoration(Transform(Vec3(0, -0.21+0.1715, 0)),
DecorativeCylinder(0.04, 0.21).setColor(Orange).setOpacity(.5));
//Shank
Body::Rigid tibia(MassProperties(3.510, Vec3(0), Inertia(Vec3(0.0477, 0.0048, 0.0484))));
tibia.addDecoration(Transform(Vec3(0, -0.235+0.1862, 0)),
DecorativeCylinder(0.035, 0.235).setColor(Red));
//--------------------------------------------------------------------------
// Build the multibody system by adding mobilized bodies to the matter subsystem
//--------------------------------------------------------------------------
// Add the thigh via hip joint
MobilizedBody::Pin thigh(matter.Ground(), Transform(Vec3(0)), femur, Transform(Vec3(0.0020, 0.1715, 0)));
// This is how you might model the knee if you could only use a pin joint.
//MobilizedBody::Pin shank(thigh, Transform(Vec3(0.0033, -0.2294, 0)),
// tibia, Transform(Vec3(0.0, 0.1862, 0.0)));
// NOTE: function of Y-translation data was defined int the femur frame
// according to Yamaguchi and Zajac, which had the orgin at the hip joint
// center and the Y along the long-axis of the femur and Z out of the page.
MobilizedBody::FunctionBased shank(thigh, Transform(Vec3(0.0020, 0.1715, 0)),
tibia, Transform(Vec3(0.0, 0.1862, 0.0)),
1, // # of mobilities (dofs) for this joint
functions, coordIndices);
// Add some stop springs so the knee angle won't get outside the range of spline
// data we have. This custom force element is defined above.
Force::Custom(forces, new MyStop(shank, -Pi/2, 0*Pi, 100));
//--------------------------------------------------------------------------
// Setup reporters so we can get some output.
//--------------------------------------------------------------------------
// Vizualizer Animation
Visualizer viz(system);
system.addEventReporter(new Visualizer::Reporter(viz, 0.01));
// Energy -- reporter defined above.
system.addEventReporter(new MyEnergyReporter(system, 0.01));
//--------------------------------------------------------------------------
// Complete the construction of the "const" part of the System and
// allocate the default state.
//--------------------------------------------------------------------------
system.realizeTopology();
// Get a copy of the default state to work with.
State state = system.getDefaultState();
//--------------------------------------------------------------------------
// Set modeling options if any (this one is not actually needed here).
//--------------------------------------------------------------------------
matter.setUseEulerAngles(state, true);
// Complete construction of the model, allocating additional state variables
// if necessary.
system.realizeModel(state);
//--------------------------------------------------------------------------
// Set initial conditions.
//--------------------------------------------------------------------------
// Hip and knee coordinates and speeds similar to early swing
double hip_angle = -45*Pi/180;
double knee_angle = 0*Pi/180;
double hip_vel = 1;
double knee_vel = -5.0;
// Set initial states (Q's and U's)
// Position
thigh.setOneQ(state, 0, hip_angle);
shank.setOneQ(state, 0, knee_angle);
// Speed
thigh.setOneU(state, 0, hip_vel);
shank.setOneU(state, 0, knee_vel);
//--------------------------------------------------------------------------
// Run simulation.
//--------------------------------------------------------------------------
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(Accuracy);
TimeStepper ts(system, integ);
ts.initialize(state); // set IC's
ts.stepTo(5.0);
}
catch (const exception& e) {
printf("EXCEPTION THROWN: %s\n", e.what());
exit(1);
}
return 0;
}