I'm working on a lower body model. I created a simple model with a Pelvis, a femur and two antagonistic muscles (Thelen2003). Now I want to use a controller to control the activations of the muscles.
But the bone keeps switching back and forth like a pendulum no matter if i use the controller or not.
How can I properly control the muscles?
Any help would be appreciated.
Code: Select all
Body *addPelvis(Model &model) {
// Properties of the pelvis (taken from the Gait 2392 model)
double pelvisMass = 11.777;
SimTK::Vec3 pelvisMassCenter(-0.0707, 0.0, 0.0);
SimTK::Inertia pelvisInertia(0.1028, 0.0871, 0.0579);
Body &ground = model.getGroundBody();
Body *pelvis = new Body("pelvis", pelvisMass, pelvisMassCenter, pelvisInertia);
pelvis->addDisplayGeometry("pelvis.vtp");
pelvis->addDisplayGeometry("l_pelvis.vtp");
pelvis->addDisplayGeometry("sacrum.vtp");
WeldJoint *ground_pelvis = new WeldJoint("ground_pelvis",
ground, Vec3(0, 1, 0), Vec3(0),
*pelvis, Vec3(0), Vec3(0));
model.addBody(pelvis);
return pelvis;
}
Body *addFemur(Model &model, Body* pelvis) {
// Properties of the femur (taken from the Gait 2392 model)
double femurMass = 9.3014;
Vec3 femurMassCenter(0.0, -0.17, 0.0);
Inertia femurInertia(0.1339, 0.0351, 0.1412);
Body &ground = model.getGroundBody();
Body *femur = new OpenSim::Body("femur_r", femurMass,
femurMassCenter, femurInertia);
femur->addDisplayGeometry("femur.vtp");
double x = -0.0707, y = -0.0661, z = 0.0835;
Vec3 locInParent(x, y, z);
PinJoint *hip = new PinJoint("hip_r",
*pelvis, locInParent, Vec3(0),
*femur, Vec3(0), Vec3(0));
CoordinateSet &jointCoordinateSet = hip->upd_CoordinateSet();
double angleRange[2] = { -2.0*SimTK::Pi/3.0, 2.0*SimTK::Pi/3.0 };
jointCoordinateSet[0].setRange(angleRange);
model.addBody(femur);
return femur;
}
Muscle *addRightRectusFemoris(Model &model, Body* pelvis, Body* femur) {
double maxIsometricForce = 1000.0, optFiberLength = .1, tendonSlackLength = .2, pennationAngle = 0.0;
Thelen2003Muscle *rf = new Thelen2003Muscle("rectfem_r", maxIsometricForce, optFiberLength, tendonSlackLength, pennationAngle);
rf->addNewPathPoint("rectfem_r-p1", *pelvis, Vec3(-0.03, -0.031, 0.097));
rf->addNewPathPoint("bifemlh_r-p2", *femur, Vec3(0.033, -0.403, 0.002));
rf->setOptimalFiberLength(optFiberLength);
model.addForce(rf);
return rf;
}
//Just a pseudo biceps femoris (not attached to tibia)
Muscle *addRightBicepsFemoris(Model &model, Body *pelvis, Body *femur) {
double maxIsometricForce = 1000.0, optFiberLength = .1, tendonSlackLength = .2, pennationAngle = 0.0;
Thelen2003Muscle *bf = new Thelen2003Muscle("bifemlh_r", maxIsometricForce, optFiberLength, tendonSlackLength, pennationAngle);
bf->addNewPathPoint("bifemlh_r-p1", *pelvis, SimTK::Vec3(-0.126, -0.103, 0.069));
bf->addNewPathPoint("bifemlh_r-p2", *femur, SimTK::Vec3(0.007, -0.384, -0.027));
bf->setOptimalFiberLength(optFiberLength);
model.addForce(bf);
return bf;
}
void addController(Model &model, double initialTime, double finalTime) {
// Create a prescribed controller that simply applies controls as function of time
// For muscles, controls are normalized motor-neuron excitations
PrescribedController *muscleController = new PrescribedController();
muscleController->setActuators(model.updActuators());
// Define linear functions for the control values for the two muscles
OpenSim::Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles
OpenSim::Array<double> slopeAndIntercept2(0.0, 2);
// muscle1 control has slope of -1 starting 1 at t = 0
slopeAndIntercept1[0] = -1.0/(finalTime - initialTime); slopeAndIntercept1[1] = 1.0;
// muscle2 control has slope of 0.95 starting 0.05 at t = 0
slopeAndIntercept2[0] = 0.95/(finalTime - initialTime); slopeAndIntercept2[1] = 0.05;
// Set the indiviudal muscle control functions for the prescribed muscle controller
muscleController->prescribeControlForActuator("rectfem_r", new OpenSim::LinearFunction(slopeAndIntercept1));
muscleController->prescribeControlForActuator("bifemlh_r", new OpenSim::LinearFunction(slopeAndIntercept1));
// Add the muscle controller to the model
model.addController(muscleController);
}
void constructModel(Model &model) {
model.setGravity(Vec3(0));
cout << "creating pelvis..." << endl;
Body *pelvis = addPelvis(model);
cout << "creating femur..." << endl;
Body *femur = addFemur(model, pelvis);
cout << "creating right rectus femoris..." << endl;
Muscle *rectfem = addRightRectusFemoris(model, pelvis, femur);
cout << "creating right biceps femoris..." << endl;
Muscle *bifem = addRightBicepsFemoris(model, pelvis, femur);
cout << "creating floor..." << endl;
ContactHalfSpace *floor = new OpenSim::ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK_PI), model.getGroundBody(), "floor");
model.addContactGeometry(floor);
ForceReporter *reporter = new OpenSim::ForceReporter(&model);
model.addAnalysis(reporter);
cout << "model ready!" << endl;
}
void doSimulation(OpenSim::Model &model, double initialTime, double finalTime)
{
State &si = model.initSystem();
RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
integrator.setAccuracy(1.0e-2);
Manager manager(model, integrator);
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
manager.integrate(si);
Storage statesDegrees(manager.getStateStorage());
statesDegrees.print("FullLegs_states.sto");
// Save the forces
OpenSim::Analysis &reporter = model.getAnalysisSet()[0];
((OpenSim::ForceReporter &)reporter).getForceStorage().print("FullLegs_forces.mot");
}
int main()
{
cout << "start..." << endl;
double initialTime = 0.0;
double finalTime = 1.0;
clock_t ts_start = clock();
try {
Model osimModel;
osimModel.setName("FullLegs");
constructModel(osimModel);
//addController(osimModel, initialTime, finalTime);
osimModel.print("FullLegs.osim");
osimModel.setUseVisualizer(false);
cout << "Do integration..." << endl;
try {
doSimulation(osimModel, initialTime, finalTime);
} catch (exception ex) {
cerr << ex.what() << endl;
cin.get();
return 1;
}
cout << "done!" << endl;
} catch (OpenSim::Exception ex) {
cout << ex.getMessage() << endl;
cin.get();
return 1;
} catch (exception ex) {
cout << ex.what() << endl;
cin.get();
return 1;
} catch (...) {
cin.get();
return 1;
}
cin.get();
return 0;
}