//==============================================================================
void FreeJoint::setTransform(Joint* joint,
const Eigen::Isometry3d& tf,
const Frame* withRespectTo)
{
if (nullptr == joint)
return;
FreeJoint* freeJoint = dynamic_cast<FreeJoint*>(joint);
if (nullptr == freeJoint)
{
dtwarn << "[FreeJoint::setTransform] Invalid joint type. Setting transform "
<< "is only allowed to FreeJoint. The joint type of given joint ["
<< joint->getName() << "] is [" << joint->getType() << "].\n";
return;
}
freeJoint->setTransform(tf, withRespectTo);
}
/**
* 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;
}
/**
* 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;
}
/**
* 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;
}
//==============================================================================
TEST_F(JOINTS, CONVENIENCE_FUNCTIONS)
{
// -- set up the root BodyNode
BodyNode* root = new BodyNode("root");
WeldJoint* rootjoint = new WeldJoint("base");
root->setParentJoint(rootjoint);
// -- set up the FreeJoint
BodyNode* freejoint_bn = new BodyNode("freejoint_bn");
FreeJoint* freejoint = new FreeJoint("freejoint");
freejoint_bn->setParentJoint(freejoint);
root->addChildBodyNode(freejoint_bn);
freejoint->setTransformFromParentBodyNode(random_transform());
freejoint->setTransformFromChildBodyNode(random_transform());
// -- set up the EulerJoint
BodyNode* eulerjoint_bn = new BodyNode("eulerjoint_bn");
EulerJoint* eulerjoint = new EulerJoint("eulerjoint");
eulerjoint_bn->setParentJoint(eulerjoint);
root->addChildBodyNode(eulerjoint_bn);
eulerjoint->setTransformFromParentBodyNode(random_transform());
eulerjoint->setTransformFromChildBodyNode(random_transform());
// -- set up the BallJoint
BodyNode* balljoint_bn = new BodyNode("balljoint_bn");
BallJoint* balljoint = new BallJoint("balljoint");
balljoint_bn->setParentJoint(balljoint);
root->addChildBodyNode(balljoint_bn);
balljoint->setTransformFromParentBodyNode(random_transform());
balljoint->setTransformFromChildBodyNode(random_transform());
// -- set up Skeleton and compute forward kinematics
Skeleton* skel = new Skeleton;
skel->addBodyNode(root);
skel->addBodyNode(freejoint_bn);
skel->addBodyNode(eulerjoint_bn);
skel->addBodyNode(balljoint_bn);
skel->init();
// Test a hundred times
for(size_t n=0; n<100; ++n)
{
// -- convert transforms to positions and then positions back to transforms
Eigen::Isometry3d desired_freejoint_tf = random_transform();
freejoint->setPositions(FreeJoint::convertToPositions(desired_freejoint_tf));
Eigen::Isometry3d actual_freejoint_tf = FreeJoint::convertToTransform(
freejoint->getPositions());
Eigen::Isometry3d desired_eulerjoint_tf = random_transform();
desired_eulerjoint_tf.translation() = Eigen::Vector3d::Zero();
eulerjoint->setPositions(
eulerjoint->convertToPositions(desired_eulerjoint_tf.linear()));
Eigen::Isometry3d actual_eulerjoint_tf = eulerjoint->convertToTransform(
eulerjoint->getPositions());
Eigen::Isometry3d desired_balljoint_tf = random_transform();
desired_balljoint_tf.translation() = Eigen::Vector3d::Zero();
balljoint->setPositions(
BallJoint::convertToPositions(desired_balljoint_tf.linear()));
Eigen::Isometry3d actual_balljoint_tf = BallJoint::convertToTransform(
balljoint->getPositions());
skel->computeForwardKinematics(true, false, false);
// -- collect everything so we can cycle through the tests
std::vector<Joint*> joints;
std::vector<BodyNode*> bns;
std::vector<Eigen::Isometry3d> desired_tfs;
std::vector<Eigen::Isometry3d> actual_tfs;
joints.push_back(freejoint);
bns.push_back(freejoint_bn);
desired_tfs.push_back(desired_freejoint_tf);
actual_tfs.push_back(actual_freejoint_tf);
joints.push_back(eulerjoint);
bns.push_back(eulerjoint_bn);
desired_tfs.push_back(desired_eulerjoint_tf);
actual_tfs.push_back(actual_eulerjoint_tf);
joints.push_back(balljoint);
bns.push_back(balljoint_bn);
desired_tfs.push_back(desired_balljoint_tf);
actual_tfs.push_back(actual_balljoint_tf);
for(size_t i=0; i<joints.size(); ++i)
{
Joint* joint = joints[i];
BodyNode* bn = bns[i];
Eigen::Isometry3d tf = desired_tfs[i];
bool check_transform_conversion =
equals(predict_joint_transform(joint, tf).matrix(),
get_relative_transform(bn, bn->getParentBodyNode()).matrix());
EXPECT_TRUE(check_transform_conversion);
if(!check_transform_conversion)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Predicted:\n" << predict_joint_transform(joint, tf).matrix()
<< "\n\nActual:\n"
<< get_relative_transform(bn, bn->getParentBodyNode()).matrix()
<< "\n\n";
}
bool check_full_cycle = equals(desired_tfs[i].matrix(),
actual_tfs[i].matrix());
EXPECT_TRUE(check_full_cycle);
if(!check_full_cycle)
{
std::cout << "[" << joint->getName() << " Failed]\n";
std::cout << "Desired:\n" << desired_tfs[i].matrix()
<< "\n\nActual:\n" << actual_tfs[i].matrix()
<< "\n\n";
}
}
}
}
