void handHandler(const bool b)
{
if (b)
{
if (s1==idle)
s1=triggered;
else if (s1==triggered)
{
if (++c1*getPeriod()>0.5)
{
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
s1=running;
}
}
else
ivel->velocityMove(joints.size(),joints.getFirst(),vels.data());
}
else
{
if (s1==triggered)
vels=-1.0*vels;
if (c1!=0)
ivel->stop(joints.size(),joints.getFirst());
s1=idle;
c1=0;
}
}
bool imuIdentifierThread::setHeadCtrlModes(const string &_s)
{
printMessage(1,"Setting %s mode for head joints..\n",_s.c_str());
if (_s!="position" && _s!="velocity")
return false;
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
VectorOf<int> modes;
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
}
imodH -> setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
Time::delay(0.1);
return true;
}
bool reactCtrlThread::setCtrlModes(const VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
if (_s!="position" && _s!="velocity")
return false;
if (jointsToSet.size()==0)
return true;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
}
}
if (_p=="arm")
{
imodA->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else if (_p=="torso")
{
imodT->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else
return false;
return true;
}
bool close()
{
iarm->stopControl();
iarm->restoreContext(startup_context);
drvCart.close();
ivel->stop(joints.size(),joints.getFirst());
for (size_t i=0; i<modes.size(); i++)
modes[i]=VOCAB_CM_POSITION;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
drvHand.close();
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
simPort.write(cmd,reply);
simPort.close();
}
if (gaze)
{
igaze->stopControl();
drvGaze.close();
}
igeo->stopFeedback();
igeo->setTransformation(eye(4,4));
drvGeomagic.close();
forceFbPort.close();
return true;
}
bool imuIdentifierThread::goHome()
{
setHeadCtrlModes("position");
// Vector pos0(6,0.0);
// iposH -> positionMove(pos0.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
Vector poss(3,0.0);
iposH -> positionMove(jointsToSet.size(),
jointsToSet.getFirst(),
poss.data());
return true;
}
bool reactCtrlThread::areJointsHealthyAndSet(VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
VectorOf<int> modes;
if (_p=="arm")
{
modes=encsA->size();
imodA->getControlModes(modes.getFirst());
}
else if (_p=="torso")
{
modes=encsT->size();
imodT->getControlModes(modes.getFirst());
}
else
return false;
for (size_t i=0; i<modes.size(); i++)
{
if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
return false;
if (_s=="velocity")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
jointsToSet.push_back(i);
}
else if (_s=="position")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
jointsToSet.push_back(i);
}
}
return true;
}
void checkSendReceiveInt()
{
report(0, "check VectorO<int> send receive");
{
Port portIn;
Port portOut;
portOut.open("/harness_sig/vtest/o");
portIn.open("/harness_sig/vtest/i");
Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");
portOut.enableBackgroundWrite(true);
VectorOf<int> vector;
vector.resize(10);
for (unsigned int k = 0; k < vector.size(); k++)
{
vector[k] = k;
}
bool success = true;
portOut.write(vector);
VectorOf<int> tmp;
portIn.read(tmp);
//compare vector and tmp
if (tmp.size() != vector.size())
{
success = false;
}
else
{
for (unsigned int k = 0; k < vector.size(); k++)
{
if (tmp[k] != vector[k])
success = false;
}
}
checkTrue(success, "VectorOf<int> was sent and received correctly");
portOut.interrupt();
portOut.close();
portIn.interrupt();
portIn.close();
}
report(0, "check VectorOf<int> bottle compatibility");
{
//write the same vector again and receive it as a bottle
Port portIn;
Port portOut;
bool success = true;
portOut.open("/harness_sig/vtest/o");
portIn.open("/harness_sig/vtest/i");
Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");
portOut.enableBackgroundWrite(true);
VectorOf<int> vector;
vector.resize(10);
for (unsigned int k = 0; k < vector.size(); k++)
{
vector[k] = k;
}
portOut.write(vector);
Bottle tmp2;
success = portIn.read(tmp2);
checkTrue(success,"correctly read from the port");
//compare vector and tmp
success = true;
if (tmp2.size() != vector.size())
{
success = false;
}
else
{
for (unsigned int k = 0; k < vector.size(); k++)
{
if (tmp2.get(k).asInt32() != vector[k])
success = false;
}
}
checkTrue(success, "VectorOf<int> was received correctly in a Bottle");
portOut.interrupt();
portOut.close();
portIn.interrupt();
portIn.close();
}
}
bool imuIdentifierThread::processWayPoint()
{
processIMU();
if (wayPoints[currentWaypoint].name == "START " ||
wayPoints[currentWaypoint].name == "END " ||
wayPoints[currentWaypoint].name == "MIDDLE " )
{
if (posCtrlFlag)
{
printMessage(1,"Putting head in home position..\n");
goHome();
posCtrlFlag = false;
}
if (yarp::os::Time::now() - timeNow > CTRL_PERIOD)
{
posCtrlFlag = true;
return false;
}
}
else
{
Vector jls = wayPoints[currentWaypoint].jntlims;
Vector vls = wayPoints[currentWaypoint].vels;
bool flag = false;
iencsH->getEncoders(encsH->data());
yarp::sig::Vector head = *encsH;
// ivelH -> velocityMove(vls.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
ivelH -> velocityMove(jointsToSet.size(),
jointsToSet.getFirst(),
vls.data());
for (int i = 0; i < 3; i++)
{
if (vls(i) > 0.0)
{
if (jls(i) - head(i) > 0.0)
{
flag = true;
}
}
else if (vls(i) < 0.0)
{
if (jls(i) - head(i) < 0.0)
{
flag = true;
}
}
}
return flag;
}
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("teleop-icub")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str();
double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble();
part=rf.check("part",Value("right_arm")).asString().c_str();
simulator=rf.check("simulator",Value("off")).asString()=="on";
gaze=rf.check("gaze",Value("off")).asString()=="on";
minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble());
maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble());
bool torso=rf.check("torso",Value("on")).asString()=="on";
Property optGeo("(device hapticdeviceclient)");
optGeo.put("remote",("/"+geomagic).c_str());
optGeo.put("local",("/"+name+"/geomagic").c_str());
if (!drvGeomagic.open(optGeo))
return false;
drvGeomagic.view(igeo);
if (simulator)
{
simPort.open(("/"+name+"/simulator:rpc").c_str());
if (!Network::connect(simPort.getName().c_str(),"/icubSim/world"))
{
yError("iCub simulator is not running!");
drvGeomagic.close();
simPort.close();
return false;
}
}
if (gaze)
{
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
{
drvGeomagic.close();
simPort.close();
return false;
}
drvGaze.view(igaze);
}
Property optCart("(device cartesiancontrollerclient)");
optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
optCart.put("local",("/"+name+"/cartesianController/"+part).c_str());
if (!drvCart.open(optCart))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
return false;
}
drvCart.view(iarm);
Property optHand("(device remote_controlboard)");
optHand.put("remote",("/"+robot+"/"+part).c_str());
optHand.put("local",("/"+name+"/"+part).c_str());
if (!drvHand.open(optHand))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
drvCart.close();
return false;
}
drvHand.view(imod);
drvHand.view(ipos);
drvHand.view(ivel);
iarm->storeContext(&startup_context);
iarm->restoreContext(0);
Vector dof(10,1.0);
if (!torso)
dof[0]=dof[1]=dof[2]=0.0;
else
dof[1]=0.0;
iarm->setDOF(dof,dof);
iarm->setTrajTime(Tp2p);
Vector accs,poss;
for (int i=0; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_POSITION);
accs.push_back(1e9);
vels.push_back(100.0);
poss.push_back(0.0);
}
poss[0]=20.0;
poss[1]=70.0;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
ipos->positionMove(joints.size(),joints.getFirst(),poss.data());
joints.clear();
modes.clear();
vels.clear();
for (int i=2; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_VELOCITY);
vels.push_back(40.0);
}
vels[vels.length()-1]=100.0;
s0=s1=idle;
c0=c1=0;
onlyXYZ=true;
stateStr[idle]="idle";
stateStr[triggered]="triggered";
stateStr[running]="running";
Matrix T=zeros(4,4);
T(0,1)=1.0;
T(1,2)=1.0;
T(2,0)=1.0;
T(3,3)=1.0;
igeo->setTransformation(SE3inv(T));
igeo->setCartesianForceMode();
igeo->getMaxFeedback(maxFeedback);
Tsim=zeros(4,4);
Tsim(0,1)=-1.0;
Tsim(1,2)=1.0; Tsim(1,3)=0.5976;
Tsim(2,0)=-1.0; Tsim(2,3)=-0.026;
Tsim(3,3)=1.0;
pos0.resize(3,0.0);
rpy0.resize(3,0.0);
x0.resize(3,0.0);
o0.resize(4,0.0);
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("ssph");
// radius
cmd.addDouble(0.02);
// position
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
// color
cmd.addInt(1);
cmd.addInt(0);
cmd.addInt(0);
// collision
cmd.addString("FALSE");
simPort.write(cmd,reply);
}
forceFbPort.open(("/"+name+"/force-feedback:i").c_str());
feedback.resize(3,0.0);
return true;
}