void ForceSensorVisualizerItem::updateForceSensorState(int index)
{
if(index < static_cast<int>(forceSensors.size())){
ForceSensor* sensor = forceSensors[index];
Vector3 v = sensor->link()->T() * sensor->T_local() * sensor->f();
forceSensorArrows[index]->setVector(v * visualRatio);
}
}
void ForceSensorVisualizerItem::updateSensorPositions()
{
for(size_t i=0; i < forceSensors.size(); ++i){
ForceSensor* sensor = forceSensors[i];
Vector3 p = sensor->link()->T() * sensor->localTranslation();
forceSensorArrows[i]->setTranslation(p);
}
}
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();
}
}
virtual bool control() {
switch(phase){
case 0 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
phase = 1;
}
break;
case 1:
if(currentFrame < qseq->numFrames()){
MultiValueSeq::Frame frame = qseq->frame(currentFrame++);
for(int i=0; i < numJoints; ++i){
qref[i] = frame[i];
}
}else{
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_shoulder_r] = -0.4;
q1[rarm_shoulder_p] = 0.75;
q1[rarm_elbow] = -2.0;
interpolator.appendSample(time + 3.0, q1);
q1[rarm_elbow] = -1.57;
q1[rarm_shoulder_p] = -0.2;
q1[rarm_wrist_r] = 1.5;
interpolator.appendSample(time + 5.0, q1);
q1[rarm_elbow] = -1.3;
q1[rarm_wrist_y] = -0.24;
interpolator.appendSample(time + 6.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 2;
}
break;
case 2 :
qref = interpolator.interpolate(time);
if(time > interpolator.domainUpper()){
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_y] = 0.0;
q1[rarm_shoulder_r] = 0.1;
interpolator.appendSample(time + 5.0, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 3;
}
break;
case 3:
qref = interpolator.interpolate(time);
if( rhsensor->F()[1] < -2 ) {
interpolator.clear();
interpolator.appendSample(time, qref);
VectorXd q1(numJoints);
q1 = qref;
q1[rarm_wrist_r] = -0.3;;
interpolator.appendSample(time + 2.0, q1);
interpolator.appendSample(time + 2.5, q1);
q1[rarm_shoulder_p] = -0.13;
q1[rarm_elbow] = -1.8;
interpolator.appendSample(time + 3.5, q1);
interpolator.update();
qref = interpolator.interpolate(time);
phase = 4;
}
break;
case 4 :
qref = interpolator.interpolate(time);
}
for(int i=0; i < body->numJoints(); ++i){
Link* joint = body->joint(i);
double q = joint->q();
double dq_ref = (qref[i] - qref_old[i]) / timeStep_;
double dq = (q - qold[i]) / timeStep_;
joint->u() = (qref[i] - q) * pgain[i] + (dq_ref - dq) * dgain[i];
qold[i] = q;
}
qref_old = qref;
time += timeStep_;
return true;
}
