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