//------------------------- ENSURE FORCE CACHE VALID --------------------------- // This will also calculate potential energy since we can do it on the cheap // simultaneously with the force. Note that if the strength of gravity was set // to zero then we already zeroed out the forces and pe during realizeInstance() // so all we have to do in that case is mark the cache valid now. Also, any // immune bodies have their force set to zero in realizeInstance() so we don't // have to do it again here. void Force::GravityImpl:: ensureForceCacheValid(const State& state) const { if (isForceCacheValid(state)) return; SimTK_STAGECHECK_GE_ALWAYS(state.getSystemStage(), Stage::Position, "Force::GravityImpl::ensureForceCacheValid()"); const Parameters& p = getParameters(state); if (p.g == 0) { // no gravity markForceCacheValid(state); // zeroes must have been precalculated return; } // Gravity is non-zero and gravity forces are not up to date, so this counts // as an evaluation. ++numEvaluations; const Vec3 gravity = p.g * p.d; const Real zeroPEOffset = p.g * p.z; ForceCache& fc = updForceCache(state); fc.pe = 0; const int nb = matter.getNumBodies(); // Skip Ground since we know it is immune. for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) { if (p.mobodIsImmune[mbx]) continue; // don't apply gravity to this body; F already zero const MobilizedBody& mobod = matter.getMobilizedBody(mbx); const MassProperties& mprops = mobod.getBodyMassProperties(state); const Transform& X_GB = mobod.getBodyTransform(state); Real m = mprops.getMass(); const Vec3& p_CB = mprops.getMassCenter(); // in B const Vec3 p_CB_G = X_GB.R()*p_CB; // exp. in G; 15 flops const Vec3 p_G_CB = X_GB.p() + p_CB_G; // meas. in G; 3 flops const Vec3 F_CB_G = m*gravity; // force at mass center; 3 flops fc.F_GB[mbx] = SpatialVec(p_CB_G % F_CB_G, F_CB_G); // body frc; 9 flops // odd signs here because height is in -gravity direction. fc.pe -= m*(~gravity*p_G_CB + zeroPEOffset); // 8 flops } const int np = matter.getNumParticles(); if (np) { const Vector& m = matter.getAllParticleMasses(state); const Vector_<Vec3>& p_GP = matter.getAllParticleLocations(state); for (ParticleIndex px(0); px < np; ++px) { fc.f_GP[px] = m[px] * gravity; // 3 flops fc.pe -= m[px]*(~gravity*p_GP[px] + zeroPEOffset); // 8 flops } } markForceCacheValid(state); }
//----------------------------- REALIZE TOPOLOGY ------------------------------- // Allocate the state variables and cache entries. The force cache is a lazy- // evaluation entry - although it can be calculated any time after // Stage::Position, it won't be unless someone asks for it. And if it is // evaluated early, it should not be re-evaluated when used as a force during // Stage::Dynamics (via the calcForce() call). // // In addition, the force cache has a dependency on the parameter values that // are stored in a discrete state variable. Changes to that variable // automatically invalidate Dynamics stage, but must be carefully managed also // to invalidate the force cache here since it is only Position-dependent. void Force::GravityImpl:: realizeTopology(State& s) const { GravityImpl* mThis = const_cast<GravityImpl*>(this); const int nb=matter.getNumBodies(), np=matter.getNumParticles(); // In case more mobilized bodies were added after this Gravity element // was constructed, make room for the rest now. Earlier default immunity // settings are preserved. if (defMobodIsImmune.size() != nb) mThis->defMobodIsImmune.resize(nb, false); // Allocate a discrete state variable to hold parameters; see above comment. const Parameters p(defDirection,defMagnitude,defZeroHeight, defMobodIsImmune); // initial value mThis->parametersIx = getForceSubsystem() .allocateDiscreteVariable(s, Stage::Dynamics, new Value<Parameters>(p)); // Don't allocate force cache space yet since we would have to copy it. // Caution -- dependence on Parameters requires manual invalidation. mThis->forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s, Stage::Position, new Value<ForceCache>()); // Now allocate the appropriate amount of space, and set to zero now // any forces that we know will end up zero so we don't have to calculate // them at run time. Precalculated zeroes must be provided for any // immune elements, or for all elements if g==0, and this must be kept // up to date if there are runtime changes to the parameters. ForceCache& fc = updForceCache(s); if (defMagnitude == 0) { fc.allocate(nb, np, true); // initially zero since no gravity } else { fc.allocate(nb, np, false); // initially NaN except for Ground for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) if (defMobodIsImmune[mbx]) fc.F_GB[mbx] = SpatialVec(Vec3(0),Vec3(0)); // This doesn't mean the ForceCache is valid yet. } }
void flexionFDSimulationWithHitMap(Model& model) { addFlexionController(model); //addExtensionController(model); //addTibialLoads(model, 0); model.setUseVisualizer(true); // init system std::time_t result = std::time(nullptr); std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl; SimTK::State& si = model.initSystem(); result = std::time(nullptr); std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl; // set gravity //model.updGravityForce().setGravityVector(si, Vec3(-9.80665,0,0)); //model.updGravityForce().setGravityVector(si, Vec3(0,0,0)); //setHipAngle(model, si, 90); //setKneeAngle(model, si, 0, false, false); //model.equilibrateMuscles( si); MultibodySystem& system = model.updMultibodySystem(); SimbodyMatterSubsystem& matter = system.updMatterSubsystem(); GeneralForceSubsystem forces(system); ContactTrackerSubsystem tracker(system); CompliantContactSubsystem contactForces(system, tracker); contactForces.setTrackDissipatedEnergy(true); //contactForces.setTransitionVelocity(1e-3); for (int i=0; i < matter.getNumBodies(); ++i) { MobilizedBodyIndex mbx(i); if (i==19 || i==20 || i==22)// || i==15 || i==16) { MobilizedBody& mobod = matter.updMobilizedBody(mbx); std::filebuf fb; //cout << mobod.updBody(). if (i==19) fb.open ( "../resources/femur_lat_r.obj",std::ios::in); else if (i==20) fb.open ( "../resources/femur_med_r.obj",std::ios::in); else if (i==22) fb.open ( "../resources/tibia_upper_r.obj",std::ios::in); //else if (i==15) //fb.open ( "../resources/meniscus_lat_r.obj",std::ios::in); //else if (i==16) //fb.open ( "../resources/meniscus_med_r.obj",std::ios::in); std::istream is(&fb); PolygonalMesh polMesh; polMesh.loadObjFile(is); fb.close(); SimTK::ContactGeometry::TriangleMesh mesh(polMesh); ContactSurface contSurf;//(mesh, ContactMaterial(1.0e6, 1, 1, 0.03, 0.03), 0.001); if (i==19 || i==20 || i==22) contSurf = ContactSurface(mesh, ContactMaterial(10, 1, 1, 0.03, 0.03), 0.001); //else if (i==15 || i==16) //contSurf = ContactSurface(mesh, ContactMaterial(10, 3, 1, 0.03, 0.03), 0.001); DecorativeMesh showMesh(mesh.createPolygonalMesh()); showMesh.setOpacity(0.5); mobod.updBody().addDecoration( showMesh); mobod.updBody().addContactSurface(contSurf); } } ModelVisualizer& viz(model.updVisualizer()); //Visualizer viz(system); viz.updSimbodyVisualizer().addDecorationGenerator(new HitMapGenerator(system,contactForces)); viz.updSimbodyVisualizer().setMode(Visualizer::RealTime); viz.updSimbodyVisualizer().setDesiredBufferLengthInSec(1); viz.updSimbodyVisualizer().setDesiredFrameRate(30); viz.updSimbodyVisualizer().setGroundHeight(-3); viz.updSimbodyVisualizer().setShowShadows(true); Visualizer::InputSilo* silo = new Visualizer::InputSilo(); viz.updSimbodyVisualizer().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.updSimbodyVisualizer().addMenu("Run", RunMenuId, runMenuItems); Array_<std::pair<String,int> > helpMenuItems; helpMenuItems.push_back(std::make_pair("TBD - Sorry!", 1)); viz.updSimbodyVisualizer().addMenu("Help", HelpMenuId, helpMenuItems); system.addEventReporter(new MyReporter(system,contactForces,ReportInterval)); //system.addEventReporter(new Visualizer::Reporter(viz.updSimbodyVisualizer(), ReportInterval)); // Check for a Run->Quit menu pick every <x> second. system.addEventHandler(new UserInputHandler(*silo, 0.001)); system.realizeTopology(); //Show ContactSurfaceIndex for each contact surface // for (int i=0; i < matter.getNumBodies(); ++i) { //MobilizedBodyIndex mbx(i); // const MobilizedBody& mobod = matter.getMobilizedBody(mbx); // const int nsurfs = mobod.getBody().getNumContactSurfaces(); //printf("mobod %d has %d contact surfaces\n", (int)mbx, nsurfs); ////cout << "mobod with mass: " << (float)mobod.getBodyMass(si) << " has " << nsurfs << " contact surfaces" << endl; // //for (int i=0; i<nsurfs; ++i) { // //printf("%2d: index %d\n", i, // //(int)tracker.getContactSurfaceIndex(mbx,i)); // //} // } //cout << "tracker num of surfaces: " << tracker.getNumSurfaces() << endl; //State state = system.getDefaultState(); //viz.report(state); State& state = model.initializeState(); viz.updSimbodyVisualizer().report(state); // Add reporters ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); CustomAnalysis* customReporter = new CustomAnalysis(&model, "r"); model.addAnalysis(customReporter); // Create the integrator and manager for the simulation. SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); //SimTK::CPodesIntegrator integrator(model.getMultibodySystem()); //integrator.setAccuracy(.01); //integrator.setAccuracy(1e-3); //integrator.setFixedStepSize(0.001); Manager manager(model, integrator); // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 0.2; // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<std::endl; result = std::time(nullptr); std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl; manager.integrate(state); result = std::time(nullptr); std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl; // Save the simulation results Storage statesDegrees(manager.getStateStorage()); statesDegrees.print("../outputs/states_flex.sto"); model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); statesDegrees.setWriteSIMMHeader(true); statesDegrees.print("../outputs/states_degrees_flex.mot"); // force reporter results forceReporter->getForceStorage().print("../outputs/force_reporter_flex.mot"); //customReporter->print( "../outputs/custom_reporter_flex.mot"); //cout << "You can choose 'Replay'" << endl; int menuId, item; unsigned int frameRate = 5; do { cout << "Please choose 'Replay' or 'Quit'" << endl; viz.updInputSilo().waitForMenuPick(menuId, item); if (item != ReplayItem && item != QuitItem) cout << "\aDude... follow instructions!\n"; if (item == ReplayItem) { cout << "Type desired frame rate (integer) for playback and press Enter (default = 1) : "; //frameRate = cin.get(); cin >> frameRate; if (cin.fail()) { cout << "Not an int. Setting default frame rate." << endl; cin.clear(); cin.ignore(std::numeric_limits<int>::max(),'\n'); frameRate = 1; } //cout << "saveEm size: " << saveEm.size() << endl; for (unsigned int i=0; i<saveEm.size(); i++) { viz.updSimbodyVisualizer().drawFrameNow(saveEm.getElt(i)); if (frameRate == 0) frameRate = 1; usleep(1000000/frameRate); } } } while (menuId != RunMenuId || item != QuitItem); }