grasp
All Data Structures Namespaces Functions Modules
psoThread.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 "psoThread.h"
18 
19 using namespace std;
20 using namespace yarp::os;
21 using namespace yarp::dev;
22 using namespace yarp::sig;
23 using namespace yarp::math;
24 using namespace iCub::ctrl;
25 using namespace iCub::iKin;
26 using namespace iCub::grasp;
27 
28 /************************************************************************/
29 PsoThread::PsoThread() : RateThread(20), cloud(new pcl::PointCloud<pcl::PointXYZ>),
30  normals (new pcl::PointCloud <pcl::Normal>)
31 {
32  done=true;
33  work=false;
34  init=true;
35  set=false;
36  fg=1e10;
37  cost=0.06;
38  /*float resolution = 128.0f;
39  octree.setResolution(resolution);*/
40 }
41 
42 /************************************************************************/
43 bool PsoThread::open(const Property &options)
44 {
45  Property &opt=const_cast<Property&>(options);
46  particles=opt.check("particles",Value(20)).asInt();
47  omega=opt.check("omega",Value(1.0)).asDouble();
48  phi_p=opt.check("phi_p",Value(0.02)).asDouble();
49  phi_g=opt.check("phi_g",Value(0.02)).asDouble();
50  iterations=opt.check("iterations",Value(300)).asInt();
51  limit_finger_max=opt.check("limit_finger_max",Value(0.08)).asDouble();
52  limit_finger_min=opt.check("limit_finger_min",Value(0.02)).asDouble();
53  hand_area=opt.check("hand_area",Value(0.00225)).asDouble();
54  dimension=opt.check("dimension",Value(9)).asInt();
55  vmax=opt.check("vmax",Value(0.3)).asDouble();
56  vmin=opt.check("vmin",Value(-0.3)).asDouble();
57  fp.resize(particles);
58  nsg.resize(dimension);
59  g.resize(dimension);
60 
61  this->start();
62  return true;
63 }
64 
65 /************************************************************************/
66 void PsoThread::setData(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud <pcl::Normal>::Ptr normals, double alpha, int overlapping_cones)
67 {
68  this->cloud->clear();
69  this->normals->clear();
70  copyPointCloud(*cloud,*this->cloud);
71  copyPointCloud(*normals,*this->normals);
72  this->overlapping_cones=overlapping_cones;
73  this->alpha=alpha;
74  work=true;
75  done=false;
76  this->resume();
77  fg=1e10;
78 }
79 
80 /************************************************************************/
81 void PsoThread::run()
82 {
83  if (work)
84  {
85  set=false;
86  int seed=(int)Time::now()*1000;
87  Random::seed(seed);
88  kdtree.setInputCloud(cloud);
89  //octree.addPointsFromInputCloud();
90  visited.clear();
91 
92  if (cloud->size()<65)
93  rand_method=0;
94  else
95  rand_method=2;
96  rand_method=0;
97  //rand_method=3;
98 
99  double time=Time::now();
100  if (rand_method==0)
101  {
102  for (int i=0; i<cloud->size(); i++)
103  {
104  for (int j=i+1; j<cloud->size(); j++)
105  {
106  for (int k=j+1; k<cloud->size(); k++)
107  {
108  yarp::sig::Vector ids(3); ids[0]=i; ids[1]=j; ids[2]=k;
109  yarp::sig::Vector p=assignPoints(ids);
110  yarp::sig::Vector n=assignNormals(ids);
111  ContactPoints triplet=fillContactPoints(p,n);
112  double f=fitness(triplet);
113  if (f<fg)
114  {
115  g=p;
116  nsg=n;
117  fg=f;
118  set=true;
119  }
120  }
121  }
122  }
123  printf("Time exhaustive search %g\n", Time::now()-time);
124  printf("best fg %g\n", fg);
125  }
126  else if (rand_method==3)
127  {
128  int iter=1;
129  int iterationsRandom=6000;
130 
131  while (iter<=iterationsRandom && fg>cost)
132  {
133  yarp::sig::Vector ids(3);
134  ids=findRandomId();
135  yarp::sig::Vector p=assignPoints(ids);
136  yarp::sig::Vector n=assignNormals(ids);
137  ContactPoints triplet=fillContactPoints(p,n);
138  double f=fitness(triplet);
139  if (f<fg)
140  {
141  g=p;
142  nsg=n;
143  fg=f;
144  set=true;
145  }
146  iter++;
147  }
148  if (fg<cost)
149  printf("iteration %i\n", iter);
150  printf("fg %g\n", fg);
151  }
152  else
153  {
154  if (rand_method==1)
155  {
156  for (int i=0; i<cloud->size(); i++)
157  {
158  for (int j=i+1; j<cloud->size(); j++)
159  {
160  for (int k=j+1; k<cloud->size(); k++)
161  {
162  yarp::sig::Vector tmp(3);
163  tmp[0]=i; tmp[1]=j; tmp[2]=k;
164  visited.push_back(tmp);
165  }
166  }
167  }
168  printf("time for structure %d %g\n", overlapping_cones, Time::now()-time);
169  }
170  double iter=1.0;
171  double tot=iterations;
172  double counter=iter;
173 
174  initialization();
175  p=x;
176  double time1=Time::now();
177  while (iter<=iterations && fg>cost)
178  {
179  for (int i=0; i<particles; i++)
180  {
181  yarp::sig::Vector ids(3,0.0);
182  if (!set)
183  {
184  if (rand_method==1)
185  {
186  int id=Random::uniform(0,visited.size()-1);
187  ids=visited[id];
188  visited.erase(visited.begin()+id);
189  }
190  else
191  ids=findRandomId();
192  }
193  else
194  {
195  for (int j=0; j<dimension; j++)
196  {
197  double rand_p=phi_p*Random::uniform();
198  double rand_g=phi_g*Random::uniform();
199  double velocity=omega*v.at(i)[j]+(rand_p*(p.at(i)[j]-x.at(i)[j]))+(rand_g*(g[j]-x.at(i)[j]));
200  if (velocity<vmin)
201  v.at(i)[j]=vmin;
202  else if (velocity>vmax)
203  v.at(i)[j]=vmax;
204  else
205  v.at(i)[j]=velocity;
206  }
207  x.at(i)+=v.at(i);
208  ids=findTriplet(x.at(i));
209  }
210 
211  x.at(i)=assignPoints(ids);
212  ns.at(i)=assignNormals(ids);
213  ContactPoints triplet=fillContactPoints(x.at(i),ns.at(i));
214  double f=fitness(triplet);
215 
216  if (f<fp[i])
217  {
218  p.at(i)=x.at(i);
219  fp[i]=f;
220  }
221 
222  if (fp[i]<fg)
223  {
224  g=p.at(i);
225  nsg=ns.at(i);
226  fg=fp[i];
227  set=true;
228  printf("Time inizialization %d %g\n", overlapping_cones, Time::now()-time1);
229  }
230  }
231  if (set)
232  omega=(1-(counter/tot));
233  else
234  omega=0.8;
235 
236  iter+=1.0;
237  counter+=1.0;
238 
239  if ((int)iter%50==0 && set)
240  {
241  double distances;
242  for (int j=0; j<x.size(); j++)
243  distances+=norm(x.at(j)-g);
244  distances/=x.size();
245 
246  if (distances<0.2)
247  {
248  tot=iterations-iter+1;
249  counter=1;
250  omega=1;
251  initialization();
252  }
253  }
254  }
255  printf("Time pso %d %g\n", overlapping_cones, Time::now()-time1);
256  printf("iteration %g\n", iter);
257  printf("fg %g\n", fg);
258  }
259  done=true;
260  work=false;
261  }
262  this->suspend();
263 }
264 
265 /************************************************************************/
266 yarp::sig::Vector PsoThread::findRandomId()
267 {
268  yarp::sig::Vector ids(3);
269  bool assigned=false;
270  while(!assigned)
271  {
272  yarp::sig::Vector tmp(3);
273  tmp[0]=Random::uniform(0,cloud->size()-1);
274  tmp[1]=Random::uniform(0,cloud->size()-2);
275  tmp[2]=Random::uniform(0,cloud->size()-3);
276  ids=findDifferentIds(tmp);
277  if (visited.size()==0)
278  {
279  visited.push_back(ids);
280  assigned=true;
281  }
282  else
283  {
284  bool equal=false;
285  for (std::vector<yarp::sig::Vector>::iterator it=visited.begin(); it!=visited.end(); it++)
286  {
287  if ((*it)[0]==ids[0] && (*it)[1]==ids[1] && (*it)[2]==ids[2])
288  {
289  equal=true;
290  break;
291  }
292  }
293  if (!equal)
294  {
295  assigned=true;
296  visited.push_back(ids);
297  }
298  }
299  }
300  return ids;
301 }
302 
303 /************************************************************************/
304 void PsoThread::initialization()
305 {
306  yarp::sig::Vector idx(3);
307  fp=1e20;
308  for (int i=0; i<particles; i++)
309  {
310  if (rand_method==1)
311  {
312  int id=Random::uniform(0,visited.size()-1);
313  idx=visited[id];
314  visited.erase(visited.begin()+id);
315  }
316  else
317  idx=findRandomId();
318 
319  if (init)
320  {
321  x.push_back(assignPoints(idx));
322  p.push_back(assignPoints(idx));
323  ns.push_back(assignNormals(idx));
324  yarp::sig::Vector tmp(dimension);
325  for (int j=0; j<tmp.size(); j++)
326  tmp[j]=fmod(Random::uniform(),vmax)*pow(-1.0,Random::uniform(1,2));
327  v.push_back(tmp);
328  }
329  else
330  {
331  x.at(i)=assignPoints(idx);
332  ns.at(i)=assignNormals(idx);
333  for (int j=0; j<dimension; j++)
334  v.at(i)[j]=fmod(Random::uniform(),vmax)*pow(-1.0,Random::uniform(1,2));
335  }
336  }
337  init=false;
338 }
339 
340 /************************************************************************/
341 bool PsoThread::isSuccessful()
342 {
343  return set;
344 }
345 
346 /************************************************************************/
347 ContactPoints PsoThread::getBestTriplet()
348 {
349  ContactPoints triplet=fillContactPoints(g,nsg);
350  bool a=isForceClosure(triplet);
351  printf("fc %d %s\n", overlapping_cones, a?"yes":"no");
352 
353  int n_cones=3;
354  yarp::sig::Vector angles(3);
355  yarp::sig::Vector idx(3);
356  if (!counterOverlap(triplet.n1,triplet.n2,angles[0]))
357  n_cones-=1;
358  if (!counterOverlap(triplet.n1,triplet.n3,angles[1]))
359  n_cones-=1;
360  if (!counterOverlap(triplet.n2,triplet.n3,angles[2]))
361  n_cones-=1;
362  printf("ov cones %d %d\n", overlapping_cones, n_cones);
363  triplet.ov_cones=n_cones;
364 
365  yarp::sig::Vector n1=triplet.n1/norm(triplet.n1)*0.003;
366  yarp::sig::Vector n2=triplet.n2/norm(triplet.n2)*0.003;
367  yarp::sig::Vector n3=triplet.n3/norm(triplet.n3)*0.003;
368 
369  triplet.c1-=n1;
370  triplet.c2-=n2;
371  triplet.c3-=n3;
372 
373  return triplet;
374 }
375 
376 /************************************************************************/
377 double PsoThread::getCost()
378 {
379  return fg;
380 }
381 
382 /************************************************************************/
383 ContactPoints PsoThread::fillContactPoints(yarp::sig::Vector &xi, yarp::sig::Vector &nsi)
384 {
385  yarp::sig::Vector c1(3),c2(3),c3(3),n1(3),n2(3),n3(3);
386  ContactPoints triplet;
387  c1[0]=xi[0];
388  c1[1]=xi[1];
389  c1[2]=xi[2];
390  c2[0]=xi[3];
391  c2[1]=xi[4];
392  c2[2]=xi[5];
393  c3[0]=xi[6];
394  c3[1]=xi[7];
395  c3[2]=xi[8];
396 
397  n1[0]=nsi[0];
398  n1[1]=nsi[1];
399  n1[2]=nsi[2];
400  n2[0]=nsi[3];
401  n2[1]=nsi[4];
402  n2[2]=nsi[5];
403  n3[0]=nsi[6];
404  n3[1]=nsi[7];
405  n3[2]=nsi[8];
406 
407  triplet.c1=c1;
408  triplet.c2=c2;
409  triplet.c3=c3;
410  triplet.n1=n1;
411  triplet.n2=n2;
412  triplet.n3=n3;
413  triplet.alpha1=alpha;
414  triplet.alpha2=alpha;
415  triplet.alpha3=alpha;
416 
417  return triplet;
418 }
419 
420 /************************************************************************/
421 double PsoThread::fitness(ContactPoints &triplet)
422 {
423  double f=0.0;
424  double dist1=norm(triplet.c1-triplet.c2);
425  double dist2=norm(triplet.c1-triplet.c3);
426  double dist3=norm(triplet.c2-triplet.c3);
427 
428  if (dist1>limit_finger_max || dist2>limit_finger_max || dist3>limit_finger_max)
429  return 1e10;
430  if (dist1<0.01 || dist2<0.01 || dist3<0.01)
431  return 1e10;
432  if (dist1>limit_finger_min && dist2>limit_finger_min && dist3>limit_finger_min)
433  return 1e10;
434 
435  if (!isForceClosure(triplet))
436  return 1e10;
437 
438  int n_cones=3;
439  yarp::sig::Vector angles(3);
440  yarp::sig::Vector idx(3);
441  if (!counterOverlap(triplet.n1,triplet.n2,angles[0]))
442  {
443  idx[0]=0;
444  n_cones-=1;
445  }
446  else
447  idx[0]=1;
448  if (!counterOverlap(triplet.n1,triplet.n3,angles[1]))
449  {
450  idx[1]=0;
451  n_cones-=1;
452  }
453  else
454  idx[1]=1;
455  if (!counterOverlap(triplet.n2,triplet.n3,angles[2]))
456  {
457  idx[2]=0;
458  n_cones-=1;
459  }
460  else
461  idx[2]=1;
462 
463  if (n_cones!=overlapping_cones)
464  f+=penaltyCones(n_cones,idx,angles);
465 
466  double diff=(double)(abs(overlapping_cones-n_cones));
467  double w=1e-01*(pow(2,diff)+1);
468  double current_area=triangleArea(triplet.c1,triplet.c2,triplet.c3);
469 
470  //the penalty increases like a line
471  if (current_area>hand_area)
472  f+=1e03*current_area;
473  else
474  {
475  //the penalty decreases like a paraboloid
476  double diff_area2=(current_area-hand_area)*(current_area-hand_area);
477  double diff_area4=diff_area2*diff_area2;
478  f+=w*1e11*abs(diff_area4);
479  }
480 
481  return f;
482 }
483 
484 /************************************************************************/
485 double PsoThread::penaltyCones(int n_cones, yarp::sig::Vector &idx, yarp::sig::Vector &angles)
486 {
487  double f=0.0;
488  int diff=overlapping_cones-n_cones;
489  int test;
490  if (diff>0)
491  test=0;
492  else
493  test=1;
494  //if the number of wanted overlapping_cones is very different from the number of
495  //effectively overlapping cones, then the angles summed to the penalty function are
496  //weighted more
497  double w=abs(diff*diff*diff);
498  std::vector<double> missingAngles;
499  for (int i=0; i<idx.size(); i++)
500  {
501  if (idx[i]==test)
502  missingAngles.push_back(angles[i]);
503  }
504  std::sort(missingAngles.begin(), missingAngles.end());
505  for (int i=0; i<abs(diff); i++)
506  f+=(w*missingAngles[i]);
507  return f;
508 }
509 
510 /************************************************************************/
511 double PsoThread::triangleArea(yarp::sig::Vector &p1, yarp::sig::Vector &p2, yarp::sig::Vector &p3)
512 {
513  return norm(cross(p2-p1,p3-p1))/2;
514 }
515 
516 /************************************************************************/
517 bool PsoThread::counterOverlap(yarp::sig::Vector &n1, yarp::sig::Vector &n2, double &angle)
518 {
519  n1=n1/norm(n1);
520  n2=n2/norm(n2);
521 
522  double theta=abs(M_PI-acos(dot(n1,n2)));
523  if (theta<2*this->alpha)
524  {
525  angle=(2*this->alpha)-theta;
526  return true;
527  }
528  else
529  {
530  angle=theta-(2*this->alpha);
531  return false;
532  }
533 }
534 
535 /************************************************************************/
536 yarp::sig::Vector PsoThread::assignPoints(const yarp::sig::Vector &ids)
537 {
538  yarp::sig::Vector res(dimension);
539  res[0]=cloud->at(ids[0]).x;
540  res[1]=cloud->at(ids[0]).y;
541  res[2]=cloud->at(ids[0]).z;
542  res[3]=cloud->at(ids[1]).x;
543  res[4]=cloud->at(ids[1]).y;
544  res[5]=cloud->at(ids[1]).z;
545  res[6]=cloud->at(ids[2]).x;
546  res[7]=cloud->at(ids[2]).y;
547  res[8]=cloud->at(ids[2]).z;
548  return res;
549 }
550 
551 /************************************************************************/
552 yarp::sig::Vector PsoThread::assignNormals(const yarp::sig::Vector &ids)
553 {
554  yarp::sig::Vector res(dimension);
555  res[0]=normals->at(ids[0]).normal_x;
556  res[1]=normals->at(ids[0]).normal_y;
557  res[2]=normals->at(ids[0]).normal_z;
558  res[3]=normals->at(ids[1]).normal_x;
559  res[4]=normals->at(ids[1]).normal_y;
560  res[5]=normals->at(ids[1]).normal_z;
561  res[6]=normals->at(ids[2]).normal_x;
562  res[7]=normals->at(ids[2]).normal_y;
563  res[8]=normals->at(ids[2]).normal_z;
564  return res;
565 }
566 
567 /************************************************************************/
568 int PsoThread::findClosestPoint(pcl::PointXYZ &point)
569 {
570  int k=1;
571  std::vector<int> pointIdxNKNSearch;
572  std::vector<float> pointNKNSquaredDistance;
573 
574  int id;
575  if (kdtree.nearestKSearch (point, k, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
576  id=pointIdxNKNSearch[0];
577  else
578  id=-1;
579  return id;
580 }
581 
582 /************************************************************************/
583 yarp::sig::Vector PsoThread::findTriplet(const yarp::sig::Vector &vect)
584 {
585  pcl::PointXYZ p1(vect[0],vect[1],vect[2]);
586  pcl::PointXYZ p2(vect[3],vect[4],vect[5]);
587  pcl::PointXYZ p3(vect[6],vect[7],vect[8]);
588 
589  yarp::sig::Vector ids(3);
590 
591  ids[0]=findClosestPoint(p1);
592  ids[1]=findClosestPoint(p2);
593  ids[2]=findClosestPoint(p3);
594 
595  if (ids[0]!=ids[1] && ids[0]!=ids[2] && ids[1]!=ids[2])
596  return ids;
597  else
598  {
599  if (rand_method==1)
600  {
601  int id=Random::uniform(0,visited.size()-1);
602  ids=visited[id];
603  visited.erase(visited.begin()+id);
604  }
605  else
606  {
607  yarp::sig::Vector tmp(3);
608  tmp[0]=Random::uniform(0,cloud->size()-1);
609  tmp[1]=Random::uniform(0,cloud->size()-2);
610  tmp[2]=Random::uniform(0,cloud->size()-3);
611  ids=findDifferentIds(tmp);
612  }
613  }
614  return ids;
615 }
616 
617 /************************************************************************/
618 yarp::sig::Vector PsoThread::findDifferentIds(const yarp::sig::Vector &ids)
619 {
620  yarp::sig::Vector idsNew(3);
621  idsNew[0]=ids[0];
622  double small=0.0;
623  double big=0.0;
624 
625  if (ids[1]>=ids[0])
626  {
627  idsNew[1]=ids[1]+1;
628  big=ids[1]+1;
629  small=ids[0];
630  }
631  else
632  {
633  idsNew[1]=ids[1];
634  big=ids[0];
635  small=ids[1];
636  }
637 
638  if (ids[2]<small)
639  idsNew[2]=ids[2];
640  else if (ids[2]>=small)
641  idsNew[2]=ids[2]+1;
642  if (idsNew[2]>=big)
643  idsNew[2]=idsNew[2]+1;
644 
645  return idsNew;
646 }
647 
648 /************************************************************************/
649 bool PsoThread::checkDone()
650 {
651  return done;
652 }
653 
654 /************************************************************************/
655 void PsoThread::close()
656 {
657 
658 }
659 
660 /************************************************************************/
661 void PsoThread::threadRelease()
662 {
663 
664 }
665 
Definition of the ForceClosure.
Definition: forceClosure.h:60