Esempio n. 1
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;
}
Esempio n. 2
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;
}