예제 #1
0
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);
}
예제 #2
0
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;
}
예제 #4
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;
}
예제 #5
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;
}
예제 #6
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()
예제 #7
0
/*==============================================================================  
    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;
    }


}
예제 #8
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;
}