void addExtensionController(Model& model) { PrescribedController* controller = new PrescribedController(); controller->setName( "extension_controller"); controller->setActuators( model.updActuators()); double control_time[2] = {0.01, 0.02}; // time nodes for linear function double control_acts[2] = {0.0, 1.0}; // force values at t1 and t2 //control_func->setName( "constant_control_func"); string muscle_name; for (int i=0; i<model.getActuators().getSize(); i++) { muscle_name = model.getActuators().get(i).getName(); // activate quadriceps if (muscle_name == "rect_fem_r" || muscle_name == "vas_med_r" || muscle_name == "vas_int_r" || muscle_name == "vas_lat_r" ) { Constant* ccf = new Constant(0.8); //PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts); controller->prescribeControlForActuator( i, ccf); } else { Constant* zccf = new Constant(0); controller->prescribeControlForActuator( i, zccf); } } model.addController( controller); }
void addFlexionController(Model& model) { PrescribedController* controller = new PrescribedController(); controller->setName( "flexion_controller"); controller->setActuators( model.updActuators()); double control_time[2] = {0, 0.05}; // time nodes for linear function double control_acts[2] = {1.0, 0}; // force values at t1 and t2 string muscle_name; for (int i=0; i<model.getActuators().getSize(); i++) { muscle_name = model.getActuators().get(i).getName(); // hamstrings: bi*, semi* if ( muscle_name == "bifemlh_r" || muscle_name == "bifemsh_r" || muscle_name == "grac_r" \ || muscle_name == "lat_gas_r" || muscle_name == "med_gas_r" || muscle_name == "sar_r" \ || muscle_name == "semimem_r" || muscle_name == "semiten_r") { Constant* ccf = new Constant(1.0); //PiecewiseLinearFunction *ccf = new PiecewiseLinearFunction( 2, control_time, control_acts); controller->prescribeControlForActuator( i, ccf); } else { Constant* zccf = new Constant(0); controller->prescribeControlForActuator( i, zccf); } } model.addController( controller); }
/** * Run a simulation of block sliding with contact on by two muscles sliding with contact */ int main() { try { // Create a new OpenSim model Model osimModel; osimModel.setName("osimModel"); double Pi = SimTK::Pi; // Get the ground body OpenSim::Body& ground = osimModel.getGroundBody(); ground.addDisplayGeometry("checkered_floor.vtp"); // create linkage body double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06; Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter); Vec3 linkageMassCenter(0,linkageLength/2,0); Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0); OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia); // Graphical representation linkage1->addDisplayGeometry("cylinder.vtp"); //This cylinder.vtp geometry is 1 meter tall, 1 meter diameter. Scale and shift it to look pretty GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet(); DisplayGeometry& thinCylinder = geometry[0]; thinCylinder.setScaleFactors(linkageDimensions); thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0))); linkage1->addDisplayGeometry("sphere.vtp"); //This sphere.vtp is 1 meter in diameter. Scale it. geometry[1].setScaleFactors(Vec3(0.1)); // Creat a second linkage body OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1); linkage2->setName("linkage2"); // Creat a block to be the pelvis double blockMass = 20.0, blockSideLength = 0.2; Vec3 blockMassCenter(0); Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength); OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia); block->addDisplayGeometry("block.vtp"); //This block.vtp is 0.1x0.1x0.1 meters. scale its appearance block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0)); // Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0); PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1, locationInChild, orientationInChild); PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2, locationInChild, orientationInChild); PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block, locationInChild, orientationInChild); double range[2] = {-SimTK::Pi*2, SimTK::Pi*2}; CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet(); ankleCoordinateSet[0].setName("q1"); ankleCoordinateSet[0].setRange(range); CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet(); kneeCoordinateSet[0].setName("q2"); kneeCoordinateSet[0].setRange(range); CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet(); hipCoordinateSet[0].setName("q3"); hipCoordinateSet[0].setRange(range); // Add the bodies to the model osimModel.addBody(linkage1); osimModel.addBody(linkage2); osimModel.addBody(block); // Define contraints on the model // Add a point on line constraint to limit the block to vertical motion Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0); PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock); osimModel.addConstraint(lineConstraint); // Add PistonActuator between the first linkage and the block Vec3 pointOnBodies(0); PistonActuator *piston = new PistonActuator(); piston->setName("piston"); piston->setBodyA(linkage1); piston->setBodyB(block); piston->setPointA(pointOnBodies); piston->setPointB(pointOnBodies); piston->setOptimalForce(200.0); piston->setPointsAreGlobal(false); osimModel.addForce(piston); //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ // Added ControllableSpring between the first linkage and the second block //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ ControllableSpring *spring = new ControllableSpring; spring->setName("spring"); spring->setBodyA(block); spring->setBodyB(linkage1); spring->setPointA(pointOnBodies); spring->setPointB(pointOnBodies); spring->setOptimalForce(2000.0); spring->setPointsAreGlobal(false); spring->setRestLength(0.8); osimModel.addForce(spring); // define the simulation times double t0(0.0), tf(15); // create a controller to control the piston and spring actuators // the prescribed controller sets the controls as functions of time PrescribedController *legController = new PrescribedController(); // give the legController control over all (two) model actuators legController->setActuators(osimModel.updActuators()); // specify some control nodes for spring stiffness control double t[] = {0.0, 4.0, 7.0, 10.0, 15.0}; double x[] = {1.0, 1.0, 0.25, 0.25, 5.0}; // specify the control function for each actuator legController->prescribeControlForActuator("piston", new Constant(0.1)); legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x)); // add the controller to the model osimModel.addController(legController); // define the acceration due to gravity osimModel.setGravity(Vec3(0, -9.80665, 0)); // enable the model visualizer see the model in action, which can be // useful for debugging osimModel.setUseVisualizer(true); // Initialize system SimTK::State& si = osimModel.initSystem(); // Pin joint initial states double q1_i = -Pi/4; double q2_i = - 2*q1_i; CoordinateSet &coordinates = osimModel.updCoordinateSet(); coordinates[0].setValue(si, q1_i, true); coordinates[1].setValue(si,q2_i, true); // Setup integrator and manager SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem()); integrator.setAccuracy(1.0e-3); ForceReporter *forces = new ForceReporter(&osimModel); osimModel.updAnalysisSet().adoptAndAppend(forces); Manager manager(osimModel, integrator); //Examine the model osimModel.printDetailedInfo(si, std::cout); // Save the model osimModel.print("toyLeg.osim"); // Print out the initial position and velocity states si.getQ().dump("Initial q's"); si.getU().dump("Initial u's"); std::cout << "Initial time: " << si.getTime() << std::endl; // Integrate manager.setInitialTime(t0); manager.setFinalTime(tf); std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl; manager.integrate(si); // Save results osimModel.printControlStorage("SpringActuatedLeg_controls.sto"); Storage statesDegrees(manager.getStateStorage()); osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees); //statesDegrees.print("PistonActuatedLeg_states_degrees.mot"); statesDegrees.print("SpringActuatedLeg_states_degrees.mot"); forces->getForceStorage().print("actuator_forces.mot"); } catch (const std::exception& ex) { std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl; return 1; } std::cout << "Exiting" << std::endl; return 0; }
/** * 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; }
/** * Create a model that does nothing. */ int main() { try { /////////////////////////////////////////// // DEFINE BODIES AND JOINTS OF THE MODEL // /////////////////////////////////////////// // Create an OpenSim model and set its name Model osimModel; osimModel.setName("tugOfWar"); // GROUND BODY // Get a reference to the model's ground body Ground& ground = osimModel.updGround(); // Add display geometry to the ground to visualize in the GUI ground.addMeshGeometry("ground.vtp"); ground.addMeshGeometry("anchor1.vtp"); ground.addMeshGeometry("anchor2.vtp"); // BLOCK BODY // Specify properties of a 20 kg, 0.1 m^3 block body double blockMass = 20.0, blockSideLength = 0.1; Vec3 blockMassCenter(0); Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength); // Create a new block body with the specified properties OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia); // Add display geometry to the block to visualize in the GUI Brick brick(SimTK::Vec3(0.05, 0.05, 0.05)); block->addGeometry(brick); // FREE JOINT // Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0); FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody); // Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet(); // Set the angle and position ranges for the coordinate set double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2}; double positionRange[2] = {-1, 1}; jointCoordinateSet[0].setRange(angleRange); jointCoordinateSet[1].setRange(angleRange); jointCoordinateSet[2].setRange(angleRange); jointCoordinateSet[3].setRange(positionRange); jointCoordinateSet[4].setRange(positionRange); jointCoordinateSet[5].setRange(positionRange); // Add the block body to the model osimModel.addBody(block); osimModel.addJoint(blockToGround); /////////////////////////////////////////////// // DEFINE THE SIMULATION START AND END TIMES // /////////////////////////////////////////////// // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 3.00; ///////////////////////////////////////////// // DEFINE CONSTRAINTS IMPOSED ON THE MODEL // ///////////////////////////////////////////// // Specify properties of a constant distance constraint to limit the block's motion double distance = 0.2; Vec3 pointOnGround(0, blockSideLength/2 ,0); Vec3 pointOnBlock(0, 0, 0); // Create a new constant distance constraint ConstantDistanceConstraint *constDist = new ConstantDistanceConstraint(ground, pointOnGround, *block, pointOnBlock, distance); // Add the new point on a line constraint to the model osimModel.addConstraint(constDist); /////////////////////////////////////// // DEFINE FORCES ACTING ON THE MODEL // /////////////////////////////////////// // GRAVITY // Obtaine the default acceleration due to gravity Vec3 gravity = osimModel.getGravity(); // MUSCLE FORCES // Create two new muscles with identical properties double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0; Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle); Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle); // Specify the paths for the two muscles // Path for muscle 1 muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35)); muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05)); // Path for muscle 2 muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35)); muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05)); // Add the two muscles (as forces) to the model osimModel.addForce(muscle1); osimModel.addForce(muscle2); // PRESCRIBED FORCE // Create a new prescribed force to be applied to the block PrescribedForce *prescribedForce = new PrescribedForce(block); prescribedForce->setName("prescribedForce"); // Specify properties of the force function to be applied to the block double time[2] = {0, finalTime}; // time nodes for linear function double fXofT[2] = {0, -blockMass*gravity[1]*3.0}; // force values at t1 and t2 // Create linear function for the force components PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT); // Set the force and point functions for the new prescribed force prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0)); prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0)); // Add the new prescribed force to the model osimModel.addForce(prescribedForce); /////////////////////////////////// // DEFINE CONTROLS FOR THE MODEL // /////////////////////////////////// // Create a prescribed controller that simply applies controls as function of time // For muscles, controls are normalized motor-neuron excitations PrescribedController *muscleController = new PrescribedController(); muscleController->setActuators(osimModel.updActuators()); // Define linear functions for the control values for the two muscles Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles Array<double> slopeAndIntercept2(0.0, 2); // muscle1 control has slope of -1 starting 1 at t = 0 slopeAndIntercept1[0] = -1.0/(finalTime-initialTime); slopeAndIntercept1[1] = 1.0; // muscle2 control has slope of 0.95 starting 0.05 at t = 0 slopeAndIntercept2[0] = 0.95/(finalTime-initialTime); slopeAndIntercept2[1] = 0.05; // Set the indiviudal muscle control functions for the prescribed muscle controller muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1)); muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2)); // Add the muscle controller to the model osimModel.addController(muscleController); /////////////////////////////////// // SPECIFY MODEL DEFAULT STATES // /////////////////////////////////// // Define the default states for the two muscles // Activation muscle1->setDefaultActivation(slopeAndIntercept1[1]); muscle2->setDefaultActivation(slopeAndIntercept2[1]); // Fiber length muscle2->setDefaultFiberLength(optimalFiberLength); muscle1->setDefaultFiberLength(optimalFiberLength); // Save the model to a file osimModel.print("tugOfWar_model.osim"); ////////////////////////// // PERFORM A SIMULATION // ///////////////////////// //osimModel.setUseVisualizer(true); // Initialize the system and get the default state SimTK::State& si = osimModel.initSystem(); // Define non-zero (defaults are 0) states for the free joint CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet(); modelCoordinateSet[3].setValue(si, distance); // set x-translation value modelCoordinateSet[5].setValue(si, 0.0); // set z-translation value modelCoordinateSet[3].setSpeedValue(si, 0.0); // set x-speed value double h_start = 0.5; modelCoordinateSet[4].setValue(si, h_start); // set y-translation which is height std::cout << "Start height = "<< h_start << std::endl; osimModel.getMultibodySystem().realize(si, Stage::Velocity); // Compute initial conditions for muscles osimModel.equilibrateMuscles(si); double mfv1 = muscle1->getFiberVelocity(si); double mfv2 = muscle2->getFiberVelocity(si); // Create the force reporter for obtaining the forces applied to the model // during a forward simulation ForceReporter* reporter = new ForceReporter(&osimModel); osimModel.addAnalysis(reporter); // Create the integrator for integrating system dynamics SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem()); integrator.setAccuracy(1.0e-6); // Create the manager managing the forward integration and its outputs Manager manager(osimModel, integrator); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl; manager.integrate(si); } catch (const std::exception& ex) { std::cerr << ex.what() << std::endl; return 1; } catch (...) { std::cerr << "UNRECOGNIZED EXCEPTION" << std::endl; return 1; } std::cout << "OpenSim environment test completed successfully. You should see a block attached to two muscles visualized in a separate window." << std::endl; return 0; }
//========================================================================================================== void testPrescribedControllerOnBlock(bool disabled) { using namespace SimTK; // Create a new OpenSim model Model osimModel; osimModel.setName("osimModel"); // Get the ground body OpenSim::Body& ground = osimModel.getGroundBody(); // Create a 20 kg, 0.1 m^3 block body double blockMass = 20.0, blockSideLength = 0.1; Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0); Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength); OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia); //Create a free joint with 6 degrees-of-freedom SimTK::Vec3 noRotation(0); SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation); // Create 6 coordinates (degrees-of-freedom) between the ground and block CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet(); double posRange[2] = {-1, 1}; jointCoordinateSet[0].setName("xTranslation"); jointCoordinateSet[0].setMotionType(Coordinate::Translational); jointCoordinateSet[0].setRange(posRange); // Add the block body to the model osimModel.addBody(&block); // Define a single coordinate actuator. CoordinateActuator actuator(jointCoordinateSet[0].getName()); actuator.setName("actuator"); // Add the actuator to the model osimModel.addForce(&actuator); double initialTime = 0; double finalTime = 1.0; // Define the initial and final control values double controlForce = 100; // Create a prescribed controller that simply applies a function of the force PrescribedController actuatorController; actuatorController.setName("testPrescribedController"); actuatorController.setActuators(osimModel.updActuators()); actuatorController.prescribeControlForActuator(0, new Constant(controlForce)); actuatorController.setDisabled(disabled); // add the controller to the model osimModel.addController(&actuatorController); osimModel.print("blockWithPrescribedController.osim"); Model modelfileFromFile("blockWithPrescribedController.osim"); // Verify that serialization and then deserialization of the disable flag is correct ASSERT(modelfileFromFile.getControllerSet().get("testPrescribedController").isDisabled() == disabled); // Initialize the system and get the state representing the state system SimTK::State& si = osimModel.initSystem(); // Specify zero slider joint kinematic states CoordinateSet &coordinates = osimModel.updCoordinateSet(); coordinates[0].setValue(si, 0.0); // x translation coordinates[0].setSpeedValue(si, 0.0); // x speed // Create the integrator and manager for the simulation. double accuracy = 1.0e-3; SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem()); integrator.setAccuracy(accuracy); Manager manager(osimModel, integrator); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl; manager.integrate(si); si.getQ().dump("Final position:"); double expected = disabled ? 0 : 0.5*(controlForce/blockMass)*finalTime*finalTime; ASSERT_EQUAL(expected, coordinates[0].getValue(si), accuracy, __FILE__, __LINE__, "PrescribedController failed to produce the expected motion of block."); // Save the simulation results Storage states(manager.getStateStorage()); states.print("block_push.sto"); osimModel.disownAllComponents(); }// end of testPrescribedControllerOnBlock()
/*============================================================================== Main test driver to be used on any muscle model (derived from Muscle) so new cases should be easy to add currently, the test only verifies that the work done by the muscle corresponds to the change in system energy. TODO: Test will fail wih prescribe motion until the work done by this constraint is accounted for. ================================================================================ */ void simulateMuscle( const Muscle &aMuscModel, double startX, double act0, const Function *motion, // prescribe motion of free end of muscle const Function *control, // prescribed excitation signal to the muscle double integrationAccuracy, int testType, double testTolerance, bool printResults) { string prescribed = (motion == NULL) ? "." : " with Prescribed Motion."; cout << "\n******************************************************" << endl; cout << "Test " << aMuscModel.getConcreteClassName() << " Model" << prescribed << endl; cout << "******************************************************" << endl; using SimTK::Vec3; //========================================================================== // 0. SIMULATION SETUP: Create the block and ground //========================================================================== // Define the initial and final simulation times double initialTime = 0.0; double finalTime = 4.0; //Physical properties of the model double ballMass = 10; double ballRadius = 0.05; double anchorWidth = 0.1; // Create an OpenSim model Model model; double optimalFiberLength = aMuscModel.getOptimalFiberLength(); double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength(); double tendonSlackLength = aMuscModel.getTendonSlackLength(); // Use a copy of the muscle model passed in to add path points later PathActuator *aMuscle = aMuscModel.clone(); // Get a reference to the model's ground body Body& ground = model.getGroundBody(); ground.addDisplayGeometry("box.vtp"); ground.updDisplayer() ->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth)); OpenSim::Body * ball = new OpenSim::Body("ball", ballMass , Vec3(0), ballMass*SimTK::Inertia::sphere(ballRadius)); ball->addDisplayGeometry("sphere.vtp"); ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius)); // ball connected to ground via a slider along X double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength; SliderJoint* slider = new SliderJoint( "slider", ground, Vec3(anchorWidth/2+xSinG, 0, 0), Vec3(0), *ball, Vec3(0), Vec3(0)); CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet(); jointCoordinateSet[0].setName("tx"); jointCoordinateSet[0].setDefaultValue(1.0); jointCoordinateSet[0].setRangeMin(0); jointCoordinateSet[0].setRangeMax(1.0); if(motion != NULL){ jointCoordinateSet[0].setPrescribedFunction(*motion); jointCoordinateSet[0].setDefaultIsPrescribed(true); } // add ball to model model.addBody(ball); model.addJoint(slider); //========================================================================== // 1. SIMULATION SETUP: Add the muscle //========================================================================== //Attach the muscle const string &actuatorType = aMuscle->getConcreteClassName(); aMuscle->setName("muscle"); aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0)); aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0)); ActivationFiberLengthMuscle_Deprecated *aflMuscle = dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle); if(aflMuscle){ // Define the default states for the muscle that has //activation and fiber-length states aflMuscle->setDefaultActivation(act0); aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength()); }else{ ActivationFiberLengthMuscle *aflMuscle2 = dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle); if(aflMuscle2){ // Define the default states for the muscle //that has activation and fiber-length states aflMuscle2->setDefaultActivation(act0); aflMuscle2->setDefaultFiberLength(aflMuscle2 ->getOptimalFiberLength()); } } model.addForce(aMuscle); // Create a prescribed controller that simply //applies controls as function of time PrescribedController * muscleController = new PrescribedController(); if(control != NULL){ muscleController->setActuators(model.updActuators()); // Set the indiviudal muscle control functions //for the prescribed muscle controller muscleController->prescribeControlForActuator("muscle",control->clone()); // Add the control set controller to the model model.addController(muscleController); } // Set names for muscles / joints. Array<string> muscNames; muscNames.append(aMuscle->getName()); Array<string> jointNames; jointNames.append("slider"); //========================================================================== // 2. SIMULATION SETUP: Instrument the test with probes //========================================================================== Array<string> muscNamesTwice = muscNames; muscNamesTwice.append(muscNames.get(0)); cout << "------------\nPROBES\n------------" << endl; int probeCounter = 1; // Add ActuatorPowerProbe to measure work done by the muscle ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1); //muscWorkProbe->setName("ActuatorWork"); muscWorkProbe->setOperation("integrate"); SimTK::Vector ic1(1); ic1 = 9.0; // some arbitary initial condition. muscWorkProbe->setInitialConditions(ic1); model.addProbe(muscWorkProbe); model.setup(); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl; if (muscWorkProbe->getName() != "UnnamedProbe") { string errorMessage = "Incorrect default name for unnamed probe: " + muscWorkProbe->getName(); throw (OpenSim::Exception(errorMessage.c_str())); } // Add ActuatorPowerProbe to measure power generated by the muscle ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor muscPowerProbe->setName("ActuatorPower"); muscPowerProbe->setOperation("value"); model.addProbe(muscPowerProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl; // Add ActuatorPowerProbe to report the muscle power MINIMUM ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinimum->setName("ActuatorPowerMinimum"); powerProbeMinimum->setOperation("minimum"); model.addProbe(powerProbeMinimum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMinAbs->setName("ActuatorPowerMinAbs"); powerProbeMinAbs->setOperation("minabs"); model.addProbe(powerProbeMinAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl; // Add ActuatorPowerProbe to report the muscle power MAXIMUM ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaximum->setName("ActuatorPowerMaximum"); powerProbeMaximum->setOperation("maximum"); model.addProbe(powerProbeMaximum); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl; // Add ActuatorPowerProbe to report the muscle power MAXABS ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor powerProbeMaxAbs->setName("ActuatorPowerMaxAbs"); powerProbeMaxAbs->setOperation("maxabs"); model.addProbe(powerProbeMaxAbs); cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl; // Add ActuatorPowerProbe to measure the square of the power generated by the muscle ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor muscPowerSquaredProbe->setName("ActuatorPowerSquared"); muscPowerSquaredProbe->setExponent(2.0); model.addProbe(muscPowerSquaredProbe); cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl; // Add JointInternalPowerProbe to measure work done by the joint JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1); jointWorkProbe->setName("JointWork"); jointWorkProbe->setOperation("integrate"); jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(jointWorkProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl; // Add JointPowerProbe to measure power generated by the joint JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor jointPowerProbe->setName("JointPower"); jointPowerProbe->setOperation("value"); model.addProbe(jointPowerProbe); cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl; // Add ActuatorForceProbe to measure the impulse of the muscle force ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1); impulseProbe->setName("ActuatorImpulse"); impulseProbe->setOperation("integrate"); impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0)); model.addProbe(impulseProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl; // Add ActuatorForceProbe to report the muscle force ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbe->setName("ActuatorForce"); forceProbe->setOperation("value"); model.addProbe(forceProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl; // Add ActuatorForceProbe to report the square of the muscle force ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbe->setName("ActuatorForceSquared"); forceSquaredProbe->setExponent(2.0); model.addProbe(forceSquaredProbe); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice"); forceSquaredProbeTwice->setSumForcesTogether(true); forceSquaredProbeTwice->setActuatorNames(muscNamesTwice); model.addProbe(forceSquaredProbeTwice); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl; // Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5 ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved"); double gain1 = 0.5; forceSquaredProbeTwiceScaled->setGain(gain1); model.addProbe(forceSquaredProbeTwiceScaled); cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl; // Add ActuatorForceProbe to report -3.5X the muscle force double gain2 = -3.50; ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeScale->setName("ScaleActuatorForce"); forceProbeScale->setOperation("value"); forceProbeScale->setGain(gain2); model.addProbe(forceProbeScale); cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl; // Add ActuatorForceProbe to report the differentiated muscle force ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor forceProbeDiff->setName("DifferentiateActuatorForce"); forceProbeDiff->setOperation("differentiate"); model.addProbe(forceProbeDiff); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl; // Add SystemEnergyProbe to measure the system KE+PE SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true); sysEnergyProbe->setName("SystemEnergy"); sysEnergyProbe->setOperation("value"); sysEnergyProbe->setComputeKineticEnergy(true); sysEnergyProbe->setComputePotentialEnergy(true); model.addProbe(sysEnergyProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl; // Add SystemEnergyProbe to measure system power (d/dt system KE+PE) SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor sysPowerProbe->setName("SystemPower"); sysPowerProbe->setDisabled(false); sysPowerProbe->setOperation("differentiate"); model.addProbe(sysPowerProbe); cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl; // Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR"); forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice); //cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl; forceSquaredProbeTwiceReportedIndividually1->setOperation("value"); model.addProbe(forceSquaredProbeTwiceReportedIndividually1); cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate"); model.addProbe(forceSquaredProbeTwiceReportedIndividually2); cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; // Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR"); forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate"); SimTK::Vector initCondVec(2); initCondVec(0) = 0; initCondVec(1) = 10; forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec); model.addProbe(forceSquaredProbeTwiceReportedIndividually3); cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl; cout << "initCondVec = " << initCondVec << endl; /* Since all components are allocated on the stack don't have model own them (and try to free)*/ // model.disownAllComponents(); model.setName("testProbesModel"); cout << "Saving model... " << endl; model.print("testProbesModel.osim"); cout << "Re-loading model... " << endl; Model reloadedModel = Model("testProbesModel.osim"); /* Setup analyses and reporters. */ ProbeReporter* probeReporter = new ProbeReporter(&model); model.addAnalysis(probeReporter); ForceReporter* forceReporter = new ForceReporter(&model); model.addAnalysis(forceReporter); MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model); model.addAnalysis(muscleReporter); model.print("testProbesModel.osim"); model.printBasicInfo(cout); //========================================================================== // 3. SIMULATION Initialization //========================================================================== // Initialize the system and get the default state SimTK::State& si = model.initSystem(); SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si); model.getMultibodySystem().realize(si,SimTK::Stage::Dynamics); model.equilibrateMuscles(si); CoordinateSet& modelCoordinateSet = model.updCoordinateSet(); // Define non-zero (defaults are 0) states for the free joint // set x-translation value modelCoordinateSet[0].setValue(si, startX, true); //Copy the initial state SimTK::State initialState(si); // Check muscle is setup correctly const PathActuator &muscle = dynamic_cast<const PathActuator&>(model.updActuators().get("muscle")); double length = muscle.getLength(si); double trueLength = startX + xSinG - anchorWidth/2; ASSERT_EQUAL(length/trueLength, 1.0, testTolerance, __FILE__, __LINE__, "testMuscles: path failed to initialize to correct length." ); model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0); //cout << "Muscle initial energy = " << Emuscle0 << endl; double Esys0 = model.getMultibodySystem().calcEnergy(si); Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0)); double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si); //cout << "Total initial system energy = " << Esys0 << endl; //========================================================================== // 4. SIMULATION Integration //========================================================================== // Create the integrator SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); integrator.setAccuracy(integrationAccuracy); // Create the manager Manager manager(model, integrator); // Integrate from initial time to final time manager.setInitialTime(initialTime); manager.setFinalTime(finalTime); cout<<"\nIntegrating from " << initialTime<< " to " << finalTime << endl; // Start timing the simulation const clock_t start = clock(); // simulate manager.integrate(si); // how long did it take? double comp_time = (double)(clock()-start)/CLOCKS_PER_SEC; //========================================================================== // 5. SIMULATION Reporting //========================================================================== double realTimeMultiplier = ((finalTime-initialTime)/comp_time); printf("testMuscles: Realtime Multiplier: %f\n" " : simulation duration / clock duration\n" " > 1 : faster than real time\n" " = 1 : real time\n" " < 1 : slower than real time\n", realTimeMultiplier ); /* ASSERT(comp_time <= (finalTime-initialTime)); printf("testMuscles: PASSED Realtime test\n" " %s simulation time: %f with accuracy %f\n\n", actuatorType.c_str(), comp_time , accuracy); */ //An analysis only writes to a dir that exists, so create here. if(printResults == true){ Storage states(manager.getStateStorage()); states.print("testProbes_states.sto"); probeReporter->getProbeStorage().print("testProbes_probes.sto"); forceReporter->getForceStorage().print("testProbes_forces.sto"); muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto"); cout << "\nDone with printing results..." << endl; } double muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; // Test the resetting of probes cout << "Resetting muscle work probe..." << endl; muscWorkProbe->reset(si); muscleWork = muscWorkProbe->getProbeOutputs(si)(0); cout << "Muscle work = " << muscleWork << endl; ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe."); //========================================================================== // 6. SIMULATION Tests //========================================================================== model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration); ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe."); ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe."); ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2*pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe."); for (int i=0; i<initCondVec.size(); ++i) { stringstream myError; //myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl; //ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str()); //if (testRealInitConditions(i) != initCondVec(i)) // cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl; } }
/** * 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; }