/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared 
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
	using namespace SimTK;
	// start timing
	std::clock_t startTime = std::clock();

	// Setup OpenSim model
	Model *model = new Model;

	// turn off gravity
	model->setGravity(Vec3(0));

	//OpenSim body 1: Ground
	OpenSim::Body& ground = model->getGroundBody();

	// OpenSim body 2: A Block
	// Geometrical/Inertial properties for the block
	double blockMass = 1.0, blockSideLength = 1;
	Vec3 blockMassCenter(0);
	Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
		blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick 

	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");

	Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0), 
		locationInBody(0), orientationInBody(0);
	FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall", 
		ground, locationInParent, orientationInParent, 
		*block, locationInBody, orientationInBody);
	
	model->addBody(block);
	model->addJoint(blockToGroundFree);
	
	// specify magnitude and direction of applied force and torque
	double forceMag = 1.0;
	Vec3 forceAxis(1, 1, 1);
	Vec3 forceInG = forceMag * forceAxis;

	double torqueMag = 1.0;
	Vec3 torqueAxis(1, 1, 1);
	Vec3 torqueInG = torqueMag*torqueAxis;

	// ---------------------------------------------------------------------------
	// Use MobilityForces to Apply the given Torques and Forces to the body
	// ---------------------------------------------------------------------------
	State& state = model->initSystem();

	model->getMultibodySystem().realize(state, Stage::Dynamics);
	Vector_<SpatialVec>& bodyForces =
		model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
	bodyForces.dump("Body Forces before applying 6D spatial force:");

	model->getMatterSubsystem().addInBodyTorque(state, block->getIndex(),
		torqueInG, bodyForces);
	model->getMatterSubsystem().addInStationForce(state, block->getIndex(),
		Vec3(0), forceInG, bodyForces);

	bodyForces.dump("Body Forces after applying 6D spatial force to the block");

	model->getMultibodySystem().realize(state, Stage::Acceleration);
	Vector udotBody = state.getUDot();
	udotBody.dump("Accelerations due to body forces");

	// clear body forces
	bodyForces *= 0;

	// update mobility forces
	Vector& mobilityForces = model->getMultibodySystem()
		.updMobilityForces(state, Stage::Dynamics);

	// Apply torques as mobility forces of the ball joint
	for (int i = 0; i<3; ++i){
		mobilityForces[i] = torqueInG[i];
		mobilityForces[i+3] = forceInG[i];
	}
	mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");


	model->getMultibodySystem().realize(state, Stage::Acceleration);
	Vector udotMobility = state.getUDot();
	udotMobility.dump("Accelerations due to mobility forces");

	// First make sure that accelerations are not zero accidentally
	ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
	// Then check if they are equal
	for (int i = 0; i<udotMobility.size(); ++i){
		ASSERT_EQUAL(udotMobility[i], udotBody[i], SimTK::Eps);
	}

	// clear the mobility forces
	mobilityForces = 0;

	// ---------------------------------------------------------------------------
	// Use a BodyActuator to Apply the same given Torques and Forces to the body
	// ---------------------------------------------------------------------------

	// Create and add the body actuator to the model
	BodyActuator* actuator = new BodyActuator(*block);
	actuator->setName("BodyAct");
	model->addForce(actuator);

	model->print("TestBodyActuatorModel.osim");
	model->setUseVisualizer(false);

	// get a new system and state to reflect additions to the model
	State& state1 = model->initSystem();

	// -------------- Provide control signals for bodyActuator ----------
	// Get the default control vector of the model
	Vector modelControls = model->getDefaultControls();
	
	// Spedicfy a vector of control signals to include desired torques and forces
	Vector fixedControls(6);
	for (int i = 0; i < 3; i++){
		fixedControls(i) = torqueInG(i);
		fixedControls(i + 3) = forceInG(i);
	}
	fixedControls.dump("Spatial forces applied by body Actuator:");

	// Add control values and set their values
	actuator->addInControls(fixedControls, modelControls);
	model->setDefaultControls(modelControls);

	// ------------------- Compute Acc and Compare -------------
	// Compute the acc due to spatial forces applied by body actuator
	model->computeStateVariableDerivatives(state1);

	Vector udotBodyActuator = state1.getUDot();
	udotBodyActuator.dump("Accelerations due to body actuator");

	// First make sure that accelerations are not zero accidentally
	ASSERT(udotMobility.norm() != 0.0 || udotBodyActuator.norm() != 0.0);
	// Then verify that the BodyActuator also generates the same acceleration
	// as the equivalent applied mobility force
	for (int i = 0; i<udotBodyActuator.size(); ++i){
		ASSERT_EQUAL(udotMobility[i], udotBodyActuator[i], SimTK::Eps);
	}

	// -------------- Setup integrator and manager -------------------
	RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
	integrator.setAccuracy(integ_accuracy);
	Manager manager(*model, integrator);

	manager.setInitialTime(0.0);
	double final_t = 1.00;
	manager.setFinalTime(final_t);
	manager.integrate(state1);

	// ----------------- Test Copying the model -------------------
	// Before exiting lets see if copying the actuator works
	BodyActuator* copyOfActuator = actuator->clone();
	ASSERT(*copyOfActuator == *actuator);

	// Check that de/serialization works
	Model modelFromFile("TestBodyActuatorModel.osim");
	ASSERT(modelFromFile == *model, __FILE__, __LINE__,
		"Model from file FAILED to match model in memory.");

	std::cout << " ********** Test BodyActuator time = ********** " <<
		1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#2
