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