void ForwardDynamicsABM::updateForceSensors()
{
    const DeviceList<ForceSensor>& sensors = sensorHelper.forceSensors();
    for(size_t i=0; i < sensors.size(); ++i){
        ForceSensor* sensor = sensors[i];
        const DyLink* link = static_cast<DyLink*>(sensor->link());

        //    | f   | = | Ivv  trans(Iwv) | * | dvo | + | pf   |
        //    | tau |   | Iwv     Iww     |   | dw  |   | ptau |
        const Vector3 f = -(link->Ivv() * link->dvo() + link->Iwv().transpose() * link->dw() + link->pf());
        const Vector3 tau = -(link->Iwv() * link->dvo() + link->Iww() * link->dw() + link->ptau());
        
        const Matrix3 R = link->R() * sensor->R_local();
        const Vector3 p = link->p() + link->R() * sensor->p_local();
        sensor->f().noalias()   = R.transpose() * f;
        sensor->tau().noalias() = R.transpose() * (tau - p.cross(f));
        sensor->notifyStateChange();
    }
}
Esempio n. 2
0
void ODEBody::updateForceSensors(bool flipYZ)
{
    const DeviceList<ForceSensor>& forceSensors = sensorHelper.forceSensors();
    for(int i=0; i < forceSensors.size(); ++i){
        ForceSensor* sensor = forceSensors.get(i);
        const Link* link = sensor->link();
        const dJointFeedback& fb = forceSensorFeedbacks[i];
        Vector3 f, tau;
        if(!flipYZ){
            f   << fb.f2[0], fb.f2[1], fb.f2[2];
            tau << fb.t2[0], fb.t2[1], fb.t2[2];
        } else {
            f   << fb.f2[0], -fb.f2[2], fb.f2[1];
            tau << fb.t2[0], -fb.t2[2], fb.t2[1];
        }
        const Matrix3 R = link->R() * sensor->R_local();
        const Vector3 p = link->R() * sensor->p_local();

        sensor->f()   = R.transpose() * f;
        sensor->tau() = R.transpose() * (tau - p.cross(f));
        sensor->notifyStateChange();
    }
}