src/taskspace/src/InverseDynamicsModel.cpp
Vector calcTotalForces(const State& s, const Model& model) {
    // create a working instance of the model, upon first entrance to this
    // function
    static Model* workingModel = NULL;
    if (workingModel == NULL) {
        workingModel = model.clone();
        workingModel->setUseVisualizer(false);
        // remove controllers to avoid infinite loop
        workingModel->updControllerSet().setSize(0);
        // disable any actuators when computing the total force
        auto& fs = workingModel->updForceSet();
        for (int i = 0; i < fs.getSize(); i++) {
            auto pathActuator = dynamic_cast<PathActuator*>(&fs[i]);
            if (pathActuator) {
                pathActuator->set_appliesForce(false);
            }
        }
        workingModel->finalizeFromProperties();
        workingModel->initSystem();
    }
    // initialize working state from s
    auto workingState = workingModel->updWorkingState();
    workingState.setTime(s.getTime());
    workingState.setQ(s.getQ());
    workingState.setU(s.getU());
    workingState.setZ(s.getZ());
    workingState.setY(s.getY());
    // generalized forces and torques should be accessed at a Dynamics stage
    workingModel->realizeDynamics(workingState);
    // get acting torques and body forces
    auto bodyForces = workingModel->getMultibodySystem()
        .getRigidBodyForces(workingState, Stage::Dynamics);
    auto generalizedForces = workingModel->getMultibodySystem()
        .getMobilityForces(workingState, Stage::Dynamics);
    // map body forces to joint forces
    Vector jointForces;
    workingModel->getMatterSubsystem()
        .multiplyBySystemJacobianTranspose(workingState, bodyForces, jointForces);
    // in our conviction we subtract their contribution
    return -1.0 * generalizedForces - jointForces;
}
/**
 * This function calculates the total applied forces. In order to calculate them
 * the model must be realized to Stage::Dynamics. This method is typically
 * called in mixed dynamics scheme thus while the model is numerically
 * integrated (forward dynamics) we may be interested in the acting forces so
 * that a controller (e.g. inverse dynamics torque controller) can calculate the
 * controls correctly. If however the original model is realized to
 * Stage::Dynamics the controller's computeControls will be called again and
 * this will cause an infinite loop. To avoid this problem upon first entrance
 * to this function a working copy of the model is created without the
 * controller and this working model is realized to Stage::Dynamics. Each time
 * the working state is updated from the current state of the simulating model
 * and the variables of the working model's working state.
 */
This function calculates the total applied forces. In order to calculate them
the model must be realized to Stage::Dynamics. This method is typically
called in mixed dynamics scheme thus while the model is numerically
integrated (forward dynamics) we may be interested in the acting forces so
that a controller  can calculate the
controls correctly. If however the original model is realized to
Stage::Dynamics the controller's computeControls will be called again and
this will cause an infinite loop. To avoid this problem upon first entrance
to this function a working copy of the model is created without the
controller and this working model is realized to Stage::Dynamics. Each time
the working state is updated from the current state of the simulating model
and the variables of the working model's working state.
This function calculates the total applied forces . In order to calculate them the model must be realized to Stage :: Dynamics . This method is typically called in mixed dynamics scheme thus while the model is numerically integrated ( forward dynamics ) we may be interested in the acting forces so that a controller can calculate the controls correctly . If however the original model is realized to Stage :: Dynamics the controller ' s computeControls will be called again and this will cause an infinite loop . To avoid this problem upon first entrance to this function a working copy of the model is created without the controller and this working model is realized to Stage :: Dynamics . Each time the working state is updated from the current state of the simulating model and the variables of the working model ' s working state .
Vector calcTotalForces(const State& s, const Model& model) {
    static Model* workingModel = NULL;
    if (workingModel == NULL) {
        workingModel = model.clone();
        workingModel->setUseVisualizer(false);
        workingModel->updControllerSet().setSize(0);
        auto& fs = workingModel->updForceSet();
        for (int i = 0; i < fs.getSize(); i++) {
            auto pathActuator = dynamic_cast<PathActuator*>(&fs[i]);
            if (pathActuator) {
                pathActuator->set_appliesForce(false);
            }
        }
        workingModel->finalizeFromProperties();
        workingModel->initSystem();
    }
    auto workingState = workingModel->updWorkingState();
    workingState.setTime(s.getTime());
    workingState.setQ(s.getQ());
    workingState.setU(s.getU());
    workingState.setZ(s.getZ());
    workingState.setY(s.getY());
    workingModel->realizeDynamics(workingState);
    auto bodyForces = workingModel->getMultibodySystem()
        .getRigidBodyForces(workingState, Stage::Dynamics);
    auto generalizedForces = workingModel->getMultibodySystem()
        .getMobilityForces(workingState, Stage::Dynamics);
    Vector jointForces;
    workingModel->getMatterSubsystem()
        .multiplyBySystemJacobianTranspose(workingState, bodyForces, jointForces);
    return -1.0 * generalizedForces - jointForces;
}
Vector calcTotalForces ( const State & s , const Model & model ) { static Model * workingModel = NULL ; if ( workingModel == NULL ) { workingModel = model . clone ( ) ; workingModel -> setUseVisualizer ( false ) ; workingModel -> updControllerSet ( ) . setSize ( 0 ) ; auto & fs = workingModel -> updForceSet ( ) ; for ( int i = 0 ; i < fs . getSize ( ) ; i ++ ) { auto pathActuator = dynamic_cast < PathActuator * > ( & fs [ i ] ) ; if ( pathActuator ) { pathActuator -> set_appliesForce ( false ) ; } } workingModel -> finalizeFromProperties ( ) ; workingModel -> initSystem ( ) ; } auto workingState = workingModel -> updWorkingState ( ) ; workingState . setTime ( s . getTime ( ) ) ; workingState . setQ ( s . getQ ( ) ) ; workingState . setU ( s . getU ( ) ) ; workingState . setZ ( s . getZ ( ) ) ; workingState . setY ( s . getY ( ) ) ; workingModel -> realizeDynamics ( workingState ) ; auto bodyForces = workingModel -> getMultibodySystem ( ) . getRigidBodyForces ( workingState , Stage :: Dynamics ) ; auto generalizedForces = workingModel -> getMultibodySystem ( ) . getMobilityForces ( workingState , Stage :: Dynamics ) ; Vector jointForces ; workingModel -> getMatterSubsystem ( ) . multiplyBySystemJacobianTranspose ( workingState , bodyForces , jointForces ) ; return -1.0 * generalizedForces - jointForces ; }
This function calculates the total applied forces.
This function calculates the total applied forces .
// create a working instance of the model, upon first entrance to this // function // remove controllers to avoid infinite loop // disable any actuators when computing the total force // initialize working state from s // generalized forces and torques should be accessed at a Dynamics stage // get acting torques and body forces // map body forces to joint forces // in our conviction we subtract their contribution