/**
* 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");
osimModel.setAuthors("Matt DeMers");
double Pi = SimTK::Pi;
// Get the ground body
Ground& ground = osimModel.updGround();
ground.addMeshGeometry("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
Cylinder cyl;
cyl.set_scale_factors(linkageDimensions);
Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
cyl1Frame->setName("Cyl1_frame");
osimModel.addFrame(cyl1Frame);
cyl.setFrameName("Cyl1_frame");
linkage1->addGeometry(cyl);
Sphere sphere(0.1);
linkage1->addGeometry(sphere);
// Creat a second linkage body
OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
linkage2->setName("linkage2");
Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2, Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
cyl2Frame->setName("Cyl2_frame");
osimModel.addFrame(cyl2Frame);
(linkage2->upd_geometry(0)).setFrameName("Cyl2_frame");
// 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);
Brick brick(SimTK::Vec3(0.05, 0.05, 0.05));
block->addGeometry(brick);
// 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);
// Add the joints to the model
osimModel.addJoint(ankle);
osimModel.addJoint(knee);
osimModel.addJoint(hip);
// 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(false);
// 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;
osimModel.dumpPathName();
// 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 << "Done." << std::endl;
return 0;
}
/**
* 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;
}
