void ConstraintDynamics::applySolution() {
VectorXd contactForces(VectorXd::Zero(getTotalNumDofs()));
VectorXd jointLimitForces(VectorXd::Zero(getTotalNumDofs()));
if (getNumContacts() > 0) {
VectorXd f_n = mX.head(getNumContacts());
VectorXd f_d = mX.segment(getNumContacts(), getNumContacts() * mNumDir);
contactForces.noalias() = mN * f_n;
contactForces.noalias() += mB * f_d;
for (int i = 0; i < getNumContacts(); i++) {
Contact& contact = mCollisionChecker->getContact(i);
contact.force.noalias() = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir);
contact.force.noalias() += contact.normal * f_n[i];
}
}
for (int i = 0; i < mLimitingDofIndex.size(); i++) {
if (mLimitingDofIndex[i] > 0) { // hitting upper bound
jointLimitForces[mLimitingDofIndex[i] - 1] = -mX[getNumContacts() * (2 + mNumDir) + i];
}else{
jointLimitForces[abs(mLimitingDofIndex[i]) - 1] = mX[getNumContacts() * (2 + mNumDir) + i];
}
}
VectorXd lambda = VectorXd::Zero(mGInv.rows());
for (int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState())
continue;
mContactForces[i] = contactForces.segment(mIndices[i], mSkels[i]->getNumDofs());
mTotalConstrForces[i] = mContactForces[i] + jointLimitForces.segment(mIndices[i], mSkels[i]->getNumDofs());
if (mConstraints.size() > 0) {
VectorXd tempVec = mGInv * (mTauHat - mJMInv[i] * (contactForces.segment(mIndices[i], mSkels[i]->getNumDofs()) + jointLimitForces.segment(mIndices[i], mSkels[i]->getNumDofs())));
mTotalConstrForces[i] += mJ[i].transpose() * tempVec;
lambda += tempVec;
}
}
int count = 0;
for (int i = 0; i < mConstraints.size(); i++) {
mConstraints[i]->setLagrangeMultipliers(lambda.segment(count, mConstraints[i]->getNumRows()));
count += mConstraints[i]->getNumRows();
}
}
void main_simulation()
#endif
{
// inputs
double fitness;
#ifdef OPTI
double *optiParams;
#endif
Loop_inputs *loop_inputs;
// initialization
loop_inputs = init_simulation();
// optimization init
#ifdef OPTI
optiParams = (double*) malloc(NB_PARAMS_TO_OPTIMIZE*sizeof(double));
get_real_params_to_opt(optiNorms, optiParams);
erase_for_opti(optiParams, loop_inputs->MBSdata);
free(optiParams);
#endif
// -- Simbody -- //
#ifdef SIMBODY
// / Create the system. Define all "system" objects - system, matterm forces, tracker, contactForces.
MultibodySystem system;
SimbodyMatterSubsystem matter(system);
ContactTrackerSubsystem tracker(system);
CompliantContactSubsystem contactForces(system, tracker);
system.setUpDirection(+ZAxis); // that is for visualization only. The default direction is +X
SimbodyVariables simbodyVariables;
// set all the mechanical parameters of the contact
simbodyVariables.p_system = &system;
simbodyVariables.p_matter = &matter;
simbodyVariables.p_tracker = &tracker;
simbodyVariables.p_contactForces = &contactForces;
// cout<<"BoxInd in Main = "<<BoxInd<<" -- should be default \n";
//init_Simbody(&simbodyVariables);
init_Simbody(&simbodyVariables, loop_inputs->MBSdata->user_IO->simbodyBodies);
//it is "system" commands. We cannot avoid them.
system.realizeTopology();
State state = system.getDefaultState();
simbodyVariables.p_state = &state;
//it is "system" command. We cannot avoid them.
system.realizeModel(state);
p_simbodyVariables = &simbodyVariables;
#endif
// loop
fitness = loop_simulation(loop_inputs);
// end of the simulation
finish_simulation(loop_inputs);
#ifdef OPTI
return fitness;
#else
#if defined(PRINT_REPORT)
printf("fitness: %f\n", fitness);
#endif
#endif
}
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);
}
