/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
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 body 1: Ground
const Ground& ground = model->getGround();
// OpenSim body 2: A Block
// Geometrical/Inertial properties for the block
double blockMass = 1.0, blockSideLength = 1;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick
OpenSim::Body *block = new OpenSim::Body("block", blockMass,
blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->attachGeometry(new Brick(Vec3(blockSideLength/2,
blockSideLength/2,
blockSideLength/2)));
Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0),
locationInBody(0), orientationInBody(0);
FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall",
ground, locationInParent, orientationInParent,
*block, locationInBody, orientationInBody);
model->addBody(block);
model->addJoint(blockToGroundFree);
// specify magnitude and direction of applied force and torque
double forceMag = 1.0;
Vec3 forceAxis(1, 1, 1);
Vec3 forceInG = forceMag * forceAxis;
double torqueMag = 1.0;
Vec3 torqueAxis(1, 1, 1);
Vec3 torqueInG = torqueMag*torqueAxis;
// ---------------------------------------------------------------------------
// Use MobilityForces to Apply the given Torques and Forces to the body
// ---------------------------------------------------------------------------
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 6D spatial force:");
model->getMatterSubsystem().addInBodyTorque(state, block->getMobilizedBodyIndex(),
torqueInG, bodyForces);
model->getMatterSubsystem().addInStationForce(state, block->getMobilizedBodyIndex(),
Vec3(0), forceInG, bodyForces);
bodyForces.dump("Body Forces after applying 6D spatial force to the block");
model->getMultibodySystem().realize(state, Stage::Acceleration);
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[i] = torqueInG[i];
mobilityForces[i+3] = forceInG[i];
}
mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");
model->getMultibodySystem().realize(state, Stage::Acceleration);
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], SimTK::Eps);
}
// clear the mobility forces
mobilityForces = 0;
// ---------------------------------------------------------------------------
// Use a BodyActuator to Apply the same given Torques and Forces to the body
// ---------------------------------------------------------------------------
// Create and add the body actuator to the model
BodyActuator* actuator = new BodyActuator(*block);
actuator->setName("BodyAct");
model->addForce(actuator);
model->setUseVisualizer(false);
// get a new system and state to reflect additions to the model
State& state1 = model->initSystem();
model->print("TestBodyActuatorModel.osim");
// -------------- Provide control signals for bodyActuator ----------
// Get the default control vector of the model
Vector modelControls = model->getDefaultControls();
// Specify a vector of control signals to include desired torques and forces
Vector fixedControls(6);
for (int i = 0; i < 3; i++){
fixedControls(i) = torqueInG(i);
fixedControls(i + 3) = forceInG(i);
}
fixedControls.dump("Spatial forces applied by body Actuator:");
// Add control values and set their values
actuator->addInControls(fixedControls, modelControls);
model->setDefaultControls(modelControls);
// ------------------- Compute Acc and Compare -------------
// Compute the acc due to spatial forces applied by body actuator
model->computeStateVariableDerivatives(state1);
Vector udotBodyActuator = state1.getUDot();
udotBodyActuator.dump("Accelerations due to body actuator");
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotBodyActuator.norm() != 0.0);
// Then verify that the BodyActuator also generates the same acceleration
// as the equivalent applied mobility force
for (int i = 0; i<udotBodyActuator.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotBodyActuator[i], SimTK::Eps);
}
// -------------- Setup integrator and manager -------------------
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(state1);
// ----------------- Test Copying the model -------------------
// Before exiting lets see if copying the actuator works
BodyActuator* copyOfActuator = actuator->clone();
ASSERT(*copyOfActuator == *actuator);
// Check that de/serialization works
Model modelFromFile("TestBodyActuatorModel.osim");
ASSERT(modelFromFile == *model, __FILE__, __LINE__,
"Model from file FAILED to match model in memory.");
std::cout << " ********** Test BodyActuator time = ********** " <<
1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
/**
* This test verifies if using a BodyActuator generates equivalent result in the body
* acceleration compared to when using a combination of PointActuaor, TorqueActuaor
* and BodyActuator.
* It therefore also verifies model consistency when user defines and uses a
* combination of these 3 actuators.
*
* @author Soha Pouya
*/
void testActuatorsCombination()
{
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: 1) The ground
const Ground& ground = model->getGround();
//ground.addDisplayGeometry("block.vtp");
// OpenSim bodies: 2) A Block
// Geometrical/Inertial properties for the block
double blockMass = 1.0, blockSideLength = 1.0;
Vec3 blockMassCenter(0);
// Brick Inertia: for the halves see doxygen
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
blockSideLength/2, blockSideLength/2);
std::cout << "blockInertia: " << blockInertia << std::endl;
OpenSim::Body *block = new OpenSim::Body("block", blockMass,
blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->attachGeometry(new Brick(Vec3(blockSideLength/2,
blockSideLength/2,
blockSideLength/2)));
// Make a FreeJoint from block to ground
Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), //locationInParent(0, blockSideLength, 0)
locationInBody(0), orientationInBody(0);
FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundFreeJoint",
ground, locationInParent, orientationInParent,
*block, locationInBody, orientationInBody);
// Add the body and joint to the model
model->addBody(block);
model->addJoint(blockToGroundFree);
// specify magnitude and direction of desired force and torque vectors to apply
double forceMag = 1.0;
Vec3 forceAxis(1, 1, 1);
SimTK::UnitVec3 forceUnitAxis(forceAxis); // to normalize
Vec3 forceInG = forceMag * forceUnitAxis;
double torqueMag = 1.0;
Vec3 torqueAxis(1, 2, 1);
SimTK::UnitVec3 torqueUnitAxis(torqueAxis); // to normalize
Vec3 torqueInG = torqueMag*torqueUnitAxis;
// needed to be called here once to build controller for body actuator
/*State& state = */model->initSystem();
// ---------------------------------------------------------------------------
// Add a set of PointActuator, TorqueActuator and BodyActuator to the model
// ---------------------------------------------------------------------------
// Create and add a body actuator to the model
BodyActuator* bodyActuator1 = new BodyActuator(*block);
bodyActuator1->setName("BodyAct1");
bodyActuator1->set_point(Vec3(0, blockSideLength/2, 0));
model->addForce(bodyActuator1);
// Create and add a torque actuator to the model
TorqueActuator* torqueActuator =
new TorqueActuator(*block, ground, torqueUnitAxis, true);
torqueActuator->setName("torqueAct");
model->addForce(torqueActuator);
// Create and add a point actuator to the model
PointActuator* pointActuator =
new PointActuator("block");
pointActuator->setName("pointAct");
pointActuator->set_direction(forceUnitAxis);
pointActuator->set_point(Vec3(0, blockSideLength/2,0));
model->addForce(pointActuator);
// ------ build the model -----
model->print("TestActuatorCombinationModel.osim");
model->setUseVisualizer(false);
// get a new system and state to reflect additions to the model
State& state1 = model->initSystem();
// ------------------- Provide control signals for bodyActuator ----------------
// Get the default control vector of the model
Vector modelControls = model->getDefaultControls();
// Specify a vector of control signals for desired torques and forces
Vector bodyActuator1Controls(6,0.0);
for (int i=0; i<3; i++) bodyActuator1Controls(i) = torqueInG(i); // torque in 3 axes
for (int i=0; i<3; i++) bodyActuator1Controls(i+3) = forceInG(i); // force along 3 axes
bodyActuator1Controls.dump("Spatial forces applied by first Body Actuator:");
// Add control values and set their values
bodyActuator1->addInControls(bodyActuator1Controls, modelControls);
model->setDefaultControls(modelControls);
// ---------------- Provide control signals for torqueActuator -----------------
Vector torqueActuatorControls(1); // input to addInControl should be a Vector
torqueActuatorControls(0) = torqueMag; // axis already defined when initializing
Vector torqueActuatorVector(3); // to print out the 3D vector of applied torque
for (int i = 0; i < 3; i++){
torqueActuatorVector(i) = torqueInG(i);
}
torqueActuatorVector.dump("Torques applied by the Torque Actuator:");
// Add control values and set their values
torqueActuator->addInControls(torqueActuatorControls, modelControls);
model->setDefaultControls(modelControls);
// ------------------ Provide control signals for pointActuator ----------------
Vector pointActuatorControls(1); // input to addInControl should be a Vector
pointActuatorControls(0) = forceMag; // axis already defined when initializing
Vector pointActuatorVector(3); // to print out the whole force vector
for (int i = 0; i < 3; i++){
pointActuatorVector(i) = forceInG(i);
}
pointActuatorVector.dump("Forces applied by the point Actuator:");
// Add control values and set their values
pointActuator->addInControls(pointActuatorControls, modelControls);
model->setDefaultControls(modelControls);
// ----------------------- Compute the acc to Compare later --------------------
// compare the acc due to forces/torques applied by all actuator
model->computeStateVariableDerivatives(state1);
Vector udotActuatorsCombination = state1.getUDot();
udotActuatorsCombination.dump("Accelerations due to all 3 actuators");
// -----------------------------------------------------------------------------
// Add a BodyActuator to enclose all of the above spatial forces in one Actuator
// -----------------------------------------------------------------------------
// Create and add a body actuator to the model
BodyActuator* bodyActuator_sum = new BodyActuator(*block);
bodyActuator_sum->setName("BodyAct_Sum");
model->addForce(bodyActuator_sum);
bodyActuator_sum->set_point(Vec3(0, blockSideLength / 2, 0));
State& state2 = model->initSystem();
model->setUseVisualizer(true);
// Get the default control vector of the model
Vector modelControls_2 = model->getDefaultControls();
// Specify a vector of control signals for desired torques and forces
Vector bodyActuatorSum_Controls(6,0.0);
// make the torque component as the sum of body, torque and point actuators used
// in previous test case
for (int i = 0; i < 3; i++){
bodyActuatorSum_Controls(i) = 2*torqueInG(i);
bodyActuatorSum_Controls(i+3) = 2*forceInG(i);
}
bodyActuatorSum_Controls.dump("Spatial forces applied by 2nd Body Actuator:");
std::cout <<"(encloses sum of the above spatial forces in one BodyActuator)"<< std::endl;
// Add control values and set their values
bodyActuator_sum->addInControls(bodyActuatorSum_Controls, modelControls_2);
model->setDefaultControls(modelControls_2);
// --------------------------- Compute Acc and Compare -------------------------
// now compare the acc due to forces/torques applied by this body actuator
model->computeStateVariableDerivatives(state2);
Vector udotOnlyBodyActuator = state2.getUDot();
udotOnlyBodyActuator.dump("Accelerations due to only-one body actuator");
// Verify that the bodyActuator_sum also generates the same acceleration
// as the equivalent applied by 3 Actuators in previous test case
// Also make sure that accelerations are not zero accidentally
ASSERT(udotOnlyBodyActuator.norm() != 0.0 || udotActuatorsCombination.norm() != 0.0);
for (int i = 0; i<udotActuatorsCombination.size(); ++i){
ASSERT_EQUAL(udotOnlyBodyActuator[i], udotActuatorsCombination[i], 1.0e-12);
}
// ------------------------ Setup integrator and manager -----------------------
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(state2);
std::cout << " ********** Test Actuator Combination 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;
}
/**
* 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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("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->attachGeometry(new Mesh("luxo_head_meters.obj"));
head->attachGeometry(new Mesh("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));
}
}
/**
* 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.attachGeometry(new Mesh("checkered_floor.vtp"));
// create linkage body
double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
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);
linkage1->attachGeometry(new Sphere(0.1));
// Graphical representation
Cylinder cyl(linkageDiameter/2, linkageLength);
Frame* cyl1Frame = new PhysicalOffsetFrame(*linkage1,
Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
cyl1Frame->setName("Cyl1_frame");
cyl1Frame->attachGeometry(cyl.clone());
osimModel.addFrame(cyl1Frame);
// Create a second linkage body as a clone of the first
OpenSim::Body* linkage2 = linkage1->clone();
linkage2->setName("linkage2");
Frame* cyl2Frame = new PhysicalOffsetFrame(*linkage2,
Transform(Vec3(0.0, linkageLength / 2.0, 0.0)));
cyl2Frame->setName("Cyl2_frame");
osimModel.addFrame(cyl2Frame);
// Create 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->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));
// Create 1 degree-of-freedom pin joints between the bodies to create 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};
ankle->updCoordinate().setName("q1");
ankle->updCoordinate().setRange(range);
knee->updCoordinate().setName("q2");
knee->updCoordinate().setRange(range);
hip->updCoordinate().setName("q3");
hip->updCoordinate().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 constraints 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 acceleration 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;
// Integrate
manager.setInitialTime(t0);
manager.setFinalTime(tf);
std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
manager.integrate(si);
// Save results
auto controlsTable = osimModel.getControlsTable();
STOFileAdapter_<double>::write(controlsTable,
"SpringActuatedLeg_controls.sto");
auto statesTable = manager.getStatesTable();
osimModel.updSimbodyEngine().convertRadiansToDegrees(statesTable);
STOFileAdapter_<double>::write(statesTable,
"SpringActuatedLeg_states_degrees.sto");
auto forcesTable = forces->getForcesTable();
STOFileAdapter_<double>::write(forcesTable, "actuator_forces.sto");
}
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 FRAME
// Get a reference to the model's ground frame
Ground& ground = osimModel.updGround();
// Attach 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, 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
block->attachGeometry(new Brick(SimTK::Vec3(0.05, 0.05, 0.05)));
// FREE JOINT
// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground frames
Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(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 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
// Obtain 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("prescribedForce", *block);
// 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 individual 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 manager managing the forward integration and its outputs
Manager manager(osimModel);
manager.setIntegratorAccuracy(1.0e-6);
// Integrate from initial time to final time
si.setTime(initialTime);
manager.initialize(si);
std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(finalTime);
}
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;
}