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;
}
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 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 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;
}
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;
}
void Controller::setJointsCtrlMode()
{
if (jointsToSet.size()==0)
return;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (jointsToSet[i]<eyesJoints[0])
{
if (neckPosCtrlOn)
modes.push_back(VOCAB_CM_POSITION_DIRECT);
else
modes.push_back(VOCAB_CM_VELOCITY);
}
else
modes.push_back(VOCAB_CM_MIXED);
}
modHead->setControlModes(jointsToSet.size(),jointsToSet.getFirst(),
modes.getFirst());
}
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;
}
