void printStatus()
{
if (t-t1>=PRINT_STATUS_PER)
{
Vector x,o,xdhat,odhat,qdhat;
// we get the current arm pose in the
// operational space
icart->getPose(x,o);
// we get the final destination of the arm
// as found by the solver: it differs a bit
// from the desired pose according to the tolerances
icart->getDesired(xdhat,odhat,qdhat);
double e_x=norm(xdhat-x);
double e_o=norm(odhat-o);
fprintf(stdout,"+++++++++\n");
fprintf(stdout,"xd [m] = %s\n",xd.toString().c_str());
fprintf(stdout,"xdhat [m] = %s\n",xdhat.toString().c_str());
fprintf(stdout,"x [m] = %s\n",x.toString().c_str());
fprintf(stdout,"od [rad] = %s\n",od.toString().c_str());
fprintf(stdout,"odhat [rad] = %s\n",odhat.toString().c_str());
fprintf(stdout,"o [rad] = %s\n",o.toString().c_str());
fprintf(stdout,"norm(e_x) [m] = %g\n",e_x);
fprintf(stdout,"norm(e_o) [rad] = %g\n",e_o);
fprintf(stdout,"---------\n\n");
t1=t;
}
}
bool cartesianMover::display_axis_pose(cartesianMover *cm)
{
ICartesianControl *icrt = cm->crt;
GtkEntry **entry = (GtkEntry **) cm->po;
//fprintf(stderr, "Trying to get the cartesian position...");
Vector x;
Vector axis;
if(!icrt->getPose(x,axis))
fprintf(stderr, "Troubles in gettin the cartesian pose for %s", cm->partLabel);
//else
//fprintf(stderr, "got x=%s, o=%s\n", x.toString().c_str(), o.toString().c_str());
char buffer[40] = {'i', 'n', 'i', 't'};
for (int k = 0; k < 4; k++)
{
if (k == 3)
sprintf(buffer, "%.2f", axis(k)*180/M_PI);
else
sprintf(buffer, "%.2f", axis(k));
gtk_entry_set_text((GtkEntry*) entry[k], buffer);
}
return true;
}
void cartesianMover::cartesian_line_click(GtkTreeView *tree_view, GtkTreePath *path, GtkTreeViewColumn *column, cartesianMover *cm)
{
ICartesianControl *icrt = cm->crt;
int *i = gtk_tree_path_get_indices (path);
//double currPos[NUMBER_OF_CARTESIAN_COORDINATES];
Vector x;
Vector axis;
if(!icrt->getPose(x,axis))
fprintf(stderr, "Troubles in getting the cartesian pose for %s", cm->partLabel);
Matrix R = axis2dcm(axis);
Vector eu = dcm2euler(R);
//fprintf(stderr, "Storing euler angles: %s...", eu.toString().c_str());
//fprintf(stderr, "...corrsponding to axis: %s", axis.toString().c_str());
for (int k =0; k < NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
if(k<3)
cm->STORED_POS[*i][k] = x(k);
else
cm->STORED_POS[*i][k] = eu(k-3)*180/M_PI;
}
gtk_tree_view_set_model (GTK_TREE_VIEW (tree_view), refresh_cartesian_list_model(cm));
gtk_widget_draw(GTK_WIDGET(tree_view), NULL);
return;
}
void limitTorsoPitch()
{
int axis=0; // pitch joint
double min, max;
arm->getLimits(axis,&min,&max);
arm->setLimits(axis,min,MAX_TORSO_PITCH);
}
static void stop_motion(GtkWidget *box, gpointer user_data)
{
ICartesianControl *crt = (ICartesianControl *) user_data;
bool stopped = crt->stopControl();
if(!stopped)
fprintf(stderr, "There were problems in trying stopping cartesian control\n");
return;
}
bool cartesianMover::display_cartesian_pose(cartesianMover *cm)
{
ICartesianControl *icrt = cm->crt;
GtkEntry **entry = (GtkEntry **) cm->currPosArray;
GtkWidget **sliderPosArray = (GtkWidget **) cm->sliderArray;
//fprintf(stderr, "Trying to get the cartesian position...");
Vector x;
Vector axis;
if(!icrt->getPose(x,axis))
fprintf(stderr, "Troubles in gettin the cartesian pose for %s", cm->partLabel);
//else
//fprintf(stderr, "got x=%s, o=%s\n", x.toString().c_str(), o.toString().c_str());
//converting to a Rotation Matrix
Matrix R = axis2dcm(axis);
//converting to a Euler angles
Vector eulerAngles = dcm2euler(R, 1);
//back to the Roation Matrix
//Matrix Rr = euler2dcm(eulerAngles);
//fprintf(stderr, "v: %s\n Res Matrix: %s\n", eulerAngles.toString().c_str(), (Rr-R).toString().c_str());
gboolean focus = false;
char buffer[40] = {'i', 'n', 'i', 't'};
for (int k = 0; k < NUMBER_OF_CARTESIAN_COORDINATES; k++)
{
if (k<3)
sprintf(buffer, "%.2f", x(k));
if (k>=3 && k<= 5)
sprintf(buffer, "%.1f", eulerAngles(k-3)*180/M_PI);
gtk_entry_set_text((GtkEntry*) entry[k], buffer);
//if changing orientation do not update all the three sliders
//if changing position update other sliders
if(k<3)
{
g_object_get((gpointer) sliderPosArray[k], "has-focus", &focus, NULL);
if(!focus)
gtk_range_set_value ((GtkRange *) (sliderPosArray[k]), x(k));
}
else
{
for (int i = 3; i < NUMBER_OF_CARTESIAN_COORDINATES; i++)
{
gboolean tmp;
g_object_get((gpointer) sliderPosArray[i], "has-focus", &tmp, NULL);
focus = focus || tmp;
}
if(!focus)
gtk_range_set_value ((GtkRange *) (sliderPosArray[k]), eulerAngles(k-3) * 180/M_PI);
}
}
return true;
}
static void toggle_tracking_mode(GtkWidget *box, gpointer user_data)
{
ICartesianControl *crt = (ICartesianControl *) user_data;
bool trck;
crt->getTrackingMode(&trck);
if (trck)
crt->setTrackingMode(false);
else
crt->setTrackingMode(true);
return;
}
void setStraightness(const double straightness)
{
ICartesianControl *icart;
action->getCartesianIF(icart);
Bottle options;
Bottle &straightOpt=options.addList();
straightOpt.addString("straightness");
straightOpt.addDouble(straightness);
icart->tweakSet(options);
}
virtual void threadRelease()
{
// we require an immediate stop
// before closing the client for safety reason
icart->stopControl();
// it's a good rule to restore the controller
// context as it was before opening the module
icart->restoreContext(startup_context_id);
client.close();
}
void limitTorsoPitch()
{
int axis=0; // pitch joint
double min, max;
// sometimes it may be helpful to reduce
// the range of variability of the joints;
// for example here we don't want the torso
// to lean out more than 30 degrees forward
// we keep the lower limit
icart->getLimits(axis,&min,&max);
icart->setLimits(axis,min,MAX_TORSO_PITCH);
}
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 updateModule()
{
if (imgOutPort.getOutputCount()>0)
{
if (ImageOf<PixelBgr> *pImgBgrIn=imgInPort.read(false))
{
Vector xa,oa;
iarm->getPose(xa,oa);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Vector v(4,0.0); v[3]=1.0;
Vector c=Ha*v;
v=0.0; v[0]=0.05; v[3]=1.0;
Vector x=Ha*v;
v=0.0; v[1]=0.05; v[3]=1.0;
Vector y=Ha*v;
v=0.0; v[2]=0.05; v[3]=1.0;
Vector z=Ha*v;
v=solution; v.push_back(1.0);
Vector t=Ha*v;
Vector pc,px,py,pz,pt;
int camSel=(eye=="left")?0:1;
igaze->get2DPixel(camSel,c,pc);
igaze->get2DPixel(camSel,x,px);
igaze->get2DPixel(camSel,y,py);
igaze->get2DPixel(camSel,z,pz);
igaze->get2DPixel(camSel,t,pt);
CvPoint point_c=cvPoint((int)pc[0],(int)pc[1]);
CvPoint point_x=cvPoint((int)px[0],(int)px[1]);
CvPoint point_y=cvPoint((int)py[0],(int)py[1]);
CvPoint point_z=cvPoint((int)pz[0],(int)pz[1]);
CvPoint point_t=cvPoint((int)pt[0],(int)pt[1]);
cvCircle(pImgBgrIn->getIplImage(),point_c,4,cvScalar(0,255,0),4);
cvCircle(pImgBgrIn->getIplImage(),point_t,4,cvScalar(255,0,0),4);
cvLine(pImgBgrIn->getIplImage(),point_c,point_x,cvScalar(0,0,255),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_y,cvScalar(0,255,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_z,cvScalar(255,0,0),2);
cvLine(pImgBgrIn->getIplImage(),point_c,point_t,cvScalar(255,255,255),2);
tip.clear();
tip.addInt(point_t.x);
tip.addInt(point_t.y);
imgOutPort.prepare()=*pImgBgrIn;
imgOutPort.write();
}
}
return true;
}
bool interruptModule()
{
imgLPortIn.interrupt();
imgRPortIn.interrupt();
igaze->stopControl();
iarm->stopControl();
return true;
}
virtual bool threadInit()
{
string name=rf.check("name",Value("faceTracker")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
int period=rf.check("period",Value(50)).asInt();
eye=rf.check("eye",Value("left")).asString().c_str();
arm=rf.check("arm",Value("right")).asString().c_str();
eyeDist=rf.check("eyeDist",Value(1.0)).asDouble();
holdoff=rf.check("holdoff",Value(3.0)).asDouble();
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze_client").c_str());
if (!clientGaze.open(optGaze))
return false;
clientGaze.view(igaze);
igaze->storeContext(&startup_gazeContext_id);
igaze->blockNeckRoll(0.0);
if (arm!="none")
{
Property optArm("(device cartesiancontrollerclient)");
optArm.put("remote",("/"+robot+"/cartesianController/"+arm+"_arm").c_str());
optArm.put("local",("/"+name+"/arm_client").c_str());
if (!clientArm.open(optArm))
return false;
clientArm.view(iarm);
iarm->storeContext(&startup_armContext_id);
}
portImgIn.open(("/"+name+"/img:i").c_str());
portImgOut.open(("/"+name+"/img:o").c_str());
portTopDown.open(("/"+name+"/topdown:i").c_str());
portSetFace.open(("/"+name+"/setFace:rpc").c_str());
if (!fd.init(rf.findFile("descriptor").c_str()))
{
fprintf(stdout,"Cannot load descriptor!\n");
return false;
}
Rand::init();
resetState();
armCmdState=0;
queuedFaceExprFlag=false;
setRate(period);
cvSetNumThreads(1);
t0=Time::now();
return true;
}
void approachTargetWithHand(const Vector &x, const Vector &o)
{
// we assume that only right hand is used
// and we approach slightly on the right side of the target
Vector pos(x);
pos[0] += 0.05;
pos[1] += 0.10;
pos[2] += 0.05;
iarm->goToPose(pos, o);
}
void home()
{
// these values should be loaded from a config file
// for now we just put them in the code
Vector x(3);
x[0] = -0.3;
x[1] = 0.4;
x[2] = 0.2;
iarm->goToPosition(x);
// look down for the homing position
look_down();
}
virtual void run()
{
t=Time::now();
generateTarget();
// go to the target :)
// (in streaming)
icart->goToPose(xd,od);
// some verbosity
printStatus();
}
bool read(ConnectionReader &connection)
{
Bottle data;
data.read(connection);
if ((data.size()>=2) && enabled)
{
Vector xa,oa;
iarm->getPose(xa,oa);
Vector xe,oe;
if (eye=="left")
igaze->getLeftEyePose(xe,oe);
else
igaze->getRightEyePose(xe,oe);
Matrix Ha=axis2dcm(oa);
Ha.setSubcol(xa,0,3);
Matrix He=axis2dcm(oe);
He.setSubcol(xe,0,3);
Matrix H=Prj*SE3inv(He)*Ha;
Vector p(2);
p[0]=data.get(0).asDouble();
p[1]=data.get(1).asDouble();
if (logPort.getOutputCount()>0)
{
Vector &log=logPort.prepare();
log=p;
for (int i=0; i<H.rows(); i++)
log=cat(log,H.getRow(i));
for (int i=0; i<Prj.rows(); i++)
log=cat(log,Prj.getRow(i));
for (int i=0; i<Ha.rows(); i++)
log=cat(log,Ha.getRow(i));
for (int i=0; i<He.rows(); i++)
log=cat(log,He.getRow(i));
logPort.write();
}
mutex.wait();
solver.addItem(p,H);
mutex.post();
}
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 close()
{
igaze->restoreContext(startup_context_gaze);
drvGaze.close();
iarm->restoreContext(startup_context_arm);
drvArm.close();
imgLPortIn.close();
imgRPortIn.close();
imgLPortOut.close();
imgRPortOut.close();
rpcPort.close();
return true;
}
void pickUp(char ch) {
int lr;
if(ch == 'l')
lr = 0;
else if(ch == 'r')
lr = 1;
int joints = 0;
armp[lr]->getAxes(&joints);
int i;
for(i = 10; i < joints; i++) {
armp[lr]->setRefSpeed(i, 10);
armp[lr]->setRefAcceleration(i, 50);
}
armp[lr]->setRefSpeed(5, 10);
armp[lr]->setRefAcceleration(5, 50);
disconnectCartesian('l');
disconnectCartesian('r');
armp[lr]->positionMove(5, -40);
armp[lr]->positionMove(7, 15);
waitUntilFinish(armp[lr]);
connectCartesian('l');
connectCartesian('r');
if(ch == 'l') {
arm->getPose(xd, od);
xd[0] -= 0.02;
xd[2] -= 0.06;
arm->goToPose(xd, od);
Time::delay(3);
arm->stopControl();
}
else if(ch == 'r') {
right_arm->getPose(xd, od);
xd[0] -= 0.02;
xd[2] -= 0.06;
right_arm->goToPose(xd, od);
Time::delay(3);
right_arm->stopControl();
}
disconnectCartesian('l');
disconnectCartesian('r');
for(i = 10; i < joints; i++) {
armp[lr]->positionMove(i, 80);
}
Time::delay(2);
connectCartesian('l');
connectCartesian('r');
}
void armRest()
{
if (arm!="none")
{
Vector x(3);
x[0]=REACH_REST_X;
x[1]=REACH_REST_Y;
x[2]=REACH_REST_Z;
if (arm=="left")
x[1]=-x[1];
iarm->goToPosition(x);
armCmdState=0;
}
}
void armTrack()
{
if (arm!="none")
{
if (ARMISTRACKING(armCmdState))
{
Vector fp;
igaze->getFixationPoint(fp);
if (fp[0]>REACH_X_MAX)
fp[0]=REACH_X_MAX;
fp[2]+=REACH_OFFS_Z;
iarm->goToPosition(fp);
fprintf(stdout,"Reaching for: (%.1f,%.1f,%.1f) m\n",fp[0],fp[1],fp[2]);
}
else
armCmdState++;
}
}
virtual void threadRelease()
{
armRest();
gazeRest();
igaze->restoreContext(startup_gazeContext_id);
clientGaze.close();
if (arm!="none")
{
iarm->restoreContext(startup_armContext_id);
clientArm.close();
}
portImgIn.interrupt();
portImgOut.interrupt();
portTopDown.interrupt();
portSetFace.interrupt();
portImgIn.close();
portImgOut.close();
portTopDown.close();
portSetFace.close();
}
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;
}
bool configure(ResourceFinder &rf)
{
std::string robotname = rf.check("robot",yarp::os::Value("icubSim")).asString();
Property options;
options.put("robot", robotname); // typically from the command line.
options.put("device", "remote_controlboard");
string s("/");
s += robotname;
s += "/right_arm/keyBabbling";
options.put("local", s.c_str());
s.clear();
s += "/";
s += robotname;
s += "/right_arm";
options.put("remote", s.c_str());
if (!dd.open(options)) {
cout << "Device not available. Here are the known devices:\n"<< endl;
cout << Drivers::factory().toString().c_str() << endl;;
return false;
}
portStreamer.open("/keyboardBabbling/stream:o");
portFromCart.open("/keyboardBabbling/stream:i");
portRpc.open("/keyboarBabbling/rpc");
attach(portRpc);
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote", "/" + robotname +"/cartesianController/right_arm");
optionCart.put("local","/keyboardBabbling/cartesian_client/right_arm");
if (!driverCart.open(optionCart))
return false;
bool ok;
ok = dd.view(pos);
ok &= dd.view(vel);
ok &= dd.view(pid);
ok &= dd.view(amp);
ok &= dd.view(armCtrlModeUsed);
ok &= dd.view(armImpUsed);
ok &= driverCart.view(armCart);
ok &= dd.view(ilimRight);
iCubFinger* fingerRight = new iCubFinger("right_index");
deque<IControlLimits*> limRight;
limRight.push_back(ilimRight);
fingerRight->alignJointsBounds(limRight);
if (!ok) {
cout << "Device not able to acquire views" << endl;
dd.close();
return false;
}
int jnts = 0;
pos->getAxes(&jnts);
printf("Working with %d axes\n", jnts);
vector<bool> mask;
mask.resize(jnts);
mask[0] = false;
mask[1] = false;
mask[2] = false;
mask[3] = false;
mask[4] = true;
mask[5] = false;
mask[6] = true;
mask[7] = false;
mask[8] = true;
mask[9] = true;
mask[10] = true;
mask[11] = true;
mask[12] = true;
mask[13] = true;
mask[14] = true;
mask[15] = true;
cout << "Closing the hand" << endl;
// closing the hand:
pos->positionMove(4, 30.0);
pos->positionMove(6, 10.0);
pos->positionMove(8, 10.0);
pos->positionMove(9, 17.0);
pos->positionMove(10, 170.0);
pos->positionMove(11, 0.0);
pos->positionMove(12, 0.0);
pos->positionMove(13, 90.0);
pos->positionMove(14, 180.0);
pos->positionMove(15, 180.0);
cout << "Waiting to be in initial position...";
bool motionDone=false;
while (!motionDone)
{
motionDone=true;
for (int i=0; i<jnts; i++)
{
if (mask[i])
{
bool jntMotionDone = false;
pos->checkMotionDone(i, &jntMotionDone);
motionDone &= jntMotionDone;
}
}
Time::yield(); // to avoid killing cpu
}
cout << "ok" << endl;
// wait for the hand to be in initial position
Matrix R=zeros(3,3);
R(0,0)=-1.0; R(1,1)=1; R(2,2)=-1.0;
orientation=dcm2axis(R);
// enable torso movements as well
// in order to enlarge the workspace
Vector dof;
armCart->getDOF(dof);
dof=1.0; dof[1]=0.0; // every dof but the torso roll
armCart->setDOF(dof,dof);
armCart->setTrajTime(1.0);
Vector initPos(3,0.0);
if (rf.check("rightHandInitial"))
{
Bottle *botPos = rf.find("rightHandInitial").asList();
initPos[0] = botPos->get(0).asDouble();
initPos[1] = botPos->get(1).asDouble();
initPos[2] = botPos->get(2).asDouble();
cout<<"Reaching initial position with right hand"<<initPos.toString(3,3).c_str()<<endl;
armCart->goToPoseSync(initPos,orientation);
armCart->waitMotionDone(0.1,3.0);
}
else
{
cout << "Cannot find the inital position" << endl << "closing module" << endl;
return false;
}
minY = rf.find("min").asDouble();
maxY = rf.find("max").asDouble();
gap = rf.find("gap").asDouble();
delay = rf.find("delay").asDouble();
thrMove = rf.find("threshold_move").asDouble();
yDivisions = rf.check("yDivisions",Value(10)).asInt();
Bottle* bsequenceToPlay = rf.find("sequence").asList();
if (bsequenceToPlay)
{
cout << "Using sequence, not random." << endl;
for (int i = 0; i < bsequenceToPlay->size(); i++)
sequenceToPlay.push_back(bsequenceToPlay->get(i).asInt());
indexInSequence = 0;
}
tempPos=initPos;
state=up;
forward = false;
timeBeginIdle = Time::now();
Rand::init();
return true;
}
bool updateModule()
{
//Always send the current position of the end effector
Vector toSend, toSend2;
armCart->getPose(toSend, toSend2);
portStreamer.write(toSend);
if (!forward)
{
bool done;
armCart->checkMotionDone(&done);
/*
if (!done)
return true;*/
if (state==idle)
{
if (Time::now() - timeBeginIdle > delay)
{
tempPos[2] += gap;
state = up;
timeBeginIdle = Time::now();
}
else
return true;
}
else if (state == up)
{
if (done || Time::now() - timeBeginIdle > thrMove)
{
if (sequenceToPlay.size() > 0)
{
int nextIndex = sequenceToPlay[indexInSequence];
if (nextIndex >= yDivisions)
{
cout << "The index in the sequence is larger than the number of division. Clamping." << endl;
nextIndex = yDivisions - 1;
}
tempPos[1] = minY + ((maxY - minY) / (double)yDivisions)*sequenceToPlay[indexInSequence];
indexInSequence++;
if (indexInSequence >= (int)sequenceToPlay.size())
indexInSequence = 0;
}
else
{
tempPos[1] = minY + ((maxY - minY) / (double)yDivisions)*Random::uniform(0, yDivisions);
}
state = side;
timeBeginIdle = Time::now();
}
}
else if (state == side)
{
if (done || Time::now() - timeBeginIdle > thrMove)
{
tempPos[2] -= gap;
state = down;
timeBeginIdle = Time::now();
}
}
else if (state == down)
{
if (done || Time::now() - timeBeginIdle > thrMove)
{
state = idle;
timeBeginIdle = Time::now();
}
}
armCart->goToPoseSync(tempPos,orientation);
printf("Going to (%s)\n",tempPos.toString(3,3).c_str());
}
else
{
Vector cartPos;
portFromCart.read(cartPos);
armCart->goToPoseSync(cartPos, orientation);
}
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();
}
}
}
virtual bool threadInit() {
/* Read in parameters from resource finder */
loadParams();
/* Start up gaze control client interface */
Property option("(device gazecontrollerclient)");
option.put("remote","/iKinGazeCtrl");
option.put("local","/client/gaze");
clientGazeCtrl.open(option);
gaze=NULL;
if (clientGazeCtrl.isValid()) {
clientGazeCtrl.view(gaze);
} else {
printf("could not initialize gaze control interface, failing...\n");
return false;
}
//set gaze control interface params
gaze->setNeckTrajTime(neckTT);
gaze->setEyesTrajTime(eyeTT);
gaze->bindNeckPitch(-30,30);
gaze->bindNeckYaw(-25,25);
gaze->bindNeckRoll(-10,10);
/* Start up arm cartesian control client interface */
Property optiona("(device cartesiancontrollerclient)");
if (armInUse) {
string tmpcrname = "/" + robotname + "/cartesianController/right_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/right_arm");
}
else {
string tmpcrname = "/" + robotname + "/cartesianController/left_arm";
optiona.put("remote",tmpcrname.c_str());
optiona.put("local","/cartesian_client/left_arm");
}
clientArmCart.open(optiona);
carm = NULL;
if (clientArmCart.isValid())
{
clientArmCart.view(carm);
} else {
printf("could not initialize arm cartesian control interface, failing...\n");
return false;
}
// set trajectory time
carm->setTrajTime(trajTime); //slow for safety
// get the torso dofs
Vector newDof, curDof;
carm->getDOF(curDof);
newDof=curDof;
//enable torso pitch and yaw, also wrist movements
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
newDof[7]=1;
newDof[8]=1;
newDof[9]=1;
// impose some restriction on the torso pitch
double tpmin, tpmax;
carm->getLimits(0,&tpmin,&tpmax);
carm->setLimits(0,tpmin,maxPitch);
// send the request for dofs reconfiguration
carm->setDOF(newDof,curDof);
string lname, rname;
Property doption;
doption.put("device", "remote_controlboard");
lname = "/"+name+"/torso"; rname = "/"+robotname+"/torso";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotTorso.open(doption);
if (!robotTorso.isValid()) {
printf("could not initialize torso control interface, failing...\n");
return false;
}
robotTorso.view(tPos); robotTorso.view(tEnc);
lname = "/"+name+"/head"; rname = "/"+robotname+"/head";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotHead.open(doption);
if (!robotHead.isValid()) {
printf("could not initialize head control interface, failing...\n");
return false;
}
robotHead.view(hPos); robotHead.view(hEnc);
lname = "/"+name+"/"+armname+"_arm"; rname = "/"+robotname+"/"+armname+"_arm";
doption.put("local", lname.c_str());
doption.put("remote", rname.c_str());
robotArm.open(doption);
if (!robotArm.isValid()) {
printf("could not initialize arm control interface, failing...\n");
return false;
}
robotArm.view(aPos); robotArm.view(aEnc);
for (int i=0; i<7; i++) {
aPos->setRefSpeed(i, 10.0);
}
/* Create and open ports */
cportL=new ClickPort(bfL);
string cplName="/"+name+"/clk:l";
cportL->open(cplName.c_str());
cportL->useCallback();
cportR=new ClickPort(bfR);
string cprName="/"+name+"/clk:r";
cportR->open(cprName.c_str());
cportR->useCallback();
susPort=new Port;
string suspName="/"+name+"/sus:o";
susPort->open(suspName.c_str());
return true;
}
virtual bool threadInit()
{
// open a client interface to connect to the cartesian server of the simulator
// we suppose that:
//
// 1 - the iCub simulator is running
// (launch: iCub_SIM)
//
// 2 - the cartesian server is running
// (launch: yarprobotinterface --context simCartesianControl)
//
// 3 - the cartesian solver for the left arm is running too
// (launch: iKinCartesianSolver --context simCartesianControl --part left_arm)
//
Property option("(device cartesiancontrollerclient)");
option.put("remote","/icubSim/cartesianController/left_arm");
option.put("local","/cartesian_client/left_arm");
if (!client.open(option))
return false;
// open the view
client.view(icart);
// latch the controller context in order to preserve
// it after closing the module
// the context contains the dofs status, the tracking mode,
// the resting positions, the limits and so on.
icart->storeContext(&startup_context_id);
// set trajectory time
icart->setTrajTime(1.0);
// get the torso dofs
Vector newDof, curDof;
icart->getDOF(curDof);
newDof=curDof;
// enable the torso yaw and pitch
// disable the torso roll
newDof[0]=1;
newDof[1]=0;
newDof[2]=1;
// send the request for dofs reconfiguration
icart->setDOF(newDof,curDof);
// impose some restriction on the torso pitch
limitTorsoPitch();
// print out some info about the controller
Bottle info;
icart->getInfo(info);
fprintf(stdout,"info = %s\n",info.toString().c_str());
// register the event, attaching the callback
icart->registerEvent(*this);
xd.resize(3);
od.resize(4);
return true;
}
