void performMCFD_flexion(Model model, int iterations)
{
std::default_random_engine gen;
random_device rd;
gen.seed(rd());
//std::uniform_real_distribution<double> random_stiff(5*1.e8, 5*1.e9);
//std::uniform_real_distribution<double> random_diss(0.0, 20.0);
//std::uniform_real_distribution<double> random_aPCL_force(4800, 6200);
std::uniform_real_distribution<double> random_PCL_length_range(0, 0.01); // length diff
// flexion controller
addFlexionController(model);
// init system
std::time_t result = std::time(nullptr);
std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl;
SimTK::State& si = model.initSystem();
result = std::time(nullptr);
std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl;
// disable muscles
string muscle_name;
for (int i=0; i<model.getActuators().getSize(); i++)
{
muscle_name = model.getActuators().get(i).getName();
model.getActuators().get(i).setDisabled(si, true);
if (muscle_name == "bifemlh_r" || muscle_name == "bifemsh_r" || muscle_name == "grac_r" \
|| muscle_name == "lat_gas_r" || muscle_name == "med_gas_r" || muscle_name == "sar_r" \
|| muscle_name == "semimem_r" || muscle_name == "semiten_r" \
|| muscle_name == "rect_fem_r" || muscle_name == "vas_med_r" || muscle_name == "vas_int_r" || muscle_name == "vas_lat_r" )
model.getActuators().get(i).setDisabled(si, false);
}
model.equilibrateMuscles( si);
Array<double> samplingArray1;
Array<double> samplingArray2;
for (int i = 50; i < iterations; i++)
{
// Add reporters
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
CustomAnalysis* customReporter = new CustomAnalysis(&model, "r");
model.addAnalysis(customReporter);
//string outputFile = changeToString(i) + "_fd_.sto";
//double this_random_stiff = random_stiff(gen);
//double this_random_diss = random_diss(gen);
//double this_random_aPCL = random_aPCL_force(gen);
//double this_random_pPCL = this_random_aPCL * 0.8363;
double this_random_PCL_diff = random_PCL_length_range(gen);
double this_random_aPCL_length = (0.031 - 0.005) + this_random_PCL_diff;
double this_random_pPCL_length = (0.030 - 0.005) + this_random_PCL_diff;
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setStiffness(this_random_stiff);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setStiffness(this_random_stiff);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setDissipation(this_random_diss);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setDissipation(this_random_diss);
//static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aPCL_R")).set_stiffness(this_random_aPCL);
//static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pPCL_R")).set_stiffness(this_random_pPCL);
static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aPCL_R")).setRestingLength(this_random_aPCL_length);
static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pPCL_R")).setRestingLength(this_random_pPCL_length);
samplingArray1.append(this_random_aPCL_length);
samplingArray2.append(this_random_pPCL_length);
setHipAngle(model, si, 90);
setKneeAngle(model, si, 0, false, false);
si = model.initSystem();
// Create the integrator and manager for the simulation.
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
//integrator.setAccuracy(1.0e-3);
//integrator.setFixedStepSize(0.0001);
Manager manager(model, integrator);
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 0.25;
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<". "<< i <<std::endl;
result = std::time(nullptr);
std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl;
manager.integrate(si);
result = std::time(nullptr);
std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl;
// Save the simulation results
Storage statesDegrees(manager.getStateStorage());
//statesDegrees.print("../outputs/MonteCarlo/states_rads/" + changeToString(i) + "_states_flexion.sto");
model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
statesDegrees.setWriteSIMMHeader(true);
statesDegrees.print("../outputs/MC_flexion/MC_PclLength_Flexion_v6/states/" + changeToString(i) + "_states_degrees_flexion.mot");
// force reporter results
forceReporter->getForceStorage().print("../outputs/MC_flexion/MC_PclLength_Flexion_v6/ForceReporter/" + changeToString(i) + "_force_reporter_flexion.mot");
customReporter->print( "../outputs/MC_flexion/MC_PclLength_Flexion_v6/CustomReporter/" + changeToString(i) + "_custom_reporter_flexion.mot");
model.removeAnalysis(forceReporter);
model.removeAnalysis(customReporter);
writeArrayToFile("../outputs/MC_flexion/MC_PclLength_Flexion_v6/aPCL_length.txt", samplingArray1);
writeArrayToFile("../outputs/MC_flexion/MC_PclLength_Flexion_v6/pPCL_length.txt", samplingArray2);
}
}
/**
* 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
OpenSim::Body& ground = osimModel.getGroundBody();
ground.addDisplayGeometry("checkered_floor.vtp");
// create linkage body
double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
Vec3 linkageMassCenter(0,linkageLength/2,0);
Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);
OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
// Graphical representation
linkage1->addDisplayGeometry("cylinder.vtp");
//This cylinder.vtp geometry is 1 meter tall, 1 meter diameter. Scale and shift it to look pretty
GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet();
DisplayGeometry& thinCylinder = geometry[0];
thinCylinder.setScaleFactors(linkageDimensions);
thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0)));
linkage1->addDisplayGeometry("sphere.vtp");
//This sphere.vtp is 1 meter in diameter. Scale it.
geometry[1].setScaleFactors(Vec3(0.1));
// Creat a second linkage body
OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
linkage2->setName("linkage2");
// Creat a block to be the pelvis
double blockMass = 20.0, blockSideLength = 0.2;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
block->addDisplayGeometry("block.vtp");
//This block.vtp is 0.1x0.1x0.1 meters. scale its appearance
block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0));
// Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);
PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1,
locationInChild, orientationInChild);
PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
locationInChild, orientationInChild);
PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
locationInChild, orientationInChild);
double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
ankleCoordinateSet[0].setName("q1");
ankleCoordinateSet[0].setRange(range);
CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
kneeCoordinateSet[0].setName("q2");
kneeCoordinateSet[0].setRange(range);
CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
hipCoordinateSet[0].setName("q3");
hipCoordinateSet[0].setRange(range);
// Add the bodies to the model
osimModel.addBody(linkage1);
osimModel.addBody(linkage2);
osimModel.addBody(block);
// Define contraints on the model
// Add a point on line constraint to limit the block to vertical motion
Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
osimModel.addConstraint(lineConstraint);
// Add PistonActuator between the first linkage and the block
Vec3 pointOnBodies(0);
PistonActuator *piston = new PistonActuator();
piston->setName("piston");
piston->setBodyA(linkage1);
piston->setBodyB(block);
piston->setPointA(pointOnBodies);
piston->setPointB(pointOnBodies);
piston->setOptimalForce(200.0);
piston->setPointsAreGlobal(false);
osimModel.addForce(piston);
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Added ControllableSpring between the first linkage and the second block
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ControllableSpring *spring = new ControllableSpring;
spring->setName("spring");
spring->setBodyA(block);
spring->setBodyB(linkage1);
spring->setPointA(pointOnBodies);
spring->setPointB(pointOnBodies);
spring->setOptimalForce(2000.0);
spring->setPointsAreGlobal(false);
spring->setRestLength(0.8);
osimModel.addForce(spring);
// define the simulation times
double t0(0.0), tf(15);
// create a controller to control the piston and spring actuators
// the prescribed controller sets the controls as functions of time
PrescribedController *legController = new PrescribedController();
// give the legController control over all (two) model actuators
legController->setActuators(osimModel.updActuators());
// specify some control nodes for spring stiffness control
double t[] = {0.0, 4.0, 7.0, 10.0, 15.0};
double x[] = {1.0, 1.0, 0.25, 0.25, 5.0};
// specify the control function for each actuator
legController->prescribeControlForActuator("piston", new Constant(0.1));
legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));
// add the controller to the model
osimModel.addController(legController);
// define the acceration due to gravity
osimModel.setGravity(Vec3(0, -9.80665, 0));
// enable the model visualizer see the model in action, which can be
// useful for debugging
osimModel.setUseVisualizer(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
osimModel.printControlStorage("SpringActuatedLeg_controls.sto");
Storage statesDegrees(manager.getStateStorage());
osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
//statesDegrees.print("PistonActuatedLeg_states_degrees.mot");
statesDegrees.print("SpringActuatedLeg_states_degrees.mot");
forces->getForceStorage().print("actuator_forces.mot");
}
catch (const std::exception& ex)
{
std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
return 1;
}
std::cout << "Exiting" << std::endl;
return 0;
}
void performMCFD_atl(Model model, int iterations)
{
// gaussian distribution setup
std::default_random_engine gen;
random_device rd;
gen.seed(rd());
//std::uniform_real_distribution<double> random_stiff(5*1.e8, 5*1.e9);
//std::uniform_real_distribution<double> random_diss(0.0, 20.0);
//std::uniform_real_distribution<double> random_aACL_force(1300,1700);
std::uniform_real_distribution<double> random_ACL_length_range(0, 0.005); // length diff
// init system
std::time_t result = std::time(nullptr);
std::cout << "\nBefore initSystem() " << std::asctime(std::localtime(&result)) << endl;
SimTK::State& si = model.initSystem();
result = std::time(nullptr);
std::cout << "\nAfter initSystem() " << std::asctime(std::localtime(&result)) << endl;
// set gravity
model.updGravityForce().setGravityVector(si, Vec3(0,0,0));
model.equilibrateMuscles( si);
Array<double> samplingArray1;
Array<double> samplingArray2;
for (int i = 20; i < iterations; i++)
{
double kneeAngle [6] = {0, -15, -30, -60, -90};
//double this_random_stiff = random_stiff(gen);
//double this_random_diss = random_diss(gen);
//double this_random_aACL = random_aACL_force(gen);
//double this_random_pACL = this_random_aACL * 1.266;
double this_random_ACL_diff = random_ACL_length_range(gen);
double this_random_aACL_length = (0.031 - 0.0025) + this_random_ACL_diff;
double this_random_pACL_length = (0.0253 - 0.0025) + this_random_ACL_diff;
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setStiffness(this_random_stiff);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setStiffness(this_random_stiff);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_lat_meniscii_r")).setDissipation(this_random_diss);
//static_cast<OpenSim::ElasticFoundationForce&>( model.updForceSet().get("femur_med_meniscii_r")).setDissipation(this_random_diss);
//static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aACL_R")).set_stiffness(this_random_aACL);
//static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pACL_R")).set_stiffness(this_random_pACL);
static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("aACL_R")).setRestingLength(this_random_aACL_length);
static_cast<OpenSim::CustomLigament&>( model.updForceSet().get("pACL_R")).setRestingLength(this_random_pACL_length);
samplingArray1.append(this_random_aACL_length);
samplingArray2.append(this_random_pACL_length);
for (int j=3; j<4; j++)
{
// Add reporters
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
CustomAnalysis* customReporter = new CustomAnalysis(&model, "r");
model.addAnalysis(customReporter);
//string outputFile = changeToString(i) + "_fd_.sto";
// edit prescribed force for anterior tibial load
addTibialLoads(model, kneeAngle[j]);
si = model.initSystem();
setKneeAngle(model, si, kneeAngle[j], true, true);
// Create the integrator and manager for the simulation.
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
//integrator.setAccuracy(1.0e-3);
//integrator.setFixedStepSize(0.0001);
Manager manager(model, integrator);
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 0.8;
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to " <<finalTime<<". "<< i <<std::endl;
result = std::time(nullptr);
std::cout << "\nBefore integrate(si) " << std::asctime(std::localtime(&result)) << endl;
manager.integrate(si);
result = std::time(nullptr);
std::cout << "\nAfter integrate(si) " << std::asctime(std::localtime(&result)) << endl;
// Save the simulation results
Storage statesDegrees(manager.getStateStorage());
//statesDegrees.print("../outputs/MonteCarlo/states_rads/" + changeToString(i) + "_states_flexion.sto");
model.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
statesDegrees.setWriteSIMMHeader(true);
statesDegrees.print("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/states/" + changeToString(i) + "_states_degrees_atl_" + changeToString(abs(kneeAngle[j])) + ".mot" );
// force reporter results
forceReporter->getForceStorage().print("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/ForceReporter/" + changeToString(i) + "_force_reporter_atl_" + changeToString(abs(kneeAngle[j])) + ".mot");
customReporter->print( "../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/CustomReporter/" + changeToString(i) + "_custom_reporter_atl_" + changeToString(abs(kneeAngle[j])) + ".mot");
model.removeAnalysis(forceReporter);
model.removeAnalysis(customReporter);
writeArrayToFile("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/aACL_length.txt", samplingArray1);
writeArrayToFile("../outputs/MC_anterior_loads/MC_AclLength_Atl_v6/pACL_length.txt", samplingArray2);
}
}
}
void testExternalLoad()
{
using namespace SimTK;
Model model("Pendulum.osim");
State &s = model.initSystem();
// Simulate gravity
double init_t =-1e-8;
double final_t = 2.0;
int nsteps = 10;
double dt = final_t/nsteps;
//initial state
double q_init = Pi/4;
model.updCoordinateSet()[0].setValue(s, q_init);
Vector_<double> q_grav(nsteps+1);
// Integrator and integration manager
double integ_accuracy = 1e-6;
RungeKuttaMersonIntegrator integrator(model.getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(model, integrator);
manager.setInitialTime(init_t);
for(int i = 0; i < nsteps+1; i++){
manager.setFinalTime(dt*i);
manager.integrate(s);
q_grav[i] = model.updCoordinateSet()[0].getValue(s);
manager.setInitialTime(dt*i);
}
//q_grav.dump("Coords due to gravity.");
/***************************** CASE 1 ************************************/
// Simulate the same system without gravity but with an equivalent external load
OpenSim::Body &pendulum = model.getBodySet().get(model.getNumBodies()-1);
string pendBodyName = pendulum.getName();
Vec3 comInB;
pendulum.getMassCenter(comInB);
Storage forceStore;
addLoadToStorage(forceStore, pendulum.getMass()*model.getGravity(), comInB, Vec3(0, 0, 0));
forceStore.setName("test_external_loads.sto");
forceStore.print(forceStore.getName());
// Apply external force with force in ground, point in body, zero torque
ExternalForce xf(forceStore, "force", "point", "torque", pendBodyName, "ground", pendBodyName);
xf.setName("grav");
ExternalLoads* extLoads = new ExternalLoads(model);
extLoads->adoptAndAppend(&xf);
extLoads->print("ExternalLoads_test.xml");
for(int i=0; i<extLoads->getSize(); i++)
model.addForce(&(*extLoads)[i]);
// Create the force reporter
ForceReporter* reporter = new ForceReporter(&model);
model.addAnalysis(reporter);
Kinematics* kin = new Kinematics(&model);
kin->setInDegrees(false);
model.addAnalysis(kin);
PointKinematics* pKin = new PointKinematics(&model);
pKin->setBody(&pendulum);
pKin->setPoint(comInB);
pKin->setPointName(pendulum.getName()+"_com_p");
model.addAnalysis(pKin);
SimTK::State &s2 = model.initSystem();
// Turn-off gravity in the model
model.updGravityForce().disable(s2);
// initial position
model.updCoordinateSet()[0].setValue(s2, q_init);
RungeKuttaMersonIntegrator integrator2(model.getMultibodySystem() );
integrator2.setAccuracy(integ_accuracy);
Manager manager2(model, integrator2);
manager2.setInitialTime(init_t);
// Simulate with the external force applied instead of gravity
Vector_<double> q_xf(nsteps+1);
Vector_<Vec3> pcom_xf(nsteps+1);
for(int i = 0; i < nsteps+1; i++){
manager2.setFinalTime(dt*i);
manager2.integrate(s2);
q_xf[i] = model.updCoordinateSet()[0].getValue(s2);
manager2.setInitialTime(dt*i);
}
//q_xf.dump("Coords due to external force point expressed in pendulum.");
Vector err = q_xf-q_grav;
double norm_err = err.norm();
// kinematics should match to within integ accuracy
ASSERT_EQUAL(0.0, norm_err, integ_accuracy);
/***************************** CASE 2 ************************************/
// Simulate the same system without gravity but with an equivalent external
// force but this time with the point expressed in ground and using
// previous kinematics to transform point to pendulum.
// Construct a new Storage for ExternalForce data source with point
// described in ground
Storage forceStore2 = reporter->getForceStorage();
forceStore2.print("ForcesTest.sto");
Storage *pStore = pKin->getPositionStorage();
pStore->print("PointInGroundTest.sto");
pStore->addToRdStorage(forceStore2, init_t, final_t);
forceStore2.setName("ExternalForcePointInGround.sto");
forceStore2.print(forceStore2.getName());
Storage *qStore = kin->getPositionStorage();
qStore->print("LoadKinematics.sto");
string id_base = pendBodyName+"_"+xf.getName();
string point_id = pKin->getPointName();
ExternalForce xf2(forceStore2, id_base+"_F", point_id, id_base+"_T", pendBodyName, "ground", "ground");
xf2.setName("xf_pInG");
// Empty out existing external forces
extLoads->setMemoryOwner(false);
extLoads->setSize(0);
extLoads->adoptAndAppend(&xf2);
//Ask external loads to transform point expressed in ground to the applied body
extLoads->setDataFileName(forceStore2.getName());
extLoads->invokeConnectToModel(model);
extLoads->transformPointsExpressedInGroundToAppliedBodies(*qStore);
// remove previous external force from the model too
model.disownAllComponents();
model.updForceSet().setSize(0);
// after external loads has transformed the point of the force, then add it the model
for(int i=0; i<extLoads->getSize(); i++)
model.addForce(&(*extLoads)[i]);
// recreate dynamical system to reflect new force
SimTK::State &s3 = model.initSystem();
// Turn-off gravity in the model
model.updGravityForce().disable(s3);
// initial position
model.updCoordinateSet()[0].setValue(s3, q_init);
RungeKuttaMersonIntegrator integrator3(model.getMultibodySystem() );
integrator3.setAccuracy(integ_accuracy);
Manager manager3(model, integrator3);
manager3.setInitialTime(init_t);
// Simulate with the external force applied instead of gravity
Vector_<double> q_xf2(nsteps+1);
for(int i = 0; i < nsteps+1; i++){
manager3.setFinalTime(dt*i);
manager3.integrate(s3);
q_xf2[i] = model.updCoordinateSet()[0].getValue(s3);
manager3.setInitialTime(dt*i);
}
//q_xf2.dump("Coords due to external force point expressed in ground.");
err = q_xf2-q_grav;
//err.dump("Coordinate error after transforming point.");
norm_err = err.norm();
// kinematics should match to within integ accuracy
ASSERT_EQUAL(0.0, norm_err, integ_accuracy);
}
/*==============================================================================
Main test driver to be used on any muscle model (derived from Muscle) so new
cases should be easy to add currently, the test only verifies that the work
done by the muscle corresponds to the change in system energy.
TODO: Test will fail wih prescribe motion until the work done by this
constraint is accounted for.
================================================================================
*/
void simulateMuscle(
const Muscle &aMuscModel,
double startX,
double act0,
const Function *motion, // prescribe motion of free end of muscle
const Function *control, // prescribed excitation signal to the muscle
double integrationAccuracy,
int testType,
double testTolerance,
bool printResults)
{
string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";
cout << "\n******************************************************" << endl;
cout << "Test " << aMuscModel.getConcreteClassName()
<< " Model" << prescribed << endl;
cout << "******************************************************" << endl;
using SimTK::Vec3;
//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 4.0;
//Physical properties of the model
double ballMass = 10;
double ballRadius = 0.05;
double anchorWidth = 0.1;
// Create an OpenSim model
Model model;
double optimalFiberLength = aMuscModel.getOptimalFiberLength();
double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength();
double tendonSlackLength = aMuscModel.getTendonSlackLength();
// Use a copy of the muscle model passed in to add path points later
PathActuator *aMuscle = aMuscModel.clone();
// Get a reference to the model's ground body
Ground& ground = model.updGround();
ground.addDisplayGeometry("box.vtp");
ground.updDisplayer()
->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2 * anchorWidth));
OpenSim::Body * ball = new OpenSim::Body("ball",
ballMass,
Vec3(0),
ballMass*SimTK::Inertia::sphere(ballRadius));
ball->addDisplayGeometry("sphere.vtp");
ball->updDisplayer()->setScaleFactors(Vec3(2 * ballRadius));
// ball connected to ground via a slider along X
double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength;
SliderJoint* slider = new SliderJoint("slider",
ground,
Vec3(anchorWidth / 2 + xSinG, 0, 0),
Vec3(0),
*ball,
Vec3(0),
Vec3(0));
CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
jointCoordinateSet[0].setName("tx");
jointCoordinateSet[0].setDefaultValue(1.0);
jointCoordinateSet[0].setRangeMin(0);
jointCoordinateSet[0].setRangeMax(1.0);
if (motion != NULL){
jointCoordinateSet[0].setPrescribedFunction(*motion);
jointCoordinateSet[0].setDefaultIsPrescribed(true);
}
// add ball to model
model.addBody(ball);
model.addJoint(slider);
//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================
//Attach the muscle
const string &actuatorType = aMuscle->getConcreteClassName();
aMuscle->setName("muscle");
aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0));
aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0));
ActivationFiberLengthMuscle_Deprecated *aflMuscle
= dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
if (aflMuscle){
// Define the default states for the muscle that has
//activation and fiber-length states
aflMuscle->setDefaultActivation(act0);
aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
}
else{
ActivationFiberLengthMuscle *aflMuscle2
= dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
if (aflMuscle2){
// Define the default states for the muscle
//that has activation and fiber-length states
aflMuscle2->setDefaultActivation(act0);
aflMuscle2->setDefaultFiberLength(aflMuscle2
->getOptimalFiberLength());
}
}
model.addForce(aMuscle);
// Create a prescribed controller that simply
//applies controls as function of time
PrescribedController * muscleController = new PrescribedController();
if (control != NULL){
muscleController->setActuators(model.updActuators());
// Set the indiviudal muscle control functions
//for the prescribed muscle controller
muscleController->prescribeControlForActuator("muscle", control->clone());
// Add the control set controller to the model
model.addController(muscleController);
}
// Set names for muscles / joints.
Array<string> muscNames;
muscNames.append(aMuscle->getName());
Array<string> jointNames;
jointNames.append("slider");
//==========================================================================
// 2. SIMULATION SETUP: Instrument the test with probes
//==========================================================================
Array<string> muscNamesTwice = muscNames;
muscNamesTwice.append(muscNames.get(0));
cout << "------------\nPROBES\n------------" << endl;
int probeCounter = 1;
// Add ActuatorPowerProbe to measure work done by the muscle
ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
//muscWorkProbe->setName("ActuatorWork");
muscWorkProbe->setOperation("integrate");
SimTK::Vector ic1(1);
ic1 = 9.0; // some arbitary initial condition.
muscWorkProbe->setInitialConditions(ic1);
model.addProbe(muscWorkProbe);
model.setup();
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl;
// Add ActuatorPowerProbe to measure power generated by the muscle
ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor
muscPowerProbe->setName("ActuatorPower");
muscPowerProbe->setOperation("value");
model.addProbe(muscPowerProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl;
// Add ActuatorPowerProbe to report the muscle power MINIMUM
ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinimum->setName("ActuatorPowerMinimum");
powerProbeMinimum->setOperation("minimum");
model.addProbe(powerProbeMinimum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM
ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinAbs->setName("ActuatorPowerMinAbs");
powerProbeMinAbs->setOperation("minabs");
model.addProbe(powerProbeMinAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXIMUM
ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaximum->setName("ActuatorPowerMaximum");
powerProbeMaximum->setOperation("maximum");
model.addProbe(powerProbeMaximum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXABS
ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaxAbs->setName("ActuatorPowerMaxAbs");
powerProbeMaxAbs->setOperation("maxabs");
model.addProbe(powerProbeMaxAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl;
// Add ActuatorPowerProbe to measure the square of the power generated by the muscle
ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
muscPowerSquaredProbe->setName("ActuatorPowerSquared");
muscPowerSquaredProbe->setExponent(2.0);
model.addProbe(muscPowerSquaredProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl;
// Add JointInternalPowerProbe to measure work done by the joint
JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1);
jointWorkProbe->setName("JointWork");
jointWorkProbe->setOperation("integrate");
jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(jointWorkProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl;
// Add JointPowerProbe to measure power generated by the joint
JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor
jointPowerProbe->setName("JointPower");
jointPowerProbe->setOperation("value");
model.addProbe(jointPowerProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl;
// Add ActuatorForceProbe to measure the impulse of the muscle force
ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1);
impulseProbe->setName("ActuatorImpulse");
impulseProbe->setOperation("integrate");
impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(impulseProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl;
// Add ActuatorForceProbe to report the muscle force
ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbe->setName("ActuatorForce");
forceProbe->setOperation("value");
model.addProbe(forceProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl;
// Add ActuatorForceProbe to report the square of the muscle force
ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbe->setName("ActuatorForceSquared");
forceSquaredProbe->setExponent(2.0);
model.addProbe(forceSquaredProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice
ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice");
forceSquaredProbeTwice->setSumForcesTogether(true);
forceSquaredProbeTwice->setActuatorNames(muscNamesTwice);
model.addProbe(forceSquaredProbeTwice);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5
ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved");
double gain1 = 0.5;
forceSquaredProbeTwiceScaled->setGain(gain1);
model.addProbe(forceSquaredProbeTwiceScaled);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl;
// Add ActuatorForceProbe to report -3.5X the muscle force
double gain2 = -3.50;
ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeScale->setName("ScaleActuatorForce");
forceProbeScale->setOperation("value");
forceProbeScale->setGain(gain2);
model.addProbe(forceProbeScale);
cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force
ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeDiff->setName("DifferentiateActuatorForce");
forceProbeDiff->setOperation("differentiate");
model.addProbe(forceProbeDiff);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl;
// Add SystemEnergyProbe to measure the system KE+PE
SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true);
sysEnergyProbe->setName("SystemEnergy");
sysEnergyProbe->setOperation("value");
sysEnergyProbe->setComputeKineticEnergy(true);
sysEnergyProbe->setComputePotentialEnergy(true);
model.addProbe(sysEnergyProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl;
// Add SystemEnergyProbe to measure system power (d/dt system KE+PE)
SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor
sysPowerProbe->setName("SystemPower");
sysPowerProbe->setDisabled(false);
sysPowerProbe->setOperation("differentiate");
model.addProbe(sysPowerProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl;
// Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR");
forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice);
//cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl;
forceSquaredProbeTwiceReportedIndividually1->setOperation("value");
model.addProbe(forceSquaredProbeTwiceReportedIndividually1);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate");
model.addProbe(forceSquaredProbeTwiceReportedIndividually2);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate");
SimTK::Vector initCondVec(2);
initCondVec(0) = 0;
initCondVec(1) = 10;
forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec);
model.addProbe(forceSquaredProbeTwiceReportedIndividually3);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
cout << "initCondVec = " << initCondVec << endl;
/* Since all components are allocated on the stack don't have model
own them (and try to free)*/
// model.disownAllComponents();
model.setName("testProbesModel");
cout << "Saving model... " << endl;
model.print("testProbesModel.osim");
cout << "Re-loading model... " << endl;
Model reloadedModel = Model("testProbesModel.osim");
/* Setup analyses and reporters. */
ProbeReporter* probeReporter = new ProbeReporter(&model);
model.addAnalysis(probeReporter);
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model);
model.addAnalysis(muscleReporter);
model.print("testProbesModel.osim");
model.printBasicInfo(cout);
//==========================================================================
// 3. SIMULATION Initialization
//==========================================================================
// Initialize the system and get the default state
SimTK::State& si = model.initSystem();
SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si);
model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics);
model.equilibrateMuscles(si);
CoordinateSet& modelCoordinateSet = model.updCoordinateSet();
// Define non-zero (defaults are 0) states for the free joint
// set x-translation value
modelCoordinateSet[0].setValue(si, startX, true);
//Copy the initial state
SimTK::State initialState(si);
// Check muscle is setup correctly
const PathActuator &muscle
= dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
double length = muscle.getLength(si);
double trueLength = startX + xSinG - anchorWidth / 2;
ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__,
"testMuscles: path failed to initialize to correct length.");
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
//cout << "Muscle initial energy = " << Emuscle0 << endl;
double Esys0 = model.getMultibodySystem().calcEnergy(si);
Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
//cout << "Total initial system energy = " << Esys0 << endl;
//==========================================================================
// 4. SIMULATION Integration
//==========================================================================
// Create the integrator
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
integrator.setAccuracy(integrationAccuracy);
// Create the manager
Manager manager(model, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl;
// Start timing the simulation
const clock_t start = clock();
// simulate
manager.integrate(si);
// how long did it take?
double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC;
//==========================================================================
// 5. SIMULATION Reporting
//==========================================================================
double realTimeMultiplier = ((finalTime - initialTime) / comp_time);
printf("testMuscles: Realtime Multiplier: %f\n"
" : simulation duration / clock duration\n"
" > 1 : faster than real time\n"
" = 1 : real time\n"
" < 1 : slower than real time\n",
realTimeMultiplier);
/*
ASSERT(comp_time <= (finalTime-initialTime));
printf("testMuscles: PASSED Realtime test\n"
" %s simulation time: %f with accuracy %f\n\n",
actuatorType.c_str(), comp_time , accuracy);
*/
//An analysis only writes to a dir that exists, so create here.
if (printResults == true){
Storage states(manager.getStateStorage());
states.print("testProbes_states.sto");
probeReporter->getProbeStorage().print("testProbes_probes.sto");
forceReporter->getForceStorage().print("testProbes_forces.sto");
muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto");
cout << "\nDone with printing results..." << endl;
}
double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
// Test the resetting of probes
cout << "Resetting muscle work probe..." << endl;
muscWorkProbe->reset(si);
muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe.");
//==========================================================================
// 6. SIMULATION Tests
//==========================================================================
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe.");
ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe.");
for (int i = 0; i<initCondVec.size(); ++i) {
stringstream myError;
//myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl;
//ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str());
//if (testRealInitConditions(i) != initCondVec(i))
// cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl;
}
}
// test sphere to sphere contact using elastic foundation with and without
// meshes and their combination
int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2)
{
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
Vec3(0), Inertia());
osimModel->addBody(&ground);
OpenSim::Body ball;
ball.setName("ball");
ball.setMass(mass);
ball.setMassCenter(Vec3(0));
ball.setInertia(Inertia(1.0));
// Add joints
FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));
osimModel->addBody(&ball);
osimModel->addJoint(&free);
// Create ContactGeometry.
OpenSim::ContactGeometry *ball1, *ball2;
if (useElasticFoundation && useMesh1)
ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1");
else
ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1");
if (useElasticFoundation && useMesh2)
ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2");
else
ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2");
osimModel->addContactGeometry(ball1);
osimModel->addContactGeometry(ball2);
OpenSim::Force* force;
std::string prefix;
if (useElasticFoundation){
}
else{
}
if (useElasticFoundation)
{
// Add an ElasticFoundationForce.
OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball1");
contactParams->addGeometry("ball2");
force = new OpenSim::ElasticFoundationForce(contactParams);
prefix = "EF_";
prefix += useMesh1 ?"Mesh":"noMesh";
prefix += useMesh2 ? "_to_Mesh":"_to_noMesh";
}
else
{
// Add a Hertz HuntCrossleyForce.
OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball1");
contactParams->addGeometry("ball2");
force = new OpenSim::HuntCrossleyForce(contactParams);
prefix = "Hertz";
}
force->setName("contact");
osimModel->addForce(force);
osimModel->setGravity(gravity_vec);
osimModel->setName(prefix);
osimModel->clone()->print(prefix+".osim");
Kinematics* kin = new Kinematics(osimModel);
osimModel->addAnalysis(kin);
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
osim_state.updQ()[4] = height;
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//==========================================================================================================
// Simulate it and see if it bounces correctly.
cout << "stateY=" << osim_state.getY() << std::endl;
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
integrator.setMaximumStepSize(100*integ_accuracy);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
manager.setFinalTime(duration);
manager.integrate(osim_state);
kin->printResults(prefix);
reporter->printResults(prefix);
osimModel->disownAllComponents();
// model takes ownership of components unless container set is told otherwise
delete osimModel;
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;
}
void testCoordinateLimitForce()
{
using namespace SimTK;
double mass = 1;
double ball_radius = 0.25;
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setName("CoordinateLimitForceTest");
//OpenSim bodies
OpenSim::Body& ground = osimModel->getGroundBody();
OpenSim::Body ball("ball", mass ,Vec3(0), mass*SimTK::Inertia::sphere(0.1));
ball.addDisplayGeometry("sphere.vtp");
ball.scale(Vec3(ball_radius), false);
// Add joints
SliderJoint slider("", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {0.1, 2};
// Rename coordinates for a slider joint
CoordinateSet &slider_coords = slider.upd_CoordinateSet();
slider_coords[0].setName("ball_h");
slider_coords[0].setRange(positionRange);
slider_coords[0].setMotionType(Coordinate::Translational);
osimModel->addBody(&ball);
osimModel->addJoint(&slider);
osimModel->setGravity(gravity_vec);
// Define the parameters of the Coordinate Limit Force
double K_upper = 200.0;
double K_lower = 1000.0;
double damping = 0.01;
double trans = 0.05;
CoordinateLimitForce limitForce("ball_h", positionRange[1], K_upper,
positionRange[0], K_lower, damping, trans, true);
osimModel->addForce(&limitForce);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
osimModel->print("CoordinateLimitForceTest.osim");
// Check serialization and deserilaization
Model* loadedModel = new Model("CoordinateLimitForceTest.osim");
ASSERT(*loadedModel == *osimModel,
"Deserialized CoordinateLimitForceTest failed to be equivalent to original.");
// check copy
Model *copyModel = osimModel->clone();
ASSERT(*copyModel == *loadedModel,
"Clone of CoordinateLimitForceTest failed to be equivalent to original.");
copyModel->print("cloneCoordinateLimitForceTest.osim");
delete osimModel;
osimModel = copyModel;
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
double dh = 0.2;
double start_h = positionRange[1];
double start_v = 2.0;
const Coordinate &q_h = osimModel->getCoordinateSet()[0];
q_h.setValue(osim_state, start_h);
q_h.setSpeedValue(osim_state, start_v);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration );
CoordinateLimitForce *clf = dynamic_cast<CoordinateLimitForce *>(&osimModel->getForceSet()[0]);
// initial energy of the system;
double clfPE = clf->computePotentialEnergy(osim_state);
double constStiffnessPE = 0.5*K_upper*dh*dh;
ASSERT(clfPE < constStiffnessPE);
double energy0 = clfPE + mass*(-gravity_vec[1])*start_h + 0.5*mass*start_v*start_v;
// system KE + PE including strain energy in CLF
double eSys0 = osimModel->getMultibodySystem().calcEnergy(osim_state);
// instantaneous power dissipation
double clfPowerDissipation = clf->getPowerDissipation(osim_state);
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 1.0;
double nsteps = 20;
double dt = final_t/nsteps;
for(int i = 1; i <=nsteps; i++){
manager.setFinalTime(dt*i);
manager.integrate(osim_state);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
double h = q_h.getValue(osim_state);
double v = q_h.getSpeedValue(osim_state);
//Now check that the force reported by spring
Array<double> model_force = clf->getRecordValues(osim_state);
double ediss = clf->getDissipatedEnergy(osim_state);
double clfE = clf->computePotentialEnergy(osim_state) + ediss;
// EK + EM of mass alone
double eMass = 0.5*mass*v*v - mass*gravity_vec[1]*h;
double eSpringGuess = energy0-eMass;
double e = eMass+clfE;
// system KE + PE including strain energy in CLF
double eSys = osimModel->getMultibodySystem().calcEnergy(osim_state)+ ediss;
ASSERT_EQUAL(1.0, e/energy0, integ_accuracy, "CoordinateLimitForce Failed to conserve energy");
ASSERT_EQUAL(1.0, eSys/eSys0, integ_accuracy, "CoordinateLimitForce Failed to conserve system energy");
// get the forces applied to the ball by the limit force
double analytical_force = 0;
if(h > (positionRange[1]+trans)){
ASSERT_EQUAL(-K_upper*(h-positionRange[1])-damping*v, model_force[0], 1e-4);
}
else if( h < (positionRange[0]-trans)){
ASSERT_EQUAL(K_lower*(positionRange[0]-h)-damping*v, model_force[0], 1e-4);
}
else if( (h < positionRange[1]) && (h > positionRange[0])){
// Verify no force is being applied when within limits
ASSERT_EQUAL(0.0, model_force[0], 1e-5);
}
manager.setInitialTime(dt*i);
}
manager.getStateStorage().print("coordinte_limit_force_model_states.sto");
// Save the forces
reporter->getForceStorage().print("limit_forces.mot");
}
// Test our wraapping of Hunt-Crossley force in OpenSim
// Simple simulation of bouncing ball with dissipation should generate contact
// forces that settle to ball weight.
void testHuntCrossleyForce()
{
using namespace SimTK;
double start_h = 0.5;
// Setup OpenSim model
Model *osimModel = new Model("BouncingBall_HuntCrossley.osim");
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
osimModel->getCoordinateSet()[4].setValue(osim_state, start_h);
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
const OpenSim::Body &ball = osimModel->getBodySet().get("ball");
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 2.0;
manager.setFinalTime(final_t);
// start timing
clock_t startTime = clock();
manager.integrate(osim_state);
// end timing
cout << "Hunt Crossley simulation time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms" << endl;
//make sure we can access dynamic variables
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
// Print out the motion for visualizing/debugging
manager.getStateStorage().print("bouncing_ball_HC_states.sto");
// Save the forces
reporter->getForceStorage().print("HuntCrossley_contact_forces.mot");
// Bouncing ball should have settled to rest on groun due to dissipation
// In that case the force generated by contact should be identically body weight
// in vertical and zero else where.
OpenSim::HuntCrossleyForce &contact = (OpenSim::HuntCrossleyForce &)osimModel->getForceSet().get("contact");
Array<double> contact_force = contact.getRecordValues(osim_state);
ASSERT_EQUAL(contact_force[0], 0.0, 1e-4); // no horizontal force on the ball
ASSERT_EQUAL(contact_force[1], -ball.getMass()*gravity_vec[1], 1e-3); // vertical is weight
ASSERT_EQUAL(contact_force[2], 0.0, 1e-4); // no horizontal force on the ball
ASSERT_EQUAL(contact_force[3], 0.0, 1e-4); // no torque on the ball
ASSERT_EQUAL(contact_force[4], 0.0, 1e-4); // no torque on the ball
ASSERT_EQUAL(contact_force[5], 0.0, 1e-4); // no torque on the ball
// Before exiting lets see if copying the force works
OpenSim::HuntCrossleyForce *copyOfForce = contact.clone();
bool isEqual = (*copyOfForce == contact);
if(!isEqual){
contact.print("originalForce.xml");
copyOfForce->print("copyOfForce.xml");
}
ASSERT(isEqual);
}
void testFunctionBasedBushingForce()
{
using namespace SimTK;
double mass = 1;
double stiffness = 10;
double restlength = 0.0;
double h0 = 0;
double start_h = 0.5;
double ball_radius = 0.25;
double omega = sqrt(stiffness/mass);
double dh = mass*gravity_vec(1)/stiffness;
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setName("FunctionBasedBushingTest");
//OpenSim bodies
OpenSim::Body& ground = osimModel->getGroundBody();
OpenSim::Body ball("ball", mass, Vec3(0), mass*SimTK::Inertia::sphere(0.1));
ball.addDisplayGeometry("sphere.vtp");
ball.scale(Vec3(ball_radius), false);
// Add joints
SliderJoint slider("", ground, Vec3(0), Vec3(0,0,Pi/2), ball, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {-10, 10};
// Rename coordinates for a slider joint
CoordinateSet &slider_coords = slider.upd_CoordinateSet();
slider_coords[0].setName("ball_h");
slider_coords[0].setRange(positionRange);
slider_coords[0].setMotionType(Coordinate::Translational);
osimModel->addBody(&ball);
osimModel->addJoint(&slider);
Vec3 rotStiffness(0);
Vec3 transStiffness(stiffness);
Vec3 rotDamping(0);
Vec3 transDamping(0);
osimModel->setGravity(gravity_vec);
FunctionBasedBushingForce spring("ground", Vec3(0), Vec3(0), "ball", Vec3(0), Vec3(0), transStiffness, rotStiffness, transDamping, rotDamping);
spring.setName("linear_bushing");
osimModel->addForce(&spring);
osimModel->print("FunctionBasedBushingForceModel.osim");
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
slider_coords[0].setValue(osim_state, start_h);
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 2.0;
double nsteps = 10;
double dt = final_t/nsteps;
for(int i = 1; i <=nsteps; i++){
manager.setFinalTime(dt*i);
manager.integrate(osim_state);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
Vec3 pos;
osimModel->updSimbodyEngine().getPosition(osim_state, ball, Vec3(0), pos);
double height = (start_h-dh)*cos(omega*osim_state.getTime())+dh;
ASSERT_EQUAL(height, pos(1), 1e-4);
//Now check that the force reported by spring
Array<double> model_force = spring.getRecordValues(osim_state);
// get the forces applied to the ground and ball
double analytical_force = -stiffness*height;
// analytical force corresponds in direction to the force on the ball Y index = 7
ASSERT_EQUAL(analytical_force, model_force[7], 2e-4);
manager.setInitialTime(dt*i);
}
osimModel->disownAllComponents();
manager.getStateStorage().print("function_based_bushing_model_states.sto");
// Save the forces
reporter->getForceStorage().print("function_based_bushing_forces.mot");
// Before exiting lets see if copying the spring works
FunctionBasedBushingForce *copyOfSpring = spring.clone();
ASSERT(*copyOfSpring == spring);
}
void testPathSpring()
{
using namespace SimTK;
double mass = 1;
double stiffness = 10;
double restlength = 0.5;
double dissipation = 0.1;
double h0 = 0;
double start_h = 0.5;
double ball_radius = 0.25;
double omega = sqrt(stiffness/mass);
double dh = mass*gravity_vec(1)/stiffness;
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setName("PathSpring");
//OpenSim bodies
OpenSim::Body& ground = osimModel->getGroundBody();
OpenSim::Body pulleyBody("PulleyBody", mass ,Vec3(0), mass*SimTK::Inertia::brick(0.1, 0.1, 0.1));
OpenSim::Body block("block", mass ,Vec3(0), mass*SimTK::Inertia::brick(0.2, 0.1, 0.1));
block.addDisplayGeometry("box.vtp");
block.scale(Vec3(0.2, 0.1, 0.1), false);
WrapCylinder* pulley = new WrapCylinder();
pulley->setRadius(0.1);
pulley->setLength(0.05);
// Add the wrap object to the body, which takes ownership of it
pulleyBody.addWrapObject(pulley);
// Add joints
WeldJoint weld("pulley", ground, Vec3(0, 1.0, 0), Vec3(0), pulleyBody, Vec3(0), Vec3(0));
SliderJoint slider("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
CoordinateSet &slider_coords = slider.upd_CoordinateSet();
slider_coords[0].setName("block_h");
slider_coords[0].setRange(positionRange);
slider_coords[0].setMotionType(Coordinate::Translational);
osimModel->addBody(&block);
osimModel->addJoint(&weld);
osimModel->addBody(&pulleyBody);
osimModel->addJoint(&slider);
osimModel->setGravity(gravity_vec);
PathSpring spring("spring", restlength, stiffness, dissipation);
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
osimModel->addForce(&spring);
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
//osimModel->setUseVisualizer(true);
SimTK::State& osim_state = osimModel->initSystem();
slider_coords[0].setValue(osim_state, start_h);
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-6);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 10.0;
manager.setFinalTime(final_t);
manager.integrate(osim_state);
// tension should only be velocity dependent
osimModel->getMultibodySystem().realize(osim_state, Stage::Velocity);
// Now check that the force reported by spring
double model_force = spring.getTension(osim_state);
// get acceleration of the block
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
double hddot = osimModel->getCoordinateSet().get("block_h").getAccelerationValue(osim_state);
// the tension should be half the weight of the block
double analytical_force = -0.5*(gravity_vec(1)-hddot)*mass;
// Save the forces
reporter->getForceStorage().print("path_spring_forces.mot");
// something is wrong if the block does not reach equilibrium
ASSERT_EQUAL(analytical_force, model_force, 1e-3);
// Before exiting lets see if copying the spring works
PathSpring *copyOfSpring = spring.clone();
ASSERT(*copyOfSpring == spring);
osimModel->disownAllComponents();
}
void testCoordinateLimitForceRotational()
{
using namespace SimTK;
double mass = 1;
double edge = 0.2;
// Setup OpenSim model
Model *osimModel = new Model;
osimModel->setName("RotationalCoordinateLimitForceTest");
//OpenSim bodies
OpenSim::Body& ground = osimModel->getGroundBody();
OpenSim::Body block("block", mass ,Vec3(0), mass*SimTK::Inertia::brick(edge,edge,edge));
block.addDisplayGeometry("box.vtp");
block.scale(Vec3(edge), false);
// Add joints
PinJoint pin("", ground, Vec3(0), Vec3(0,0,0), block, Vec3(0,-edge,0), Vec3(0,0,0));
// NOTE: Angular limits are in degrees NOT radians
double positionRange[2] = {-30, 90};
// Rename coordinates for a slider joint
CoordinateSet &pin_coords = pin.upd_CoordinateSet();
pin_coords[0].setName("theta");
pin_coords[0].setRange(positionRange);
pin_coords[0].setMotionType(Coordinate::Rotational);
osimModel->addBody(&block);
osimModel->addJoint(&pin);
osimModel->setGravity(Vec3(0));
// Define the parameters of the Coordinate Limit Force
// For rotational coordinates, these are in Nm/degree
double K_upper = 10.0;
double K_lower = 20.0;
double damping = 0.1;
double trans = 0.05;
CoordinateLimitForce limitForce("theta", positionRange[1], K_upper,
positionRange[0], K_lower, damping, trans, true);
osimModel->addForce(&limitForce);
// BAD: have to set memoryOwner to false or program will crash when this test is complete.
osimModel->disownAllComponents();
// Create the force reporter
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
// Start 2 degrees beyond the upper limit
double start_q = SimTK_DEGREE_TO_RADIAN*positionRange[1] + SimTK::Pi/90;
double start_v = 0.0;
const Coordinate &coord = osimModel->getCoordinateSet()[0];
coord.setValue(osim_state, start_q);
coord.setSpeedValue(osim_state, start_v);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration );
CoordinateLimitForce *clf = dynamic_cast<CoordinateLimitForce *>(&osimModel->getForceSet()[0]);
//Now check that the force reported by spring
Array<double> model_force = clf->getRecordValues(osim_state);
ASSERT_EQUAL(model_force[0]/(-2*K_upper), 1.0, integ_accuracy);
double clfPE = clf->computePotentialEnergy(osim_state);
double constSpringPE = 0.5*(K_upper*2.0)*2.0*SimTK_DEGREE_TO_RADIAN;
ASSERT_EQUAL(clfPE/constSpringPE, 1.0, 0.001, "Specified upper rotational stiffness not met.");
ASSERT(clfPE < constSpringPE);
//Now test lower bound
start_q = SimTK_DEGREE_TO_RADIAN*positionRange[0] - SimTK::Pi/90;
coord.setValue(osim_state, start_q);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration );
model_force = clf->getRecordValues(osim_state);
ASSERT_EQUAL(model_force[0]/(2*K_lower), 1.0, integ_accuracy);
clfPE = clf->computePotentialEnergy(osim_state);
constSpringPE = 0.5*(K_lower*2.0)*2.0*SimTK_DEGREE_TO_RADIAN;
ASSERT_EQUAL(clfPE/constSpringPE, 1.0, 0.001);
ASSERT(clfPE < constSpringPE);
// total system energy prior to simulation
double eSys0 = osimModel->getMultibodySystem().calcEnergy(osim_state);
//==========================================================================
// Perform a simulation an monitor energy conservation
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(1e-8);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
double final_t = 1.0;
double nsteps = 20;
double dt = final_t/nsteps;
for(int i = 1; i <=nsteps; i++){
manager.setFinalTime(dt*i);
manager.integrate(osim_state);
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
double q = coord.getValue(osim_state);
double u = coord.getSpeedValue(osim_state);
double ediss = clf->getDissipatedEnergy(osim_state);
// system KE + PE including strain energy in CLF
double eSys = osimModel->getMultibodySystem().calcEnergy(osim_state)+ ediss;
double EKsys = osimModel->getMultibodySystem().calcKineticEnergy(osim_state);
ASSERT_EQUAL(eSys/eSys0, 1.0, integ_accuracy);
manager.setInitialTime(dt*i);
}
manager.getStateStorage().print("rotational_coordinte_limit_force_states.sto");
// Save the forces
reporter->getForceStorage().print("rotational_limit_forces.mot");
}
