예제 #1
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);
}
예제 #2
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);
}
예제 #3
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()