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.
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;
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 {
// setup test problem
double r = .5;
double uP = -Pi/2;
double vP = Pi/3;
double uQ = 0;
double vQ = 2;
Vec3 O(-r, -r, 0.2);
Vec3 I(r, r, -r);
Vec3 P(r*cos(uP)*sin(vP), r*sin(uP)*sin(vP), r*cos(vP));
Vec3 Q(r*cos(uQ)*sin(vQ), r*sin(uQ)*sin(vQ), r*cos(vQ));
Vec3 r_OP = P-O;
Vec3 r_IQ = Q-I;
Vec3 tP = r_OP.normalize();
Vec3 tQ = r_IQ.normalize();
int n = 6; // problem size
Vector x(n), dx(n), Fx(n), xold(n);
Matrix J(n,n);
ContactGeometry::Sphere geom(r);
// r = 2;
// Vec3 radii(1,2,3);
// ContactGeometry::Ellipsoid geom(radii);
Geodesic geod;
// Create a dummy MultibodySystem for visualization purposes
MultibodySystem dummySystem;
SimbodyMatterSubsystem matter(dummySystem);
matter.updGround().addBodyDecoration(Transform(), geom.createDecorativeGeometry()
.setColor(Gray)
.setOpacity(0.5)
.setResolution(5));
// Visualize with default options; ask for a report every 1/30 of a second
// to match the Visualizer's default 30 frames per second rate.
Visualizer viz(dummySystem);
viz.setBackgroundType(Visualizer::SolidColor);
dummySystem.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// add vizualization callbacks for geodesics, contact points, etc.
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodP(), Red));
viz.addDecorationGenerator(new GeodesicDecorator(geom.getGeodQ(), Blue));
viz.addDecorationGenerator(new GeodesicDecorator(geod, Orange));
viz.addDecorationGenerator(new PlaneDecorator(geom.getPlane(), Gray));
viz.addDecorationGenerator(new PathDecorator(x, O, I, Green));
dummySystem.realizeTopology();
State dummyState = dummySystem.getDefaultState();
// calculate the geodesic
geom.addVizReporter(new VizPeriodicReporter(viz, dummyState, vizInterval));
viz.report(dummyState);
// creat path error function
//PathError pathErrorFnc(n, n, geom, geod, O, I);
PathErrorSplit pathErrorFnc(n, n, geom, geod, O, I);
pathErrorFnc.setEstimatedAccuracy(estimatedPathErrorAccuracy);
Differentiator diff(pathErrorFnc);
// set initial conditions
x[0]=P[0]; x[1]=P[1]; x[2]=P[2];
x[3]=Q[0]; x[4]=Q[1]; x[5]=Q[2];
Real f, fold, lam;
pathErrorFnc.f(x, Fx);
viz.report(dummyState);
sleepInSec(pauseBetweenPathIterations);
f = std::sqrt(~Fx*Fx);
for (int i = 0; i < maxNewtonIterations; ++i) {
if (f < ftol) {
std::cout << "path converged in " << i << " iterations" << std::endl;
// cout << "obstacle err = " << Fx << ", x = " << x << endl;
break;
}
diff.calcJacobian(x, Fx, J, Differentiator::ForwardDifference);
dx = J.invert()*Fx;
fold = f;
xold = x;
// backtracking
lam = 1;
while (true) {
x = xold - lam*dx;
cout << "TRY stepsz=" << lam << " sz*dx=" << lam*dx << endl;
pathErrorFnc.f(x, Fx);
f = std::sqrt(~Fx*Fx);
if (f > fold && lam > minlam) {
lam = lam / 2;
} else {
break;
}
}
if (maxabsdiff(x,xold) < xtol) {
std::cout << "converged on step size after " << i << " iterations" << std::endl;
std::cout << "error = " << Fx << std::endl;
break;
}
viz.report(dummyState);
sleepInSec(pauseBetweenPathIterations);
}
cout << "obstacle error = " << Fx << endl;
cout << "num geodP pts = " << geom.getGeodP().getNumPoints() << endl;
} 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;
}
void testConservationOfEnergy() {
// Create the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -9.8, 0));
const Real Mass = 1;
const Vec3 HalfShape = Vec3(1,.5,.25)/2;
const Transform BodyAttach(Rotation(), Vec3(HalfShape[0],0,0));
Body::Rigid brickBody(MassProperties(Mass, Vec3(.1,.2,.3),
Mass*Inertia(1,1.1,1.2,0.01,0.02,0.03)));
//Body::Rigid brickBody(MassProperties(Mass, Vec3(0),
// Mass*UnitInertia::ellipsoid(HalfShape)));
brickBody.addDecoration(Transform(), DecorativeEllipsoid(HalfShape)
.setOpacity(0.25)
.setColor(Blue));
brickBody.addDecoration(BodyAttach,
DecorativeFrame(0.5).setColor(Red));
const int NBod=50;
MobilizedBody::Free brick1(matter.Ground(), Transform(),
brickBody, BodyAttach);
MobilizedBody::Free brick2(brick1, Transform(),
brickBody, BodyAttach);
MobilizedBody prev=brick2;
MobilizedBody body25;
for (int i=0; i<NBod; ++i) {
MobilizedBody::Ball next(prev, -1*BodyAttach.p(),
brickBody, BodyAttach);
if (i==25) body25=next;
//Force::TwoPointLinearSpring(forces,
// prev, Vec3(0), next, Vec3(0), 1000, 1);
prev=next;
}
Constraint::Ball(matter.Ground(), Vec3(0,1,0)-2*NBod/3*BodyAttach.p(),
prev, BodyAttach.p());
Constraint::Ball(matter.Ground(), Vec3(0,1,0)-1*NBod/3*BodyAttach.p(),
body25, BodyAttach.p());
Vec6 k1(1,100,1,100,100,100), c1(0);
Force::LinearBushing(forces, matter.Ground(), -2*NBod/3*BodyAttach.p(),
prev, BodyAttach.p(), k1, c1);
matter.Ground().addBodyDecoration(-2*NBod/3*BodyAttach.p(),
DecorativeFrame().setColor(Green));
Force::Thermostat thermo(forces, matter,
SimTK_BOLTZMANN_CONSTANT_MD,
5000,
.1);
Vec6 k(1,100,1,100,100,100), c(0);
Force::LinearBushing bushing1(forces, matter.Ground(), -1*NBod/3*BodyAttach.p(),
brick1, BodyAttach, k, c);
Force::LinearBushing bushing2(forces, brick1, Transform(),
brick2, BodyAttach, k, c);
matter.Ground().addBodyDecoration(-1*NBod/3*BodyAttach.p(),
DecorativeFrame().setColor(Green));
Visualizer viz(system);
Visualizer::Reporter* reporter = new Visualizer::Reporter(viz, 1./30);
viz.setBackgroundType(Visualizer::SolidColor);
system.addEventReporter(reporter);
ThermoReporter* thermoReport = new ThermoReporter
(system, thermo, bushing1, bushing2, 1./10);
system.addEventReporter(thermoReport);
// Initialize the system and state.
system.realizeTopology();
State state = system.getDefaultState();
viz.report(state);
printf("Default state -- hit ENTER\n");
cout << "t=" << state.getTime()
<< " q=" << brick1.getQAsVector(state) << brick2.getQAsVector(state)
<< " u=" << brick1.getUAsVector(state) << brick2.getUAsVector(state)
<< "\nnChains=" << thermo.getNumChains(state)
<< " T=" << thermo.getBathTemperature(state)
<< "\nt_relax=" << thermo.getRelaxationTime(state)
<< " kB=" << thermo.getBoltzmannsConstant()
<< endl;
getchar();
state.setTime(0);
system.realize(state, Stage::Acceleration);
Vector initU(state.getNU());
initU = Test::randVector(state.getNU());
state.updU()=initU;
RungeKuttaMersonIntegrator integ(system);
//integ.setMinimumStepSize(1e-1);
integ.setAccuracy(1e-2);
TimeStepper ts(system, integ);
ts.initialize(state);
const State& istate = integ.getState();
viz.report(integ.getState());
viz.zoomCameraToShowAllGeometry();
printf("After initialize -- hit ENTER\n");
cout << "t=" << integ.getTime()
<< "\nE=" << system.calcEnergy(istate)
<< "\nEbath=" << thermo.calcBathEnergy(istate)
<< endl;
thermoReport->handleEvent(istate);
getchar();
// Simulate it.
ts.stepTo(20.0);
viz.report(integ.getState());
viz.zoomCameraToShowAllGeometry();
printf("After simulation:\n");
cout << "t=" << integ.getTime()
<< "\nE=" << system.calcEnergy(istate)
<< "\nEbath=" << thermo.calcBathEnergy(istate)
<< "\nNsteps=" << integ.getNumStepsTaken()
<< endl;
thermoReport->handleEvent(istate);
}
//==============================================================================
// 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;
}
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
{ // 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;
}
int main() {
// Define the system.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.8/10);
//Force::GlobalDamper damp(forces, matter, 1);
// Describe mass and visualization properties for a generic body.
Real mass = 2;
Vec3 hdim(1,.5,.25);
Body::Rigid bodyInfo(MassProperties(mass, Vec3(0), UnitInertia::brick(hdim)));
bodyInfo.addDecoration(Transform(),
DecorativeBrick(hdim).setColor(Orange).setOpacity(.3));
Real pmass = .1;
Vec3 phdim(5,.5,2);
Body::Rigid platformBody(MassProperties(10*mass,
Vec3(0), UnitInertia::ellipsoid(phdim)));
platformBody.addDecoration(Transform(),
DecorativeEllipsoid(phdim).setColor(Cyan).setOpacity(.1)
.setResolution(5));
MobilizedBody::Ball platform(matter.Ground(), Vec3(0),
platformBody, phdim/2);
//MobilizedBody platform = matter.Ground();
// Create the moving (mobilized) bodies of the pendulum.
//MobilizedBody::Free brick(platform, Transform(Vec3(0)),
// bodyInfo, Transform(Vec3(0)));
MobilizedBody::Free brick(matter.Ground(), Transform(Vec3(0)),
bodyInfo, Transform(Vec3(0)));
Array_<Constraint::SphereOnPlaneContact> balls;
Array_<Constraint::SphereOnSphereContact> sphsph;
Array_<Constraint::Rod> rods;
Rotation ZtoY(-Pi/2, XAxis);
//Constraint::PointInPlaneWithStiction pt1(platform,
// Transform(ZtoY, Vec3(0,1,0)),
// brick, hdim);
//pt1.setPlaneDisplayHalfWidth(5);
//Constraint::SphereOnPlaneContact ball1(platform,
// Transform(ZtoY, Vec3(0,1,0)),
// brick, hdim, 0.5, false);
//ball1.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball1);
//Constraint::SphereOnPlaneContact ball2(brick,
// Transform(Vec3(0,0,-hdim[2])),
// platform, -phdim/2, 0.5, false);
//ball2.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball2);
//Constraint::SphereOnPlaneContact ball3(brick,
// Transform(Vec3(0,0,-hdim[2])),
// platform, Vec3(-2,3,-.5), .7, true);
//ball3.setPlaneDisplayHalfWidth(5);
//balls.push_back(ball3);
//MobilizedBody::Free ball(matter.Ground(), Vec3(0),
// MassProperties(1,Vec3(0),UnitInertia(1,1,1)),
// Vec3(0));
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,1,-.5), .7,
// ball, Vec3(0), 1.2, true);
//Constraint::SphereOnSphereContact bb(brick, hdim, 0.5,
// ball, Vec3(0), 1.2, true);
//sphsph.push_back(bb);
Constraint::SphereOnSphereContact ss(brick, hdim, 0.5,
platform, Vec3(-3,1,-.5), 1.2,
false);
sphsph.push_back(ss);
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7,
// brick, hdim, 0.5, false);
//Constraint::SphereOnSphereContact ss(platform, Vec3(-2,3,-.5), .7,
// brick, hdim, 0.5, false);
//Constraint::SphereOnSphereContact ss(brick, hdim, 0.5,
// matter.Ground(), Vec3(-2,3,-.5), .7,true);
Constraint::Rod rod1(brick, Vec3(0,hdim[1],hdim[2]),
platform, Vec3(0,3,-.5), 1.5*1.2);
// Spring to keep the brick near 000.
//Force::TwoPointLinearSpring(forces, platform, Vec3(0),
// brick, Vec3(0), 4, 1);
// Rod to keep the brick near 000.
//Constraint::Rod rod1(platform, Vec3(0,0,2),
// brick, -hdim, 3);
//rods.push_back(rod1);
// Try edge/edge contact.
Constraint::LineOnLineContact ll(platform,
Transform(Rotation(UnitVec3(1,1,1), XAxis, UnitVec3(-XAxis), ZAxis),
Vec3(1,1,1)),
2, // hlen
brick,
Transform(Rotation(UnitVec3(ZAxis), XAxis, Vec3(-1,-1,0), ZAxis),
Vec3(-hdim[0],-hdim[1],0)),
2, // hlen
true);
// Set up visualization at 30 fps.
Visualizer viz(system);
viz.setBackgroundType(Visualizer::SolidColor);
viz.setShowFrameRate(true);
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
// Initialize the system and acquire default state.
State state = system.realizeTopology();
brick.setQToFitTransform(state, Vec3(0,5,0));
brick.setUToFitAngularVelocity(state, Vec3(10,10,10));
//rod1.setRodLength(state, 5);
viz.report(state);
printf("Initial config. Ready to assemble.\n"); getchar();
Assembler asmb(system);
asmb.assemble(state);
viz.report(state);
printf("Assembled. Ready to initialize.\n"); getchar();
//printf("Changed ball3 from rad=%g to rad=%g\n",
// ball3.getSphereRadius(state), 1.5);
//ball3.setSphereRadius(state, 1.5);
//viz.report(state); getchar();
//asmb.assemble(state);
//viz.report(state);
//printf("Re-assembled. Ready to simulate.\n"); getchar();
// Choose integrator and simulate for 10 seconds.
RungeKuttaMersonIntegrator integ(system);
//RungeKutta3Integrator integ(system);
integ.setAccuracy(1e-8);
//integ.setConstraintTolerance(1e-3);
TimeStepper ts(system, integ);
ts.initialize(state);
viz.report(ts.getState());
printf("Initialized. Ready to simulate.\n"); getchar();
viz.addDecorationGenerator(new ShowEnergy(system,brick,balls,sphsph,rods));
ts.stepTo(100.0);
printf("# steps=%d/%d\n",
integ.getNumStepsTaken(), integ.getNumStepsAttempted());
}
//==============================================================================
// MAIN
//==============================================================================
int main() {
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
matter.setShowDefaultGeometry(false);
GeneralForceSubsystem forces(system);
Force::Gravity gravity(forces, matter, -YAxis, 9.80665);
Visualizer viz(system);
MobilizedBody& Ground = matter.updGround(); // short name for Ground
// Calculate the mass properties for a half-ellipsoid. a1,b1,c1 are the
// radii (semi-axis lengths) of the full ellipsoid in x,y,z resp.
// Body origin is still center of full ellipsoid (0,0,0) but the COM moves
// lower in y which affects the xx and zz inertias. Note that Simbody
// requires the inertias to be given about the body origin, *not* COM.
// Unit inertia about the body origin is the same as for a full ellipsoid,
// but will be weighted by half as much mass.
// TODO: is this right?
Real m1 = 1.0, a1 = 0.25, b1 = 0.083333333333333, c1 = 0.083333333333333;
Real comShiftY = (3./8.)*b1; // Because it's a half ellipsoid.
Body::Rigid halfEllipsoid(MassProperties(m1, Vec3(0, -comShiftY, 0),
UnitInertia::ellipsoid(Vec3(a1,b1,c1))));
// Add some artwork -- don't have a half-ellipsoid unfortunately.
halfEllipsoid.addDecoration(Transform(),
DecorativeEllipsoid(Vec3(a1,b1,c1)).setColor(Red).setResolution(10));
// Now define a rectangular solid that we'll weld to the rattleback to
// give it asymmetrical mass properties.
Real m2 = 2.0, a2 = 2.0*a1, b2 = 0.02, c2 = 0.05;
const Vec3 barHalfDims = Vec3(a2,b2,c2)/2;
Body::Rigid barBody(MassProperties(m2, Vec3(0),
UnitInertia::brick(barHalfDims)));
barBody.addDecoration(Transform(), DecorativeBrick(barHalfDims)
.setColor(Blue).setOpacity(1.));
// Create a massless x-z base to provide the two slipping dofs.
MobilizedBody::Slider xdir(Ground, Transform(),
Body::Massless(), Transform());
const Rotation x2z(-Pi/2, YAxis); // rotate so +x moves to +z
MobilizedBody::Slider base(xdir, x2z, Body::Massless(), x2z);
base.addBodyDecoration(Transform(), DecorativeBrick(Vec3(0.25, 0.001, 0.25))
.setColor(Orange).setOpacity(0.50));
// Use a reverse mobilizer so that the contact point remains in a fixed
// location of the base body.
MobilizedBody::Ellipsoid rattle(base, Rotation(Pi/2, XAxis),
halfEllipsoid, Rotation(Pi/2, XAxis),
Vec3(a1,b1,c1), // ellipsoid half-radii
MobilizedBody::Reverse);
// Weld the bar to the ellipsoid at a 45 degree angle to produce lopsided
// inertia properties.
MobilizedBody::Weld bar(rattle, Transform(Rotation(45*Pi/180, YAxis),
Vec3(0,-b2/2.1,0)),
barBody, Transform());
// Finally, the rattle cannot just slide on the surface of the ground, it
// must roll.
#ifdef USE_BAD_CONSTRAINTS
// TODO: (sherm 20130620) these are the wrong constraints because they
// ignore the acceleration term caused by the contact point moving on the
// ellipsoid's surface. The correct constraint has to cognizant of the
// ellipsoid geometry at the contact point. Use of these constraints fails
// to conserve energy.
viz.addDecoration(Ground, Vec3(0),
DecorativeText("TODO: BROKEN -- USING INVALID NOSLIP CONSTRAINTS")
.setIsScreenText(true));
Constraint::NoSlip1D contactPointXdir(base, Vec3(0), UnitVec3(1,0,0),
matter.updGround(), rattle);
Constraint::NoSlip1D contactPointZdir(base, Vec3(0), UnitVec3(0,0,1),
matter.updGround(), rattle);
#endif
// Draw a cute green box to rattle around in.
Ground.addBodyDecoration(Vec3(0, 0*1e-5, 0), // floor
DecorativeBrick(Vec3(.5, .00001, .5)).setColor(Green).setOpacity(.1));
Ground.addBodyDecoration(Vec3(0.5, 0.25, 0), // right wall
DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
Ground.addBodyDecoration(Vec3(-.5, .25, 0), // left
DecorativeBrick(Vec3(1e-5, .25, .5)).setColor(Green).setOpacity(.25));
Ground.addBodyDecoration(Vec3(0, .25, -.5), // back
DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.25));
Ground.addBodyDecoration(Vec3(0, .25, .5), // front
DecorativeBrick(Vec3(.5, .25, 1e-5)).setColor(Green).setOpacity(.1));
// Output a visualization frame every 1/30 of a second, and output
// energy information every 1/4 second.
system.addEventReporter(new Visualizer::Reporter(viz, 1./30));
system.addEventReporter(new EnergyReporter(system, rattle, 1./4));
// We're done building the system. Create it and obtain a copy of the
// default state.
State state = system.realizeTopology();
// Start this off at an angle so it will do something.
// Caution -- this joint is reversed.
rattle.setQToFitRotation(state, ~Rotation(10*Pi/180., YAxis));
//rattle.setUToFitAngularVelocity(state, Vec3(0, -0.5*Pi, 1.0*Pi));
// Set up simulation.
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(1e-5);
TimeStepper ts(system, integ);
ts.initialize(state);
// Simulate.
ts.stepTo(50);
}
void testRiboseMobilizer()
{
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
DecorationSubsystem decorations(system);
matter.setShowDefaultGeometry(false);
// Put some hastily chosen mass there (doesn't help)
Body::Rigid rigidBody;
rigidBody.setDefaultRigidBodyMassProperties(MassProperties(
mass_t(20.0*daltons),
location_t(Vec3(0,0,0)*nanometers),
Inertia(20.0)
));
// One body anchored at C4 atom,
MobilizedBody::Weld c4Body(
matter.updGround(),
Rotation(-120*degrees, XAxis),
rigidBody,
Transform());
// sphere for C4 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for C5 atom
decorations.addBodyFixedDecoration(
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) )
);
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c4Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// One body anchored at C3 atom -- works
// Pin version
//MobilizedBody::Pin c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
// Function based pin version -- works
//TestPinMobilizer c3Body(
// c4Body,
// Transform(),
// rigidBody,
// Transform(location_t(Vec3(0,0,1.5)*angstroms))
// );
PseudorotationMobilizer c3Body(
c4Body,
Transform(),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(36.4*degrees), // amplitude
angle_t(-161.8*degrees) // phase
);
// sphere for C3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O3 atom
decorations.addBodyFixedDecoration(
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c3Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c2Body(
c3Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(35.8*degrees), // amplitude
angle_t(-91.3*degrees) // phase
);
// sphere for C2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for O2 atom
decorations.addBodyFixedDecoration(
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c3Body.getMobilizedBodyIndex(),
Vec3(0),
c2Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
PseudorotationMobilizer c1Body(
c2Body,
Rotation( angle_t(-80*degrees), YAxis ),
rigidBody,
Transform(location_t(Vec3(0,0,1.5)*angstroms)),
angle_t(37.6*degrees), // amplitude
angle_t(52.8*degrees) // phase
);
// sphere for C1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
Transform(),
DecorativeSphere( length_t(0.5*angstroms) )
);
// sphere for N1 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(0,0,1))
);
// sphere for O4 atom
decorations.addBodyFixedDecoration(
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeSphere( length_t(0.5*angstroms) ).setColor(Vec3(1,0,0))
);
decorations.addRubberBandLine(
c2Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
Vec3(0),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c1Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(-1.0,-1.0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
decorations.addRubberBandLine(
c4Body.getMobilizedBodyIndex(),
Vec3(0),
c1Body.getMobilizedBodyIndex(),
location_t(Vec3(1.0,0,-0.5)*angstroms),
DecorativeLine().setColor(Vec3(0,0,0)).setLineThickness(6));
// Prescribed motion
Constraint::ConstantSpeed(c3Body, 0.5);
// Two constraint way works; one constraint way does not
bool useTwoConstraints = true;
if (useTwoConstraints) {
// Constraints to make three generalized coordinates identical
std::vector<MobilizedBodyIndex> c32bodies(2);
c32bodies[0] = c3Body.getMobilizedBodyIndex();
c32bodies[1] = c2Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coordinates(2, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c32bodies, coordinates);
std::vector<MobilizedBodyIndex> c21bodies(2);
c21bodies[0] = c2Body.getMobilizedBodyIndex();
c21bodies[1] = c1Body.getMobilizedBodyIndex();
Constraint::CoordinateCoupler(matter, new DifferenceFunction, c21bodies, coordinates);
}
else { // trying to get single constraint way to work
// Try one constraint for all three mobilizers
std::vector<MobilizedBodyIndex> c123Bodies(3);
c123Bodies[0] = c1Body.getMobilizedBodyIndex();
c123Bodies[1] = c2Body.getMobilizedBodyIndex();
c123Bodies[2] = c3Body.getMobilizedBodyIndex();
std::vector<MobilizerQIndex> coords3(3, MobilizerQIndex(0));
Constraint::CoordinateCoupler(matter, new ThreeDifferencesFunction, c123Bodies, coords3);
}
Visualizer viz(system);
viz.setBackgroundType(Visualizer::SolidColor);
system.addEventReporter(new Visualizer::Reporter(viz, 0.10));
system.realizeTopology();
State& state = system.updDefaultState();
// Simulate it.
VerletIntegrator integ(system);
//RungeKuttaMersonIntegrator integ(system);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(50.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";
}
}
//------------------------------------------------------------------------------
// 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;
}