bool icubFinger::readEncoders(yarp::sig::Vector &encoderValues){
    bool ret = false;


    encoderValues.resize(3);
    encoderValues.zero();

    int pendingReads = _fingerEncoders->getPendingReads();
    Bottle *handEnc = NULL;

    for(int i = 0; i <= pendingReads; i++){
        handEnc = _fingerEncoders->read();
    }

    if(handEnc != NULL)
    {
        encoderValues[0] = handEnc->get(_proximalEncoderIndex).asDouble();
        encoderValues[1] = handEnc->get(_middleEncoderIndex).asDouble();
        encoderValues[2] = handEnc->get(_distalEncoderIndex).asDouble();
        ret = true;

        adjustMinMax(encoderValues[0], _minProximal, _maxProximal);
        adjustMinMax(encoderValues[1], _minMiddle, _maxMiddle);
        adjustMinMax(encoderValues[2], _minDistal, _maxDistal);
    }


    //cout << "Encoders: " << encoderValues.toString() << endl;
    return ret;
}
void MiddleFinger::getTactileDataComp(yarp::sig::Vector &tactileData){
    tactileData.resize(12);
    tactileData.zero();

    Bottle *tactileDataPort = _tactileDataComp_in->read();

    int startIndex = 12;
    if(tactileDataPort != NULL){
        for(int i = startIndex; i < startIndex + 12; ++i){
            tactileData[i - startIndex] = tactileDataPort->get(i).asDouble();
        }
    }

}
Example #3
0
bool yarpWholeBodyModelV1::convertBaseVelocity(const double *dxB, yarp::sig::Vector & v_b, yarp::sig::Vector & omega_b)
{
    if (!dxB) {
        //shortcut to zero the vectors
        v_b.zero();
        omega_b.zero();
        return true;
    }
    v_b[0] = dxB[0];
    v_b[1] = dxB[1];
    v_b[2] = dxB[2];
    omega_b[0] = dxB[3];
    omega_b[1] = dxB[4];
    omega_b[2] = dxB[5];
    return true;
}
Example #4
0
bool BoschIMU::read(yarp::sig::Vector &out)
{
    mutex.wait();
    out.resize(nChannels);
    out.zero();

    out = data;
    if(nChannels == 16)
    {
        out[12] = quaternion.w();
        out[13] = quaternion.x();
        out[14] = quaternion.y();
        out[15] = quaternion.z();
    }

    mutex.post();
    return true;
};
Example #5
0
bool yarpWholeBodyModelV1::convertQ(const double *_q_input, yarp::sig::Vector & q_complete_output)
{
    if (!_q_input) {
        //shortcut to zero vector
        q_complete_output.zero();
        return true;
    }

    for(int wbi_joint_numeric_id=0; wbi_joint_numeric_id < this->dof; wbi_joint_numeric_id++ )
    {
            double tmp;
            tmp = _q_input[wbi_joint_numeric_id];
            assert(wbiToiDynTreeJointId[wbi_joint_numeric_id] >= 0);
            assert(wbiToiDynTreeJointId[wbi_joint_numeric_id] < (int)q_complete_output.size());
            q_complete_output[wbiToiDynTreeJointId[wbi_joint_numeric_id]] = tmp;
    }
    return true;
}
void MiddleFinger::getTactileDataRaw(yarp::sig::Vector &rawTactileData){

    // Read from the port
    rawTactileData.resize(12);
    rawTactileData.zero();

    Bottle* tactileData;
    tactileData = _rawTactileData_in->read();

    //cout << tactileData->toString() << endl;
    int startIndex = 12;
    if(tactileData != NULL){
        for (int i = startIndex; i < 12; ++i)
        {
            rawTactileData[i - startIndex] = tactileData->get(i).asDouble();
        }
    }
}