Ejemplo n.º 1
0
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);
    }
}
Ejemplo n.º 2
0
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);
    }
}
Ejemplo n.º 3
0
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();
    }
}
Ejemplo n.º 4
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();
    }
}
    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;
    }