Esempio n. 1
0
//==============================================================================
// 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);

    // 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;
}
Esempio n. 2
0
/**
* 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);
    }

    // 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->setUseVisualizer(false);

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

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

    // -------------- Provide control signals for bodyActuator ----------
    // Get the default control vector of the model
    Vector modelControls = model->getDefaultControls();
    
    // Specify 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;
}
Esempio n. 3
0
const SimTK::MobilizedBodyIndex Joint::
    getMobilizedBodyIndex(const OpenSim::Body& body) const
{
        return body.getMobilizedBodyIndex();
}