virtual void threadRelease() {
/* Stop and close ports */
cportL->interrupt();
cportR->interrupt();
susPort->interrupt();
cportL->close();
cportR->close();
susPort->close();
delete cportL;
delete cportR;
delete susPort;
/* stop motor interfaces and close */
gaze->stopControl();
clientGazeCtrl.close();
carm->stopControl();
clientArmCart.close();
robotTorso.close();
robotHead.close();
robotArm.close();
}
bool interruptModule()
{
imgLPortIn.interrupt();
imgRPortIn.interrupt();
igaze->stopControl();
iarm->stopControl();
return true;
}
virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
igaze->stopControl();
// it's a good rule to restore the controller
// context as it was before opening the module
igaze->restoreContext(startup_context_id);
clientGaze.close();
clientTorso.close();
}
virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
// (anyway it's already done internally in the
// destructor)
igaze->stopControl();
// it's a good rule to reinstate the tracking mode
// as it was
igaze->setTrackingMode(false);
delete clientGaze;
// delete clientTorso; //pramod
}
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 close()
{
handlerPort.close();
objectsPort.close();
igaze->stopControl();
igaze->restoreContext(startupGazeContextID);
igaze->deleteContext(startupGazeContextID);
igaze->deleteContext(currentGazeContextID);
if (clientGazeCtrl.isValid())
clientGazeCtrl.close();
icartLeft->stopControl();
icartLeft->restoreContext(startupArmLeftContextID);
icartLeft->deleteContext(startupArmLeftContextID);
icartLeft->deleteContext(currentArmLeftContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
icartRight->stopControl();
icartRight->restoreContext(startupArmRightContextID);
icartRight->deleteContext(startupArmRightContextID);
icartRight->deleteContext(currentArmRightContextID);
if (clientArmLeft.isValid())
clientArmLeft.close();
itorsoVelocity->stop();
if (clientTorso.isValid())
clientTorso.close();
return true;
}
virtual void run() {
while (isStopping() != true) {
/* poll the click ports containers to see if we have left/right ready to go */
bfL.lock(); bfR.lock();
if (bfL.size() == 2 && bfR.size() == 2) {
printf("got a hit!\n");
/* if they are, raise the flag that action is beginning, save current joint configuration */
Bottle susmsg;
susmsg.addInt(1);
susPort->write(susmsg);
//get the current joint configuration for the torso, head, and arm
tang.clear(); tang.resize(3);
tEnc->getEncoders(tang.data());
hang.clear(); hang.resize(6);
hEnc->getEncoders(hang.data());
aang.clear(); aang.resize(16);
aEnc->getEncoders(aang.data());
/* get the xyz location of the gaze point */
Vector bvL(2); Vector bvR(2);
bvL[0] = bfL.get(0).asDouble(); bvL[1] = bfL.get(1).asDouble();
bvR[0] = bfR.get(0).asDouble(); bvR[1] = bfR.get(1).asDouble();
objPos.clear(); objPos.resize(3);
gaze->triangulate3DPoint(bvL,bvR,objPos);
/* servo the head to gaze at that point */
//gaze->lookAtStereoPixels(bvL,bvR);
gaze->lookAtFixationPoint(objPos);
gaze->waitMotionDone(1.0,10.0);
gaze->stopControl();
printf("object position estimated as: %f, %f, %f\n", objPos[0], objPos[1], objPos[2]);
printf("is this ok?\n");
string posResp = Network::readString().c_str();
if (posResp == "yes" || posResp == "y") {
/* move to hover the hand over the XY position of the target: [X, Y, Z=0.2], with palm upright */
objPos[2] = 0.1;
Vector axa(4);
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(objPos,axa);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
//curl fingers and thumb slightly to hold object
Vector armCur(16);
aEnc->getEncoders(armCur.data());
armCur[8] = 3; armCur[10] = 25;
armCur[11] = 25; armCur[12] = 25;
armCur[13] = 25; armCur[14] = 25;
armCur[15] = 55;
aPos->positionMove(armCur.data());
Time::delay(2.0);
/* wait for terminal signal from user that object has been moved to the hand */
bool validTarg = false;
printf("object position reached, place in hand and enter target xy position\n");
while (!validTarg) {
string objResp = Network::readString().c_str();
/* ask the user to enter in an XY target location, or confirm use of previous one */
Bottle btarPos(objResp.c_str());
if (btarPos.size() < 2) {
//if user enters no target position, try to use last entered position
if (targetPos.length() != 3) {
printf("no previous target position available, please re-enter:\n");
} else {
validTarg = true;
}
} else {
targetPos.clear(); targetPos.resize(3);
targetPos[0] = btarPos.get(0).asDouble();
targetPos[1] = btarPos.get(1).asDouble();
targetPos[2] = 0.1;
validTarg = true;
}
}
/* move the arm to above the target location */
axa.zero();
if (armInUse) {
axa[2] = 1.0; axa[3] = M_PI;
} else {
axa[1] = 1.0; axa[3] = M_PI;
}
carm->goToPoseSync(targetPos,axa);
//carm->goToPosition(targetPos);
carm->waitMotionDone(1.0,10.0);
Time::delay(2.0);
/* wait for user signal that the object has been removed */
printf("object has been moved to target location. please remove object and hit enter\n");
string tarResp = Network::readString().c_str();
}
/* return to saved motor configuration, clear click buffers, lower flag signaling action done */
printf("gaze done, attempting reset\n");
tPos->positionMove(tang.data());
hPos->positionMove(hang.data());
aPos->positionMove(aang.data());
bfL.clear(); bfR.clear();
bfL.unlock(); bfR.unlock();
susmsg.clear();
susmsg.addInt(0);
susPort->write(susmsg);
}
else {
bfL.unlock(); bfR.unlock();
}
}
}
void reachingHandler(const bool b, const Vector &pos,
const Vector &rpy)
{
if (b)
{
if (s0==idle)
s0=triggered;
else if (s0==triggered)
{
if (++c0*getPeriod()>0.5)
{
pos0[0]=pos[0];
pos0[1]=pos[1];
pos0[2]=pos[2];
rpy0[0]=rpy[0];
rpy0[1]=rpy[1];
rpy0[2]=rpy[2];
iarm->getPose(x0,o0);
s0=running;
}
}
else
{
Vector xd(4,0.0);
xd[0]=pos[0]-pos0[0];
xd[1]=pos[1]-pos0[1];
xd[2]=pos[2]-pos0[2];
xd[3]=1.0;
Matrix H0=eye(4,4);
H0(0,3)=x0[0];
H0(1,3)=x0[1];
H0(2,3)=x0[2];
xd=H0*xd;
Matrix Rd;
if (onlyXYZ)
Rd=axis2dcm(o0);
else
{
Vector drpy(3);
drpy[0]=rpy[0]-rpy0[0];
drpy[1]=rpy[1]-rpy0[1];
drpy[2]=rpy[2]-rpy0[2];
Vector ax(4,0.0),ay(4,0.0),az(4,0.0);
ax[0]=1.0; ax[3]=drpy[2];
ay[1]=1.0; ay[3]=drpy[1]*((part=="left_arm")?1.0:-1.0);
az[2]=1.0; az[3]=drpy[0]*((part=="left_arm")?1.0:-1.0);
Rd=axis2dcm(o0)*axis2dcm(ax)*axis2dcm(ay)*axis2dcm(az);
}
Vector od=dcm2axis(Rd);
iarm->goToPose(xd,od);
if (gaze)
igaze->lookAtFixationPoint(xd);
yInfo("going to (%s) (%s)",
xd.toString(3,3).c_str(),od.toString(3,3).c_str());
if (simulator)
updateSim(xd);
}
}
else
{
if (s0==triggered)
onlyXYZ=!onlyXYZ;
if (c0!=0)
{
iarm->stopControl();
if (simulator)
{
Vector x,o;
iarm->getPose(x,o);
updateSim(x);
}
if (gaze)
igaze->stopControl();
}
s0=idle;
c0=0;
}
}
