bool YarpJointDev::getDOF ( yarp::sig::Vector& curDof ) {
curDof.clear();
for ( unsigned i = 0; i < _chain->GetNumberOfJoints(); ++i )
curDof.push_back ( 1 );
return true;
}
bool IndexFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){
bool ret = true;
Vector joints;
int nEncs;
position.clear();
position.resize(3); //x,y, z position
ret = ret && _armEncoder->getAxes(&nEncs);
Vector encs(nEncs);
if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
{
cerr << _dbgtag << "Failed to read arm encoder data" << endl;
}
//cout << encs.toString() << endl;
ret = ret && _iCubFinger->getChainJoints(encs, joints);
if(ret == false){
cout << "failed to get chain joints" << endl;
return false;
}
// Replace the joins with the encoder readings
joints[0] = 20; // The index fingertip calibration procedure used this value.
joints[1] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
joints[2] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
joints[3] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
//cout << joints.size() << endl;
//Convert the joints to radians.
for (int j = 0; j < joints.size(); j++)
joints[j] *= DEG2RAD;
yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
position = tipFrame.getCol(3); // Tip's position in the hand coordinate
return ret;
}
void YarpJointDev::PosEulerSingle_To_PosAxisAngle ( const std::vector< float >& pos,
yarp::sig::Vector& xd,
yarp::sig::Vector& od ) {
assert ( pos.size() == 6 );
xd.clear();
od.clear();
xd.push_back ( pos[0] );
xd.push_back ( pos[1] );
xd.push_back ( pos[2] );
float xa, ya, za, theta;
Euler_To_AxisAngle ( pos[3], pos[4], pos[5], xa, ya, za, theta );
od.push_back ( xa );
od.push_back ( ya );
od.push_back ( za );
od.push_back ( theta );
}
bool MiddleFinger::getPositionHandFrame(yarp::sig::Vector &position, yarp::sig::Vector &fingerEncoders){
bool ret = true;
Vector joints;
int nEncs;
position.clear();
position.resize(3); //x,y, z position
ret = ret && _armEncoder->getAxes(&nEncs);
Vector encs(nEncs);
if(! (ret = ret && _armEncoder->getEncoders(encs.data())))
{
cerr << _dbgtag << "Failed to read arm encoder data" << endl;
}
//cout << encs.toString() << endl;
//cout << fingerEncoders.toString() << endl;
ret = ret && _iCubFinger->getChainJoints(encs, joints);
if(ret == false){
cout << "failed to get chain joints" << endl;
return false;
}
//std::cout << "From iCubFinger: " << joints.toString() << endl;
//This is where it is different from the index finger
joints[0] = 90 * (1 - (fingerEncoders[0] - _minProximal) / (_maxProximal - _minProximal) );
joints[1] = 90 * (1 - (fingerEncoders[1] - _minMiddle) / (_maxMiddle - _minMiddle) );
joints[2] = 90 * (1 - (fingerEncoders[2] - _minDistal) / (_maxDistal - _minDistal) );
//cout << "From mycalculat: " << joints.toString() << endl;
//cout << joints.size() << endl;
//Convert the joints to radians.
for (int j = 0; j < joints.size(); j++)
joints[j] *= M_PI/180;
yarp::sig::Matrix tipFrame = _iCubFinger->getH(joints);
position = tipFrame.getCol(3); // Tip's position in the hand coordinate
}
bool JoypadControlClient::getRawTouch(unsigned int touch_id, yarp::sig::Vector& value)
{
value.clear();
if(m_rpc_only)
{
Bottle cmd, response;
cmd.addVocab(VOCAB_IJOYPADCTRL);
cmd.addVocab(VOCAB_GET);
cmd.addVocab(VOCAB_TOUCH);
cmd.addVocab(VOCAB_VALUE);
cmd.addInt32(touch_id);
m_rpcPort.write(cmd, response);
if(response.get(0).asVocab() == VOCAB_OK && response .get(1).isFloat64() && response.get(2).isFloat64())
{
value.push_back(response.get(1).asFloat64());
value.push_back(response.get(2).asFloat64());
return true;
}
else
{
return false;
}
}
else
{
LockGuard l(m_touchPort.mutex);
if(touch_id < m_touchPort.storage.size()/2)
{
value.push_back(m_touchPort.storage[touch_id * 2]);
value.push_back(m_touchPort.storage[touch_id * 2 + 1]);
return true;
}
else
{
return false;
}
}
}
bool JoypadControlClient::getRawStick(unsigned int stick_id, yarp::sig::Vector& value, JoypadCtrl_coordinateMode coordinate_mode)
{
value.clear();
if(m_rpc_only || coordinate_mode == IJoypadController::JypCtrlcoord_POLAR)
{
Bottle cmd, response;
int dof, coordmode;
coordmode = coordinate_mode == IJoypadController::JypCtrlcoord_CARTESIAN ? VOCAB_CARTESIAN : VOCAB_POLAR;
cmd.addVocab(VOCAB_IJOYPADCTRL);
cmd.addVocab(VOCAB_GET);
cmd.addVocab(VOCAB_STICKDOF);
cmd.addVocab(VOCAB_VALUE);
cmd.addInt32(stick_id);
m_rpcPort.write(cmd, response);
if(response.get(0).asVocab() == VOCAB_OK && response.get(1).isInt32())
{
dof = response.get(1).asInt32();
}
else
{
return false;
}
cmd.addVocab(VOCAB_IJOYPADCTRL);
cmd.addVocab(VOCAB_GET);
cmd.addVocab(VOCAB_STICK);
cmd.addVocab(VOCAB_VALUE);
cmd.addVocab(coordmode);
cmd.addInt32(stick_id);
m_rpcPort.write(cmd, response);
if(response.get(0).asVocab() == VOCAB_OK)
{
for(int i = 0; i < dof; i++)
{
if(response.get(i).isFloat64())
{
value.push_back(response.get(i).asFloat64());
}
else
{
return false;
}
}
return true;
}
else
{
return false;
}
}
else
{
LockGuard l(m_stickPort.mutex);
int offset = 0;
unsigned int i;
if(getStickCount(i), stick_id >= i)
{
yError() << "JoypadControlCLient: GetStick() error.. Stick_id out of bound";
return false;
}
for(size_t j = 0; j < stick_id; j++)
{
offset += m_stickDof[j];
}
for(size_t i = 0; i < m_stickDof[stick_id]; ++i)
{
value.push_back(m_stickPort.storage[offset + i]);
}
return true;
}
}