inverse.cpp
Go to the documentation of this file.
1 /* *********************
2  * @file inverse.cpp
3  *
4  * @author Stefan Ulbrich
5  * @date 2013-2014
6  *
7  * @brief This file contains the implementation the methods of the KBM::Models::KBM class related to subdivision and the global IK algorithms.
8  * *********************/
9 
10 #include "kbm.h"
11 #include <cmath>
12 #include <assert.h>
13 #include <queue>
14 #include <boost/multi_array.hpp>
15 
18 
20 {
21 
22  /* Removec can be evaluated with subdivide(center,Eigen3::VectorXf::Constant(x,y)
23  * void KBM::subdivide(const Vector &center, Real newSpreadAngle){
24  Vector newSpreadAngles = Vector::Constant(this->nDoF,newSpreadAngle);
25  return this->subdivide(center,newSpreadAngles);
26  }*/
27 
28  void KBM::subdivide(const Vector& center, const Vector& newSpreadAngles)
29  {
30  this->getSubdivisedNet(center, newSpreadAngles, this->controlNet);
31  this->spreadAngles = newSpreadAngles;
32  this->center = center;
33  }
34 
35  void KBM::getSubdivisedNet(unsigned int subdividedDoF, Real newCenter, Real newSpreadAngle, Matrix& resultingControlNet) const
36  {
37  Matrix SLE = Matrix::Constant(nControlPoints, nControlPoints, 1.0);
38  using CacheType = boost::multi_array<Real, 3>;
39  CacheType cache(boost::extents[this->nDoF][3][3]);
40 
41  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
42  {
43  for (int idx = 0; idx < 3; idx++)
44  for (int idx2 = 0; idx2 < 3; idx2++)
45  {
46  assert(iDoF >= 0);
47  if (static_cast<unsigned int>(iDoF) != subdividedDoF)
48  {
49  if (idx != idx2)
50  {
51  cache[iDoF][idx][idx2] = 0.0f;
52  }
53  else
54  {
55  cache[iDoF][idx][idx2] = 1.0f;
56  }
57  }
58  else
59  {
60  Real a1, a2;
61 
62  switch (idx2)
63  {
64  case 0:
65  a1 = (newCenter - this->center(iDoF)) - newSpreadAngle;
66  a2 = (newCenter - this->center(iDoF)) - newSpreadAngle;
67  break;
68 
69  case 1:
70  a1 = (newCenter - this->center(iDoF)) - newSpreadAngle;
71  a2 = (newCenter - this->center(iDoF)) + newSpreadAngle;
72  break;
73 
74  case 2:
75  a1 = (newCenter - this->center(iDoF)) + newSpreadAngle;
76  a2 = (newCenter - this->center(iDoF)) + newSpreadAngle;
77  break;
78 
79  }
80 
81  Real t1 = std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
82  Real t2 = std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
83 
84  //ARMARX_DEBUG_S << idx2 << "," << iDoF<< ": " << a1 << "," << a2 << "," << t1 << ","<< t2<<std::endl;
85  switch (idx)
86  {
87  case 0:
88  cache[iDoF][idx][idx2] = (1 - t1) * (1 - t2);
89  break;
90 
91  case 1:
92  cache[iDoF][idx][idx2] = ((1 - t1) * t2 + (1 - t2) * t1) * std::cos(this->spreadAngles[iDoF]);
93  break;
94 
95  case 2:
96  cache[iDoF][idx][idx2] = t1 * t2;
97  break;
98  }
99 
100  }
101 
102  //ARMARX_DEBUG_S << iDoF << "," << idx << "," << idx2 << "," << cache[iDoF][idx][idx2] <<std::endl;
103  }
104 
105  }
106 
107  for (int iSamples = 0; iSamples < nControlPoints; iSamples++)
108  {
109  Real weight = 0;
110 
111  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
112  {
113  // SLE(iControlPoints,iSamples)=1.0;
114  // ARMARX_DEBUG_S << SLE;
115  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
116  {
117  int idx = this->index(iControlPoints, iDoF);
118  int idx2 = this->index(iSamples, iDoF);
119 
120  SLE(iControlPoints, iSamples) *= cache[iDoF][idx][idx2];
121  }
122 
123  // ARMARX_DEBUG_S << SLE << std::endl;
124  weight += SLE(iControlPoints, iSamples);
125  }
126 
127  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
128  {
129  SLE(iControlPoints, iSamples) /= weight;
130  }
131  }
132 
133  // ARMARX_DEBUG_S << std::endl << SLE <<std::endl;
134  resultingControlNet = this->controlNet * SLE;
135  // ARMARX_DEBUG_S << this->controlNet << std::endl;
136  }
137 
138  void KBM::getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix& resultingControlNet) const
139  {
140 
141  Matrix SLE = Matrix::Constant(nControlPoints, nControlPoints, 1.0);
142 
143  using CacheType = boost::multi_array<Real, 3>;
144  CacheType cache(boost::extents[this->nDoF][3][3]);
145 
146  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
147  {
148  for (int idx = 0; idx < 3; idx++)
149  for (int idx2 = 0; idx2 < 3; idx2++)
150  {
151  Real a1, a2;
152 
153  switch (idx2)
154  {
155  case 0:
156  a1 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
157  a2 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
158  break;
159 
160  case 1:
161  a1 = (newCenter(iDoF) - this->center(iDoF)) - newSpreadAngles[iDoF];
162  a2 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
163  break;
164 
165  case 2:
166  a1 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
167  a2 = (newCenter(iDoF) - this->center(iDoF)) + newSpreadAngles[iDoF];
168  break;
169 
170  }
171 
172  Real t1 = std::tan(a1 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
173  Real t2 = std::tan(a2 / 2.0) / tan(this->spreadAngles[iDoF] / 2.0) / 2.0 + 0.5;
174 
175  //ARMARX_DEBUG_S << idx2 << "," << iDoF<< ": " << a1 << "," << a2 << "," << t1 << ","<< t2<<std::endl;
176  switch (idx)
177  {
178  case 0:
179  cache[iDoF][idx][idx2] = (1 - t1) * (1 - t2);
180  break;
181 
182  case 1:
183  cache[iDoF][idx][idx2] = ((1 - t1) * t2 + (1 - t2) * t1) * std::cos(this->spreadAngles[iDoF]);
184  break;
185 
186  case 2:
187  cache[iDoF][idx][idx2] = t1 * t2;
188  break;
189  }
190  }
191 
192  }
193 
194 
195  for (int iSamples = 0; iSamples < nControlPoints; iSamples++)
196  {
197  Real weight = 0;
198 
199  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
200  {
201  // SLE(iControlPoints,iSamples)=1.0;
202  // ARMARX_DEBUG_S << SLE;
203  for (int iDoF = 0; iDoF < this->nDoF; iDoF++)
204  {
205  int idx = this->index(iControlPoints, iDoF);
206  int idx2 = this->index(iSamples, iDoF);
207  SLE(iControlPoints, iSamples) *= cache[iDoF][idx][idx2];
208  }
209 
210  //ARMARX_DEBUG_S << SLE;
211  weight += SLE(iControlPoints, iSamples);
212  }
213 
214  for (int iControlPoints = 0; iControlPoints < nControlPoints; iControlPoints++)
215  {
216  SLE(iControlPoints, iSamples) /= weight;
217  }
218  }
219 
220  // ARMARX_DEBUG_S << std::endl << SLE <<std::endl;
221  resultingControlNet = this->controlNet * SLE;
222  // ARMARX_DEBUG_S << this->controlNet << std::endl;
223 
224  }
225  /*Removed -- can be emulated by checkBBox(point,point)
226  * bool KBM::checkBBox(const Vector &point) const
227  {
228  return KBM::checkBBoxes(point,this->controlNet);
229  }*/
230 
231  KBM::BBCheckType KBM::checkBBox(const Vector& lower, const Vector& upper, Real tolerance) const
232  {
233  return KBM::checkBBoxes(lower, upper, this->controlNet, tolerance);
234  }
235 
236  KBM::BBCheckType KBM::checkBBoxes(const Vector& lower, const Vector& upper, const Matrix& controlNet, Real tolerance)
237  {
238  Vector BBlower, BBupper;
239  KBM::getBBox(BBlower, BBupper, controlNet);
240  //ARMARX_DEBUG_S << "lower: " << BBlower.transpose() << ", upper: " << BBupper.transpose() << std::endl << controlNet << std::endl
241  // << "Target lower: " << lower.transpose() << ", uppder: " << upper.transpose() << std::endl;
242  bool covered = true;
243  bool included = true;
244 
245  for (int i = 0; i < lower.size(); i++)
246  {
247  if ((BBlower(i) > (upper(i)) + tolerance) || BBupper(i) <= (lower(i) - tolerance))
248  {
249  return KBM::DISJOINT;
250  }
251 
252  if ((BBlower(i) < lower(i)) || (BBupper(i) > upper(i)))
253  {
254  covered = false;
255  }
256 
257  if ((BBlower(i) > lower(i)) || (BBupper(i) < upper(i)))
258  {
259  included = false;
260  }
261 
262  }
263 
264  if (covered)
265  {
266  return KBM::COVERED;
267  }
268 
269  if (included)
270  {
271  return KBM::INCLUDED;
272  }
273 
274  return KBM::OVERLAPPING;
275  }
276 
277  /*Removed -- can be emulated by checkBBoxes(point,point)
278  bool KBM::checkBBoxes(const Vector &point, const Matrix &controlNet)
279  {
280  return KBM::checkBBoxes(point,point,controlNet);
281  }*/
282 
283  void KBM::getBBox(Vector& lower, Vector& upper, const Matrix& controlNet)
284  {
285  lower = controlNet.rowwise().minCoeff();
286  upper = controlNet.rowwise().maxCoeff();
287  }
288 
289  void KBM::getBBox(Vector& lower, Vector& upper)
290  {
291  getBBox(lower, upper, this->controlNet);
292  }
293 
295  {
296  Vector lower, upper;
297  this->getBBox(lower, upper);
298  Vector size = upper - lower;
299  return size.prod();
300  }
301 
302  Real KBM::getOverlappingVolumeRatio(const Vector& lower, const Vector& upper, Real targetVolume)
303  {
304  Real volume = this->getOverlappingVolume(lower, upper);
305  return volume / targetVolume;
306  }
307 
308  Real KBM::getOverlappingVolume(const Vector& lower, const Vector& upper)
309  {
310  Vector BBlower, BBupper;
311  this->getBBox(BBlower, BBupper);
312  Real volume = 1.0;
313 
314  for (int i = 0; i < lower.size(); i++)
315  {
316  volume *= std::min(upper(i), BBupper(i)) - std::max(lower(i), BBlower(i));
317 
318  if (volume <= 0.0f)
319  {
320  return 0.0f;
321  }
322  }
323 
324  return volume;
325  }
326 
327 }
328 //int KBM::predictInverse(const Vector &shape, int nSubdivisions, unsigned int dim)
329 //{
330 // KBM childA(*this), childB(*this);
331 // // TODO: Check if solution lies in control net (this->controlNet.[min|max]Coefficient) [>|<] shape
332 // // return 0 if not
333 // // Todo compute the new centers and spread angles for the children
334 // Vector centerA,centerB;
335 // std::vector<Real> spreadAnglesA, spreadAnglesB;
336 // childA.subdivide(centerA,spreadAnglesA);
337 // childB.subdivide(centerB,spreadAnglesB);
338 // return childA.predictInverse(shape,nSubdivisions,dim+1) + childB.predictInverse(shape,nSubdivisions,dim+1);
339 //}
340 
342 {
343  // Recursive search
344  void solveGlobalIK(unsigned int recursion, int side,
345  SolutionSet& solutionSet,
346  Models::KBM_ptr kbm,
347  const Vector& lower,
348  const Vector& upper,
349  Real resolution,
350  Vector spreadAngles,
351  Vector center)
352  {
353 
354  // 1. Check if target is in BBox. Else stop
355  Matrix controlNet;
356  kbm->getSubdivisedNet(center, spreadAngles, controlNet);
357 
358  KBM::Models::KBM::BBCheckType check = Models::KBM::checkBBoxes(lower, upper, controlNet);
359 
360  // <for ARMARX_DEBUG_S>
361  Vector BBlower, BBupper;
362  Models::KBM::getBBox(BBlower, BBupper, controlNet);
363  // <for ARMARX_DEBUG_S/>
364  Solution solution(center, spreadAngles, BBupper, BBlower, check);
365 
366  // if completely disjoint stop search in this interval.
367  if (check == KBM::Models::KBM::DISJOINT) //The tolerance should be implemented by upper lower.
368  {
369  solutionSet.push_back(solution);
370  ARMARX_DEBUG_S << "Disjoint: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl;/*
371  << "lower: " << lower.transpose() << ", upper: " << upper.transpose() << std::endl
372  << "BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;*/
373  return;
374  }
375 
376 
377  // if the interval covers the bounding box, a solution has been found.
378  if (check == KBM::Models::KBM::COVERED)
379  {
380  solutionSet.push_back(solution);
381  ARMARX_DEBUG_S << "Covered: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl; /* << "center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
382  << ", lower: " << lower.transpose() << ", upper: " << upper.transpose()
383  << ", BB-lower: " << BBlower << ", BB-upper: " << BBupper << std::endl;*/
384  return;
385  }
386 
387  // 2. Now check if resolution has been reached.
388  if (spreadAngles.maxCoeff() <= resolution)
389  {
390  // Include those solutions as well?
391  if (check == KBM::Models::KBM::INCLUDED)
392  {
393  ARMARX_DEBUG_S << "Max Res: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl << "center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
394  << ", lower: " << lower.transpose() << ", upper: " << upper.transpose()
395  << ", BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;
396  }
397 
398 
399  solutionSet.push_back(solution);
400 
401  //ARMARX_DEBUG_Sging stuff
402  ARMARX_DEBUG_S << "Max Res: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl; /* <<"center: " << center.transpose() << ", max. angle: " << spreadAngles.maxCoeff()
403  << ", lower: " << lower.transpose() << ", upper: " << upper.transpose()
404  << ", BB-lower: " << BBlower.transpose() << ", BB-upper: " << BBupper.transpose() << std::endl;
405  */
406  }
407  // subdivide the input interval
408  else
409  {
410 
411  ARMARX_DEBUG_S << "Subdivide: " << std::string(recursion * 2, '-') << recursion << ":" << side << std::endl;
412  Vector center1 = center;
413  Vector center2 = center;
414 
415  unsigned int dof = recursion % kbm->getNDoF();
416  center1(dof) = center(dof) + spreadAngles[dof] / 2.0f;
417  center2(dof) = center(dof) - spreadAngles[dof] / 2.0f;
418  spreadAngles[dof] /= 2.0f;
419  solveGlobalIK(recursion + 1, 1, solutionSet, kbm, lower, upper, resolution, spreadAngles, center1);
420  solveGlobalIK(recursion + 1, 2, solutionSet, kbm, lower, upper, resolution, spreadAngles, center2);
421  }
422  }
423 
424  SolutionSet solveGlobalIK(Models::KBM_ptr kbm, const Vector& lower, const Vector& upper, Real resolution)
425  {
426  SolutionSet solution;
427  solveGlobalIK(0, 0, solution, kbm, lower, upper, resolution, kbm->getSpreadAngles(), kbm->getCenter());
428  return solution;
429  }
430 
432  {
433  SolutionSet solution;
434  solveGlobalIK(0, 0, solution, kbm, target, target, resolution, kbm->getSpreadAngles(), kbm->getCenter());
435  return solution;
436  }
437 
438 
439  void GlobalIKBase::solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution)
440  {
441  this->targetLower = lower;
442  this->targetUpper = upper;
443  this->resolution = resolution;
444  this->recurse(0, kbm);
445  }
446 
448  {
449  Vector center;
450  Vector spreadAngles;
451  this->subdivideAngles(side, recursion, kbm, center, spreadAngles);
452  Matrix controlNet;
453 #if 1
454  unsigned int dof = recursion % kbm->getNDoF();
455  kbm->getSubdivisedNet(dof, center[dof], spreadAngles[dof], controlNet);
456 #else
457  Matrix controlNet2;
458  kbm->getSubdivisedNet(center, spreadAngles, controlNet2);
459  ARMARX_DEBUG_S << "Similarity of Dof " << dof << ": " << controlNet.isApprox(controlNet2) << std::endl;
460 #endif
461 
462 
463  return Models::KBM::createKBM(kbm->getNDoF(), kbm->getNDim(), center, spreadAngles, controlNet);
464  }
465 
466  void GlobalIKBase::subdivideAngles(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector& center, Vector& spreadAngles)
467  {
468  unsigned int dof = recursion % kbm->getNDoF();
469  center = kbm->getCenter();
470  spreadAngles = kbm->getSpreadAngles();
471 
472  if (side == GlobalIKBase::LOWER)
473  {
474  center(dof) = center(dof) - spreadAngles[dof] / 2.0f;
475  }
476  else
477  {
478  center(dof) = center(dof) + spreadAngles[dof] / 2.0f;
479  }
480 
481  spreadAngles[dof] /= 2.0f;
482  }
483 
484  unsigned int GlobalIKExtensive::recurse(unsigned int level, Models::KBM_ptr kbm)
485  {
486  KBM::Models::KBM::BBCheckType check = kbm->checkBBox(this->targetLower, this->targetUpper);
487  // <for ARMARX_DEBUG_S/>
488  Vector BBupper, BBlower;
489  kbm->getBBox(BBupper, BBlower);
490  Solution solution(kbm->getCenter(), kbm->getSpreadAngles(), BBupper, BBlower, check);
491 
492  // 1. Definitively a Solution or no definitively solution
493  if (check == KBM::Models::KBM::DISJOINT)
494  {
495  solutions.push_back(solution);
496  return 0;
497  }
498 
499  if (check == KBM::Models::KBM::COVERED)
500  {
501  solutions.push_back(solution);
502  return 1;
503  }
504 
505  // 2. Now check if resolution has been reached.
506  if (kbm->getSpreadAngles().maxCoeff() <= resolution)
507  {
508  solutions.push_back(solution);
509  return 0;
510  }
511  else
512  {
513  //solutions.push_back(solution);
514 
516  int n = this->recurse(level + 1, sub);
517  sub = this->subdivideKBM(GlobalIKBase::UPPER, level, kbm);
518  kbm.reset();
519  n += this->recurse(level + 1, sub);
520  return n;
521  }
522 
523  return 0;
524  }
525 
526  unsigned int GlobalIKSemiBreadth::recurse(unsigned int level, Models::KBM_ptr kbm)
527  {
528  this->targetVolume = (this->targetUpper - this->targetLower).prod();
529  Real overlap = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
530 
531  KBM::Models::KBM::BBCheckType check = kbm->checkBBox(this->targetLower, this->targetUpper);
532  // <for ARMARX_DEBUG_S/>
533  Solution solution(kbm, check);
534  bool breadth = kbm->getSpreadAngles().maxCoeff() > this->semiBreadthRecursion ;
535 
536  if (breadth)
537  {
538  ARMARX_DEBUG_S << "breadth " << std::string(level * 2, '-') << level << std::endl;
539  }
540  else
541  {
542  ARMARX_DEBUG_S << "depth " << std::string(level * 2, '-') << level << ": " << kbm->getSpreadAngles().maxCoeff() / M_PI * 180.0f << std::endl;
543  }
544 
545  // 1. Definitively a Solution or no definitively solution
546  if (check == KBM::Models::KBM::DISJOINT)
547  {
548  if (breadth && (check & solutionSelect))
549  {
550  solutions.push_back(solution);
551  }
552 
553  ARMARX_DEBUG_S << "Dead End: " << overlap << std::endl;
554  assert(overlap == 0.0f);
555  return 0;
556  }
557 
558  if (check == KBM::Models::KBM::COVERED)
559  {
560  ARMARX_DEBUG_S << "Solution found: " << overlap << std::endl;
561  solutions.push_back(solution);
562  Real overlap = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume());
563  ARMARX_CHECK_EXPRESSION(overlap == 1.0f);
564  return 1;
565  }
566 
567  // 2. Now check if resolution has been reached.
568  if (kbm->getSpreadAngles().maxCoeff() <= resolution)
569  {
570  ARMARX_DEBUG_S << "Max recursion, target overlap " << overlap * 100.f << ", " << kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume()) << "%"
571  << std::endl;
572  misses++;
573  return 0;
574  }
575  else
576  {
577  if (!breadth && (check & solutionSelect))
578 
579  {
580  solutions.push_back(solution);
581  }
582 
583 
584  if (!breadth)
585  {
586  Real ratio = kbm->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, kbm->getVolume());
587  GraphNode initial(kbm, ratio, level, kbm->getVolume());
588  SolutionSet s = this->runDijkstra(initial);
589  solutions.insert(solutions.end(), s.begin(), s.end());
590  return s.size();
591 
592  }
593  else
594  {
595  Models::KBM_ptr sub1 = this->subdivideKBM(GlobalIKBase::LOWER, level, kbm);
596  Models::KBM_ptr sub2 = this->subdivideKBM(GlobalIKBase::UPPER, level, kbm);
597  Real ratio1 = sub1->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
598  Real ratio2 = sub2->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, this->targetVolume);
599  //Real ratio1 = sub1->getOverlappingVolumeRatio(this->targetLower,this->targetUpper, kbm->getVolume());
600  //Real ratio2 = sub2->getOverlappingVolumeRatio(this->targetLower,this->targetUpper, kbm->getVolume());
601  ARMARX_DEBUG_S << "Reduction by subdivision: " << 100.f* sub1->getVolume() / kbm->getVolume() << "%, " << sub2->getVolume() / kbm->getVolume() * 100.f << "%" << std::endl;
602  ARMARX_DEBUG_S << "Target overlapping: " << ratio1 * 100.f << "%, " << ratio2 * 100.f << "%, " << this->targetVolume << std::endl;
603  kbm.reset();
604 
605  //if (std::max(ratio1,ratio2)<0.75){
606  // ARMARX_DEBUG_S << "Not enough overlap\n";
607  // return 0;
608  // }
609  int n;
610 
611  if (ratio1 > ratio2)
612  {
613  n = this->recurse(level + 1, sub1);
614  }
615  else
616  {
617  n = this->recurse(level + 1, sub2);
618  }
619 
620  if (breadth || n == 0)
621  {
622  //sub = this->subdivideKBM(GlobalIKBase::UPPER,level,kbm);
623  //kbm.reset();
624  if (ratio1 > ratio2)
625  {
626  n += this->recurse(level + 1, sub2);
627  }
628  else
629  {
630  n += this->recurse(level + 1, sub1);
631  }
632  }
633 
634  return n;
635  }
636  }
637 
638  assert(0);
639  return 0;
640 
641  }
642 
643  // ReachabilityTree Klasse anlegen!
644 
645  // Reuse GraphNode for extensive search too
646 
647  bool operator<(const GraphNode& left, const GraphNode& right)
648  {
649  if (left.ratio != right.ratio)
650  {
651  return left.ratio < right.ratio;
652  }
653 
654  return false;
655  }
656 
657 
659  {
660  SolutionSet s;
661  std::priority_queue<GraphNode> priority_queue;
662  //priority_queue.push(GraphNode(model,this->targetLower,this->targetUpper,this->targetVolume));
663  priority_queue.push(initial);
664  int counter = 0;
665 
666  while (!priority_queue.empty())
667  {
668  GraphNode topNode = priority_queue.top();
669  ARMARX_DEBUG_S << topNode.ratio << ", " ;
670  priority_queue.pop();
671 
672  if (topNode.ratio == 1.0)
673  {
674  s.push_back(Solution(topNode.model, Models::KBM::COVERED));
675  return s;
676  }
677 
678  if (topNode.model->getSpreadAngles().maxCoeff() >= resolution)
679  for (unsigned int side = GlobalIKBase::LOWER; side != GlobalIKBase::END; side++)
680  {
681  Models::KBM_ptr child = this->subdivideKBM((GlobalIKBase::Side) side, topNode.level + 1, topNode.model);
682  Real volume = child->getVolume();
683  Real ratio = child->getOverlappingVolumeRatio(this->targetLower, this->targetUpper, volume);
684 
685  if (ratio != 0.0f)
686  {
687  counter++;
688  priority_queue.push(GraphNode(child, ratio, topNode.level + 1, volume));
689  std::cout << side + 1 << ". Ratio: " << ratio << " (" << ratio / topNode.ratio * 100.0f << "%), Vol.: " << volume / topNode.volume * 100.f << "%, ";
690  }
691  else
692  {
693  std::cout << "0 ";
694  }
695  }
696  else
697  {
698  std::cout << "Resolution: " << topNode.model->getSpreadAngles().maxCoeff() ;
699  }
700 
701  std::cout << ", " << topNode.model->getSpreadAngles().maxCoeff() / M_PI * 180.0f << "(" << resolution / M_PI * 180.0f << ")" << ", Level: " << topNode.level << std::endl;
702  }
703 
704  ARMARX_DEBUG_S << counter << std::endl;
705  return s;
706  }
707 
708  GlobalIKSemiBreadth::GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect) : semiBreadthRecursion(_semiBreadthRecursion), solutionSelect(_solutionSelect)
709  {
710  this->misses = 0;
711  }
712 }
memoryx::KBM::Inverse::GraphNode::model
Models::KBM_ptr model
Definition: kbm.h:330
kbm.h
memoryx::KBM::Models::KBM::checkBBoxes
static BBCheckType checkBBoxes(const Vector &lower, const Vector &upper, const Matrix &controlNet, Real tolerance=0.0f)
Check whether an interval overlaps the the bounding box of a given control net.
Definition: inverse.cpp:236
memoryx::KBM::Inverse::GlobalIKBase::SolutionSet
std::vector< Solution > SolutionSet
Definition: kbm.h:299
memoryx::KBM::Models::KBM_ptr
std::shared_ptr< KBM > KBM_ptr
Definition: kbm.h:68
memoryx::KBM::Inverse::GraphNode::volume
Real volume
Definition: kbm.h:333
memoryx::KBM::Models::KBM::INCLUDED
@ INCLUDED
The interval is included in the bounding box .
Definition: kbm.h:95
memoryx::KBM::Models::KBM::subdivide
void subdivide(const Vector &center, const Vector &newSpreadAngles)
Work in progress This method subdivides the KBM representation.
Definition: inverse.cpp:28
memoryx::KBM::Matrix
Eigen::MatrixXd Matrix
Definition: kbm.h:38
memoryx::KBM::Inverse::GlobalIKSemiBreadth::GlobalIKSemiBreadth
GlobalIKSemiBreadth(Real _semiBreadthRecursion, int _solutionSelect=Models::KBM::COVERED)
Definition: inverse.cpp:708
memoryx::KBM::Inverse::GlobalIKBase::solutions
SolutionSet solutions
Definition: kbm.h:311
memoryx::KBM::Models::KBM::DISJOINT
@ DISJOINT
The bounding box and interval are disjoint .
Definition: kbm.h:99
memoryx::KBM::Inverse::GlobalIKBase::subdivideAngles
void subdivideAngles(Side side, unsigned int recursion, Models::KBM_ptr kbm, Vector &center, Vector &spreadAngles)
Subdivides the joint angles only. Can be used to select in which direction to subdivise first.
Definition: inverse.cpp:466
memoryx::KBM::Inverse::Solution
Definition: kbm.h:365
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:688
memoryx::KBM::Inverse::GlobalIKSemiBreadth::solutionSelect
int solutionSelect
Definition: kbm.h:360
memoryx::KBM::Models::KBM::checkBBox
BBCheckType checkBBox(const Vector &lower, const Vector &upper, Real tolerance=0.0f) const
Check whether an interval overlaps the the bounding box of the control points.
Definition: inverse.cpp:231
memoryx::KBM::Inverse::GlobalIKBase::recurse
virtual unsigned int recurse(unsigned int level, Models::KBM_ptr kbm)=0
memoryx::KBM::Inverse::GlobalIKSemiBreadth::misses
int misses
Definition: kbm.h:355
memoryx::KBM::Inverse::GlobalIKBase::Solution
Definition: kbm.h:278
memoryx::KBM::Inverse::operator<
bool operator<(const GraphNode &left, const GraphNode &right)
Definition: inverse.cpp:647
memoryx::KBM::Models::KBM::getSubdivisedNet
void getSubdivisedNet(const Vector newCenter, Vector newSpreadAngles, Matrix &resultingControlNet) const
Work in progress This method computes the control net of a subdivised KBM instance.
Definition: inverse.cpp:138
memoryx::KBM::Inverse::GlobalIKSemiBreadth::semiBreadthRecursion
Real semiBreadthRecursion
Definition: kbm.h:359
memoryx::KBM::Vector
Eigen::VectorXd Vector
Definition: kbm.h:39
memoryx::KBM::Models::KBM::COVERED
@ COVERED
The bounding box is covered by the interval (most interesting case for inverse kinematics).
Definition: kbm.h:97
memoryx::KBM::Inverse::GlobalIKBase::Side
Side
Definition: kbm.h:301
memoryx::KBM::Inverse::GlobalIKBase::END
@ END
Definition: kbm.h:305
memoryx::VariantType::GraphNode
const armarx::VariantTypeId GraphNode
Definition: GraphNode.h:39
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:198
M_PI
#define M_PI
Definition: MathTools.h:17
memoryx::KBM::Models::KBM::createKBM
static KBM_ptr createKBM(int nDoF, int dim, const Vector &center, const Vector &spreadAngles, const Matrix &controlNet)
Factory methods.
Definition: kbm.cpp:407
memoryx::KBM::Inverse::GraphNode::ratio
Real ratio
Definition: kbm.h:331
memoryx::KBM::Models
Where the model representation for a Body Schema (especially the Kinematic BĀ“ezier Maps) reside.
Definition: inverse.cpp:19
memoryx::KBM::Models::KBM::BBCheckType
BBCheckType
Cases for the check of bounding boxes (i.e., an interval ) and an interval ( )both in Cartesian coor...
Definition: kbm.h:92
memoryx::KBM::Inverse::GlobalIKSemiBreadth::recurse
unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override
Definition: inverse.cpp:526
max
T max(T t1, T t2)
Definition: gdiam.h:48
memoryx::KBM::Models::KBM::getBBox
static void getBBox(Vector &lower, Vector &upper, const Matrix &controlNet)
Helper function that extracts the bounding box of a control net.
Definition: inverse.cpp:283
memoryx::KBM::Inverse
Namespace for algorithms related to solving the inverse kinematics.
Definition: inverse.cpp:341
memoryx::KBM::Inverse::solveGlobalIK
void solveGlobalIK(unsigned int recursion, int side, SolutionSet &solutionSet, Models::KBM_ptr kbm, const Vector &lower, const Vector &upper, Real resolution, Vector spreadAngles, Vector center)
Definition: inverse.cpp:344
ExpressionException.h
memoryx::KBM::Inverse::GlobalIKBase::resolution
Real resolution
Definition: kbm.h:321
memoryx::KBM::Models::KBM::getOverlappingVolume
Real getOverlappingVolume(const Vector &lower, const Vector &upper)
Definition: inverse.cpp:308
memoryx::KBM::Inverse::GraphNode
Definition: kbm.h:325
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
memoryx::KBM::Models::KBM::getVolume
Real getVolume()
Definition: inverse.cpp:294
memoryx::KBM::Inverse::GlobalIKBase::subdivideKBM
Models::KBM_ptr subdivideKBM(GlobalIKBase::Side side, unsigned int recursion, Models::KBM_ptr kbm)
Subdivides the kbm and creates new KBM. Calls GlobalIKBase::subdivideAngles().
Definition: inverse.cpp:447
memoryx::KBM::Inverse::GlobalIKSemiBreadth::targetVolume
Real targetVolume
Definition: kbm.h:358
memoryx::KBM::Inverse::GlobalIKSemiBreadth::runDijkstra
GlobalIKBase::SolutionSet runDijkstra(KBM::Inverse::GraphNode initial)
Definition: inverse.cpp:658
memoryx::KBM::Inverse::GraphNode::level
int level
Definition: kbm.h:332
sub
Point sub(const Point &x, const Point &y)
Definition: point.hpp:43
memoryx::KBM::Models::KBM::getOverlappingVolumeRatio
Real getOverlappingVolumeRatio(const Vector &lower, const Vector &upper, Real targetVolume)
Definition: inverse.cpp:302
memoryx::KBM::Inverse::GlobalIKBase::solve
void solve(Models::KBM_ptr kbm, const Vector lower, const Vector upper, Real resolution=2.0f *M_PI/180.0f)
Definition: inverse.cpp:439
memoryx::KBM::Inverse::GlobalIKExtensive::recurse
unsigned int recurse(unsigned int level, Models::KBM_ptr kbm) override
Definition: inverse.cpp:484
memoryx::KBM::Models::KBM::OVERLAPPING
@ OVERLAPPING
The interval and bounding box are partially overlapping (if none of the above applies).
Definition: kbm.h:101
Logging.h
min
T min(T t1, T t2)
Definition: gdiam.h:42
memoryx::KBM::Inverse::GlobalIKBase::LOWER
@ LOWER
Definition: kbm.h:303
memoryx::KBM::Real
double Real
Type definition of the underlying Realing point type.
Definition: kbm.h:37
memoryx::KBM::Inverse::GlobalIKBase::targetLower
Vector targetLower
Definition: kbm.h:320
memoryx::KBM::Inverse::GlobalIKBase::UPPER
@ UPPER
Definition: kbm.h:304
memoryx::KBM::Inverse::SolutionSet
std::vector< Solution > SolutionSet
Return type of the global inverse kinematics solvers.
Definition: kbm.h:378
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
memoryx::KBM::Inverse::GlobalIKBase::targetUpper
Vector targetUpper
Definition: kbm.h:320