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

//==============================================================================
// 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
    const Ground& ground = model->getGround();

    //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->getMobilizedBodyIndex(),
        torqueMag*torqueAxis, bodyForces);
    model->getMatterSubsystem().addInBodyTorque(state, bodyB->getMobilizedBodyIndex(),
        -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);
//.........这里部分代码省略.........

/**
* 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
    const Ground& ground = model->getGround();

    // 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->attachGeometry(new Brick(Vec3(blockSideLength/2,
                                     blockSideLength/2, 
                                     blockSideLength/2)));

    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->getMobilizedBodyIndex(),
        torqueInG, bodyForces);
    model->getMatterSubsystem().addInStationForce(state, block->getMobilizedBodyIndex(),
        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);
//.........这里部分代码省略.........

const SimTK::MobilizedBodyIndex Joint::
    getMobilizedBodyIndex(const OpenSim::Body& body) const
{
        return body.getMobilizedBodyIndex();
}