void ExploreCoactivation(Model* model, State& initial_state, double ti, double tf, double slope, double activation, string dir = "")
{
Coordinate& rx = model->updCoordinateSet().get("platform_rx");
rx.set_locked(false);
rx.setDefaultValue(Pi*slope / 180.0);
rx.set_locked(true);
ControllerSet& controllers = model->updControllerSet();
// set all invertor excitations to the predetermined value
// invertors are stronger than evertors, so must scale down invertor activity to produce co-activation with no net moment
double invertorActivation = activation*invertorScaleFactor;
PrescribedController* invControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("inverter_controls_r"));
invControls->set_isDisabled(false);
for (int i = 0; i<invControls->getActuatorSet().getSize(); i++)
{
OpenSim::Constant currentAct(invertorActivation);
invControls->prescribeControlForActuator(i, currentAct.clone());
OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&invControls->updActuators().get(i));
if (muscle)
{
muscle->setDefaultActivation(invertorActivation);
}
}
// set all evertor excitations to the predetermined value
PrescribedController* evControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("everter_controls_r"));
evControls->set_isDisabled(false);
for (int i = 0; i<evControls->getActuatorSet().getSize(); i++)
{
OpenSim::Constant currentAct(activation);
evControls->prescribeControlForActuator(i, currentAct.clone());
OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&evControls->updActuators().get(i));
if (muscle)
{
muscle->setDefaultActivation(activation);
}
}
// turn off other everter and inverter controllers
//model->updControllerSet().get("AnkleEverterDelayedReflexes").set_isDisabled(true);
//model->updControllerSet().get("AnkleInverterDelayedReflexes").set_isDisabled(true);
// Create a force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
SimTK::State& osim_state = model->initSystem();
osim_state.setQ(initial_state.getQ());
osim_state.setU(initial_state.getU());
rx.set_locked(false);
rx.setValue(osim_state, Pi*slope / 180.0);
rx.set_locked(true);
model->equilibrateMuscles(osim_state);
// Create the integrator for integrating system dynamics
SimTK::RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(integratorTolerance);
integrator.setMaximumStepSize(1);
integrator.setMinimumStepSize(1e-8);
// Create the manager managing the forward integration and its outputs
Manager manager(*model, integrator);
// Integrate from initial time to final time
manager.setInitialTime(ti);
manager.setFinalTime(tf);
cout << "\nIntegrating from " << ti << " to " << tf << endl;
manager.integrate(osim_state);
//////////////////////////////
// SAVE THE RESULTS TO FILE //
//////////////////////////////
// Save the model states from forward integration
Storage statesDegrees(manager.getStateStorage());
char trial_name[100];
sprintf(trial_name, "%sincline_%.1f_activation_%.1f", dir, slope, activation);
statesDegrees.print(string(trial_name) + string("_states.sto"));
// Save the forces
reporter->getForceStorage().print(string(trial_name) + string("_forces.sto"));
// Save the controls
model->printControlStorage(trial_name + string("_controls.sto"));
// save the model
model->print(string(trial_name) + string("_model.osim"));
}
int main(int argc, char* argv[])
{
try {
std::clock_t startTime = std::clock();
string modelFile = "bestModel.osim";
string ctrlFile = "reflexControllers.xml";
string kinematicsFile = "initialState.sto";
double ti = initialTime;
double tf = finalTime;
// Options
for (int i = 0; i < argc; i++) {
if (strcmp(argv[i], "-m") == 0) {
modelFile = argv[i+1];
}
if (strcmp(argv[i], "-csfile") == 0) {
ctrlFile = argv[i+1];
}
if (strcmp(argv[i], "-kfile") == 0) {
kinematicsFile = argv[i + 1];
}
if (strcmp(argv[i], "-ti") == 0) {
ti = atof(argv[i + 1]);
}
if (strcmp(argv[i], "-tf") == 0) {
tf = atof(argv[i + 1]);
}
}
OpenSim::Model osimModel(modelFile);
Vec3 platform_origin(0.0, 0.015, 0.45);
//Vec3 twist_y(0.0, 0.20943951023931953, 0.0);
osimModel.updJointSet().get("ground_platform").setLocationInParent(platform_origin);
//osimModel.updJointSet().get("ground_platform").setOrientationInParent(twist_y);
//osimModel.updForceSet().get("foot_floor_l").set_isDisabled(true);
CoordinateSet& coords = osimModel.updCoordinateSet();
//coords.get("platform_rx").set_locked(false);
//coords.get("platform_rx").setDefaultValue(platformAngle*Pi/180);
/*
coords.get("platform_rx").setDefaultValue(0.0);
coords.get("platform_rx").set_locked(true);
coords.get("platform_ry").set_locked(false);
coords.get("platform_ry").setDefaultValue(0.0);
coords.get("platform_ry").set_locked(true);
coords.get("platform_rz").set_locked(false);
coords.get("platform_rz").setDefaultValue(0.0);
coords.get("platform_rz").set_locked(true);
coords.get("platform_ty").set_locked(false);
coords.get("platform_ty").setDefaultValue(0.0);
coords.get("platform_ty").set_locked(true);
*/
Array<double> actLevels;
actLevels.append(0.0);
actLevels.append(0.1);
actLevels.append(0.2);
actLevels.append(0.3);
actLevels.append(0.4);
actLevels.append(0.5);
actLevels.append(0.6);
actLevels.append(0.7);
actLevels.append(0.8);
actLevels.append(0.9);
actLevels.append(1.0);
Array<double> reflexGains;
reflexGains.append(0.0);
reflexGains.append(0.5);
reflexGains.append(1.0);
reflexGains.append(2.0);
reflexGains.append(5.0);
reflexGains.append(10.0);
//reflexGains.append(100.0);
//reflexGains.append(1000.0);
ControllerSet inputControllerSet(osimModel,ctrlFile);
cout << "Found " << inputControllerSet.getSize() << " controllers." << endl;
for (int i = 0; i < inputControllerSet.getSize(); i++)
{
DelayedPathReflexController* reflex = dynamic_cast<DelayedPathReflexController*>(&inputControllerSet.get(i));
if (reflex) {
for (int i = 0; i<reflex->getActuatorSet().getSize(); i++)
{
OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&reflex->updActuators().get(i));
if (muscle)
{
muscle->setDefaultActivation(0.0);
}
}
reflex->set_isDisabled(true);
osimModel.addController(reflex->clone());
}
}
for (int i = 0; i < inputControllerSet.getSize(); i++)
{
PrescribedController* prescribed = dynamic_cast<PrescribedController*>(&inputControllerSet.get(i));
if (prescribed) {
prescribed->set_isDisabled(true);
osimModel.addController(prescribed->clone());
}
}
ControllerSet& controllers = osimModel.updControllerSet();
cout << "Model has " << controllers.getSize() << " controllers." << endl;
int numControllers, numParameters;
numControllers = controllers.getSize();
//optLog << "numControllers = " << numControllers << endl;
State& osimState = osimModel.initSystem();
/* Read desired kinematics*/
Storage k_desired = Storage(kinematicsFile);
Storage *q_desired, *u_desired;
osimModel.getSimbodyEngine().formCompleteStorages(osimState, k_desired, q_desired, u_desired);
cout << "Joint velocities estimated." << endl;
if (q_desired->isInDegrees())
{
cout << "Converting coordinates from degrees to radians." << endl;
osimModel.getSimbodyEngine().convertDegreesToRadians(*q_desired);
}
cout << "All desired coordinates are in radians." << endl;
if (u_desired->isInDegrees())
{
cout << "Converting velocities from degrees to radians." << endl;
osimModel.getSimbodyEngine().convertDegreesToRadians(*u_desired);
}
cout << "All desired velocities are in radians." << endl;
Vector qi(osimState.getNQ(), 0.0), ui(osimState.getNU(),0.0);
// get and set initial position
q_desired->getDataAtTime(ti, osimState.getNQ(), qi);
//cout << "Setting q's." << endl;
//qi.dump();
osimState.setQ(qi);
// get and set initial velocity
u_desired->getDataAtTime(ti, osimState.getNU(), ui);
//cout << "Setting u's" << endl;
//ui.dump();
osimState.setU(ui);
cout << "Initial position and velocity set." << endl;
State initial_state = osimState;
for (int j = 0; j<actLevels.getSize(); j++)
{
cout << "\tCo-contraction level " << actLevels[j]
<< " (" << j + 1 << "/" << actLevels.getSize() << "):" << endl;
Model* modelCopy = osimModel.clone();
ExploreCoactivation(modelCopy, initial_state, ti, tf, platformAngle, actLevels[j], resultDir);
}
for (int j = 0; j<reflexGains.getSize(); j++)
{
cout << "\tReflex Gain " << reflexGains[j]
<< " (" << j + 1 << "/" << reflexGains.getSize() << "):" << endl;
Model* modelCopy = osimModel.clone();
ExploreReflexes(modelCopy, initial_state, ti, tf, platformAngle, reflexGains[j], reflexDelay,resultDir);
//simulateDropHeightAndReflexes(dropModel, dropHeights[i], platformAngle, reflexGains[j], reflexDelay);
}
clock_t endTime = std::clock();
fwdLog << "computeTime: " << endTime - startTime << "ms" << endl;
}
catch (const std::exception& ex)
{
std::cout << ex.what() << std::endl;
return 1;
}
catch (...)
{
std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
return 1;
}
std::cout << "\nSimulations Completed\n";
return 0;
// End of main() routine.
}
void simulateDropHeightAndCocontraction(Model* model, double height, double slope, double activation, string dir = "")
{
// get the component of gravity in the y direction
double g = model->getGravity()[1];
// use gravity and height to compute the velocity after falling a distance equal to the input height.
double landingVelocity = g*sqrt(-2 * height / g);
Coordinate& ty = model->updCoordinateSet().get("pelvis_ty");
ty.set_locked(false);
ty.setDefaultSpeedValue(landingVelocity);
/*ty.set_locked(false);
ty.setDefaultValue(-1*height);
ty.set_locked(true);*/
Coordinate& rx = model->updCoordinateSet().get("platform_rx");
rx.set_locked(false);
rx.setDefaultValue(Pi*slope / 180.0);
rx.set_locked(true);
// set all invertor excitations to the predetermined value
PrescribedController* invControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("inverter_controls_r"));
invControls->set_isDisabled(false);
for (int i = 0; i<invControls->getActuatorSet().getSize(); i++)
{
OpenSim::Constant currentAct(activation);
invControls->prescribeControlForActuator(i, currentAct.clone());
OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&invControls->updActuators().get(i));
if (muscle)
{
muscle->setDefaultActivation(activation);
}
}
// set all evertor excitations to the predetermined value
PrescribedController* evControls = dynamic_cast<PrescribedController*> (&model->updControllerSet().get("everter_controls_r"));
evControls->set_isDisabled(false);
for (int i = 0; i<evControls->getActuatorSet().getSize(); i++)
{
OpenSim::Constant currentAct(activation);
evControls->prescribeControlForActuator(i, currentAct.clone());
OpenSim::ActivationFiberLengthMuscle* muscle = dynamic_cast<OpenSim::ActivationFiberLengthMuscle*> (&evControls->updActuators().get(i));
if (muscle)
{
muscle->setDefaultActivation(activation);
}
}
// turn off other everter and inverter controllers
model->updControllerSet().get("AnkleEverterReflexes").set_isDisabled(true);
model->updControllerSet().get("AnkleInverterReflexes").set_isDisabled(true);
model->updControllerSet().get("AnkleEverterDelayedReflexes").set_isDisabled(true);
model->updControllerSet().get("AnkleInverterDelayedReflexes").set_isDisabled(true);
// Create a force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
SimTK::State& osim_state = model->initSystem();
//ty.setValue(osim_state, -1*height);
ty.setSpeedValue(osim_state, landingVelocity);
model->getMultibodySystem().realize(osim_state, Stage::Velocity);
model->equilibrateMuscles(osim_state);
// Create the integrator for integrating system dynamics
SimTK::RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(1.0e-6);
integrator.setMaximumStepSize(1);
integrator.setMinimumStepSize(1e-8);
// Create the manager managing the forward integration and its outputs
Manager manager(*model, integrator);
// Integrate from initial time to final time
double ti = 0;
double tf = 0.3; //300 ms after landing
manager.setInitialTime(ti);
manager.setFinalTime(tf);
cout << "\nIntegrating from " << ti << " to " << tf << endl;
manager.integrate(osim_state);
//////////////////////////////
// SAVE THE RESULTS TO FILE //
//////////////////////////////
// Save the model states from forward integration
Storage statesDegrees(manager.getStateStorage());
char trial_name[100];
sprintf(trial_name, "%sheight_%.1f_incline_%.1f_activation_%.1f", dir, height, slope, activation);
statesDegrees.print(string(trial_name) + string("_states.sto"));
// Save the forces
reporter->getForceStorage().print(string(trial_name) + string("_forces.sto"));
// Save the controls
model->printControlStorage(trial_name + string("_controls.sto"));
// save the model
model->print(string(trial_name) + string("_model.osim"));
}