0
void Force::applyTorque(const SimTK::State &s, const OpenSim::Body &aBody,
                        const Vec3& aTorque, Vector_<SpatialVec> &bodyForces) const
{
    _model->getMatterSubsystem().addInBodyTorque(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
            aTorque, bodyForces);
}
//==============================================================================
// Test Cases
//==============================================================================
void testTorqueActuator()
{
	using namespace SimTK;
	// start timing
	std::clock_t startTime = std::clock();

	// Setup OpenSim model
	Model *model = new Model;

	// turn off gravity
	model->setGravity(Vec3(0));

	//OpenSim bodies
    OpenSim::Body& ground = model->getGroundBody();

	//Cylindrical bodies
	double r = 0.25, h = 1.0;
	double m1 = 1.0, m2 = 2.0;
	Inertia j1 = m1*Inertia::cylinderAlongY(r, h);
	Inertia j2 = m2*Inertia::cylinderAlongY(r, h);

	//OpenSim bodies
	OpenSim::Body* bodyA 
		= new OpenSim::Body("A", m1, Vec3(0), j1);
	
	OpenSim::Body* bodyB 
		= new OpenSim::Body("B", m2, Vec3(0), j2);

	// connect bodyA to ground with 6dofs
	FreeJoint* base = 
		new FreeJoint("base", ground, Vec3(0), Vec3(0), *bodyA, Vec3(0), Vec3(0));

	model->addBody(bodyA);
	model->addJoint(base);

	// connect bodyA to bodyB by a Ball joint
	BallJoint* bInA = new BallJoint("bInA", *bodyA, Vec3(0,-h/2, 0), Vec3(0), 
		                   *bodyB, Vec3(0, h/2, 0), Vec3(0));

	model->addBody(bodyB);
	model->addJoint(bInA);

	// specify magnitude and direction of applied torque
	double torqueMag = 2.1234567890;
	Vec3 torqueAxis(1/sqrt(2.0), 0, 1/sqrt(2.0));
	Vec3 torqueInG = torqueMag*torqueAxis;

	State state = model->initSystem();

	model->getMultibodySystem().realize(state, Stage::Dynamics);
	Vector_<SpatialVec>& bodyForces = 
		model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
	bodyForces.dump("Body Forces before applying torque");
	model->getMatterSubsystem().addInBodyTorque(state, bodyA->getIndex(), 
		torqueMag*torqueAxis, bodyForces);
	model->getMatterSubsystem().addInBodyTorque(state, bodyB->getIndex(), 
		-torqueMag*torqueAxis, bodyForces);
	bodyForces.dump("Body Forces after applying torque to bodyA and bodyB");

	model->getMultibodySystem().realize(state, Stage::Acceleration);
	const Vector& udotBody = state.getUDot();
	udotBody.dump("Accelerations due to body forces");

	// clear body forces
	bodyForces *= 0;

	// update mobility forces
	Vector& mobilityForces = model->getMultibodySystem()
		.updMobilityForces(state, Stage::Dynamics);

	// Apply torques as mobility forces of the ball joint
	for(int i=0; i<3; ++i){
		mobilityForces[6+i] = torqueInG[i];	
	}

	model->getMultibodySystem().realize(state, Stage::Acceleration);
	const Vector& udotMobility = state.getUDot();
	udotMobility.dump("Accelerations due to mobility forces");

	// First make sure that accelerations are not zero accidentally
	ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
	// Then check if they are equal
	for(int i=0; i<udotMobility.size(); ++i){
		ASSERT_EQUAL(udotMobility[i], udotBody[i], 1.0e-12);
	}

	// clear the mobility forces
	mobilityForces = 0;

	//Now add the actuator to the model and control it to generate the same
	//torque as applied directly to the multibody system (above)

	// Create and add the torque actuator to the model
	TorqueActuator* actuator =
		new TorqueActuator(*bodyA, *bodyB, torqueAxis, true);
	actuator->setName("torque");
	model->addForce(actuator);

	// Create and add a controller to control the actuator
	PrescribedController* controller = 	new PrescribedController();
	controller->addActuator(*actuator);
	// Apply torque about torqueAxis
	controller->prescribeControlForActuator("torque", new Constant(torqueMag));

	model->addController(controller);

	/*
	ActuatorPowerProbe* powerProbe = new ActuatorPowerProbe(Array<string>("torque",1),false, 1); 
	powerProbe->setOperation("integrate");
	powerProbe->setInitialConditions(Vector(1, 0.0));
	*/

	//model->addProbe(powerProbe);

	model->print("TestTorqueActuatorModel.osim");
	model->setUseVisualizer(false);

	// get a new system and state to reflect additions to the model
	state = model->initSystem();

	model->computeStateVariableDerivatives(state);

	const Vector &udotTorqueActuator = state.getUDot();

	// First make sure that accelerations are not zero accidentally
	ASSERT(udotMobility.norm() != 0.0 || udotTorqueActuator.norm() != 0.0);

	// Then verify that the TorqueActuator also generates the same acceleration
	// as the equivalent applied mobility force
	for(int i=0; i<udotMobility.size(); ++i){
		ASSERT_EQUAL(udotMobility[i], udotTorqueActuator[i], 1.0e-12);
	}

	// determine the initial kinetic energy of the system
	double iKE = model->getMatterSubsystem().calcKineticEnergy(state);

	RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
	integrator.setAccuracy(integ_accuracy);
    Manager manager(*model,  integrator);

	manager.setInitialTime(0.0);

	double final_t = 1.00;

	manager.setFinalTime(final_t);
	manager.integrate(state);

	model->computeStateVariableDerivatives(state);

	double fKE = model->getMatterSubsystem().calcKineticEnergy(state);

	// Change in system kinetic energy can only be attributable to actuator work
	//double actuatorWork = (powerProbe->getProbeOutputs(state))[0];
	// test that this is true
	//ASSERT_EQUAL(actuatorWork, fKE-iKE, integ_accuracy);

	// Before exiting lets see if copying the spring works
	TorqueActuator* copyOfActuator = actuator->clone();
	ASSERT(*copyOfActuator == *actuator);
	
	// Check that de/serialization works
	Model modelFromFile("TestTorqueActuatorModel.osim");
	ASSERT(modelFromFile == *model, __FILE__, __LINE__,
		"Model from file FAILED to match model in memory.");

	std::cout << " ********** Test TorqueActuator time =  ********** " << 
		1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
示例#4
0
//-----------------------------------------------------------------------------
// METHODS TO APPLY FORCES AND TORQUES
//-----------------------------------------------------------------------------
void Force::applyForceToPoint(const SimTK::State &s, const OpenSim::Body &aBody, const Vec3& aPoint,
                              const Vec3& aForce, Vector_<SpatialVec> &bodyForces) const
{
    _model->getMatterSubsystem().addInStationForce(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
            aPoint, aForce, bodyForces);
}