void addExtensionController(Model& model)
{
PrescribedController* controller = new PrescribedController();
controller->setName( "extension_controller");
controller->setActuators( model.updActuators());
double control_time[2] = {0.01, 0.02}; // time nodes for linear function
double control_acts[2] = {0.0, 1.0}; // force values at t1 and t2
//control_func->setName( "constant_control_func");
string muscle_name;
for (int i=0; i<model.getActuators().getSize(); i++)
{
muscle_name = model.getActuators().get(i).getName();
// activate quadriceps
if (muscle_name == "rect_fem_r" || muscle_name == "vas_med_r" || muscle_name == "vas_int_r" || muscle_name == "vas_lat_r" )
{
Constant* ccf = new Constant(0.8);
//PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts);
controller->prescribeControlForActuator( i, ccf);
}
else
{
Constant* zccf = new Constant(0);
controller->prescribeControlForActuator( i, zccf);
}
}
model.addController( controller);
}
void addFlexionController(Model& model)
{
PrescribedController* controller = new PrescribedController();
controller->setName( "flexion_controller");
controller->setActuators( model.updActuators());
double control_time[2] = {0, 0.05}; // time nodes for linear function
double control_acts[2] = {1.0, 0}; // force values at t1 and t2
string muscle_name;
for (int i=0; i<model.getActuators().getSize(); i++)
{
muscle_name = model.getActuators().get(i).getName();
// hamstrings: bi*, semi*
if ( muscle_name == "bifemlh_r" || muscle_name == "bifemsh_r" || muscle_name == "grac_r" \
|| muscle_name == "lat_gas_r" || muscle_name == "med_gas_r" || muscle_name == "sar_r" \
|| muscle_name == "semimem_r" || muscle_name == "semiten_r")
{
Constant* ccf = new Constant(1.0);
//PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts);
controller->prescribeControlForActuator( i, ccf);
}
else
{
Constant* zccf = new Constant(0);
controller->prescribeControlForActuator( i, zccf);
}
}
model.addController( controller);
}
/**
* Run a simulation of block sliding with contact on by two muscles sliding with contact
*/
int main()
{
try {
// Create a new OpenSim model
Model osimModel;
osimModel.setName("osimModel");
double Pi = SimTK::Pi;
// Get the ground body
OpenSim::Body& ground = osimModel.getGroundBody();
ground.addDisplayGeometry("checkered_floor.vtp");
// create linkage body
double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
Vec3 linkageMassCenter(0,linkageLength/2,0);
Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);
OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
// Graphical representation
linkage1->addDisplayGeometry("cylinder.vtp");
//This cylinder.vtp geometry is 1 meter tall, 1 meter diameter. Scale and shift it to look pretty
GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet();
DisplayGeometry& thinCylinder = geometry[0];
thinCylinder.setScaleFactors(linkageDimensions);
thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0)));
linkage1->addDisplayGeometry("sphere.vtp");
//This sphere.vtp is 1 meter in diameter. Scale it.
geometry[1].setScaleFactors(Vec3(0.1));
// Creat a second linkage body
OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
linkage2->setName("linkage2");
// Creat a block to be the pelvis
double blockMass = 20.0, blockSideLength = 0.2;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
block->addDisplayGeometry("block.vtp");
//This block.vtp is 0.1x0.1x0.1 meters. scale its appearance
block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0));
// Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);
PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1,
locationInChild, orientationInChild);
PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
locationInChild, orientationInChild);
PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
locationInChild, orientationInChild);
double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
ankleCoordinateSet[0].setName("q1");
ankleCoordinateSet[0].setRange(range);
CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
kneeCoordinateSet[0].setName("q2");
kneeCoordinateSet[0].setRange(range);
CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
hipCoordinateSet[0].setName("q3");
hipCoordinateSet[0].setRange(range);
// Add the bodies to the model
osimModel.addBody(linkage1);
osimModel.addBody(linkage2);
osimModel.addBody(block);
// Define contraints on the model
// Add a point on line constraint to limit the block to vertical motion
Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
osimModel.addConstraint(lineConstraint);
// Add PistonActuator between the first linkage and the block
Vec3 pointOnBodies(0);
PistonActuator *piston = new PistonActuator();
piston->setName("piston");
piston->setBodyA(linkage1);
piston->setBodyB(block);
piston->setPointA(pointOnBodies);
piston->setPointB(pointOnBodies);
piston->setOptimalForce(200.0);
piston->setPointsAreGlobal(false);
osimModel.addForce(piston);
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Added ControllableSpring between the first linkage and the second block
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ControllableSpring *spring = new ControllableSpring;
spring->setName("spring");
spring->setBodyA(block);
spring->setBodyB(linkage1);
spring->setPointA(pointOnBodies);
spring->setPointB(pointOnBodies);
spring->setOptimalForce(2000.0);
spring->setPointsAreGlobal(false);
spring->setRestLength(0.8);
osimModel.addForce(spring);
// define the simulation times
double t0(0.0), tf(15);
// create a controller to control the piston and spring actuators
// the prescribed controller sets the controls as functions of time
PrescribedController *legController = new PrescribedController();
// give the legController control over all (two) model actuators
legController->setActuators(osimModel.updActuators());
// specify some control nodes for spring stiffness control
double t[] = {0.0, 4.0, 7.0, 10.0, 15.0};
double x[] = {1.0, 1.0, 0.25, 0.25, 5.0};
// specify the control function for each actuator
legController->prescribeControlForActuator("piston", new Constant(0.1));
legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));
// add the controller to the model
osimModel.addController(legController);
// define the acceration due to gravity
osimModel.setGravity(Vec3(0, -9.80665, 0));
// enable the model visualizer see the model in action, which can be
// useful for debugging
osimModel.setUseVisualizer(true);
// Initialize system
SimTK::State& si = osimModel.initSystem();
// Pin joint initial states
double q1_i = -Pi/4;
double q2_i = - 2*q1_i;
CoordinateSet &coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, q1_i, true);
coordinates[1].setValue(si,q2_i, true);
// Setup integrator and manager
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-3);
ForceReporter *forces = new ForceReporter(&osimModel);
osimModel.updAnalysisSet().adoptAndAppend(forces);
Manager manager(osimModel, integrator);
//Examine the model
osimModel.printDetailedInfo(si, std::cout);
// Save the model
osimModel.print("toyLeg.osim");
// Print out the initial position and velocity states
si.getQ().dump("Initial q's");
si.getU().dump("Initial u's");
std::cout << "Initial time: " << si.getTime() << std::endl;
// Integrate
manager.setInitialTime(t0);
manager.setFinalTime(tf);
std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
manager.integrate(si);
// Save results
osimModel.printControlStorage("SpringActuatedLeg_controls.sto");
Storage statesDegrees(manager.getStateStorage());
osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
//statesDegrees.print("PistonActuatedLeg_states_degrees.mot");
statesDegrees.print("SpringActuatedLeg_states_degrees.mot");
forces->getForceStorage().print("actuator_forces.mot");
}
catch (const std::exception& ex)
{
std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
return 1;
}
std::cout << "Exiting" << std::endl;
return 0;
}
void testMcKibbenActuator()
{
double pressure = 5 * 10e5; // 5 bars
double num_turns = 1.5; // 1.5 turns
double B = 277.1 * 10e-4; // 277.1 mm
using namespace SimTK;
std::clock_t startTime = std::clock();
double mass = 1;
double ball_radius = 10e-6;
Model *model = new Model;
model->setGravity(Vec3(0));
Ground& ground = model->updGround();
McKibbenActuator *actuator = new McKibbenActuator("mckibben", num_turns, B);
OpenSim::Body* ball = new OpenSim::Body("ball", mass ,Vec3(0), mass*SimTK::Inertia::sphere(0.1));
ball->scale(Vec3(ball_radius), false);
actuator->addNewPathPoint("mck_ground", ground, Vec3(0));
actuator->addNewPathPoint("mck_ball", *ball, Vec3(ball_radius));
Vec3 locationInParent(0, ball_radius, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
SliderJoint *ballToGround = new SliderJoint("ballToGround", ground, locationInParent, orientationInParent, *ball, locationInBody, orientationInBody);
ballToGround->updCoordinate().setName("ball_d");
ballToGround->updCoordinate().setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4));
ballToGround->updCoordinate().set_prescribed(true);
model->addBody(ball);
model->addJoint(ballToGround);
model->addForce(actuator);
PrescribedController* controller = new PrescribedController();
controller->addActuator(*actuator);
controller->prescribeControlForActuator("mckibben", new Constant(pressure));
model->addController(controller);
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
SimTK::State& si = model->initSystem();
model->getMultibodySystem().realize(si, Stage::Position);
double final_t = 10.0;
double nsteps = 10;
double dt = final_t / nsteps;
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(1e-7);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
for (int i = 1; i <= nsteps; i++){
manager.setFinalTime(dt*i);
manager.integrate(si);
model->getMultibodySystem().realize(si, Stage::Velocity);
Vec3 pos;
model->updSimbodyEngine().getPosition(si, *ball, Vec3(0), pos);
double applied = actuator->computeActuation(si);;
double theoretical = (pressure / (4* pow(num_turns,2) * SimTK::Pi)) * (3*pow(pos(0), 2) - pow(B, 2));
ASSERT_EQUAL(applied, theoretical, 10.0);
manager.setInitialTime(dt*i);
}
std::cout << " ******** Test McKibbenActuator time = ********" <<
1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
void testClutchedPathSpring()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
double mass = 1;
double stiffness = 100;
double dissipation = 0.3;
double start_h = 0.5;
//double ball_radius = 0.25;
//double omega = sqrt(stiffness/mass);
// Setup OpenSim model
Model* model = new Model;
model->setName("ClutchedPathSpringModel");
model->setGravity(gravity_vec);
//OpenSim bodies
const Ground* ground = &model->getGround();
// body that acts as the pulley that the path wraps over
OpenSim::Body* pulleyBody =
new OpenSim::Body("PulleyBody", mass ,Vec3(0), mass*Inertia::brick(0.1, 0.1, 0.1));
// body the path spring is connected to at both ends
OpenSim::Body* block =
new OpenSim::Body("block", mass ,Vec3(0), mass*Inertia::brick(0.2, 0.1, 0.1));
block->attachGeometry(new Brick(Vec3(0.2, 0.1, 0.1)));
block->scale(Vec3(0.2, 0.1, 0.1), false);
//double dh = mass*gravity_vec(1)/stiffness;
WrapCylinder* pulley = new WrapCylinder();
pulley->set_radius(0.1);
pulley->set_length(0.05);
// Add the wrap object to the body, which takes ownership of it
pulleyBody->addWrapObject(pulley);
// Add joints
WeldJoint* weld =
new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
SliderJoint* slider =
new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {-10, 10};
// Rename coordinates for a slider joint
slider->updCoordinate().setName("block_h");
slider->updCoordinate().setRange(positionRange);
model->addBody(pulleyBody);
model->addJoint(weld);
model->addBody(block);
model->addJoint(slider);
ClutchedPathSpring* spring =
new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);
spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
int N = 10;
for(int i=1; i<N; ++i){
double angle = i*Pi/N;
double x = 0.1*cos(angle);
double y = 0.1*sin(angle);
spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
}
spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));
// BUG in defining wrapping API requires that the Force containing the GeometryPath be
// connected to the model before the wrap can be added
model->addForce(spring);
PrescribedController* controller = new PrescribedController();
controller->addActuator(*spring);
// Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
double timePts[4] = {0.0, 5.0, 6.0, 10.0};
double clutchOnPts[4] = {1.5, -2.0, 0.5, 0.5};
PiecewiseConstantFunction* controlfunc =
new PiecewiseConstantFunction(4, timePts, clutchOnPts);
controller->prescribeControlForActuator("clutch_spring", controlfunc);
model->addController(controller);
model->print("ClutchedPathSpringModel.osim");
//Test deserialization
delete model;
model = new Model("ClutchedPathSpringModel.osim");
// Create the force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
model->setUseVisualizer(false);
SimTK::State& state = model->initSystem();
CoordinateSet& coords = model->updCoordinateSet();
coords[0].setValue(state, start_h);
model->getMultibodySystem().realize(state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setWriteToStorage(true);
manager.setInitialTime(0.0);
double final_t = 4.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
spring = dynamic_cast<ClutchedPathSpring*>(
&model->updForceSet().get("clutch_spring"));
// Now check that the force reported by spring
double model_force = spring->getTension(state);
double stretch0 = spring->getStretch(state);
// the tension should be half the weight of the block
double analytical_force = -0.5*(gravity_vec(1))*mass;
cout << "Tension is: " << model_force << " and should be: " << analytical_force << endl;
// error if the block does not reach equilibrium since spring is clamped
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
// unclamp and continue integrating
manager.setInitialTime(final_t);
final_t = 5.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should go to zero quickly
model_force = spring->getTension(state);
cout << "Tension is: " << model_force << " and should be: 0.0" << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, 0.0, 10*integ_accuracy);
// spring is reclamped at 7s so keep integrating
manager.setInitialTime(final_t);
final_t = 10.0;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should build to support the block again
model_force = spring->getTension(state);
double stretch1 = spring->getStretch(state);
cout << "Tension is: " << model_force << " and should be: "<< analytical_force << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
cout << "Steady stretch at control = 1.0 is " << stretch0 << " m." << endl;
cout << "Steady stretch at control = 0.5 is " << stretch1 << " m." << endl;
ASSERT_EQUAL(2*stretch0, stretch1, 10*integ_accuracy);
manager.getStateStorage().print("clutched_path_spring_states.sto");
model->getControllerSet().printControlStorage("clutched_path_spring_controls.sto");
// Save the forces
reporter->getForceStorage().print("clutched_path_spring_forces.mot");
model->disownAllComponents();
cout << " ********** Test clutched spring time = ********** " <<
1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
//==============================================================================
// Test Cases
//==============================================================================
void testTorqueActuator()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
// Setup OpenSim model
Model *model = new Model;
// turn off gravity
model->setGravity(Vec3(0));
//OpenSim bodies
const Ground& ground = model->getGround();
//Cylindrical bodies
double r = 0.25, h = 1.0;
double m1 = 1.0, m2 = 2.0;
Inertia j1 = m1*Inertia::cylinderAlongY(r, h);
Inertia j2 = m2*Inertia::cylinderAlongY(r, h);
//OpenSim bodies
OpenSim::Body* bodyA
= new OpenSim::Body("A", m1, Vec3(0), j1);
OpenSim::Body* bodyB
= new OpenSim::Body("B", m2, Vec3(0), j2);
// connect bodyA to ground with 6dofs
FreeJoint* base =
new FreeJoint("base", ground, Vec3(0), Vec3(0), *bodyA, Vec3(0), Vec3(0));
model->addBody(bodyA);
model->addJoint(base);
// connect bodyA to bodyB by a Ball joint
BallJoint* bInA = new BallJoint("bInA", *bodyA, Vec3(0,-h/2, 0), Vec3(0),
*bodyB, Vec3(0, h/2, 0), Vec3(0));
model->addBody(bodyB);
model->addJoint(bInA);
// specify magnitude and direction of applied torque
double torqueMag = 2.1234567890;
Vec3 torqueAxis(1/sqrt(2.0), 0, 1/sqrt(2.0));
Vec3 torqueInG = torqueMag*torqueAxis;
State state = model->initSystem();
model->getMultibodySystem().realize(state, Stage::Dynamics);
Vector_<SpatialVec>& bodyForces =
model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
bodyForces.dump("Body Forces before applying torque");
model->getMatterSubsystem().addInBodyTorque(state, bodyA->getMobilizedBodyIndex(),
torqueMag*torqueAxis, bodyForces);
model->getMatterSubsystem().addInBodyTorque(state, bodyB->getMobilizedBodyIndex(),
-torqueMag*torqueAxis, bodyForces);
bodyForces.dump("Body Forces after applying torque to bodyA and bodyB");
model->getMultibodySystem().realize(state, Stage::Acceleration);
const Vector& udotBody = state.getUDot();
udotBody.dump("Accelerations due to body forces");
// clear body forces
bodyForces *= 0;
// update mobility forces
Vector& mobilityForces = model->getMultibodySystem()
.updMobilityForces(state, Stage::Dynamics);
// Apply torques as mobility forces of the ball joint
for(int i=0; i<3; ++i){
mobilityForces[6+i] = torqueInG[i];
}
model->getMultibodySystem().realize(state, Stage::Acceleration);
const Vector& udotMobility = state.getUDot();
udotMobility.dump("Accelerations due to mobility forces");
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
// Then check if they are equal
for(int i=0; i<udotMobility.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotBody[i], 1.0e-12);
}
// clear the mobility forces
mobilityForces = 0;
//Now add the actuator to the model and control it to generate the same
//torque as applied directly to the multibody system (above)
// Create and add the torque actuator to the model
TorqueActuator* actuator =
new TorqueActuator(*bodyA, *bodyB, torqueAxis, true);
actuator->setName("torque");
model->addForce(actuator);
// Create and add a controller to control the actuator
PrescribedController* controller = new PrescribedController();
controller->addActuator(*actuator);
// Apply torque about torqueAxis
controller->prescribeControlForActuator("torque", new Constant(torqueMag));
model->addController(controller);
/*
ActuatorPowerProbe* powerProbe = new ActuatorPowerProbe(Array<string>("torque",1),false, 1);
powerProbe->setOperation("integrate");
powerProbe->setInitialConditions(Vector(1, 0.0));
*/
//model->addProbe(powerProbe);
model->print("TestTorqueActuatorModel.osim");
model->setUseVisualizer(false);
// get a new system and state to reflect additions to the model
state = model->initSystem();
model->computeStateVariableDerivatives(state);
const Vector &udotTorqueActuator = state.getUDot();
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotTorqueActuator.norm() != 0.0);
// Then verify that the TorqueActuator also generates the same acceleration
// as the equivalent applied mobility force
for(int i=0; i<udotMobility.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotTorqueActuator[i], 1.0e-12);
}
// determine the initial kinetic energy of the system
/*double iKE = */model->getMatterSubsystem().calcKineticEnergy(state);
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
double final_t = 1.00;
manager.setFinalTime(final_t);
manager.integrate(state);
model->computeStateVariableDerivatives(state);
/*double fKE = */model->getMatterSubsystem().calcKineticEnergy(state);
// Change in system kinetic energy can only be attributable to actuator work
//double actuatorWork = (powerProbe->getProbeOutputs(state))[0];
// test that this is true
//ASSERT_EQUAL(actuatorWork, fKE-iKE, integ_accuracy);
// Before exiting lets see if copying the spring works
TorqueActuator* copyOfActuator = actuator->clone();
ASSERT(*copyOfActuator == *actuator);
// Check that de/serialization works
Model modelFromFile("TestTorqueActuatorModel.osim");
ASSERT(modelFromFile == *model, __FILE__, __LINE__,
"Model from file FAILED to match model in memory.");
std::cout << " ********** Test TorqueActuator time = ********** " <<
1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
/**
* Run a simulation of a sliding block being pulled by two muscle
*/
int main()
{
std::clock_t startTime = std::clock();
try {
///////////////////////////////////////////////
// DEFINE THE SIMULATION START AND END TIMES //
///////////////////////////////////////////////
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 10.0;
///////////////////////////////////////////
// DEFINE BODIES AND JOINTS OF THE MODEL //
///////////////////////////////////////////
// Create an OpenSim model and set its name
Model osimModel;
osimModel.setName("tugOfWar");
// GROUND FRAME
// Get a reference to the model's ground body
Ground& ground = osimModel.updGround();
// Add display geometry to the ground to visualize in the GUI
ground.attachGeometry(new Mesh("ground.vtp"));
ground.attachGeometry(new Mesh("anchor1.vtp"));
ground.attachGeometry(new Mesh("anchor2.vtp"));
// BLOCK BODY
// Specify properties of a 20 kg, 10cm length block body
double blockMass = 20.0, blockSideLength = 0.1;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength,
blockSideLength, blockSideLength);
// Create a new block body with the specified properties
OpenSim::Body *block = new OpenSim::Body("block", blockMass,
blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->attachGeometry(new Mesh("block.vtp"));
// FREE JOINT
// Create a new free joint with 6 degrees-of-freedom (coordinates)
// between the block and ground bodies
double halfLength = blockSideLength/2.0;
Vec3 locationInParent(0, halfLength, 0), orientationInParent(0);
Vec3 locationInBody(0, halfLength, 0), orientationInBody(0);
FreeJoint *blockToGround = new FreeJoint("blockToGround", ground,
locationInParent, orientationInParent,
*block, locationInBody, orientationInBody);
// Set the angle and position ranges for the free (6-degree-of-freedom)
// joint between the block and ground frames.
double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
double positionRange[2] = {-1, 1};
blockToGround->updCoordinate(FreeJoint::Coord::Rotation1X).setRange(angleRange);
blockToGround->updCoordinate(FreeJoint::Coord::Rotation2Y).setRange(angleRange);
blockToGround->updCoordinate(FreeJoint::Coord::Rotation3Z).setRange(angleRange);
blockToGround->updCoordinate(FreeJoint::Coord::TranslationX).setRange(positionRange);
blockToGround->updCoordinate(FreeJoint::Coord::TranslationY).setRange(positionRange);
blockToGround->updCoordinate(FreeJoint::Coord::TranslationZ).setRange(positionRange);
// Add the block body to the model
osimModel.addBody(block);
osimModel.addJoint(blockToGround);
///////////////////////////////////////
// DEFINE FORCES ACTING ON THE MODEL //
///////////////////////////////////////
// MUSCLE FORCES
// Create two new muscles
double maxIsometricForce = 1000.0, optimalFiberLength = 0.2,
tendonSlackLength = 0.1, pennationAngle = 0.0,
fatigueFactor = 0.30, recoveryFactor = 0.20;
// fatigable muscle (Millard2012EquilibriumMuscle with fatigue)
FatigableMuscle* fatigable = new FatigableMuscle("fatigable",
maxIsometricForce, optimalFiberLength, tendonSlackLength,
pennationAngle, fatigueFactor, recoveryFactor);
// original muscle model (muscle without fatigue)
Millard2012EquilibriumMuscle* original =
new Millard2012EquilibriumMuscle("original",
maxIsometricForce, optimalFiberLength, tendonSlackLength,
pennationAngle);
// Define the path of the muscles
fatigable->addNewPathPoint("fatigable-point1", ground,
Vec3(0.0, halfLength, -0.35));
fatigable->addNewPathPoint("fatigable-point2", *block,
Vec3(0.0, halfLength, -halfLength));
original->addNewPathPoint("original-point1", ground,
Vec3(0.0, halfLength, 0.35));
original->addNewPathPoint("original-point2", *block,
Vec3(0.0, halfLength, halfLength));
// Define the default states for the two muscles
// Activation
fatigable->setDefaultActivation(0.01);
original->setDefaultActivation(0.01);
// Fiber length
fatigable->setDefaultFiberLength(optimalFiberLength);
original->setDefaultFiberLength(optimalFiberLength);
// Add the two muscles (as forces) to the model
osimModel.addForce(fatigable);
osimModel.addForce(original);
///////////////////////////////////
// DEFINE CONTROLS FOR THE MODEL //
///////////////////////////////////
// Create a prescribed controller that simply supplies controls as
// a function of time.
// For muscles, controls are normalized stoor-neuron excitations
PrescribedController *muscleController = new PrescribedController();
muscleController->setActuators(osimModel.updActuators());
// Set the prescribed muscle controller to use the same muscle control function for each muscle
muscleController->prescribeControlForActuator("fatigable", new Constant(1.0));
muscleController->prescribeControlForActuator("original", new Constant(1.0));
// Add the muscle controller to the model
osimModel.addController(muscleController);
// Add a Muscle analysis
MuscleAnalysis* muscAnalysis = new MuscleAnalysis(&osimModel);
Array<std::string> coords(blockToGround->getCoordinate(FreeJoint::Coord::TranslationZ).getName(),1);
muscAnalysis->setCoordinates(coords);
muscAnalysis->setComputeMoments(false);
osimModel.addAnalysis(muscAnalysis);
// Turn on the visualizer to view the simulation run live.
osimModel.setUseVisualizer(false);
//////////////////////////
// PERFORM A SIMULATION //
//////////////////////////
// Initialize the system and get the state
SimTK::State& si = osimModel.initSystem();
// Init coords to 0 and lock the rotational degrees of freedom so the block doesn't twist
CoordinateSet& coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, 0);
coordinates[1].setValue(si, 0);
coordinates[2].setValue(si, 0);
coordinates[3].setValue(si, 0);
coordinates[4].setValue(si, 0);
coordinates[5].setValue(si, 0);
coordinates[0].setLocked(si, true);
coordinates[1].setLocked(si, true);
coordinates[2].setLocked(si, true);
// Last coordinate (index 5) is the Z translation of the block
coordinates[4].setLocked(si, true);
// Compute initial conditions for muscles
osimModel.equilibrateMuscles(si);
// Create the integrator, force reporter, and manager for the simulation.
// Create the integrator
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-6);
// Create the force reporter
ForceReporter* reporter = new ForceReporter(&osimModel);
osimModel.updAnalysisSet().adoptAndAppend(reporter);
// Create the manager
Manager manager(osimModel, integrator);
// Print out details of the model
osimModel.printDetailedInfo(si, std::cout);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
//////////////////////////////
// SAVE THE RESULTS TO FILE //
//////////////////////////////
// Save the simulation results
// Save the states
auto statesTable = manager.getStatesTable();
STOFileAdapter_<double>::write(statesTable,
"tugOfWar_fatigue_states.sto");
auto forcesTable = reporter->getForcesTable();
STOFileAdapter_<double>::write(forcesTable,
"tugOfWar_fatigue_forces.sto");
// Save the muscle analysis results
IO::makeDir("MuscleAnalysisResults");
muscAnalysis->printResults("fatigue", "MuscleAnalysisResults");
// Save the OpenSim model to a file
osimModel.print("tugOfWar_fatigue_model.osim");
}
catch (const std::exception& ex)
{
std::cout << ex.what() << std::endl;
return 1;
}
catch (...)
{
std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
return 1;
}
std::cout << "main() routine time = " << 1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n";
std::cout << "OpenSim example completed successfully.\n";
return 0;
}
/**
* Method for building the Luxo Jr articulating model. It sets up the system of
* rigid bodies and joint articulations to define Luxo Jr lamp geometry.
*/
void createLuxoJr(OpenSim::Model &model){
// Create base
//--------------
OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0),
Inertia::cylinderAlongY(0.1, baseHeight));
// Add visible geometry
base->attachMeshGeometry("Base_meters.obj");
// Define base to float relative to ground via free joint
FreeJoint* base_ground = new FreeJoint("base_ground",
// parent body, location in parent body, orientation in parent
model.getGround(), Vec3(0.0), Vec3(0.0),
// child body, location in child body, orientation in child
*base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0));
// add base to model
model.addBody(base); model.addJoint(base_ground);
/*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) {
base_ground->upd_CoordinateSet()[i].set_locked(true);
}*/
// Fix a frame to the base axis for attaching the bottom bracket
SimTK::Transform* shift_and_rotate = new SimTK::Transform();
//shift_and_rotate->setToZero();
shift_and_rotate->set(Rotation(-1*SimTK::Pi/2,
SimTK::CoordinateAxis::XCoordinateAxis()),
Vec3(0.0, bracket_location, 0.0));
PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base",
*base, *shift_and_rotate);
// Create bottom bracket
//-----------------------
OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket",
bracket_mass, Vec3(0.0),
Inertia::brick(0.03, 0.03, 0.015));
// add bottom bracket to model
model.addBody(bottom_bracket);
// Fix a frame to the bracket for attaching joint
shift_and_rotate->setP(Vec3(0.0));
PhysicalOffsetFrame pivot_frame_on_bottom_bracket(
"pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate);
// Add visible geometry
bottom_bracket->attachMeshGeometry("bottom_bracket_meters.obj");
// Make bottom bracket to twist on base with vertical pin joint.
// You can create a joint from any existing physical frames attached to
// rigid bodies. One way to reference them is by name, like this...
PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base,
pivot_frame_on_bottom_bracket);
base_pivot->append_frames(pivot_frame_on_base);
base_pivot->append_frames(pivot_frame_on_bottom_bracket);
// add base pivot joint to the model
model.addJoint(base_pivot);
// add some damping to the pivot
// initialized to zero stiffness and damping
BushingForce* pivotDamper = new BushingForce("pivot_bushing",
"pivot_frame_on_base",
"pivot_frame_on_bottom_bracket");
pivotDamper->set_rotational_damping(pivot_damping);
model.addForce(pivotDamper);
// Create posterior leg
//-----------------------
OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar",
bar_mass,
Vec3(0.0),
Inertia::brick(leg_bar_dimensions/2.0));
posteriorLegBar->attachMeshGeometry("Leg_meters.obj");
PhysicalOffsetFrame posterior_knee_on_bottom_bracket(
"posterior_knee_on_bottom_bracket",
*bottom_bracket, Transform(posterior_bracket_hinge_location) );
PhysicalOffsetFrame posterior_knee_on_posterior_bar(
"posterior_knee_on_posterior_bar",
*posteriorLegBar, Transform(inferior_bar_hinge_location) );
// Attach posterior leg to bottom bracket using another pin joint.
// Another way to reference physical frames in a joint is by creating them
// in place, like this...
OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee",
posterior_knee_on_bottom_bracket,
posterior_knee_on_posterior_bar);
// posteriorKnee will own and serialize the attachment offset frames
posteriorKnee->append_frames(posterior_knee_on_bottom_bracket);
posteriorKnee->append_frames(posterior_knee_on_posterior_bar);
// add posterior leg to model
model.addBody(posteriorLegBar); model.addJoint(posteriorKnee);
// allow this joint's coordinate to float freely when assembling constraints
// the joint we create next will drive the pose of the 4-bar linkage
posteriorKnee->upd_CoordinateSet()[0]
.set_is_free_to_satisfy_constraints(true);
// Create anterior leg Hlink
//----------------------------
OpenSim::Body* leg_Hlink = new OpenSim::Body("leg_Hlink",
bar_mass,
Vec3(0.0),
Inertia::brick(leg_Hlink_dimensions/2.0));
leg_Hlink->attachMeshGeometry("H_Piece_meters.obj");
PhysicalOffsetFrame anterior_knee_on_bottom_bracket(
"anterior_knee_on_bottom_bracket",
*bottom_bracket, Transform(anterior_bracket_hinge_location));
PhysicalOffsetFrame anterior_knee_on_anterior_bar(
"anterior_knee_on_anterior_bar",
*leg_Hlink, Transform(inferior_Hlink_hinge_location));
// Connect anterior leg to bottom bracket via pin joint
OpenSim::PinJoint* anterior_knee = new OpenSim::PinJoint("anterior_knee",
anterior_knee_on_bottom_bracket,
anterior_knee_on_anterior_bar);
anterior_knee->append_frames(anterior_knee_on_bottom_bracket);
anterior_knee->append_frames(anterior_knee_on_anterior_bar);
// add anterior leg to model
model.addBody(leg_Hlink); model.addJoint(anterior_knee);
// this anterior knee joint defines the motion of the lower 4-bar linkage
// set it's default coordinate value to a slightly flexed position.
anterior_knee->upd_CoordinateSet()[0].set_default_value(SimTK::Pi/6);
// Create pelvis bracket
//-----------------------
OpenSim::Body* pelvisBracket = new OpenSim::Body("pelvis_bracket",
bracket_mass,
Vec3(0.0),
Inertia::brick(pelvis_dimensions/2.0));
pelvisBracket->attachMeshGeometry("Pelvis_bracket_meters.obj");
// Connect pelvis to Hlink via pin joint
SimTK::Transform pelvis_anterior_shift(
anterior_superior_pelvis_pin_location);
PhysicalOffsetFrame anterior_hip_on_Hlink(
"anterior_hip_on_Hlink",
*leg_Hlink, Transform(superior_Hlink_hinge_location));
PhysicalOffsetFrame anterior_hip_on_pelvis(
"anterior_hip_on_pelvis",
*pelvisBracket, pelvis_anterior_shift);
OpenSim::PinJoint* anteriorHip = new OpenSim::PinJoint("anterior_hip",
anterior_hip_on_Hlink,
anterior_hip_on_pelvis);
anteriorHip->append_frames(anterior_hip_on_Hlink);
anteriorHip->append_frames(anterior_hip_on_pelvis);
// add anterior leg to model
model.addBody(pelvisBracket); model.addJoint(anteriorHip);
// since the previous, anterior knee joint drives the pose of the lower
// 4-bar linkage, set the anterior hip angle such that it's free to satisfy
// constraints that couple it to the 4-bar linkage.
anteriorHip->upd_CoordinateSet()[0]
.set_is_free_to_satisfy_constraints(true);
// Close the loop for the lower, four-bar linkage with a constraint
//------------------------------------------------------------------
// Create and configure point on line constraint
OpenSim::PointOnLineConstraint* posteriorHip =
new OpenSim::PointOnLineConstraint();
posteriorHip->setLineBodyByName(pelvisBracket->getName());
posteriorHip->setLineDirection(Vec3(0.0,0.0,1.0));
posteriorHip->setPointOnLine(inferior_pelvis_pin_location);
posteriorHip->setFollowerBodyByName(posteriorLegBar->getName());
posteriorHip->setPointOnFollower(superior_bar_hinge_location);
// add constraint to model
model.addConstraint(posteriorHip);
// Create chest piece
//-----------------------
OpenSim::Body* chest = new OpenSim::Body("chest_bar", bar_mass,
Vec3(0.0),
Inertia::brick(torso_bar_dimensions/2.0));
chest->attachMeshGeometry("Anterior_torso_bar.obj");
PhysicalOffsetFrame anterior_torso_hinge_on_pelvis(
"anterior_torso_hinge_on_pelvis",
*pelvisBracket, Transform(anterior_superior_pelvis_pin_location) );
PhysicalOffsetFrame anterior_torso_hinge_on_chest(
"anterior_torso_hinge_on_chest",
*chest, Transform(inferior_torso_hinge_location) );
// Attach chest piece to pelvice with pin joint
OpenSim::PinJoint* anteriorTorsoHinge = new OpenSim::PinJoint(
"anterior_torso_hinge",
anterior_torso_hinge_on_pelvis,
anterior_torso_hinge_on_chest);
anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_pelvis);
anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_chest);
// add posterior leg to model
model.addBody(chest); model.addJoint(anteriorTorsoHinge);
// set torso rotation slightly anterior
anteriorTorsoHinge->upd_CoordinateSet()[0].setDefaultValue(-1*SimTK::Pi/4);
// Create chest piece
//-----------------------
OpenSim::Body* back = new OpenSim::Body("back_bar", bar_mass,
Vec3(0.0),
Inertia::brick(torso_bar_dimensions/2.0));
back->attachMeshGeometry("Posterior_torso_bar.obj");
PhysicalOffsetFrame posterior_torso_hinge_on_pelvis(
"posterior_torso_hinge_on_pelvis",
*pelvisBracket, Transform(posterior_superior_pelvis_pin_location) );
PhysicalOffsetFrame posterior_torso_hinge_on_back(
"posterior_torso_hinge_on_back",
*back, Transform(back_peg_center) );
// Attach chest piece to pelvis with pin joint
OpenSim::PinJoint* posteriorTorsoHinge = new OpenSim::PinJoint(
"posterior_torso_hinge",
posterior_torso_hinge_on_pelvis,
posterior_torso_hinge_on_back);
posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_pelvis);
posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_back);
// add posterior leg to model
model.addBody(back); model.addJoint(posteriorTorsoHinge);
// set posterior back joint to freely follow anterior joint through 4-bar
// linkage coupling.
posteriorTorsoHinge->upd_CoordinateSet()[0]
.set_is_free_to_satisfy_constraints(true);
// Create shoulder bracket
//-----------------------
OpenSim::Body* shoulderBracket = new OpenSim::Body("shoulder_bracket",
bracket_mass,
Vec3(0.0),
Inertia::brick(shoulder_dimensions/2.0));
shoulderBracket->attachMeshGeometry("Shoulder_meters.obj");
// add anterior leg to model
model.addBody(shoulderBracket);
PhysicalOffsetFrame anterior_thoracic_joint_on_chest(
"anterior_thoracic_joint_on_chest",
*chest, Transform(superior_torso_hinge_location) );
PhysicalOffsetFrame anterior_thoracic_joint_on_shoulder(
"anterior_thoracic_joint_on_shoulder",
*shoulderBracket, Transform(anterior_thoracic_joint_center));
// Connect pelvis to Hlink via pin joint
OpenSim::PinJoint* anteriorThoracicJoint =
new OpenSim::PinJoint("anterior_thoracic_joint",
anterior_thoracic_joint_on_chest,
anterior_thoracic_joint_on_shoulder);
anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_chest);
anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_shoulder);
// add back joint
model.addJoint(anteriorThoracicJoint);
// since the previous, anterior thoracic joint drives the pose of the lower
// 4-bar linkage, set the anterior shoulder angle such that it's free to
// satisfy constraints that couple it to the 4-bar linkage.
anteriorThoracicJoint->upd_CoordinateSet()[0]
.set_is_free_to_satisfy_constraints(true);
// Close the loop for the lower, four-bar linkage with a constraint
//------------------------------------------------------------------
// Create and configure point on line constraint
OpenSim::PointOnLineConstraint* posteriorShoulder =
new OpenSim::PointOnLineConstraint();
posteriorShoulder->setLineBodyByName(shoulderBracket->getName());
posteriorShoulder->setLineDirection(Vec3(0.0,0.0,1.0));
posteriorShoulder->setPointOnLine(posterior_thoracic_joint_center);
posteriorShoulder->setFollowerBodyByName(back->getName());
posteriorShoulder->setPointOnFollower(superior_torso_hinge_location);
// add constraint to model
model.addConstraint(posteriorShoulder);
// Create and add luxo head
OpenSim::Body* head = new OpenSim::Body("head", head_mass, Vec3(0),
Inertia::cylinderAlongX(0.5*head_dimension[1], head_dimension[1]));
head->attachMeshGeometry("luxo_head_meters.obj");
head->attachMeshGeometry("Bulb_meters.obj");
model.addBody(head);
PhysicalOffsetFrame cervical_joint_on_shoulder("cervical_joint_on_shoulder",
*shoulderBracket, Transform(superior_shoulder_hinge_location) );
PhysicalOffsetFrame cervical_joint_on_head("cervical_joint_on_head",
*head, Transform(cervicle_joint_center));
// attach to shoulder via pin joint
OpenSim::PinJoint* cervicalJoint = new OpenSim::PinJoint("cervical_joint",
cervical_joint_on_shoulder,
cervical_joint_on_head);
cervicalJoint->append_frames(cervical_joint_on_shoulder);
cervicalJoint->append_frames(cervical_joint_on_head);
// add a neck joint
model.addJoint(cervicalJoint);
// lock the kneck coordinate so the head doens't spin without actuators or
// passive forces
cervicalJoint->upd_CoordinateSet()[0].set_locked(true);
// Coordinate Limit forces for restricting leg range of motion.
//-----------------------------------------------------------------------
CoordinateLimitForce* kneeLimitForce =
new CoordinateLimitForce(
anterior_knee->get_CoordinateSet()[0].getName(),
knee_flexion_max, joint_softstop_stiffness,
knee_flexion_min, joint_softstop_stiffness,
joint_softstop_damping,
transition_region);
model.addForce(kneeLimitForce);
// Coordinate Limit forces for restricting back range motion.
//-----------------------------------------------------------------------
CoordinateLimitForce* backLimitForce =
new CoordinateLimitForce(
anteriorTorsoHinge->get_CoordinateSet()[0].getName(),
back_extension_max, joint_softstop_stiffness,
back_extension_min, joint_softstop_stiffness,
joint_softstop_damping,
transition_region);
model.addForce(backLimitForce);
// Contact
//-----------------------------------------------------------------------
ContactHalfSpace* floor_surface = new ContactHalfSpace(SimTK::Vec3(0),
SimTK::Vec3(0, 0, -0.5*SimTK::Pi),
model.updGround(), "floor_surface");
OpenSim::ContactMesh* foot_surface = new ContactMesh(
"thin_disc_0.11_by_0.01_meters.obj",
SimTK::Vec3(0),
SimTK::Vec3(0),
*base,
"foot_surface");
// add contact geometry to model
model.addContactGeometry(floor_surface);
model.addContactGeometry(foot_surface);
// define contact as an elastic foundation force
OpenSim::ElasticFoundationForce::ContactParameters* contactParameters =
new OpenSim::ElasticFoundationForce::ContactParameters(
stiffness,
dissipation,
friction,
friction,
viscosity);
contactParameters->addGeometry("foot_surface");
contactParameters->addGeometry("floor_surface");
OpenSim::ElasticFoundationForce* contactForce =
new OpenSim::ElasticFoundationForce(contactParameters);
contactForce->setName("contact_force");
model.addForce(contactForce);
// MUSCLES
//-----------------------------------------------------------------------
// add a knee extensor to control the lower 4-bar linkage
Millard2012EquilibriumMuscle* kneeExtensorRight = new Millard2012EquilibriumMuscle(
"knee_extensor_right",
knee_extensor_F0, knee_extensor_lm0,
knee_extensor_lts, pennationAngle);
kneeExtensorRight->addNewPathPoint("knee_extensor_right_origin", *leg_Hlink,
knee_extensor_origin);
kneeExtensorRight->addNewPathPoint("knee_extensor_right_insertion",
*bottom_bracket,
knee_extensor_insertion);
kneeExtensorRight->set_ignore_tendon_compliance(true);
model.addForce(kneeExtensorRight);
// add a second copy of this knee extensor for the left side
Millard2012EquilibriumMuscle* kneeExtensorLeft =
new Millard2012EquilibriumMuscle(*kneeExtensorRight);
kneeExtensorLeft->setName("kneeExtensorLeft");
// flip the z coordinates of all path points
PathPointSet& points = kneeExtensorLeft->updGeometryPath().updPathPointSet();
for (int i=0; i<points.getSize(); ++i) {
points[i].setLocationCoord(2, -1*points[i].getLocationCoord(2));
}
kneeExtensorLeft->set_ignore_tendon_compliance(true);
model.addForce(kneeExtensorLeft);
// add a back extensor to controll the upper 4-bar linkage
Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle(
"back_extensor_right",
back_extensor_F0, back_extensor_lm0,
back_extensor_lts, pennationAngle);
backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest,
back_extensor_origin);
backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back,
back_extensor_insertion);
backExtensorRight->set_ignore_tendon_compliance(true);
model.addForce(backExtensorRight);
// copy right back extensor and use to make left extensor
Millard2012EquilibriumMuscle* backExtensorLeft =
new Millard2012EquilibriumMuscle(*backExtensorRight);
backExtensorLeft->setName("back_extensor_left");
PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath()
.updPathPointSet();
for (int i=0; i<points.getSize(); ++i) {
pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2));
}
backExtensorLeft->set_ignore_tendon_compliance(true);
model.addForce(backExtensorLeft);
// MUSCLE CONTROLLERS
//________________________________________________________________________
// specify a piecwise linear function for the muscle excitations
PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times,
excitations);
PrescribedController* kneeController = new PrescribedController();
kneeController->addActuator(*kneeExtensorLeft);
kneeController->addActuator(*kneeExtensorRight);
kneeController->prescribeControlForActuator(0, x_of_t);
kneeController->prescribeControlForActuator(1, x_of_t->clone());
model.addController(kneeController);
PrescribedController* backController = new PrescribedController();
backController->addActuator(*backExtensorLeft);
backController->addActuator(*backExtensorRight);
backController->prescribeControlForActuator(0, x_of_t->clone());
backController->prescribeControlForActuator(1, x_of_t->clone());
model.addController(backController);
/* You'll find that these muscles can make Luxo Myo stand, but not jump.
* Jumping will require an assistive device. We'll add two frames for
* attaching a point to point assistive actuator.
*/
// add frames for connecting a back assitance device between the chest
// and pelvis
PhysicalOffsetFrame* back_assist_origin_frame = new
PhysicalOffsetFrame("back_assist_origin",
*chest,
back_assist_origin_transform);
PhysicalOffsetFrame* back_assist_insertion_frame = new
PhysicalOffsetFrame("back_assist_insertion",
*pelvisBracket,
back_assist_insertion_transform);
model.addFrame(back_assist_origin_frame);
model.addFrame(back_assist_insertion_frame);
// add frames for connecting a knee assistance device between the posterior
// leg and bottom bracket.
PhysicalOffsetFrame* knee_assist_origin_frame = new
PhysicalOffsetFrame("knee_assist_origin",
*posteriorLegBar,
knee_assist_origin_transform);
PhysicalOffsetFrame* knee_assist_insertion_frame = new
PhysicalOffsetFrame("knee_assist_insertion",
*bottom_bracket,
knee_assist_insertion_transform);
model.addFrame(knee_assist_origin_frame);
model.addFrame(knee_assist_insertion_frame);
// Temporary: make the frame geometry disappear.
for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) {
const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors(
SimTK::Vec3(0.001, 0.001, 0.001));
}
}
/**
* Create a model that does nothing.
*/
int main()
{
try {
///////////////////////////////////////////
// DEFINE BODIES AND JOINTS OF THE MODEL //
///////////////////////////////////////////
// Create an OpenSim model and set its name
Model osimModel;
osimModel.setName("tugOfWar");
// GROUND BODY
// Get a reference to the model's ground body
Ground& ground = osimModel.updGround();
// Add display geometry to the ground to visualize in the GUI
ground.addMeshGeometry("ground.vtp");
ground.addMeshGeometry("anchor1.vtp");
ground.addMeshGeometry("anchor2.vtp");
// BLOCK BODY
// Specify properties of a 20 kg, 0.1 m^3 block body
double blockMass = 20.0, blockSideLength = 0.1;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
// Create a new block body with the specified properties
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
block->addGeometry(brick);
// FREE JOINT
// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();
// Set the angle and position ranges for the coordinate set
double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
double positionRange[2] = {-1, 1};
jointCoordinateSet[0].setRange(angleRange);
jointCoordinateSet[1].setRange(angleRange);
jointCoordinateSet[2].setRange(angleRange);
jointCoordinateSet[3].setRange(positionRange);
jointCoordinateSet[4].setRange(positionRange);
jointCoordinateSet[5].setRange(positionRange);
// Add the block body to the model
osimModel.addBody(block);
osimModel.addJoint(blockToGround);
///////////////////////////////////////////////
// DEFINE THE SIMULATION START AND END TIMES //
///////////////////////////////////////////////
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 3.00;
/////////////////////////////////////////////
// DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
/////////////////////////////////////////////
// Specify properties of a constant distance constraint to limit the block's motion
double distance = 0.2;
Vec3 pointOnGround(0, blockSideLength/2 ,0);
Vec3 pointOnBlock(0, 0, 0);
// Create a new constant distance constraint
ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground,
pointOnGround, *block, pointOnBlock, distance);
// Add the new point on a line constraint to the model
osimModel.addConstraint(constDist);
///////////////////////////////////////
// DEFINE FORCES ACTING ON THE MODEL //
///////////////////////////////////////
// GRAVITY
// Obtaine the default acceleration due to gravity
Vec3 gravity = osimModel.getGravity();
// MUSCLE FORCES
// Create two new muscles with identical properties
double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0;
Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
// Specify the paths for the two muscles
// Path for muscle 1
muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35));
muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05));
// Path for muscle 2
muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35));
muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05));
// Add the two muscles (as forces) to the model
osimModel.addForce(muscle1);
osimModel.addForce(muscle2);
// PRESCRIBED FORCE
// Create a new prescribed force to be applied to the block
PrescribedForce *prescribedForce = new PrescribedForce(block);
prescribedForce->setName("prescribedForce");
// Specify properties of the force function to be applied to the block
double time[2] = {0, finalTime}; // time nodes for linear function
double fXofT[2] = {0, -blockMass*gravity[1]*3.0}; // force values at t1 and t2
// Create linear function for the force components
PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
// Set the force and point functions for the new prescribed force
prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));
// Add the new prescribed force to the model
osimModel.addForce(prescribedForce);
///////////////////////////////////
// DEFINE CONTROLS FOR THE MODEL //
///////////////////////////////////
// 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(osimModel.updActuators());
// Define linear functions for the control values for the two muscles
Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles
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("muscle1", new LinearFunction(slopeAndIntercept1));
muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));
// Add the muscle controller to the model
osimModel.addController(muscleController);
///////////////////////////////////
// SPECIFY MODEL DEFAULT STATES //
///////////////////////////////////
// Define the default states for the two muscles
// Activation
muscle1->setDefaultActivation(slopeAndIntercept1[1]);
muscle2->setDefaultActivation(slopeAndIntercept2[1]);
// Fiber length
muscle2->setDefaultFiberLength(optimalFiberLength);
muscle1->setDefaultFiberLength(optimalFiberLength);
// Save the model to a file
osimModel.print("tugOfWar_model.osim");
//////////////////////////
// PERFORM A SIMULATION //
/////////////////////////
//osimModel.setUseVisualizer(true);
// Initialize the system and get the default state
SimTK::State& si = osimModel.initSystem();
// Define non-zero (defaults are 0) states for the free joint
CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet();
modelCoordinateSet[3].setValue(si, distance); // set x-translation value
modelCoordinateSet[5].setValue(si, 0.0); // set z-translation value
modelCoordinateSet[3].setSpeedValue(si, 0.0); // set x-speed value
double h_start = 0.5;
modelCoordinateSet[4].setValue(si, h_start); // set y-translation which is height
std::cout << "Start height = "<< h_start << std::endl;
osimModel.getMultibodySystem().realize(si, Stage::Velocity);
// Compute initial conditions for muscles
osimModel.equilibrateMuscles(si);
double mfv1 = muscle1->getFiberVelocity(si);
double mfv2 = muscle2->getFiberVelocity(si);
// Create the force reporter for obtaining the forces applied to the model
// during a forward simulation
ForceReporter* reporter = new ForceReporter(&osimModel);
osimModel.addAnalysis(reporter);
// Create the integrator for integrating system dynamics
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-6);
// Create the manager managing the forward integration and its outputs
Manager manager(osimModel, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
}
catch (const std::exception& ex)
{
std::cerr << ex.what() << std::endl;
return 1;
}
catch (...)
{
std::cerr << "UNRECOGNIZED EXCEPTION" << std::endl;
return 1;
}
std::cout << "OpenSim environment test completed successfully. You should see a block attached to two muscles visualized in a separate window." << std::endl;
return 0;
}
/*==============================================================================
Main test driver to be used on any muscle model (derived from Muscle) so new
cases should be easy to add currently, the test only verifies that the work
done by the muscle corresponds to the change in system energy.
TODO: Test will fail wih prescribe motion until the work done by this
constraint is accounted for.
================================================================================
*/
void simulateMuscle(
const Muscle &aMuscModel,
double startX,
double act0,
const Function *motion, // prescribe motion of free end of muscle
const Function *control, // prescribed excitation signal to the muscle
double integrationAccuracy,
int testType,
double testTolerance,
bool printResults)
{
string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";
cout << "\n******************************************************" << endl;
cout << "Test " << aMuscModel.getConcreteClassName()
<< " Model" << prescribed << endl;
cout << "******************************************************" << endl;
using SimTK::Vec3;
//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 4.0;
//Physical properties of the model
double ballMass = 10;
double ballRadius = 0.05;
double anchorWidth = 0.1;
// Create an OpenSim model
Model model;
double optimalFiberLength = aMuscModel.getOptimalFiberLength();
double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength();
double tendonSlackLength = aMuscModel.getTendonSlackLength();
// Use a copy of the muscle model passed in to add path points later
PathActuator *aMuscle = aMuscModel.clone();
// Get a reference to the model's ground body
Body& ground = model.getGroundBody();
ground.addDisplayGeometry("box.vtp");
ground.updDisplayer()
->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));
OpenSim::Body * ball = new OpenSim::Body("ball",
ballMass ,
Vec3(0),
ballMass*SimTK::Inertia::sphere(ballRadius));
ball->addDisplayGeometry("sphere.vtp");
ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
// ball connected to ground via a slider along X
double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;
SliderJoint* slider = new SliderJoint( "slider",
ground,
Vec3(anchorWidth/2+xSinG, 0, 0),
Vec3(0),
*ball,
Vec3(0),
Vec3(0));
CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
jointCoordinateSet[0].setName("tx");
jointCoordinateSet[0].setDefaultValue(1.0);
jointCoordinateSet[0].setRangeMin(0);
jointCoordinateSet[0].setRangeMax(1.0);
if(motion != NULL){
jointCoordinateSet[0].setPrescribedFunction(*motion);
jointCoordinateSet[0].setDefaultIsPrescribed(true);
}
// add ball to model
model.addBody(ball);
model.addJoint(slider);
//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================
//Attach the muscle
const string &actuatorType = aMuscle->getConcreteClassName();
aMuscle->setName("muscle");
aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));
ActivationFiberLengthMuscle_Deprecated *aflMuscle
= dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
if(aflMuscle){
// Define the default states for the muscle that has
//activation and fiber-length states
aflMuscle->setDefaultActivation(act0);
aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
}else{
ActivationFiberLengthMuscle *aflMuscle2
= dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
if(aflMuscle2){
// Define the default states for the muscle
//that has activation and fiber-length states
aflMuscle2->setDefaultActivation(act0);
aflMuscle2->setDefaultFiberLength(aflMuscle2
->getOptimalFiberLength());
}
}
model.addForce(aMuscle);
// Create a prescribed controller that simply
//applies controls as function of time
PrescribedController * muscleController = new PrescribedController();
if(control != NULL){
muscleController->setActuators(model.updActuators());
// Set the indiviudal muscle control functions
//for the prescribed muscle controller
muscleController->prescribeControlForActuator("muscle",control->clone());
// Add the control set controller to the model
model.addController(muscleController);
}
// Set names for muscles / joints.
Array<string> muscNames;
muscNames.append(aMuscle->getName());
Array<string> jointNames;
jointNames.append("slider");
//==========================================================================
// 2. SIMULATION SETUP: Instrument the test with probes
//==========================================================================
Array<string> muscNamesTwice = muscNames;
muscNamesTwice.append(muscNames.get(0));
cout << "------------\nPROBES\n------------" << endl;
int probeCounter = 1;
// Add ActuatorPowerProbe to measure work done by the muscle
ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
//muscWorkProbe->setName("ActuatorWork");
muscWorkProbe->setOperation("integrate");
SimTK::Vector ic1(1);
ic1 = 9.0; // some arbitary initial condition.
muscWorkProbe->setInitialConditions(ic1);
model.addProbe(muscWorkProbe);
model.setup();
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl;
if (muscWorkProbe->getName() != "UnnamedProbe") {
string errorMessage = "Incorrect default name for unnamed probe: " + muscWorkProbe->getName();
throw (OpenSim::Exception(errorMessage.c_str()));
}
// Add ActuatorPowerProbe to measure power generated by the muscle
ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor
muscPowerProbe->setName("ActuatorPower");
muscPowerProbe->setOperation("value");
model.addProbe(muscPowerProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl;
// Add ActuatorPowerProbe to report the muscle power MINIMUM
ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinimum->setName("ActuatorPowerMinimum");
powerProbeMinimum->setOperation("minimum");
model.addProbe(powerProbeMinimum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM
ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinAbs->setName("ActuatorPowerMinAbs");
powerProbeMinAbs->setOperation("minabs");
model.addProbe(powerProbeMinAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXIMUM
ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaximum->setName("ActuatorPowerMaximum");
powerProbeMaximum->setOperation("maximum");
model.addProbe(powerProbeMaximum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXABS
ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaxAbs->setName("ActuatorPowerMaxAbs");
powerProbeMaxAbs->setOperation("maxabs");
model.addProbe(powerProbeMaxAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl;
// Add ActuatorPowerProbe to measure the square of the power generated by the muscle
ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
muscPowerSquaredProbe->setName("ActuatorPowerSquared");
muscPowerSquaredProbe->setExponent(2.0);
model.addProbe(muscPowerSquaredProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl;
// Add JointInternalPowerProbe to measure work done by the joint
JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1);
jointWorkProbe->setName("JointWork");
jointWorkProbe->setOperation("integrate");
jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(jointWorkProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl;
// Add JointPowerProbe to measure power generated by the joint
JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor
jointPowerProbe->setName("JointPower");
jointPowerProbe->setOperation("value");
model.addProbe(jointPowerProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl;
// Add ActuatorForceProbe to measure the impulse of the muscle force
ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1);
impulseProbe->setName("ActuatorImpulse");
impulseProbe->setOperation("integrate");
impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(impulseProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl;
// Add ActuatorForceProbe to report the muscle force
ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbe->setName("ActuatorForce");
forceProbe->setOperation("value");
model.addProbe(forceProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl;
// Add ActuatorForceProbe to report the square of the muscle force
ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbe->setName("ActuatorForceSquared");
forceSquaredProbe->setExponent(2.0);
model.addProbe(forceSquaredProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice
ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice");
forceSquaredProbeTwice->setSumForcesTogether(true);
forceSquaredProbeTwice->setActuatorNames(muscNamesTwice);
model.addProbe(forceSquaredProbeTwice);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5
ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved");
double gain1 = 0.5;
forceSquaredProbeTwiceScaled->setGain(gain1);
model.addProbe(forceSquaredProbeTwiceScaled);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl;
// Add ActuatorForceProbe to report -3.5X the muscle force
double gain2 = -3.50;
ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeScale->setName("ScaleActuatorForce");
forceProbeScale->setOperation("value");
forceProbeScale->setGain(gain2);
model.addProbe(forceProbeScale);
cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force
ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeDiff->setName("DifferentiateActuatorForce");
forceProbeDiff->setOperation("differentiate");
model.addProbe(forceProbeDiff);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl;
// Add SystemEnergyProbe to measure the system KE+PE
SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true);
sysEnergyProbe->setName("SystemEnergy");
sysEnergyProbe->setOperation("value");
sysEnergyProbe->setComputeKineticEnergy(true);
sysEnergyProbe->setComputePotentialEnergy(true);
model.addProbe(sysEnergyProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl;
// Add SystemEnergyProbe to measure system power (d/dt system KE+PE)
SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor
sysPowerProbe->setName("SystemPower");
sysPowerProbe->setDisabled(false);
sysPowerProbe->setOperation("differentiate");
model.addProbe(sysPowerProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl;
// Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR");
forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice);
//cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl;
forceSquaredProbeTwiceReportedIndividually1->setOperation("value");
model.addProbe(forceSquaredProbeTwiceReportedIndividually1);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate");
model.addProbe(forceSquaredProbeTwiceReportedIndividually2);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate");
SimTK::Vector initCondVec(2);
initCondVec(0) = 0;
initCondVec(1) = 10;
forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec);
model.addProbe(forceSquaredProbeTwiceReportedIndividually3);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
cout << "initCondVec = " << initCondVec << endl;
/* Since all components are allocated on the stack don't have model
own them (and try to free)*/
// model.disownAllComponents();
model.setName("testProbesModel");
cout << "Saving model... " << endl;
model.print("testProbesModel.osim");
cout << "Re-loading model... " << endl;
Model reloadedModel = Model("testProbesModel.osim");
/* Setup analyses and reporters. */
ProbeReporter* probeReporter = new ProbeReporter(&model);
model.addAnalysis(probeReporter);
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model);
model.addAnalysis(muscleReporter);
model.print("testProbesModel.osim");
model.printBasicInfo(cout);
//==========================================================================
// 3. SIMULATION Initialization
//==========================================================================
// Initialize the system and get the default state
SimTK::State& si = model.initSystem();
SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si);
model.getMultibodySystem().realize(si,SimTK::Stage::Dynamics);
model.equilibrateMuscles(si);
CoordinateSet& modelCoordinateSet = model.updCoordinateSet();
// Define non-zero (defaults are 0) states for the free joint
// set x-translation value
modelCoordinateSet[0].setValue(si, startX, true);
//Copy the initial state
SimTK::State initialState(si);
// Check muscle is setup correctly
const PathActuator &muscle
= dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
double length = muscle.getLength(si);
double trueLength = startX + xSinG - anchorWidth/2;
ASSERT_EQUAL(length/trueLength, 1.0, testTolerance, __FILE__, __LINE__,
"testMuscles: path failed to initialize to correct length." );
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
//cout << "Muscle initial energy = " << Emuscle0 << endl;
double Esys0 = model.getMultibodySystem().calcEnergy(si);
Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
//cout << "Total initial system energy = " << Esys0 << endl;
//==========================================================================
// 4. SIMULATION Integration
//==========================================================================
// Create the integrator
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
integrator.setAccuracy(integrationAccuracy);
// Create the manager
Manager manager(model, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout<<"\nIntegrating from " << initialTime<< " to " << finalTime << endl;
// Start timing the simulation
const clock_t start = clock();
// simulate
manager.integrate(si);
// how long did it take?
double comp_time = (double)(clock()-start)/CLOCKS_PER_SEC;
//==========================================================================
// 5. SIMULATION Reporting
//==========================================================================
double realTimeMultiplier = ((finalTime-initialTime)/comp_time);
printf("testMuscles: Realtime Multiplier: %f\n"
" : simulation duration / clock duration\n"
" > 1 : faster than real time\n"
" = 1 : real time\n"
" < 1 : slower than real time\n",
realTimeMultiplier );
/*
ASSERT(comp_time <= (finalTime-initialTime));
printf("testMuscles: PASSED Realtime test\n"
" %s simulation time: %f with accuracy %f\n\n",
actuatorType.c_str(), comp_time , accuracy);
*/
//An analysis only writes to a dir that exists, so create here.
if(printResults == true){
Storage states(manager.getStateStorage());
states.print("testProbes_states.sto");
probeReporter->getProbeStorage().print("testProbes_probes.sto");
forceReporter->getForceStorage().print("testProbes_forces.sto");
muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto");
cout << "\nDone with printing results..." << endl;
}
double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
// Test the resetting of probes
cout << "Resetting muscle work probe..." << endl;
muscWorkProbe->reset(si);
muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe.");
//==========================================================================
// 6. SIMULATION Tests
//==========================================================================
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe.");
ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2*pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe.");
for (int i=0; i<initCondVec.size(); ++i) {
stringstream myError;
//myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl;
//ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str());
//if (testRealInitConditions(i) != initCondVec(i))
// cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl;
}
}
//==========================================================================================================
void testPrescribedControllerOnBlock(bool disabled)
{
using namespace SimTK;
// Create a new OpenSim model
Model osimModel;
osimModel.setName("osimModel");
// Get the ground body
OpenSim::Body& ground = osimModel.getGroundBody();
// Create a 20 kg, 0.1 m^3 block body
double blockMass = 20.0, blockSideLength = 0.1;
Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0);
Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia);
//Create a free joint with 6 degrees-of-freedom
SimTK::Vec3 noRotation(0);
SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation);
// Create 6 coordinates (degrees-of-freedom) between the ground and block
CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet();
double posRange[2] = {-1, 1};
jointCoordinateSet[0].setName("xTranslation");
jointCoordinateSet[0].setMotionType(Coordinate::Translational);
jointCoordinateSet[0].setRange(posRange);
// Add the block body to the model
osimModel.addBody(&block);
// Define a single coordinate actuator.
CoordinateActuator actuator(jointCoordinateSet[0].getName());
actuator.setName("actuator");
// Add the actuator to the model
osimModel.addForce(&actuator);
double initialTime = 0;
double finalTime = 1.0;
// Define the initial and final control values
double controlForce = 100;
// Create a prescribed controller that simply applies a function of the force
PrescribedController actuatorController;
actuatorController.setName("testPrescribedController");
actuatorController.setActuators(osimModel.updActuators());
actuatorController.prescribeControlForActuator(0, new Constant(controlForce));
actuatorController.setDisabled(disabled);
// add the controller to the model
osimModel.addController(&actuatorController);
osimModel.print("blockWithPrescribedController.osim");
Model modelfileFromFile("blockWithPrescribedController.osim");
// Verify that serialization and then deserialization of the disable flag is correct
ASSERT(modelfileFromFile.getControllerSet().get("testPrescribedController").isDisabled() == disabled);
// Initialize the system and get the state representing the state system
SimTK::State& si = osimModel.initSystem();
// Specify zero slider joint kinematic states
CoordinateSet &coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, 0.0); // x translation
coordinates[0].setSpeedValue(si, 0.0); // x speed
// Create the integrator and manager for the simulation.
double accuracy = 1.0e-3;
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(accuracy);
Manager manager(osimModel, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
si.getQ().dump("Final position:");
double expected = disabled ? 0 : 0.5*(controlForce/blockMass)*finalTime*finalTime;
ASSERT_EQUAL(expected, coordinates[0].getValue(si), accuracy, __FILE__, __LINE__, "PrescribedController failed to produce the expected motion of block.");
// Save the simulation results
Storage states(manager.getStateStorage());
states.print("block_push.sto");
osimModel.disownAllComponents();
}// end of testPrescribedControllerOnBlock()
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 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"));
}
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"));
}
/**
* Run a simulation of block sliding with contact on by two muscles sliding with contact
*/
int main()
{
clock_t startTime = clock();
try {
//////////////////////
// MODEL PARAMETERS //
//////////////////////
// Specify body mass of a 20 kg, 0.1m sides of cubed block body
double blockMass = 20.0, blockSideLength = 0.1;
// Constant distance of constraint to limit the block's motion
double constantDistance = 0.2;
// Contact parameters
double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity=0.01;
///////////////////////////////////////////
// DEFINE BODIES AND JOINTS OF THE MODEL //
///////////////////////////////////////////
// Create an OpenSim model and set its name
Model osimModel;
osimModel.setName("tugOfWar");
// GROUND BODY
// Get a reference to the model's ground body
OpenSim::Body& ground = osimModel.getGroundBody();
// Add display geometry to the ground to visualize in the Visualizer and GUI
// add a checkered floor
ground.addDisplayGeometry("checkered_floor.vtp");
// add anchors for the muscles to be fixed too
ground.addDisplayGeometry("block.vtp");
ground.addDisplayGeometry("block.vtp");
// block is 0.1 by 0.1 by 0.1m cube and centered at origin.
// transform anchors to be placed at the two extremes of the sliding block (to come)
GeometrySet& geometry = ground.updDisplayer()->updGeometrySet();
DisplayGeometry& anchor1 = geometry[1];
DisplayGeometry& anchor2 = geometry[2];
// scale the anchors
anchor1.setScaleFactors(Vec3(5, 1, 1));
anchor2.setScaleFactors(Vec3(5, 1, 1));
// reposition the anchors
anchor1.setTransform(Transform(Vec3(0, 0.05, 0.35)));
anchor2.setTransform(Transform(Vec3(0, 0.05, -0.35)));
// BLOCK BODY
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
// Create a new block body with the specified properties
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->addDisplayGeometry("block.vtp");
// FREE JOINT
// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();
// Set the angle and position ranges for the coordinate set
double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
double positionRange[2] = {-1, 1};
jointCoordinateSet[0].setRange(angleRange);
jointCoordinateSet[1].setRange(angleRange);
jointCoordinateSet[2].setRange(angleRange);
jointCoordinateSet[3].setRange(positionRange);
jointCoordinateSet[4].setRange(positionRange);
jointCoordinateSet[5].setRange(positionRange);
// GRAVITY
// Obtaine the default acceleration due to gravity
Vec3 gravity = osimModel.getGravity();
// Define non-zero default states for the free joint
jointCoordinateSet[3].setDefaultValue(constantDistance); // set x-translation value
double h_start = blockMass*gravity[1]/(stiffness*blockSideLength*blockSideLength);
jointCoordinateSet[4].setDefaultValue(h_start); // set y-translation which is height
// Add the block and joint to the model
osimModel.addBody(block);
osimModel.addJoint(blockToGround);
///////////////////////////////////////////////
// DEFINE THE SIMULATION START AND END TIMES //
///////////////////////////////////////////////
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 3.00;
/////////////////////////////////////////////
// DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
/////////////////////////////////////////////
Vec3 pointOnGround(0, blockSideLength/2 ,0);
Vec3 pointOnBlock(0, 0, 0);
// Create a new constant distance constraint
ConstantDistanceConstraint *constDist =
new ConstantDistanceConstraint(ground,
pointOnGround, *block, pointOnBlock, constantDistance);
// Add the new point on a line constraint to the model
osimModel.addConstraint(constDist);
///////////////////////////////////////
// DEFINE FORCES ACTING ON THE MODEL //
///////////////////////////////////////
// MUSCLE FORCES
// Create two new muscles with identical properties
double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0;
Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
// Specify the paths for the two muscles
// Path for muscle 1
muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35));
muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05));
// Path for muscle 2
muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35));
muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05));
// Add the two muscles (as forces) to the model
osimModel.addForce(muscle1);
osimModel.addForce(muscle2);
// CONTACT FORCE
// Define contact geometry
// Create new floor contact halfspace
ContactHalfSpace *floor = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK_PI), ground, "floor");
// Create new cube contact mesh
OpenSim::ContactMesh *cube = new OpenSim::ContactMesh("blockMesh.obj", SimTK::Vec3(0), SimTK::Vec3(0), *block, "cube");
// Add contact geometry to the model
osimModel.addContactGeometry(floor);
osimModel.addContactGeometry(cube);
// Define contact parameters for elastic foundation force
OpenSim::ElasticFoundationForce::ContactParameters *contactParams =
new OpenSim::ElasticFoundationForce::ContactParameters(stiffness, dissipation, friction, friction, viscosity);
contactParams->addGeometry("cube");
contactParams->addGeometry("floor");
// Create a new elastic foundation (contact) force between the floor and cube.
OpenSim::ElasticFoundationForce *contactForce = new OpenSim::ElasticFoundationForce(contactParams);
contactForce->setName("contactForce");
// Add the new elastic foundation force to the model
osimModel.addForce(contactForce);
// PRESCRIBED FORCE
// Create a new prescribed force to be applied to the block
PrescribedForce *prescribedForce = new PrescribedForce(block);
prescribedForce->setName("prescribedForce");
// Specify properties of the force function to be applied to the block
double time[2] = {0, finalTime}; // time nodes for linear function
double fXofT[2] = {0, -blockMass*gravity[1]*3.0}; // force values at t1 and t2
// Create linear function for the force components
PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
// Set the force and point functions for the new prescribed force
prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));
// Add the new prescribed force to the model
osimModel.addForce(prescribedForce);
///////////////////////////////////
// DEFINE CONTROLS FOR THE MODEL //
///////////////////////////////////
// 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(osimModel.updActuators());
// Define linear functions for the control values for the two muscles
Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles
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("muscle1", new LinearFunction(slopeAndIntercept1));
muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));
// Add the muscle controller to the model
osimModel.addController(muscleController);
///////////////////////////////////
// SPECIFY MODEL DEFAULT STATES //
///////////////////////////////////
// Define the default states for the two muscles
// Activation
muscle1->setDefaultActivation(slopeAndIntercept1[1]);
muscle2->setDefaultActivation(slopeAndIntercept2[1]);
// Fiber length
muscle2->setDefaultFiberLength(optimalFiberLength);
muscle1->setDefaultFiberLength(optimalFiberLength);
// Save the model to a file
osimModel.print("tugOfWar_model.osim");
//////////////////////////
// PERFORM A SIMULATION //
//////////////////////////
// set use visualizer to true to visualize the simulation live
osimModel.setUseVisualizer(false);
// Initialize the system and get the default state
SimTK::State& si = osimModel.initSystem();
// Enable constraint consistent with current configuration of the model
constDist->setDisabled(si, false);
cout << "Start height = "<< h_start << endl;
osimModel.getMultibodySystem().realize(si, Stage::Velocity);
// Compute initial conditions for muscles
osimModel.equilibrateMuscles(si);
double mfv1 = muscle1->getFiberVelocity(si);
double mfv2 = muscle2->getFiberVelocity(si);
// Create the force reporter for obtaining the forces applied to the model
// during a forward simulation
ForceReporter* reporter = new ForceReporter(&osimModel);
osimModel.addAnalysis(reporter);
// Create the integrator for integrating system dynamics
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-6);
// Create the manager managing the forward integration and its outputs
Manager manager(osimModel, integrator);
// Print out details of the model
osimModel.printDetailedInfo(si, cout);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<endl;
manager.integrate(si);
//////////////////////////////
// SAVE THE RESULTS TO FILE //
//////////////////////////////
// Save the model states from forward integration
Storage statesDegrees(manager.getStateStorage());
statesDegrees.print("tugOfWar_states.sto");
// Save the forces
reporter->getForceStorage().print("tugOfWar_forces.mot");
}
catch (const std::exception& ex)
{
cerr << ex.what() << endl;
return 1;
}
catch (...)
{
cerr << "UNRECOGNIZED EXCEPTION" << endl;
return 1;
}
cout << "main() routine time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms\n";
cout << "OpenSim example completed successfully." << endl;
return 0;
}
int main() {
Model model;
model.setName("bicep_curl");
#ifdef VISUALIZE
model.setUseVisualizer(true);
#endif
// Create two links, each with a mass of 1 kg, center of mass at the body's
// origin, and moments and products of inertia of zero.
OpenSim::Body* humerus = new OpenSim::Body("humerus", 1, Vec3(0), Inertia(0));
OpenSim::Body* radius = new OpenSim::Body("radius", 1, Vec3(0), Inertia(0));
// Connect the bodies with pin joints. Assume each body is 1 m long.
PinJoint* shoulder = new PinJoint("shoulder",
// Parent body, location in parent, orientation in parent.
model.getGround(), Vec3(0), Vec3(0),
// Child body, location in child, orientation in child.
*humerus, Vec3(0, 1, 0), Vec3(0));
PinJoint* elbow = new PinJoint("elbow",
*humerus, Vec3(0), Vec3(0), *radius, Vec3(0, 1, 0), Vec3(0));
// Add a muscle that flexes the elbow.
Millard2012EquilibriumMuscle* biceps = new
Millard2012EquilibriumMuscle("biceps", 200, 0.6, 0.55, 0);
biceps->addNewPathPoint("origin", *humerus, Vec3(0, 0.8, 0));
biceps->addNewPathPoint("insertion", *radius, Vec3(0, 0.7, 0));
// Add a controller that specifies the excitation of the muscle.
PrescribedController* brain = new PrescribedController();
brain->addActuator(*biceps);
// Muscle excitation is 0.3 for the first 0.5 seconds, then increases to 1.
brain->prescribeControlForActuator("biceps",
new StepFunction(0.5, 3, 0.3, 1));
// Add components to the model.
model.addBody(humerus); model.addBody(radius);
model.addJoint(shoulder); model.addJoint(elbow);
model.addForce(biceps);
model.addController(brain);
// Add a console reporter to print the muscle fiber force and elbow angle.
ConsoleReporter* reporter = new ConsoleReporter();
reporter->set_report_time_interval(1.0);
reporter->addToReport(biceps->getOutput("fiber_force"));
reporter->addToReport(
elbow->getCoordinate(PinJoint::Coord::RotationZ).getOutput("value"),
"elbow_angle");
model.addComponent(reporter);
// Add display geometry.
Ellipsoid bodyGeometry(0.1, 0.5, 0.1);
bodyGeometry.setColor(Gray);
// Attach an ellipsoid to a frame located at the center of each body.
PhysicalOffsetFrame* humerusCenter = new PhysicalOffsetFrame(
"humerusCenter", *humerus, Transform(Vec3(0, 0.5, 0)));
humerus->addComponent(humerusCenter);
humerusCenter->attachGeometry(bodyGeometry.clone());
PhysicalOffsetFrame* radiusCenter = new PhysicalOffsetFrame(
"radiusCenter", *radius, Transform(Vec3(0, 0.5, 0)));
radius->addComponent(radiusCenter);
radiusCenter->attachGeometry(bodyGeometry.clone());
// Configure the model.
State& state = model.initSystem();
// Fix the shoulder at its default angle and begin with the elbow flexed.
shoulder->getCoordinate().setLocked(state, true);
elbow->getCoordinate().setValue(state, 0.5 * Pi);
model.equilibrateMuscles(state);
// Configure the visualizer.
#ifdef VISUALIZE
model.updMatterSubsystem().setShowDefaultGeometry(true);
Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
viz.setBackgroundType(viz.SolidColor);
viz.setBackgroundColor(White);
#endif
// Simulate.
simulate(model, state, 10.0);
return 0;
};