grasp
•All Data Structures Namespaces Functions Modules
precisionGrasp.cpp
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ilaria Gori
3  * email: ilaria.gori@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 #include "precisionGrasp.h"
18 
19 using namespace std;
20 using namespace yarp::os;
21 using namespace yarp::sig;
22 using namespace yarp::math;
23 using namespace yarp::dev;
24 using namespace iCub::iKin;
25 using namespace iCub::ctrl;
26 using namespace iCub::data3D;
27 using namespace iCub::grasp;
28 using namespace pcl::io;
29 using namespace pcl;
30 
31 /************************************************************************/
32 PrecisionGrasp::PrecisionGrasp() : cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
33  cloudxyz(new pcl::PointCloud<pcl::PointXYZ>),
34  normals (new pcl::PointCloud <pcl::Normal>)
35 {
36  path="";
37 
38  dont=false;
39  grasped=false;
40  visualize=true;
41  straight=false;
42  rightBlocked=false;
43  leftBlocked=false;
44  rightDisabled=false;
45  leftDisabled=false;
46  fromFileFinished=false;
47  filterCloud=true;
48  clicked=false;
49  writeCloud=false;
50  nFile=0;
51  counter=0;
52  bestCost=1e20;
53  bestManipulability=0.0;
54  offsetL.resize(3,0.0);
55  offsetR.resize(3,0.0);
56  home_p_l.resize(3,0.0); home_p_l[0]=-0.27; home_p_l[2]=0.1;
57  home_p_r.resize(3,0.0); home_p_r[0]=-0.27; home_p_r[2]=0.1;
58  home_o_l.resize(4,0.0);
59  home_o_r.resize(4,0.0);
60 
61  current_state=STATE_WAIT;
62  visualizationThread=new VisualizationThread(data);
63  psoThreadFitness1=new PsoThread();
64  psoThreadFitness2=new PsoThread();
65  psoThreadFitness3=new PsoThread();
66  psoThreadFitness4=new PsoThread();
67 }
68 
69 /************************************************************************/
70 bool PrecisionGrasp::configure(ResourceFinder &rf)
71 {
72  radiusSearch=rf.check("radiusSearch",Value(0.045)).asDouble();
73  handSize=rf.check("limit_finger_max",Value(0.08)).asDouble();
74  double limit_finger_min=rf.check("limit_finger_min",Value(0.02)).asDouble();
75  string name=rf.check("name",Value("precision-grasp")).asString().c_str();
76  string useFile=rf.check("fromFile",Value("false")).asString().c_str();
77  path=rf.find("path").asString().c_str();
78  sampling=rf.check("sampling",Value(0.01f)).asDouble();
79  alpha=rf.check("alpha",Value(0.9)).asDouble();
80  robot=rf.check("robot",Value("icub")).asString();
81  double phi_p=rf.check("phi_p",Value(0.001)).asDouble();
82  double phi_g=rf.check("phi_g",Value(0.001)).asDouble();
83  int iterations=rf.check("iterations",Value(2000)).asInt();
84  double hand_area=rf.check("hand_area",Value(0.00225)).asDouble();
85  outputFile=rf.find("outputFile").asString().c_str();
86  posx=rf.check("x",Value(0)).asInt();
87  posy=rf.check("y",Value(0)).asInt();
88  visualizationThread->setPosition(posx, posy);
89  sizex=rf.check("w",Value(320)).asInt();
90  sizey=rf.check("h",Value(240)).asInt();
91  visualizationThread->setSize(sizex, sizey);
92 
93  if (rf.check("disableRight"))
94  rightDisabled=true;
95  if (rf.check("disableLeft"))
96  leftDisabled=true;
97 
98  if (!openDevices())
99  return false;
100 
101  Property psoProperty;
102  psoProperty.put("particles",20);
103  psoProperty.put("omega",1.0);
104  psoProperty.put("phi_p",phi_p);
105  psoProperty.put("phi_g",phi_g);
106  psoProperty.put("iterations",iterations);
107  psoProperty.put("limit_finger_max",handSize);
108  psoProperty.put("limit_finger_min",limit_finger_min);
109  psoProperty.put("hand_area",hand_area);
110  psoProperty.put("dimension",9);
111 
112  psoThreadFitness1->open(psoProperty);
113  psoThreadFitness2->open(psoProperty);
114  psoThreadFitness3->open(psoProperty);
115  psoThreadFitness4->open(psoProperty);
116 
117  fromFile=(useFile=="true");
118 
119  if (fromFile)
120  {
121  path=rf.find("path").asString().c_str();
122  if (path=="")
123  return false;
124  }
125 
126  //Ports opening
127  rpc.open(("/"+name+"/rpc").c_str());
128  attach(rpc);
129 
130  depth2kin.open(("/"+name+"/depth2kin:o").c_str());
131 
132  toMatlab.open(("/"+name+"/matlab").c_str());
133 
134  if (!ikPort1r.open(("/"+name+"/ik1r:o").c_str()))
135  return false;
136 
137  if (!ikPort2r.open(("/"+name+"/ik2r:o").c_str()))
138  return false;
139 
140  if (!ikPort3r.open(("/"+name+"/ik3r:o").c_str()))
141  return false;
142 
143  if (!ikPort4r.open(("/"+name+"/ik4r:o").c_str()))
144  return false;
145 
146  if (!ikPort1l.open(("/"+name+"/ik1l:o").c_str()))
147  return false;
148 
149  if (!ikPort2l.open(("/"+name+"/ik2l:o").c_str()))
150  return false;
151 
152  if (!ikPort3l.open(("/"+name+"/ik3l:o").c_str()))
153  return false;
154 
155  if (!ikPort4l.open(("/"+name+"/ik4l:o").c_str()))
156  return false;
157 
158  if (!meshPort.open(("/"+name+"/mesh:i").c_str()))
159  return false;
160 
161  if (!reconstructionPort.open(("/"+name+"/reconstruction").c_str()))
162  return false;
163 
164  if (!areCmdPort.open(("/"+name+"/are/cmd:o").c_str()))
165  return false;
166 
167  if (fromFile)
168  {
169  if (!rightDisabled)
170  {
171  Network::connect(ikPort1r.getName().c_str(), "/handIKModule1/right/rpc");
172  Network::connect(ikPort2r.getName().c_str(), "/handIKModule2/right/rpc");
173  Network::connect(ikPort3r.getName().c_str(), "/handIKModule3/right/rpc");
174  Network::connect(ikPort4r.getName().c_str(), "/handIKModule4/right/rpc");
175  }
176  if (!leftDisabled)
177  {
178  Network::connect(ikPort1l.getName().c_str(), "/handIKModule1/left/rpc");
179  Network::connect(ikPort2l.getName().c_str(), "/handIKModule2/left/rpc");
180  Network::connect(ikPort3l.getName().c_str(), "/handIKModule3/left/rpc");
181  Network::connect(ikPort4l.getName().c_str(), "/handIKModule4/left/rpc");
182  }
183  }
184 
185  return true;
186 }
187 
188 /************************************************************************/
189 bool PrecisionGrasp::openDevices()
190 {
191  Property optArmRight,optArmLeft;
192  Property optTorso;
193 
194  string remoteTorsoName="/"+robot+"/torso";
195  optTorso.put("device", "remote_controlboard");
196  optTorso.put("remote",remoteTorsoName.c_str());
197  optTorso.put("local","/localTorso");
198 
199  robotTorso.open(optTorso);
200 
201  if (!robotTorso.isValid())
202  {
203  printf("Torso not available\n");
204  return false;
205  }
206 
207  robotTorso.view(limTorso);
208 
209  if (!rightDisabled)
210  {
211  Property optCtrlRight;
212  optCtrlRight.put("device","cartesiancontrollerclient");
213  optCtrlRight.put("remote",("/"+robot+"/cartesianController/right_arm").c_str());
214  optCtrlRight.put("local","/orientation/right/cartesianRight");
215 
216  if (!dCtrlRight.open(optCtrlRight))
217  {
218  fprintf(stdout, "Right Cartesian Interface is not open\n");
219  return false;
220  }
221 
222  dCtrlRight.view(iCtrlRight);
223  iCtrlRight->storeContext(&context_in_right);
224  yarp::sig::Vector dof;
225  iCtrlRight->getDOF(dof);
226  yarp::sig::Vector newDof=dof;
227  newDof[0]=1.0;
228  newDof[2]=1.0;
229 
230  iCtrlRight->setDOF(newDof,dof);
231  iCtrlRight->storeContext(&context_right);
232  iCtrlRight->restoreContext(context_in_right);
233 
234  string remoteArmNameRight="/"+robot+"/right_arm";
235  optArmRight.put("device", "remote_controlboard");
236  optArmRight.put("remote",remoteArmNameRight.c_str());
237  optArmRight.put("local","/localArm/right");
238 
239  robotArmRight.open(optArmRight);
240 
241  if (!robotArmRight.isValid())
242  {
243  printf("Right arm not available\n");
244  return false;
245  }
246 
247  robotArmRight.view(limArmRight);
248  robotArmRight.view(posRight);
249  robotArmRight.view(encRight);
250 
251  armRight=new iCubArm("right");
252  chainRight=armRight->asChain();
253 
254  chainRight->releaseLink(0);
255  chainRight->releaseLink(1);
256  chainRight->releaseLink(2);
257 
258  deque<IControlLimits*> limRight;
259  limRight.push_back(limTorso);
260  limRight.push_back(limArmRight);
261  armRight->alignJointsBounds(limRight);
262 
263  armRight->setAllConstraints(false);
264 
265  thetaMinRight.resize(10,0.0);
266  thetaMaxRight.resize(10,0.0);
267  for (unsigned int i=0; i< chainRight->getDOF(); i++)
268  {
269  thetaMinRight[i]=(*chainRight)(i).getMin();
270  thetaMaxRight[i]=(*chainRight)(i).getMax();
271  }
272  }
273 
274  if (!leftDisabled)
275  {
276  Property optCtrlLeft;
277  optCtrlLeft.put("device","cartesiancontrollerclient");
278  optCtrlLeft.put("remote",("/"+robot+"/cartesianController/left_arm").c_str());
279  optCtrlLeft.put("local","/orientation/left/cartesianLeft");
280 
281  if (!dCtrlLeft.open(optCtrlLeft))
282  {
283  fprintf(stdout, "Left Cartesian Interface is not open\n");
284  return false;
285  }
286 
287  dCtrlLeft.view(iCtrlLeft);
288  iCtrlLeft->storeContext(&context_in_left);
289  yarp::sig::Vector dof;
290  yarp::sig::Vector newDof;
291  iCtrlLeft->getDOF(dof);
292  newDof=dof;
293  newDof[0]=1.0;
294  newDof[2]=1.0;
295 
296  iCtrlLeft->setDOF(newDof,dof);
297  iCtrlLeft->storeContext(&context_left);
298  iCtrlLeft->restoreContext(context_in_left);
299 
300  string remoteArmNameLeft="/"+robot+"/left_arm";
301  optArmLeft.put("device", "remote_controlboard");
302  optArmLeft.put("remote",remoteArmNameLeft.c_str());
303  optArmLeft.put("local","/localArm/left");
304 
305  robotArmLeft.open(optArmLeft);
306 
307  if (!robotArmLeft.isValid())
308  {
309  printf("Left arm not available\n");
310  return false;
311  }
312 
313  robotArmLeft.view(limArmLeft);
314  robotArmLeft.view(posLeft);
315  robotArmLeft.view(encLeft);
316 
317  armLeft=new iCubArm("left");
318  chainLeft=armLeft->asChain();
319 
320  chainLeft->releaseLink(0);
321  chainLeft->releaseLink(1);
322  chainLeft->releaseLink(2);
323 
324  deque<IControlLimits*> limLeft;
325  limLeft.push_back(limTorso);
326  limLeft.push_back(limArmLeft);
327  armLeft->alignJointsBounds(limLeft);
328 
329  armLeft->setAllConstraints(false);
330 
331  thetaMinLeft.resize(10,0.0);
332  thetaMaxLeft.resize(10,0.0);
333  for (unsigned int i=0; i< chainLeft->getDOF(); i++)
334  {
335  thetaMinLeft[i]=(*chainLeft)(i).getMin();
336  thetaMaxLeft[i]=(*chainLeft)(i).getMax();
337  }
338  }
339 
340  return true;
341 }
342 
343 /************************************************************************/
344 string PrecisionGrasp::extractData(const yarp::os::Bottle &data, const int t)
345 {
346  Bottle &b=const_cast<Bottle&>(data);
347  string hand=b.find("hand").asString().c_str();
348  double cost=b.find("cost").asDouble();
349  Bottle *eeB=b.find("ee").asList();
350  fillVectorFromBottle(eeB,ee_tmp);
351  Bottle *oright=b.find("or").asList();
352  fillVectorFromBottle(oright,axis_angle_tmp);
353  Bottle *j=b.find("joints").asList();
354  fillVectorFromBottle(j,joints_tmp);
355  Bottle* combination=b.find("combination").asList();
356  fillVectorFromBottle(combination,combination_tmp);
357 
358  yarp::dev::ICartesianControl *iCtrl;
359  iCub::iKin::iCubArm *arm;
360  yarp::sig::Vector *thetaMin=&thetaMinRight;
361  yarp::sig::Vector *thetaMax=&thetaMaxRight;
362  q.resize(10,0.0);
363 
364  if (hand=="right")
365  {
366  iCtrl=iCtrlRight;
367  arm=armRight;
368  thetaMin=&thetaMinRight;
369  thetaMax=&thetaMaxRight;
370  }
371  else
372  {
373  iCtrl=iCtrlLeft;
374  arm=armLeft;
375  thetaMin=&thetaMinLeft;
376  thetaMax=&thetaMaxLeft;
377  }
378 
379  double manipulability=0.0;
380 
381  iCtrl->askForPose(ee_tmp,axis_angle_tmp,xdhat,odhat,q);
382 
383  q=q*M_PI/180.0;
384  arm->setAng(q);
385 
386  yarp::sig::Vector od(3);
387  od[0]=axis_angle_tmp[0]*axis_angle_tmp[3];
388  od[1]=axis_angle_tmp[1]*axis_angle_tmp[3];
389  od[2]=axis_angle_tmp[2]*axis_angle_tmp[3];
390 
391  yarp::sig::Vector odhattmp(3);
392  odhattmp[0]=odhat[0]*odhat[3];
393  odhattmp[1]=odhat[1]*odhat[3];
394  odhattmp[2]=odhat[2]*odhat[3];
395 
396  double xdist=norm(ee_tmp-xdhat);
397  double odist=norm(od-odhattmp);
398 
399  printf("\n\nodist %g xdist %g\n", odist, xdist);
400 
401  if (xdist>0.01 || odist>0.05)
402  {
403  manipulability=0.0;
404  }
405  else
406  {
407  Matrix jacobian=arm->GeoJacobian();
408  Matrix mulJac=jacobian*(jacobian.transposed());
409 
410  manipulability=sqrt(det(mulJac));
411 
412  double limits=0.0;
413  for (unsigned int k=0; k<thetaMin->size(); k++)
414  {
415  limits+=(q[k]-((*thetaMin)[k]))*((*thetaMax)[k]-q[k])/(((*thetaMax)[k]-(*thetaMin)[k])*((*thetaMax)[k]-(*thetaMin)[k]));
416  }
417 
418  manipulability*=(1-exp(-limits));
419  }
420 
421  printf("manipulability %g\n", manipulability);
422  printf("cost %g\n\n", cost);
423 
424  double toll=0.2;
425  if (cost<bestCost+toll && manipulability>(bestManipulability-toll))
426  {
427  string tag_0=b.get(0).asString().c_str();
428  if (tag_0=="IK1")
429  winner_ov_cones=ov_cones1;
430  else if (tag_0=="IK2")
431  winner_ov_cones=ov_cones2;
432  else if (tag_0=="IK3")
433  winner_ov_cones=ov_cones3;
434  else
435  winner_ov_cones=ov_cones4;
436  winner_joints=joints_tmp;
437  winner_ee=ee_tmp;
438  winner_axis=axis_angle_tmp;
439  winner_combination=combination_tmp;
440  winner_hand=hand;
441  winner_triplet=t;
442  winner_xdhat=xdhat;
443  winner_odhat=odhat;
444  bestCost=cost;
445  bestManipulability=manipulability;
446  }
447 
448  return hand;
449 }
450 
451 /************************************************************************/
452 void PrecisionGrasp::fillVectorFromBottle(const yarp::os::Bottle* b, yarp::sig::Vector &v)
453 {
454  v.resize(b->size());
455  for (int i=0; i<b->size(); i++)
456  v[i]=b->get(i).asDouble();
457 }
458 
459 /************************************************************************/
460 bool PrecisionGrasp::interruptModule()
461 {
462  eventRpc.signal();
463  Bottle &out=toMatlab.prepare();
464  out.clear();
465  out.addString("quit");
466  toMatlab.write();
467  toMatlab.interrupt();
468  ikPort1r.interrupt();
469  ikPort2r.interrupt();
470  ikPort3r.interrupt();
471  ikPort4r.interrupt();
472  ikPort1l.interrupt();
473  ikPort2l.interrupt();
474  ikPort3l.interrupt();
475  ikPort4l.interrupt();
476  depth2kin.interrupt();
477  areCmdPort.interrupt();
478  reconstructionPort.interrupt();
479  meshPort.interrupt();
480  rpc.interrupt();
481 
482  if (psoThreadFitness1->isRunning())
483  psoThreadFitness1->stop();
484 
485  if (psoThreadFitness2->isRunning())
486  psoThreadFitness2->stop();
487 
488  if (psoThreadFitness3->isRunning())
489  psoThreadFitness3->stop();
490 
491  if (psoThreadFitness4->isRunning())
492  psoThreadFitness4->stop();
493 
494  return true;
495 }
496 
497 /************************************************************************/
498 bool PrecisionGrasp::close()
499 {
500  ikPort1r.close();
501  ikPort2r.close();
502  ikPort3r.close();
503  ikPort4r.close();
504  ikPort1l.close();
505  ikPort2l.close();
506  ikPort3l.close();
507  ikPort4l.close();
508  areCmdPort.close();
509  depth2kin.close();
510  reconstructionPort.close();
511  meshPort.close();
512  rpc.close();
513  toMatlab.close();
514 
515  if (graspFileTrain.is_open())
516  graspFileTrain.close();
517 
518  if (visualizationThread->isRunning())
519  visualizationThread->stop();
520  delete visualizationThread;
521 
522  if (psoThreadFitness1->isRunning())
523  psoThreadFitness1->stop();
524  delete psoThreadFitness1;
525 
526  if (psoThreadFitness2->isRunning())
527  psoThreadFitness2->stop();
528  delete psoThreadFitness2;
529 
530  if (psoThreadFitness3->isRunning())
531  psoThreadFitness3->stop();
532  delete psoThreadFitness3;
533 
534  if (psoThreadFitness4->isRunning())
535  psoThreadFitness4->stop();
536  delete psoThreadFitness4;
537 
538  if (dCtrlRight.isValid())
539  {
540  iCtrlRight->restoreContext(context_in_right);
541  dCtrlRight.close();
542  }
543  if (dCtrlLeft.isValid())
544  {
545  iCtrlLeft->restoreContext(context_in_left);
546  dCtrlLeft.close();
547  }
548  if (robotArmRight.isValid())
549  robotArmRight.close();
550  if (robotArmLeft.isValid())
551  robotArmLeft.close();
552  if (robotTorso.isValid())
553  robotTorso.close();
554 
555  return true;
556 }
557 
558 /************************************************************************/
559 void PrecisionGrasp::filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered, bool second)
560 {
561  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_tmp (new pcl::PointCloud<pcl::PointXYZRGB>);
562  pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
563  sor.setInputCloud (cloud_in);
564  sor.setMeanK (cloud_in->size()/2);
565  sor.setStddevMulThresh (1.0);
566  sor.filter (*cloud_in_filtered);
567 }
568 
569 /************************************************************************/
570 void PrecisionGrasp::write(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in, const string &fileName)
571 {
572  ofstream myfile;
573  myfile.open (fileName.c_str());
574  /*myfile << "ply\n";
575  myfile << "format ascii 1.0\n";
576  myfile << "element vertex " << cloud_in->size() << "\n";
577  myfile << "property float x\n";
578  myfile << "property float y\n";
579  myfile << "property float z\n";
580  myfile << "property uchar diffuse_red\n";
581  myfile << "property uchar diffuse_green\n";
582  myfile << "property uchar diffuse_blue\n";
583  myfile << "end_header\n";*/
584 
585  for (int i=0; i<cloud_in->size(); i++)
586  {
587  int r=cloud_in->at(i).r;
588  int g=cloud_in->at(i).g;
589  int b=cloud_in->at(i).b;
590  myfile << cloud_in->at(i).x << " " << cloud_in->at(i).y << " " << cloud_in->at(i).z << " " << r << " " << g << " " << b << "\n";
591  }
592  myfile.close();
593 }
594 
595 /************************************************************************/
596 void PrecisionGrasp::write(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in, pcl::PointCloud <pcl::Normal>::Ptr n, const string &fileName)
597 {
598  ofstream myfile;
599  myfile.open (fileName.c_str());
600  for (int i=0; i<cloud_in->size(); i++)
601  {
602  myfile << cloud_in->at(i).x << " " << cloud_in->at(i).y << " " << cloud_in->at(i).z << " " << n->at(i).normal_x << " " << n->at(i).normal_y << " " << n->at(i).normal_z << "\n";
603  }
604  myfile.close();
605 }
606 
607 /************************************************************************/
608 void PrecisionGrasp::fromSurfaceMesh (const SurfaceMeshWithBoundingBox& msg)
609 {
610  cloud->clear();
611  cloudxyz->clear();
612 
613  for (size_t i = 0; i<msg.mesh.points.size(); ++i)
614  {
615  PointXYZRGB pointrgb;
616  pointrgb.x=msg.mesh.points.at(i).x;
617  pointrgb.y=msg.mesh.points.at(i).y;
618  pointrgb.z=msg.mesh.points.at(i).z;
619  if (i<msg.mesh.rgbColour.size())
620  {
621  int32_t rgb= msg.mesh.rgbColour.at(i).rgba;
622  pointrgb.rgba=rgb;
623  pointrgb.r = (rgb >> 16) & 0x0000ff;
624  pointrgb.g = (rgb >> 8) & 0x0000ff;
625  pointrgb.b = (rgb) & 0x0000ff;
626  }
627  else
628  pointrgb.rgb=0;
629 
630  pcl::PointXYZ point;
631  point.x=pointrgb.x;
632  point.y=pointrgb.y;
633  point.z=pointrgb.z;
634  cloudxyz->push_back(point);
635  cloud->push_back(pointrgb);
636  }
637 
638  boundingBox.setBoundingBox(msg.boundingBox);
639 }
640 
641 /************************************************************************/
642 bool PrecisionGrasp::fillCloudFromFile()
643 {
644  struct dirent *entry;
645  DIR *dp;
646 
647  dp = opendir(path.c_str());
648  if (dp == NULL)
649  {
650  perror("opendir");
651  return false;
652  }
653 
654  while((entry = readdir(dp)))
655  {
656  if (strcmp(entry->d_name, ".") == 0 || strcmp(entry->d_name, "..") == 0)
657  continue;
658  else
659  break;
660  }
661 
662  if (entry->d_name!=NULL)
663  {
664  pcl::PointCloud<PointXYZRGB>::Ptr cloud_in_rgb (new pcl::PointCloud<PointXYZRGB>);
665 
666  string root=path+"/";
667  string name=entry->d_name;
668  string file=root+name;
669 
670  if (loadPLYFile(file.c_str(), *cloud_in_rgb) == -1)
671  {
672  cout << "cannot read file \n";
673  return false;
674  }
675 
676  if (filterCloud)
677  {
678  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
679  filter(cloud_in_rgb,cloud_in_filtered);
680  addPointCloud(cloud_in_filtered);
681  }
682  else
683  addPointCloud(cloud_in_rgb);
684  }
685  closedir(dp);
686  return true;
687 }
688 
689 /************************************************************************/
690 void PrecisionGrasp::sampleClouds(pcl::PointCloud<pcl::PointXYZ>::Ptr c, pcl::PointCloud <pcl::Normal>::Ptr n)
691 {
692  pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree (128.0f);
693  octree.setInputCloud(cloudxyz);
694  octree.addPointsFromInputCloud();
695 
696  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled (new pcl::PointCloud<pcl::PointXYZ>);
697 
698  if (cloudxyz->size()<100)
699  sampling=0.01;
700 
701  pcl::VoxelGrid<pcl::PointXYZ> voxfilter;
702  voxfilter.setInputCloud (cloudxyz);
703  voxfilter.setLeafSize (sampling,sampling,sampling);
704  voxfilter.filter(*cloud_downsampled);
705 
706  yarp::sig::Vector vx,vy,vz; boundingBox.getAxis(vx,vy,vz);
707  double zdim=getZDim(vx,vy,vz);
708  double scale=10.0;
709  double threshold=center[2]-(zdim/scale);
710  if (zdim<(handSize/2))
711  threshold=center[2]-zdim;
712  /*double numberOfPoints=percentage*((double)cloud->size());
713  int factorI=(int)((double)cloud->size()/numberOfPoints);*/
714 
715  for (int i=0; i<cloud_downsampled->size(); i++)
716  {
717  int k=1;
718  std::vector<int> pointIdxNKNSearch;
719  std::vector<float> pointNKNSquaredDistance;
720 
721  int id;
722  if (octree.nearestKSearch (cloud_downsampled->at(i), k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
723  {
724  id=pointIdxNKNSearch[0];
725  if (normalPointingOut(normals->at(id),cloudxyz->at(id)) && cloudxyz->points.at(id).z>threshold)
726  {
727  c->push_back(cloudxyz->at(id));
728  n->push_back(normals->at(id));
729  }
730  }
731  }
732  printf("points %d\n", (int)c->size());
733 }
734 
735 /************************************************************************/
736 bool PrecisionGrasp::updateModule()
737 {
738  if ((fromFile && !fromFileFinished) || (current_state==STATE_ESTIMATE))
739  {
740  double totTime=Time::now();
741 
742  if (!rightDisabled)
743  {
744  iCtrlRight->deleteContext(current_context_right);
745  iCtrlRight->storeContext(&current_context_right);
746  iCtrlRight->restoreContext(context_right);
747  }
748  if (!leftDisabled)
749  {
750  iCtrlLeft->deleteContext(current_context_left);
751  iCtrlLeft->storeContext(&current_context_left);
752  iCtrlLeft->restoreContext(context_left);
753  }
754  if (fromFile)
755  {
756  if (!fillCloudFromFile())
757  return false;
758  boundingBox=MinimumBoundingBox::getMinimumBoundingBox(cloud);
759  }
760  else if (current_state==STATE_ESTIMATE)
761  {
762  SurfaceMeshWithBoundingBox *receivedMesh=meshPort.read(false);
763  if (receivedMesh!=NULL)
764  {
765  if (receivedMesh->mesh.points.size()==0)
766  {
767  current_state=STATE_WAIT;
768  return true;
769  }
770  fromSurfaceMesh(*receivedMesh);
771  }
772  else
773  return true;
774  }
775 
776  if (writeCloud)
777  write(cloud,outputFile);
778 
779  center=boundingBox.getCenter();
780  dim=boundingBox.getDim();
781  rotation=boundingBox.getOrientation();
782 
783  associateDim();
784 
785  //Normal Estimation
786  double timeNormals=Time::now();
787  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
788  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
789  normal_estimator.setSearchMethod (tree);
790  normal_estimator.setRadiusSearch (radiusSearch);
791  normal_estimator.setInputCloud (cloudxyz);
792  normal_estimator.compute (*normals);
793  printf("Time for normal estimation %g\n", Time::now()-timeNormals);
794 
795  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_toEvaluate (new pcl::PointCloud<pcl::PointXYZ>);
796  pcl::PointCloud <pcl::Normal>::Ptr normals_toEvaluate (new pcl::PointCloud <pcl::Normal>);
797 
798  sampleClouds(cloud_toEvaluate,normals_toEvaluate);
799 
800  //write(cloud_toEvaluate,normals_toEvaluate,"C:/Users/Utente/Desktop/points.txt");
801  //readData(cloud_toEvaluate,normals_toEvaluate);
802 
803  if (visualize)
804  {
805  if (!fromFile || (fromFile && !fromFileFinished))
806  {
807  data.cloud=*cloud;
808  data.normals=*normals;
809  data.sampled_cloud=*cloud_toEvaluate;
810  data.boundingBox=boundingBox;
811  visualizationThread->start();
812  }
813  }
814 
815  bool done=false;
816  //double alphaToUse=alpha;
817  double alphaToUse=0.8;
818  double t=Time::now();
819  psoThreadFitness1->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,0);
820  psoThreadFitness2->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,1);
821  psoThreadFitness3->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,2);
822  psoThreadFitness4->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,3);
823  while(!done)
824  {
825  while(!psoThreadFitness1->checkDone() || !psoThreadFitness2->checkDone() || !psoThreadFitness3->checkDone() || !psoThreadFitness4->checkDone())
826  Time::delay(0.01);
827 
828  Bottle req; req.clear();
829  if (psoThreadFitness1->isSuccessful())
830  {
831  done=true;
832  ContactPoints triplet1=psoThreadFitness1->getBestTriplet();
833  contacts_r1.clear();
834  contacts_r1.push_back(triplet1.c1); contacts_r1.push_back(triplet1.c2); contacts_r1.push_back(triplet1.c3);
835  normals_r1.clear();
836  normals_r1.push_back(triplet1.n1); normals_r1.push_back(triplet1.n2); normals_r1.push_back(triplet1.n3);
837 
838  ov_cones1=triplet1.ov_cones;
839 
840  req=prepareData(triplet1,1);
841  if (ikPort1r.getOutputCount()>0 && !rightBlocked)
842  {
843  ikPort1r.write(req);
844  counter+=1;
845  }
846  if (ikPort1l.getOutputCount()>0 && !leftBlocked)
847  {
848  ikPort1l.write(req);
849  counter+=1;
850  }
851  }
852  if (psoThreadFitness2->isSuccessful())
853  {
854  done=true;
855  ContactPoints triplet2=psoThreadFitness2->getBestTriplet();
856  contacts_r2.clear();
857  contacts_r2.push_back(triplet2.c1); contacts_r2.push_back(triplet2.c2); contacts_r2.push_back(triplet2.c3);
858  normals_r2.clear();
859  normals_r2.push_back(triplet2.n1); normals_r2.push_back(triplet2.n2); normals_r2.push_back(triplet2.n3);
860 
861  ov_cones2=triplet2.ov_cones;
862 
863  req.clear();
864  req=prepareData(triplet2,2);
865  if (ikPort2r.getOutputCount()>0 && !rightBlocked)
866  {
867  ikPort2r.write(req);
868  counter+=1;
869  }
870  if (ikPort2l.getOutputCount()>0 && !leftBlocked)
871  {
872  ikPort2l.write(req);
873  counter+=1;
874  }
875  }
876  if (psoThreadFitness3->isSuccessful())
877  {
878  done=true;
879  ContactPoints triplet3=psoThreadFitness3->getBestTriplet();
880  contacts_r3.clear();
881  contacts_r3.push_back(triplet3.c1); contacts_r3.push_back(triplet3.c2); contacts_r3.push_back(triplet3.c3);
882  normals_r3.clear();
883  normals_r3.push_back(triplet3.n1); normals_r3.push_back(triplet3.n2); normals_r3.push_back(triplet3.n3);
884 
885  ov_cones3=triplet3.ov_cones;
886 
887  req.clear();
888  req=prepareData(triplet3,3);
889  if (ikPort3r.getOutputCount()>0 && !rightBlocked)
890  {
891  ikPort3r.write(req);
892  counter+=1;
893  }
894  if (ikPort3l.getOutputCount()>0 && !leftBlocked)
895  {
896  ikPort3l.write(req);
897  counter+=1;
898  }
899  }
900  if (psoThreadFitness4->isSuccessful())
901  {
902  done=true;
903  ContactPoints triplet4=psoThreadFitness4->getBestTriplet();
904 
905  contacts_r4.clear();
906  contacts_r4.push_back(triplet4.c1); contacts_r4.push_back(triplet4.c2); contacts_r4.push_back(triplet4.c3);
907  normals_r4.clear();
908  normals_r4.push_back(triplet4.n1); normals_r4.push_back(triplet4.n2); normals_r4.push_back(triplet4.n3);
909 
910  ov_cones4=triplet4.ov_cones;
911 
912  req.clear();
913  req=prepareData(triplet4,4);
914  if (ikPort4r.getOutputCount()>0 && !rightBlocked)
915  {
916  ikPort4r.write(req);
917  counter+=1;
918  }
919  if (ikPort4l.getOutputCount()>0 && !leftBlocked)
920  {
921  ikPort4l.write(req);
922  counter+=1;
923  }
924  }
925 
926  if ((!psoThreadFitness1->isSuccessful()) && (!psoThreadFitness2->isSuccessful()) && (!psoThreadFitness3->isSuccessful()) && (!psoThreadFitness4->isSuccessful()) && alphaToUse<=0.9)
927  {
928  alphaToUse+=0.1;
929  psoThreadFitness1->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,0);
930  psoThreadFitness2->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,1);
931  psoThreadFitness3->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,2);
932  psoThreadFitness4->setData(cloud_toEvaluate,normals_toEvaluate,alphaToUse,3);
933  }
934  else if ((!psoThreadFitness1->isSuccessful()) && (!psoThreadFitness2->isSuccessful()) && (!psoThreadFitness3->isSuccessful()) && (!psoThreadFitness4->isSuccessful()) && alphaToUse>0.9)
935  {
936  done=true;
937  grasped=false;
938  printf("Didn't find any triplet\n");
939  eventRpc.signal();
940  current_state=STATE_WAIT;
941  if (fromFile)
942  fromFileFinished=true;
943  return true;
944  }
945  }
946 
947  printf("Time %g\n", Time::now()-t);
948 
949  current_state=STATE_IK;
950 
951  if (!dont)
952  dont=false;
953 
954  if (fromFile)
955  fromFileFinished=true;
956  }
957 
958  if (current_state==STATE_GRASP)
959  {
960  mutex.wait();
961  askToGrasp();
962  mutex.post();
963  eventRpc.signal();
964  current_state=STATE_WAIT;
965  }
966 
967  return true;
968 }
969 
970 /************************************************************************/
971 void PrecisionGrasp::associateDim()
972 {
973  yarp::sig::Vector x,y,z;
974  boundingBox.getAxis(x,y,z);
975 
976  yarp::sig::Vector dim_tmp(3,0.0);
977 
978  yarp::sig::Vector tmpx(3,0.0);
979  tmpx[0]=1.0;
980 
981  yarp::sig::Vector tmpy(3,0.0);
982  tmpy[1]=1.0;
983 
984  yarp::sig::Vector tmpz(3,0.0);
985  tmpx[2]=1.0;
986 
987  double dotx1=abs(dot(x,tmpx)); double doty1=abs(dot(x,tmpy));
988  double dotx2=abs(dot(y,tmpx)); double doty2=abs(dot(y,tmpy));
989  double dotx3=abs(dot(z,tmpx)); double doty3=abs(dot(z,tmpy));
990 
991  if (dotx1<dotx2 && dotx1<dotx3)
992  {
993  dim_tmp[0]=dim[0];
994  if (doty2<doty3)
995  {
996  dim_tmp[1]=dim[1];
997  dim_tmp[2]=dim[2];
998  }
999  else if (doty2>doty3)
1000  {
1001  dim_tmp[2]=dim[1];
1002  dim_tmp[1]=dim[2];
1003  }
1004  }
1005  else if (dotx2<dotx1 && dotx2<dotx3)
1006  {
1007  dim_tmp[0]=dim[1];
1008  if (doty1<doty3)
1009  {
1010  dim_tmp[1]=dim[0];
1011  dim_tmp[2]=dim[2];
1012  }
1013  else if (doty1>doty3)
1014  {
1015  dim_tmp[1]=dim[2];
1016  dim_tmp[2]=dim[0];
1017  }
1018  }
1019  else
1020  {
1021  dim_tmp[0]=dim[2];
1022  if (doty1<doty2)
1023  {
1024  dim_tmp[1]=dim[0];
1025  dim_tmp[2]=dim[1];
1026  }
1027  else if (doty1>doty2)
1028  {
1029  dim_tmp[1]=dim[1];
1030  dim_tmp[2]=dim[0];
1031  }
1032  }
1033 
1034  dim=dim_tmp;
1035 }
1036 
1037 /************************************************************************/
1038 void PrecisionGrasp::readData(pcl::PointCloud<pcl::PointXYZ>::Ptr c, pcl::PointCloud <pcl::Normal>::Ptr n)
1039 {
1040  ifstream infile("C:\\Users\\Utente\\MATLABdata\\points\\points1.txt");
1041  double c1,c2,c3,n1,n2,n3;
1042  while(infile >> c1 >> c2 >> c3 >> n1 >> n2 >> n3)
1043  {
1044  pcl::PointXYZ p(c1,c2,c3);
1045  c->push_back(p);
1046  pcl::Normal normal(n1,n2,n3);
1047  n->push_back(normal);
1048  }
1049 }
1050 
1051 /************************************************************************/
1052 double PrecisionGrasp::getZDim(const yarp::sig::Vector &vx, const yarp::sig::Vector &vy, const yarp::sig::Vector &vz)
1053 {
1054  yarp::sig::Vector z(3,0.0); z[2]=1.0;
1055 
1056  double cx=abs(dot(vx,z));
1057  double cy=abs(dot(vy,z));
1058  double cz=abs(dot(vz,z));
1059 
1060  int index=-1;
1061  if (cx>cy && cx>cz)
1062  return norm(vx);
1063  else if (cy>cz)
1064  return norm(vy);
1065  else
1066  return norm(vz);
1067 }
1068 
1069 /************************************************************************/
1070 yarp::os::Bottle PrecisionGrasp::prepareData(const iCub::grasp::ContactPoints &triplet, const int c)
1071 {
1072  Bottle res;
1073  if (c==1)
1074  res.addString("IK1");
1075  else if (c==2)
1076  res.addString("IK2");
1077  else if (c==3)
1078  res.addString("IK3");
1079  else
1080  res.addString("IK4");
1081  Bottle &centerB=res.addList();
1082  centerB.addString("center");
1083  Bottle &center_coord=centerB.addList();
1084  for (int i=0; i<center.size(); i++)
1085  center_coord.addDouble(center[i]);
1086  Bottle &dimB=res.addList();
1087  dimB.addString("dim");
1088  Bottle &dim_coord=dimB.addList();
1089  for (int i=0; i<dim.size(); i++)
1090  dim_coord.addDouble(dim[i]);
1091  Bottle &c1=res.addList();
1092  c1.addString("c1");
1093  Bottle &c1_coord=c1.addList();
1094  for (int i=0; i<triplet.c1.size(); i++)
1095  c1_coord.addDouble(triplet.c1[i]);
1096  Bottle &c2=res.addList();
1097  c2.addString("c2");
1098  Bottle &c2_coord=c2.addList();
1099  for (int i=0; i<triplet.c2.size(); i++)
1100  c2_coord.addDouble(triplet.c2[i]);
1101  Bottle &c3=res.addList();
1102  c3.addString("c3");
1103  Bottle &c3_coord=c3.addList();
1104  for (int i=0; i<triplet.c3.size(); i++)
1105  c3_coord.addDouble(triplet.c3[i]);
1106  Bottle &n1=res.addList();
1107  n1.addString("n1");
1108  Bottle &n1_coord=n1.addList();
1109  for (int i=0; i<triplet.n1.size(); i++)
1110  n1_coord.addDouble(triplet.n1[i]);
1111  Bottle &n2=res.addList();
1112  n2.addString("n2");
1113  Bottle &n2_coord=n2.addList();
1114  for (int i=0; i<triplet.n2.size(); i++)
1115  n2_coord.addDouble(triplet.n2[i]);
1116  Bottle &n3=res.addList();
1117  n3.addString("n3");
1118  Bottle &n3_coord=n3.addList();
1119  for (int i=0; i<triplet.n3.size(); i++)
1120  n3_coord.addDouble(triplet.n3[i]);
1121  Bottle &rot=res.addList();
1122  rot.addString("rot");
1123  Bottle &rot_mat=rot.addList();
1124  for (int i=0; i<rotation.rows(); i++)
1125  for (int j=0; j<rotation.cols(); j++)
1126  rot_mat.addDouble(rotation(i,j));
1127  return res;
1128 }
1129 
1130 /************************************************************************/
1131 void PrecisionGrasp::askToGrasp()
1132 {
1133  Bottle b,reply;
1134  b.clear(); reply.clear();
1135  b.addString("getPoint");
1136  b.addString(winner_hand.c_str());
1137  b.addDouble(winner_ee[0]);
1138  b.addDouble(winner_ee[1]);
1139  b.addDouble(winner_ee[2]);
1140 
1141  depth2kin.write(b,reply);
1142 
1143  winner_ee[0]=reply.get(1).asDouble();
1144  winner_ee[1]=reply.get(2).asDouble();
1145  winner_ee[2]=reply.get(3).asDouble();
1146 
1147  if (winner_hand=="right")
1148  {
1149  printf("winner %s\n", winner_ee.toString().c_str());
1150  printf("offset %s\n", offsetR.toString().c_str());
1151  winner_ee+=offsetR;
1152  }
1153  else
1154  {
1155  printf("winner %s\n", winner_ee.toString().c_str());
1156  printf("offset %s\n", offsetL.toString().c_str());
1157  winner_ee+=offsetL;
1158  }
1159 
1160  reply.clear();
1161  Bottle cmd;
1162  cmd.addString("pgrasp");
1163  Bottle &point=cmd.addList();
1164  point.addDouble(winner_ee[0]);
1165  point.addDouble(winner_ee[1]);
1166  point.addDouble(winner_ee[2]);
1167  point.addDouble(winner_axis[0]);
1168  point.addDouble(winner_axis[1]);
1169  point.addDouble(winner_axis[2]);
1170  point.addDouble(winner_axis[3]);
1171  cmd.addString(winner_hand.c_str());
1172 
1173  Bottle &bJoints=cmd.addList();
1174  bJoints.addString("joints");
1175  Bottle &pos=bJoints.addList();
1176 
1177  yarp::sig::Vector joints(9);
1178  joints[0]=winner_joints[3]*180.0/M_PI;
1179  joints[1]=winner_joints[0]*180.0/M_PI;
1180  joints[2]=winner_joints[1]*180.0/M_PI;
1181  joints[3]=winner_joints[2]*180.0/M_PI;
1182  joints[4]=winner_joints[4]*180.0/M_PI;
1183  joints[5]=winner_joints[5]*180.0/M_PI;
1184  joints[6]=winner_joints[6]*180.0/M_PI;
1185  joints[7]=winner_joints[7]*180.0/M_PI;
1186  joints[8]=0.0;
1187 
1188  for (size_t i=0; i<joints.size(); i++)
1189  pos.addDouble(joints[i]);
1190 
1191  yarp::sig::Vector speed(joints.size(),40.0);
1192 
1193  Bottle &bVels=cmd.addList();
1194  bVels.addString("vels");
1195  Bottle &v=bVels.addList();
1196  for (size_t i=0; i<speed.size(); i++)
1197  v.addDouble(speed[i]);
1198 
1199  Bottle &bTols=cmd.addList();
1200  bTols.addString("tols");
1201  Bottle &t=bTols.addList();
1202  t.addDouble(20.0);
1203  for (int i=0; i<7; i++)
1204  t.addDouble(10.0);
1205  t.addDouble(20.0);
1206 
1207  Bottle &bThresh=cmd.addList();
1208  bThresh.addString("thres");
1209  Bottle &th=bThresh.addList();
1210  for (int i=0; i<5; i++)
1211  th.addDouble(120.0);
1212 
1213  Bottle &tmo=cmd.addList();
1214  tmo.addString("tmo");
1215  tmo.addDouble(2.0);
1216 
1217  if (areCmdPort.getOutputCount()>0)
1218  {
1219  areCmdPort.write(cmd,reply);
1220  if (reply.get(0).asString()=="ack")
1221  grasped=true;
1222  }
1223  readyToGrasp=false;
1224  /*ICartesianControl *iCtrl;
1225  IEncoders *encs;
1226  IPositionControl *pos;
1227  int curr_context;
1228  int context;
1229  yarp::sig::Vector offset;
1230  yarp::sig::Vector home_p;
1231  yarp::sig::Vector home_o;
1232  if (winner_hand=="right")
1233  {
1234  iCtrl=iCtrlRight;
1235  encs=encRight;
1236  pos=posRight;
1237  curr_context=current_context_right;
1238  context=context_right;
1239  offset=offsetR;
1240  home_p=home_p_r;
1241  home_o=home_o_r;
1242  }
1243  else
1244  {
1245  iCtrl=iCtrlLeft;
1246  encs=encLeft;
1247  pos=posLeft;
1248  curr_context=current_context_left;
1249  context=context_left;
1250  offset=offsetL;
1251  home_p=home_p_l;
1252  home_o=home_o_l;
1253  }
1254 
1255  iCtrl->deleteContext(curr_context);
1256  iCtrl->storeContext(&curr_context);
1257  iCtrl->restoreContext(context);
1258  winner_ee+=offset;
1259  iCtrl->goToPoseSync(winner_ee, winner_axis);
1260  iCtrl->waitMotionDone();
1261 
1262  yarp::sig::Vector target(16);
1263  encs->getEncoders(target.data());
1264  target[7]=winner_joints[3]*180.0/M_PI;
1265  target[8]=winner_joints[0]*180.0/M_PI;
1266  target[9]=winner_joints[1]*180.0/M_PI;
1267  target[10]=winner_joints[2]*180.0/M_PI;
1268  target[11]=winner_joints[4]*180.0/M_PI;
1269  target[12]=winner_joints[5]*180.0/M_PI;
1270  target[13]=winner_joints[6]*180.0/M_PI;
1271  target[14]=winner_joints[7]*180.0/M_PI;
1272 
1273  yarp::sig::Vector speed(target.size(),100.0);
1274  pos->positionMove(target.data());
1275  yarp::sig::Vector encoders(16);
1276  bool done=false;
1277  double t=Time::now();
1278  while (!done)
1279  {
1280  encoders=encs->getEncoders(encoders.data());
1281  if ((norm(encoders-target)<1e-01) || (Time::now()-t)>5.0)
1282  done=true;
1283  }
1284 
1285  Time::delay(1.5);
1286 
1287  winner_ee[2]+=0.1;
1288  iCtrl->goToPoseSync(home_p, home_o);
1289  iCtrl->waitMotionDone();
1290 
1291  iCtrl->restoreContext(curr_context);*/
1292 }
1293 
1294 /************************************************************************/
1295 void PrecisionGrasp::fillBottleFromVector(const yarp::sig::Vector &vect, yarp::os::Bottle *b)
1296 {
1297  for (unsigned int i=0; i<vect.size(); i++)
1298  b->addDouble(vect[i]);
1299 }
1300 
1301 /************************************************************************/
1302 void PrecisionGrasp::writeBestSolution()
1303 {
1304  std::vector<yarp::sig::Vector> contacts_r;
1305  std::vector<yarp::sig::Vector> normals_r;
1306 
1307  if (winner_triplet==1)
1308  {
1309  contacts_r=contacts_r1;
1310  normals_r=normals_r1;
1311  }
1312  else if (winner_triplet==2)
1313  {
1314  contacts_r=contacts_r2;
1315  normals_r=normals_r2;
1316  }
1317  else if (winner_triplet==3)
1318  {
1319  contacts_r=contacts_r3;
1320  normals_r=normals_r3;
1321  }
1322  else
1323  {
1324  contacts_r=contacts_r4;
1325  normals_r=normals_r4;
1326  }
1327 
1328  Bottle &out=toMatlab.prepare();
1329  out.clear();
1330  out.addString("best");
1331  Bottle &res=out.addList();
1332  Bottle &joints=res.addList();
1333  joints.addString("joints");
1334  Bottle &jointsVal=joints.addList();
1335  fillBottleFromVector(winner_joints, &jointsVal);
1336 
1337  Bottle &ee=res.addList();
1338  ee.addString("ee");
1339  Bottle &eeVal=ee.addList();
1340  fillBottleFromVector(winner_ee, &eeVal);
1341 
1342  Bottle &xdes=res.addList();
1343  xdes.addString("xdhat");
1344  Bottle &xdesVal=xdes.addList();
1345  fillBottleFromVector(winner_xdhat, &xdesVal);
1346 
1347  Bottle &axisangle=res.addList();
1348  axisangle.addString("axisangle");
1349  Bottle &axisangleVal=axisangle.addList();
1350  fillBottleFromVector(winner_axis, &axisangleVal);
1351 
1352  Bottle &odes=res.addList();
1353  odes.addString("odhat");
1354  Bottle &odesVal=odes.addList();
1355  fillBottleFromVector(winner_odhat, &odesVal);
1356 
1357  Bottle &c=res.addList();
1358  c.addString("center");
1359  Bottle &centerValue=c.addList();
1360  fillBottleFromVector(center, &centerValue);
1361 
1362  Bottle &d=res.addList();
1363  d.addString("dim");
1364  Bottle &dimValue=d.addList();
1365  fillBottleFromVector(dim, &dimValue);
1366 
1367  Bottle &c1=res.addList();
1368  c1.addString("c1");
1369  Bottle &c1Value=c1.addList();
1370  c1Value.addDouble(contacts_r.at(winner_combination[0])[0]);
1371  c1Value.addDouble(contacts_r.at(winner_combination[0])[1]);
1372  c1Value.addDouble(contacts_r.at(winner_combination[0])[2]);
1373 
1374  Bottle &c2=res.addList();
1375  c2.addString("c2");
1376  Bottle &c2Value=c2.addList();
1377  c2Value.addDouble(contacts_r.at(winner_combination[1])[0]);
1378  c2Value.addDouble(contacts_r.at(winner_combination[1])[1]);
1379  c2Value.addDouble(contacts_r.at(winner_combination[1])[2]);
1380 
1381  Bottle &c3=res.addList();
1382  c3.addString("c3");
1383  Bottle &c3Value=c3.addList();
1384  c3Value.addDouble(contacts_r.at(winner_combination[2])[0]);
1385  c3Value.addDouble(contacts_r.at(winner_combination[2])[1]);
1386  c3Value.addDouble(contacts_r.at(winner_combination[2])[2]);
1387 
1388  Bottle &n1=res.addList();
1389  n1.addString("n1");
1390  Bottle &n1Value=n1.addList();
1391  n1Value.addDouble(normals_r.at(winner_combination[0])[0]);
1392  n1Value.addDouble(normals_r.at(winner_combination[0])[1]);
1393  n1Value.addDouble(normals_r.at(winner_combination[0])[2]);
1394 
1395  Bottle &n2=res.addList();
1396  n2.addString("n2");
1397  Bottle &n2Value=n2.addList();
1398  n2Value.addDouble(normals_r.at(winner_combination[1])[0]);
1399  n2Value.addDouble(normals_r.at(winner_combination[1])[1]);
1400  n2Value.addDouble(normals_r.at(winner_combination[1])[2]);
1401 
1402  Bottle &n3=res.addList();
1403  n3.addString("n3");
1404  Bottle &n3Value=n3.addList();
1405  n3Value.addDouble(normals_r.at(winner_combination[2])[0]);
1406  n3Value.addDouble(normals_r.at(winner_combination[2])[1]);
1407  n3Value.addDouble(normals_r.at(winner_combination[2])[2]);
1408 
1409  Bottle &r=res.addList();
1410  r.addString("rotmat");
1411  Bottle &rotValue=r.addList();
1412  for (int i=0; i<rotation.rows(); i++)
1413  {
1414  for (int j=0; j<rotation.cols(); j++)
1415  {
1416  rotValue.addDouble(rotation(i,j));
1417  }
1418  }
1419 
1420  Bottle &h=res.addList();
1421  h.addString("hand");
1422  Bottle &hValue=h.addList();
1423  if (winner_hand=="right")
1424  hValue.addString("r");
1425  else
1426  hValue.addString("l");
1427 
1428  Bottle &cl=res.addList();
1429  cl.addString("cloud");
1430  Bottle &clValue=cl.addList();
1431  for (int i=0; i<cloud->size(); i++)
1432  {
1433  Bottle &point=clValue.addList();
1434  point.addDouble(cloud->at(i).x);
1435  point.addDouble(cloud->at(i).y);
1436  point.addDouble(cloud->at(i).z);
1437  point.addDouble(cloud->at(i).r);
1438  point.addDouble(cloud->at(i).g);
1439  point.addDouble(cloud->at(i).b);
1440  }
1441  if (toMatlab.getOutputCount()>0)
1442  toMatlab.writeStrict();
1443 }
1444 
1445 /************************************************************************/
1446 void PrecisionGrasp::writeToMatlab(const std::vector<yarp::sig::Vector> &contacts_r, const std::vector<yarp::sig::Vector> &normals_r, const std::string &hand)
1447 {
1448  Bottle &out=toMatlab.prepare();
1449  out.clear();
1450  Bottle &res=out.addList();
1451  Bottle &joints=res.addList();
1452  joints.addString("joints");
1453  Bottle &jointsVal=joints.addList();
1454  fillBottleFromVector(joints_tmp, &jointsVal);
1455 
1456  Bottle &ee=res.addList();
1457  ee.addString("ee");
1458  Bottle &eeVal=ee.addList();
1459  fillBottleFromVector(ee_tmp, &eeVal);
1460 
1461  Bottle &axisangle=res.addList();
1462  axisangle.addString("axisangle");
1463  Bottle &axisangleVal=axisangle.addList();
1464  fillBottleFromVector(axis_angle_tmp, &axisangleVal);
1465 
1466  Bottle &c=res.addList();
1467  c.addString("center");
1468  Bottle &centerValue=c.addList();
1469  fillBottleFromVector(center, &centerValue);
1470 
1471  Bottle &d=res.addList();
1472  d.addString("dim");
1473  Bottle &dimValue=d.addList();
1474  fillBottleFromVector(dim, &dimValue);
1475 
1476  Bottle &c1=res.addList();
1477  c1.addString("c1");
1478  Bottle &c1Value=c1.addList();
1479  c1Value.addDouble(contacts_r.at(combination_tmp[0])[0]);
1480  c1Value.addDouble(contacts_r.at(combination_tmp[0])[1]);
1481  c1Value.addDouble(contacts_r.at(combination_tmp[0])[2]);
1482 
1483  Bottle &c2=res.addList();
1484  c2.addString("c2");
1485  Bottle &c2Value=c2.addList();
1486  c2Value.addDouble(contacts_r.at(combination_tmp[1])[0]);
1487  c2Value.addDouble(contacts_r.at(combination_tmp[1])[1]);
1488  c2Value.addDouble(contacts_r.at(combination_tmp[1])[2]);
1489 
1490  Bottle &c3=res.addList();
1491  c3.addString("c3");
1492  Bottle &c3Value=c3.addList();
1493  c3Value.addDouble(contacts_r.at(combination_tmp[2])[0]);
1494  c3Value.addDouble(contacts_r.at(combination_tmp[2])[1]);
1495  c3Value.addDouble(contacts_r.at(combination_tmp[2])[2]);
1496 
1497  Bottle &n1=res.addList();
1498  n1.addString("n1");
1499  Bottle &n1Value=n1.addList();
1500  n1Value.addDouble(normals_r.at(combination_tmp[0])[0]);
1501  n1Value.addDouble(normals_r.at(combination_tmp[0])[1]);
1502  n1Value.addDouble(normals_r.at(combination_tmp[0])[2]);
1503 
1504  Bottle &n2=res.addList();
1505  n2.addString("n2");
1506  Bottle &n2Value=n2.addList();
1507  n2Value.addDouble(normals_r.at(combination_tmp[1])[0]);
1508  n2Value.addDouble(normals_r.at(combination_tmp[1])[1]);
1509  n2Value.addDouble(normals_r.at(combination_tmp[1])[2]);
1510 
1511  Bottle &n3=res.addList();
1512  n3.addString("n3");
1513  Bottle &n3Value=n3.addList();
1514  n3Value.addDouble(normals_r.at(combination_tmp[2])[0]);
1515  n3Value.addDouble(normals_r.at(combination_tmp[2])[1]);
1516  n3Value.addDouble(normals_r.at(combination_tmp[2])[2]);
1517 
1518  Bottle &r=res.addList();
1519  r.addString("rotmat");
1520  Bottle &rotValue=r.addList();
1521  for (int i=0; i<rotation.rows(); i++)
1522  {
1523  for (int j=0; j<rotation.cols(); j++)
1524  {
1525  rotValue.addDouble(rotation(i,j));
1526  }
1527  }
1528 
1529  Bottle &h=res.addList();
1530  h.addString("hand");
1531  Bottle &hValue=h.addList();
1532  if (hand=="right")
1533  hValue.addString("r");
1534  else
1535  hValue.addString("l");
1536 
1537  Bottle &cl=res.addList();
1538  cl.addString("cloud");
1539  Bottle &clValue=cl.addList();
1540  for (int i=0; i<cloud->size(); i++)
1541  {
1542  Bottle &point=clValue.addList();
1543  point.addDouble(cloud->at(i).x);
1544  point.addDouble(cloud->at(i).y);
1545  point.addDouble(cloud->at(i).z);
1546  point.addDouble(cloud->at(i).r);
1547  point.addDouble(cloud->at(i).g);
1548  point.addDouble(cloud->at(i).b);
1549  }
1550  if (toMatlab.getOutputCount()>0)
1551  toMatlab.writeStrict();
1552 }
1553 
1554 /************************************************************************/
1555 void PrecisionGrasp::writeIKBestresult()
1556 {
1557  std::vector<yarp::sig::Vector> contacts_r;
1558  std::vector<yarp::sig::Vector> normals_r;
1559 
1560  if (winner_triplet==1)
1561  {
1562  contacts_r=contacts_r1;
1563  normals_r=normals_r1;
1564  }
1565  else if (winner_triplet==2)
1566  {
1567  contacts_r=contacts_r2;
1568  normals_r=normals_r2;
1569  }
1570  else if (winner_triplet==3)
1571  {
1572  contacts_r=contacts_r3;
1573  normals_r=normals_r3;
1574  }
1575  else
1576  {
1577  contacts_r=contacts_r4;
1578  normals_r=normals_r4;
1579  }
1580 
1581  std::string filename="C:/Users/Utente/Desktop/IKResults/BestIKresult.txt";
1582  ofstream file;
1583  file.open(filename.c_str());
1584  file << "joints=[";
1585  for (unsigned int i=0; i<winner_joints.size(); i++)
1586  file << winner_joints[i] << " ";
1587  file <<"];\n";
1588  file << "ee=[" << winner_ee[0] << " " << winner_ee[1] << " " << winner_ee[2] << "];\n";
1589  file << "xdhat=[" << winner_xdhat[0] << " " << winner_xdhat[1] << " " << winner_xdhat[2] << "];\n";
1590  file << "axisangle=[" << winner_axis[0] << " " << winner_axis[1] << " " << winner_axis[2] << " " << winner_axis[3] << "];\n";
1591  file << "odhat=[" << winner_odhat[0] << " " << winner_odhat[1] << " " << winner_odhat[2] << " " << winner_odhat[3] << "];\n";
1592  file << "center=[" << center[0] << " " << center[1] << " " << center[2] << "];\n";
1593  file << "dim=[" << dim[0] << " " << dim[1] << " " << dim[2] << "];\n";
1594  file << "c1=[" << contacts_r.at(winner_combination[0])[0] << " " << contacts_r.at(winner_combination[0])[1] << " " << contacts_r.at(winner_combination[0])[2] << "];\n";
1595  file << "c2=[" << contacts_r.at(winner_combination[1])[0] << " " << contacts_r.at(winner_combination[1])[1] << " " << contacts_r.at(winner_combination[1])[2] << "];\n";
1596  file << "c3=[" << contacts_r.at(winner_combination[2])[0] << " " << contacts_r.at(winner_combination[2])[1] << " " << contacts_r.at(winner_combination[2])[2] << "];\n";
1597  file << "n1=[" << normals_r.at(winner_combination[0])[0] << " " << normals_r.at(winner_combination[0])[1] << " " << normals_r.at(winner_combination[0])[2] << "];\n";
1598  file << "n2=[" << normals_r.at(winner_combination[1])[0] << " " << normals_r.at(winner_combination[1])[1] << " " << normals_r.at(winner_combination[1])[2] << "];\n";
1599  file << "n3=[" << normals_r.at(winner_combination[2])[0] << " " << normals_r.at(winner_combination[2])[1] << " " << normals_r.at(winner_combination[2])[2] << "];\n";
1600  file << "rotmat=[";
1601  for (int i=0; i<rotation.rows(); i++)
1602  {
1603  for (int j=0; j<rotation.cols(); j++)
1604  {
1605  file << rotation(i,j);
1606  if (j!=rotation.cols()-1)
1607  file << " ";
1608  }
1609  if (i!=rotation.rows()-1)
1610  file << ";\n";
1611  }
1612 
1613  file << "];\n";
1614  if (winner_hand=="right")
1615  file << "hand='r';\n";
1616  else
1617  file << "hand='l';\n";
1618 
1619  file << "ov_cones " << winner_ov_cones << ";\n";
1620 
1621  file << "center=(" << center[0] << " " << center[1] << " " << center[2] << "];\n";
1622  file << "dim=(" << dim[0] << " " << dim[1] << " " << dim[2] << "];\n";
1623  file << "c1=(" << contacts_r.at(winner_combination[0])[0] << " " << contacts_r.at(winner_combination[0])[1] << " " << contacts_r.at(winner_combination[0])[2] << ")\n";
1624  file << "c2=(" << contacts_r.at(winner_combination[1])[0] << " " << contacts_r.at(winner_combination[1])[1] << " " << contacts_r.at(winner_combination[1])[2] << ")\n";
1625  file << "c3=(" << contacts_r.at(winner_combination[2])[0] << " " << contacts_r.at(winner_combination[2])[1] << " " << contacts_r.at(winner_combination[2])[2] << ")\n";
1626  file << "n1=(" << normals_r.at(winner_combination[0])[0] << " " << normals_r.at(winner_combination[0])[1] << " " << normals_r.at(winner_combination[0])[2] << ")\n";
1627  file << "n2=(" << normals_r.at(winner_combination[1])[0] << " " << normals_r.at(winner_combination[1])[1] << " " << normals_r.at(winner_combination[1])[2] << ")\n";
1628  file << "n3=(" << normals_r.at(winner_combination[2])[0] << " " << normals_r.at(winner_combination[2])[1] << " " << normals_r.at(winner_combination[2])[2] << ")\n";
1629 
1630  if (file.is_open())
1631  file.close();
1632 }
1633 
1634 /************************************************************************/
1635 void PrecisionGrasp::writeIKresult(const std::vector<yarp::sig::Vector> &contacts_r, const std::vector<yarp::sig::Vector> &normals_r, const int c, const string &hand, const int ov_cones)
1636 {
1637  stringstream ss;
1638  ss << c;
1639  std::string filename="C:/Users/Utente/Desktop/IKResults/IKresult_"+hand+ss.str()+".txt";
1640  ofstream file;
1641  file.open(filename.c_str());
1642  file << "joints=[";
1643  for (unsigned int i=0; i<joints_tmp.size(); i++)
1644  file << joints_tmp[i] << " ";
1645  file <<"];\n";
1646  file << "ee=[" << ee_tmp[0] << " " << ee_tmp[1] << " " << ee_tmp[2] << "];\n";
1647  file << "axisangle=[" << axis_angle_tmp[0] << " " << axis_angle_tmp[1] << " " << axis_angle_tmp[2] << " " << axis_angle_tmp[3] << "];\n";
1648  file << "center=[" << center[0] << " " << center[1] << " " << center[2] << "];\n";
1649  file << "dim=[" << dim[0] << " " << dim[1] << " " << dim[2] << "];\n";
1650  file << "c1=[" << contacts_r.at(combination_tmp[0])[0] << " " << contacts_r.at(combination_tmp[0])[1] << " " << contacts_r.at(combination_tmp[0])[2] << "];\n";
1651  file << "c2=[" << contacts_r.at(combination_tmp[1])[0] << " " << contacts_r.at(combination_tmp[1])[1] << " " << contacts_r.at(combination_tmp[1])[2] << "];\n";
1652  file << "c3=[" << contacts_r.at(combination_tmp[2])[0] << " " << contacts_r.at(combination_tmp[2])[1] << " " << contacts_r.at(combination_tmp[2])[2] << "];\n";
1653  file << "n1=[" << normals_r.at(combination_tmp[0])[0] << " " << normals_r.at(combination_tmp[0])[1] << " " << normals_r.at(combination_tmp[0])[2] << "];\n";
1654  file << "n2=[" << normals_r.at(combination_tmp[1])[0] << " " << normals_r.at(combination_tmp[1])[1] << " " << normals_r.at(combination_tmp[1])[2] << "];\n";
1655  file << "n3=[" << normals_r.at(combination_tmp[2])[0] << " " << normals_r.at(combination_tmp[2])[1] << " " << normals_r.at(combination_tmp[2])[2] << "];\n";
1656  file << "rotmat=[";
1657  for (int i=0; i<rotation.rows(); i++)
1658  {
1659  for (int j=0; j<rotation.cols(); j++)
1660  {
1661  file << rotation(i,j);
1662  if (j!=rotation.cols()-1)
1663  file << " ";
1664  }
1665  if (i!=rotation.rows()-1)
1666  file << ";\n";
1667  }
1668 
1669  file << "];\n";
1670  if (hand=="right")
1671  file << "hand='r';\n";
1672  else
1673  file << "hand='l';\n";
1674 
1675  file << "ov_cones " << ov_cones << ";\n";
1676 
1677  file << "center=(" << center[0] << " " << center[1] << " " << center[2] << ")\n";
1678  file << "dim=(" << dim[0] << " " << dim[1] << " " << dim[2] << ")\n";
1679  file << "c1=(" << contacts_r.at(combination_tmp[0])[0] << " " << contacts_r.at(combination_tmp[0])[1] << " " << contacts_r.at(combination_tmp[0])[2] << ")\n";
1680  file << "c2=(" << contacts_r.at(combination_tmp[1])[0] << " " << contacts_r.at(combination_tmp[1])[1] << " " << contacts_r.at(combination_tmp[1])[2] << ")\n";
1681  file << "c3=(" << contacts_r.at(combination_tmp[2])[0] << " " << contacts_r.at(combination_tmp[2])[1] << " " << contacts_r.at(combination_tmp[2])[2] << ")\n";
1682  file << "n1=(" << normals_r.at(combination_tmp[0])[0] << " " << normals_r.at(combination_tmp[0])[1] << " " << normals_r.at(combination_tmp[0])[2] << ")\n";
1683  file << "n2=(" << normals_r.at(combination_tmp[1])[0] << " " << normals_r.at(combination_tmp[1])[1] << " " << normals_r.at(combination_tmp[1])[2] << ")\n";
1684  file << "n3=(" << normals_r.at(combination_tmp[2])[0] << " " << normals_r.at(combination_tmp[2])[1] << " " << normals_r.at(combination_tmp[2])[2] << ")\n";
1685 
1686  if (file.is_open())
1687  file.close();
1688 }
1689 
1690 /************************************************************************/
1691 bool PrecisionGrasp::respond(const Bottle& command, Bottle& reply)
1692 {
1693  string tag_0=command.get(0).asString().c_str();
1694  if (tag_0=="set")
1695  {
1696  if (command.size()<3)
1697  {
1698  reply.addString("nack, command not recognized");
1699  return true;
1700  }
1701  string tag_1=command.get(1).asString().c_str();
1702  if (tag_1=="visualization")
1703  {
1704  if (command.get(2).asString()=="on")
1705  visualize=true;
1706  else
1707  visualize=false;
1708  reply.addString("ack");
1709  return true;
1710  }
1711  else if (tag_1=="filter")
1712  {
1713  if (command.get(2).asString()=="on")
1714  filterCloud=true;
1715  else
1716  filterCloud=false;
1717  reply.addString("ack");
1718  return true;
1719  }
1720  else if (tag_1=="x")
1721  {
1722  if (command.size()>1)
1723  {
1724  posx=command.get(1).asInt();
1725  visualizationThread->setPosition(posx,posy);
1726  reply.addString("ack");
1727  }
1728  else
1729  reply.addString("nack");
1730  return true;
1731  }
1732  else if (tag_1=="y")
1733  {
1734  if (command.size()>1)
1735  {
1736  posy=command.get(1).asInt();
1737  visualizationThread->setPosition(posx,posy);
1738  reply.addString("ack");
1739  }
1740  else
1741  reply.addString("nack");
1742  return true;
1743  }
1744  else if (tag_1=="w")
1745  {
1746  if (command.size()>1)
1747  {
1748  sizex=command.get(1).asInt();
1749  visualizationThread->setSize(sizex,sizey);
1750  reply.addString("ack");
1751  }
1752  else
1753  reply.addString("nack");
1754  return true;
1755  }
1756  else if (tag_1=="h")
1757  {
1758  if (command.size()>1)
1759  {
1760  sizey=command.get(1).asInt();
1761  visualizationThread->setSize(sizex,sizey);
1762  reply.addString("ack");
1763  }
1764  else
1765  reply.addString("nack");
1766  return true;
1767  }
1768  else if (tag_1=="write")
1769  {
1770  if (command.get(2).asString()=="on")
1771  writeCloud=true;
1772  else
1773  writeCloud=false;
1774  reply.addString("ack");
1775  return true;
1776  }
1777  else if (tag_1=="offsetR")
1778  {
1779  offsetR[0]=command.get(2).asDouble();
1780  offsetR[1]=command.get(3).asDouble();
1781  offsetR[2]=command.get(4).asDouble();
1782  reply.addString("ack");
1783  return true;
1784  }
1785  else if (tag_1=="offsetL")
1786  {
1787  offsetL[0]=command.get(2).asDouble();
1788  offsetL[1]=command.get(3).asDouble();
1789  offsetL[2]=command.get(4).asDouble();
1790  reply.addString("ack");
1791  return true;
1792  }
1793  }
1794  if (tag_0=="IK1" || tag_0=="IK2" || tag_0=="IK3" || tag_0=="IK4")
1795  {
1796  if (current_state==STATE_IK)
1797  {
1798  counter--;
1799  reply.addString("ack");
1800  if (tag_0=="IK1")
1801  {
1802  mutex_to_write.wait();
1803  string hand=extractData(command,1);
1804  //writeToMatlab(contacts_r1,normals_r1,hand);
1805  writeIKresult(contacts_r1,normals_r1,1,hand,ov_cones1);
1806  mutex_to_write.post();
1807  }
1808  else if (tag_0=="IK2")
1809  {
1810  mutex_to_write.wait();
1811  string hand=extractData(command,2);
1812  //writeToMatlab(contacts_r2,normals_r2,hand);
1813  writeIKresult(contacts_r2,normals_r2,2,hand,ov_cones2);
1814  mutex_to_write.post();
1815  }
1816  else if (tag_0=="IK3")
1817  {
1818  mutex_to_write.wait();
1819  string hand=extractData(command,3);
1820  //writeToMatlab(contacts_r3,normals_r3,hand);
1821  writeIKresult(contacts_r3,normals_r3,3,hand,ov_cones3);
1822  mutex_to_write.post();
1823  }
1824  else
1825  {
1826  mutex_to_write.wait();
1827  string hand=extractData(command,4);
1828  //writeToMatlab(contacts_r4,normals_r4,hand);
1829  writeIKresult(contacts_r4,normals_r4,4,hand,ov_cones4);
1830  mutex_to_write.post();
1831  }
1832  if (counter==0)
1833  {
1834  //Time::delay(1.0);
1835 
1836  writeBestSolution();
1837  writeIKBestresult();
1838 
1839  //Time::delay(1.0);
1840 
1841  bestCost=1e20;
1842  bestManipulability=0.0;
1843 
1844  iCtrlRight->restoreContext(current_context_right);
1845  iCtrlLeft->restoreContext(current_context_left);
1846 
1847  readyToGrasp=true;
1848  if (!dont && straight)
1849  {
1850  current_state=STATE_GRASP;
1851  straight=false;
1852  }
1853  else
1854  current_state=STATE_WAIT;
1855  printf("ready\n");
1856  }
1857  }
1858  else
1859  reply.addString("nack");
1860  return true;
1861  }
1862  if (tag_0=="go")
1863  {
1864  if (readyToGrasp)
1865  {
1866  current_state=STATE_GRASP;
1867  eventRpc.reset();
1868  eventRpc.wait();
1869  reply.addString("ack");
1870  }
1871  else
1872  reply.addString("nack");
1873  return true;
1874  }
1875  if (tag_0=="help")
1876  {
1877  reply.addString("set visualization on/off");
1878  reply.addString("set x");
1879  reply.addString("set y");
1880  reply.addString("set w");
1881  reply.addString("set h");
1882  reply.addString("set offsetL x y z");
1883  reply.addString("set offsetR x y z");
1884  reply.addString("set filter on/off");
1885  reply.addString("set write on/off");
1886  reply.addString("block right/left");
1887  reply.addString("unblock right/left");
1888  reply.addString("grasp (x y) [wait]");
1889  reply.addString("go");
1890  reply.addString("dont");
1891  reply.addString("isGrasped");
1892  return true;
1893  }
1894  if (tag_0=="block")
1895  {
1896  if (command.size()>=2)
1897  {
1898  if (command.get(1).asString()=="right")
1899  {
1900  Network::disconnect(ikPort1r.getName().c_str(), "/handIKModule1/right/rpc");
1901  Network::disconnect(ikPort2r.getName().c_str(), "/handIKModule2/right/rpc");
1902  Network::disconnect(ikPort3r.getName().c_str(), "/handIKModule3/right/rpc");
1903  Network::disconnect(ikPort4r.getName().c_str(), "/handIKModule4/right/rpc");
1904  rightBlocked=true;
1905  }
1906  else
1907  {
1908  Network::disconnect(ikPort1l.getName().c_str(), "/handIKModule1/left/rpc");
1909  Network::disconnect(ikPort2l.getName().c_str(), "/handIKModule2/left/rpc");
1910  Network::disconnect(ikPort3l.getName().c_str(), "/handIKModule3/left/rpc");
1911  Network::disconnect(ikPort4l.getName().c_str(), "/handIKModule4/left/rpc");
1912  leftBlocked=true;
1913  }
1914  reply.addString("ack");
1915  return true;
1916  }
1917  else
1918  {
1919  reply.addString("nack");
1920  return true;
1921  }
1922  }
1923  if (tag_0=="unblock")
1924  {
1925  if (command.size()>=2)
1926  {
1927  if (command.get(1).asString()=="right")
1928  {
1929  Network::connect(ikPort1r.getName().c_str(), "/handIKModule1/right/rpc");
1930  Network::connect(ikPort2r.getName().c_str(), "/handIKModule2/right/rpc");
1931  Network::connect(ikPort3r.getName().c_str(), "/handIKModule3/right/rpc");
1932  Network::connect(ikPort4r.getName().c_str(), "/handIKModule4/right/rpc");
1933  rightBlocked=false;
1934  }
1935  else
1936  {
1937  Network::connect(ikPort1l.getName().c_str(), "/handIKModule1/left/rpc");
1938  Network::connect(ikPort2l.getName().c_str(), "/handIKModule2/left/rpc");
1939  Network::connect(ikPort3l.getName().c_str(), "/handIKModule3/left/rpc");
1940  Network::connect(ikPort4l.getName().c_str(), "/handIKModule4/left/rpc");
1941  leftBlocked=false;
1942  }
1943  reply.addString("ack");
1944  return true;
1945  }
1946  else
1947  {
1948  reply.addString("nack");
1949  return true;
1950  }
1951  }
1952  if (tag_0=="isGrasped")
1953  {
1954  string r=grasped?"true":"false";
1955  reply.addString(r.c_str());
1956  return true;
1957  }
1958  if (tag_0=="dont")
1959  {
1960  dont=true;
1961  readyToGrasp=false;
1962  reply.addString("ack");
1963  current_state=STATE_WAIT;
1964  return true;
1965  }
1966  if (tag_0=="grasp")
1967  {
1968  if (current_state==STATE_ESTIMATE || current_state==STATE_IK || current_state==STATE_GRASP)
1969  {
1970  reply.addString("nack");
1971  return true;
1972  }
1973 
1974  if (visualizationThread->isRunning())
1975  visualizationThread->stop();
1976 
1977  readyToGrasp=false;
1978  grasped=false;
1979  dont=false;
1980 
1981  Bottle *pos=command.get(1).asList();
1982 
1983  Bottle cmd1;
1984  reply.clear();
1985  cmd1.addInt(pos->get(0).asInt());
1986  cmd1.addInt(pos->get(1).asInt());
1987 
1988  if (reconstructionPort.getOutputCount()>0)
1989  reconstructionPort.write(cmd1,reply);
1990 
1991  Bottle cmd2;
1992  reply.clear();
1993  cmd2.addString("3Drec");
1994 
1995  if (reconstructionPort.getOutputCount()>0)
1996  reconstructionPort.write(cmd2,reply);
1997 
1998  if (reply.size()>0 && reply.get(0).asString()=="ack")
1999  {
2000  reply.clear();
2001  current_state=STATE_ESTIMATE;
2002  straight=true;
2003  if (command.size()>=2)
2004  straight=(command.get(2).asString()!="wait");
2005  if (straight)
2006  {
2007  eventRpc.reset();
2008  eventRpc.wait();
2009  }
2010  if (grasped)
2011  reply.addString("ack");
2012  else
2013  reply.addString("nack");
2014  }
2015  else
2016  reply.addString("nack");
2017 
2018  return true;
2019  }
2020  reply.addString("nack");
2021  return true;
2022 }
2023 
2024 /************************************************************************/
2025 void PrecisionGrasp::addPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in)
2026 {
2027  cloud->clear();
2028  cloudxyz->clear();
2029 
2030  for (size_t j = 0; j < cloud_in->points.size (); ++j)
2031  {
2032  PointXYZRGB pointrgb=cloud_in->points[j];
2033  pcl::PointXYZ point;
2034  point.x=pointrgb.x;
2035  point.y=pointrgb.y;
2036  point.z=pointrgb.z;
2037  cloudxyz->push_back(point);
2038  cloud->push_back(pointrgb);
2039  }
2040 }
2041 
2042 /************************************************************************/
2043 double PrecisionGrasp::getPeriod()
2044 {
2045  return 0.1;
2046 }
2047 
2048 /************************************************************************/
2049 bool PrecisionGrasp::normalPointingOut(pcl::Normal &normal, pcl::PointXYZ &point)
2050 {
2051  yarp::sig::Vector p(3);
2052  p[0]=point.x-(normal.normal_x*0.003);
2053  p[1]=point.y-(normal.normal_y*0.003);
2054  p[2]=point.z-(normal.normal_z*0.003);
2055 
2056  yarp::sig::Vector fromCenter=(center-p)/norm(center-p);
2057 
2058  yarp::sig::Vector n(3);
2059  n[0]=normal.normal_x;
2060  n[1]=normal.normal_y;
2061  n[2]=normal.normal_z;
2062  n=n/norm(n);
2063 
2064  return (dot(n,fromCenter)<0);
2065 }
2066 
iCub::data3D::MinimumBoundingBox::getMinimumBoundingBox
iCub::data3D::BoundingBox getMinimumBoundingBox(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud)
Given a point cloud (defined in the Point Cloud Library), it computes the minimum enclosing bounding ...
Definition: minBoundBox.cpp:212
iCub::grasp::ContactPoints
Definition of the ForceClosure.
Definition: forceClosure.h:60
Generated by   doxygen 1.8.12