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