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