C++ Body::getIndex方法代码示例

/**
* 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);
	}

//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

//-----------------------------------------------------------------------------
// 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);
}