SimoxObjectWrapper.cpp
Go to the documentation of this file.
1 /*
2 * This file is part of ArmarX.
3 *
4 * ArmarX is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License version 2 as
6 * published by the Free Software Foundation.
7 *
8 * ArmarX is distributed in the hope that it will be useful, but
9 * WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11 * GNU General Public License for more details.
12 *
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 *
16 * @package MemoryX::Helpers
17 * @author ALexey Kozlov ( kozlov at kit dot edu)
18 * @date 2012
19 * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20 * GNU General Public License
21 */
22 
23 #include "SimoxObjectWrapper.h"
24 
25 // ArmarXCore
29 
30 // MemoryX
33 
34 // Simox-VirtualRobot
35 #include <VirtualRobot/XML/ObjectIO.h>
36 #include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
37 
38 #include <SimoxUtility/algorithm/string/string_tools.h>
39 
40 // Coin3D headers
41 #include <Inventor/nodes/SoSeparator.h>
42 #include <Inventor/nodes/SoNode.h>
43 #include <Inventor/nodes/SoGroup.h>
44 #include <Inventor/nodes/SoSeparator.h>
45 #include <Inventor/nodes/SoImage.h>
46 #include <Inventor/nodes/SoFile.h>
47 #include <Inventor/nodes/SoTexture2.h>
48 #include <Inventor/nodes/SoTexture3.h>
49 #include <Inventor/VRMLnodes/SoVRMLImageTexture.h>
50 #include <Inventor/actions/SoSearchAction.h>
51 
52 // Eigen
53 #include <Eigen/Eigenvalues>
54 
55 
56 #include <filesystem>
57 #include <cfloat>
59 
60 
62 {
63  namespace fs = std::filesystem;
64 
65  const std::string ATTR_TEXTURE_FILES = "texFiles";
66  const std::string ATTR_IV_FILE = "IvFile";
67  const std::string ATTR_IV_COLLISION_FILE = "IvFileCollision";
68  const std::string ATTR_MANIPULATION_FILE = "ManipulationFile";
69  const std::string ATTR_UNCERTAINTY_COLOR_MAP = "UncertaintyColorMap";
70  const std::string ATTR_UNCERTAINTY_TRANSPARENCY = "UncertaintyTransparency";
71  const std::string ATTR_PUTDOWN_ORIENTATION_RPY = "PutdownOrientationRPY";
72 
73  const std::string MO_TEMP_FILE_NAME = "___tmpManipulationObject___.xml";
74 
75  // default uncertainty visu params
76  const float ELLIPSE_RADIUS_IN_SIGMAS = 3.;
77  const float HEATMAP_MIN_VARIANCE = 1.; // mm
78  const float HEATMAP_DENSITY_THRESHOLD = expf(-15.);
79  const int HEATMAP_GRID_SIZE = 100;
80 
81  SimoxObjectWrapper::SimoxObjectWrapper(const GridFileManagerPtr& gfm, VirtualRobot::RobotIO::RobotDescription loadMode) :
83  uncertaintyVisuType(eEllipse)
84  {
85  coinVisFactory.reset(new VirtualRobot::CoinVisualizationFactory());
86 
87  cachePath = fileManager->getFileCachePath();
88  moTempFile = (cachePath / MO_TEMP_FILE_NAME).string();
89 
90  ellipseRadiusInSigmas = ELLIPSE_RADIUS_IN_SIGMAS;
91  heatmapMinVariance = HEATMAP_MIN_VARIANCE; // mm
92  heatmapDensityThreshold = HEATMAP_DENSITY_THRESHOLD;
93  heatmapGridSize = HEATMAP_GRID_SIZE;
94  if (loadMode != VirtualRobot::RobotIO::eFull && loadMode != VirtualRobot::RobotIO::eCollisionModel)
95  {
96  throw armarx::LocalException("Loadmode must either be eFull oder eCollisionModel");
97  }
98  this->loadMode = loadMode;
99  }
100 
102  {
103  }
104 
106  {
107  return new SimoxObjectWrapper(*this);
108  }
109 
110  VirtualRobot::VisualizationNodePtr SimoxObjectWrapper::getVisualization() const
111  {
112  if (simoxObject)
113  {
114  return simoxObject->getVisualization();
115  }
116  else
117  {
118  cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
119  const std::string ivFName = cacheAttributeFile(ATTR_IV_FILE, true);
120  return coinVisFactory->getVisualizationFromFile(ivFName);
121  }
122  }
123 
124  VirtualRobot::CollisionModelPtr SimoxObjectWrapper::getCollisionModel() const
125  {
126  if (simoxObject)
127  {
128  return simoxObject->getCollisionModel();
129  }
130  else
131  {
132  cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
133  const std::string ivFName = cacheAttributeFile(ATTR_IV_COLLISION_FILE, true);
134  return VirtualRobot::CollisionModelPtr(new VirtualRobot::CollisionModel(coinVisFactory->getVisualizationFromFile(ivFName)));
135  }
136  }
137 
138  VirtualRobot::ManipulationObjectPtr SimoxObjectWrapper::getManipulationObject() const
139  {
140  ARMARX_TRACE;
141  if (!simoxObject)
142  {
143  ARMARX_TRACE;
144  loadSimoxObject();
145  }
146 
147  return simoxObject;
148  }
149 
151  {
152  cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
153  const std::string ivFName = cacheAttributeFile(ATTR_IV_FILE, true);
154  const std::string ivFNameCollision = cacheAttributeFile(ATTR_IV_COLLISION_FILE, true);
155 
156  const std::string moFName = cacheAttributeFile(ATTR_MANIPULATION_FILE, true);
157 
158  return moFName;
159  }
160 
161  std::string SimoxObjectWrapper::setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string& filesDBName)
162  {
164  return storeManipulationObject(mo, filesDBName);
165  }
166 
167  void SimoxObjectWrapper::setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo)
168  {
169  // clear(false);
170  simoxObject = mo;
171  }
172 
173  void SimoxObjectWrapper::setAndStoreManipulationFile(const std::string& xmlFName, const std::string& filesDBName)
174  {
175  if (!xmlFName.empty())
176  {
177  VirtualRobot::ManipulationObjectPtr mo = loadManipulationObjectFile(xmlFName);
178  const std::string fileId = setAndStoreManipulationObject(mo, filesDBName);
179  ARMARX_INFO_S << " Saved manipulation object file: " << xmlFName << " (Id: " << fileId << ")";
180  }
181  else
182  {
183  ARMARX_WARNING_S << "xmlFName empty";
184  }
185  }
186 
187  void SimoxObjectWrapper::setManipulationFile(const std::string& xmlFName)
188  {
189  if (!xmlFName.empty())
190  {
191  ARMARX_INFO_S << " Setting manipulation object file: " << xmlFName << armarx::flush;
192  VirtualRobot::ManipulationObjectPtr mo = loadManipulationObjectFile(xmlFName);
194  }
195  }
196 
197  void SimoxObjectWrapper::setAndStoreModelIVFiles(const std::string& ivFNameVis, const std::string& ivFNameCollision,
198  const std::string& filesDBName)
199  {
200  VirtualRobot::ManipulationObjectPtr mo = createManipulationObjectFromIvFiles(getEntity()->getName(), ivFNameVis, ivFNameCollision);
201  const std::string ivFileId = setAndStoreManipulationObject(mo, filesDBName);
202  ARMARX_INFO_S << " Saved IV model file: " << ivFNameVis << " (Id: " << ivFileId << ")";
203  }
204 
205  void SimoxObjectWrapper::setModelIVFiles(const std::string& ivFNameVis, const std::string& ivFNameCollision)
206  {
207  VirtualRobot::ManipulationObjectPtr mo = createManipulationObjectFromIvFiles(getEntity()->getName(), ivFNameVis, ivFNameCollision);
209  }
210 
212  {
213  this->uncertaintyVisuType = visuType;
214  }
215 
216  void SimoxObjectWrapper::setUncertaintyVisuParams(float ellipseRadiusInSigmas, float heatmapMinVariance,
217  float heatmapDensityThreshold, int heatmapGridSize)
218  {
219  this->ellipseRadiusInSigmas = ellipseRadiusInSigmas;
220  this->heatmapMinVariance = heatmapMinVariance;
221  this->heatmapDensityThreshold = heatmapDensityThreshold;
222  this->heatmapGridSize = heatmapGridSize;
223  }
224 
225  VirtualRobot::ColorMap SimoxObjectWrapper::getUncertaintyColorMap()
226  {
227  if (getEntity()->hasAttribute(ATTR_UNCERTAINTY_COLOR_MAP))
228  {
229  const std::string mapName = getEntity()->getAttribute(ATTR_UNCERTAINTY_COLOR_MAP)->getValue()->getString();
230 
231  if (mapName == "eHot")
232  {
233  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
234  }
235  else if (mapName == "eIntensity")
236  {
237  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eIntensity);
238  }
239  else if (mapName == "eRed")
240  {
241  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eRed);
242  }
243  else if (mapName == "eGreen")
244  {
245  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGreen);
246  }
247  else if (mapName == "eBlue")
248  {
249  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eBlue);
250  }
251  else if (mapName == "eHotAlpha")
252  {
253  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHotAlpha);
254  }
255  else if (mapName == "eRedAlpha")
256  {
257  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eRedAlpha);
258  }
259  else if (mapName == "eGreenAlpha")
260  {
261  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGreenAlpha);
262  }
263  else if (mapName == "eBlueAlpha")
264  {
265  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eBlueAlpha);
266  }
267  else
268  {
269  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
270  }
271  }
272  else
273  {
274  return VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot);
275  }
276  }
277 
278  float SimoxObjectWrapper::getUncertaintyEllipseTransparency()
279  {
280  if (getEntity()->hasAttribute(ATTR_UNCERTAINTY_TRANSPARENCY))
281  {
282  return getEntity()->getAttribute(ATTR_UNCERTAINTY_TRANSPARENCY)->getValue()->getFloat();
283  }
284  else
285  {
286  return 0.3f;
287  }
288  }
289 
290 
291 
292  void SimoxObjectWrapper::updateFromEntity(const EntityBasePtr& entity)
293  {
294  setEntity(entity);
295 
296  refreshVisuPose();
297  }
298 
300  {
301  // std::cout << "Refreshing visu pose... " << std::endl;
302 
303  // only object instances are supported so far
304  ObjectInstancePtr obj = ObjectInstancePtr::dynamicCast(getEntity());
305 
306  if (!obj)
307  {
308  return;
309  }
310 
312 
313  // Position
314  armarx::FramedPositionPtr objPos = obj->getPosition();
315  if (objPos)
316  {
317  m.block(0, 3, 3, 1) = objPos->toEigen();
318  }
319 
320  // Orientation
321  armarx::FramedOrientationPtr objOrient = obj->getOrientation();
322 
323  if (objOrient)
324  {
325  m.block(0, 0, 3, 3) = objOrient->toEigen();
326  }
327 
328  VirtualRobot::ManipulationObjectPtr mo = getManipulationObject();
329 
330  if (!mo)
331  {
332  //ARMARX_WARNING_S << "Manipulation object is null";
333  }
334  else
335  {
336  mo->setGlobalPose(m);
337  }
338 
339 
340  // Position uncertainty
341  EntityAttributeBasePtr posAttr = obj->getPositionAttribute();
342  ProbabilityMeasureBasePtr posDist = posAttr->getUncertainty();
343 
344  if (posDist)
345  {
346  static const std::string UNCERTAINTY_VISU_NAME = "positionUncertainty";
347  VirtualRobot::VisualizationNodePtr moVis;
348 
349  if (mo)
350  {
351  moVis = mo->getVisualization();
352 
353  if (moVis)
354  {
355  moVis->detachVisualization(UNCERTAINTY_VISU_NAME);
356  }
357  else
358  {
359  //ARMARX_WARNING_S << "Manipulation object's visu is null";
360  }
361  }
362 
363 
364  GaussianMixtureDistributionBasePtr posGM = GaussianMixtureDistribution::FromProbabilityMeasure(posDist);
365 
366  if (posGM)
367  {
368 
369  VirtualRobot::VisualizationNodePtr posDistVisu;
370 
371  posGM->normalize();
372 
373  const VirtualRobot::ColorMap cmap = getUncertaintyColorMap();
374 
375  switch (uncertaintyVisuType)
376  {
377  case eEllipse:
378  posDistVisu = getUncertaintyEllipsesVisu(posGM, objPos->toEigen(), cmap, getUncertaintyEllipseTransparency());
379  break;
380 
381  case eHeatMap:
382  posDistVisu = getUncertaintyHeatVisu(posGM, objPos->toEigen(), false, cmap);
383  break;
384 
385  case eHeatSurface:
386  posDistVisu = getUncertaintyHeatVisu(posGM, objPos->toEigen(), true, cmap);
387  break;
388 
389  default:
390  ARMARX_ERROR_S << "Uncertainty visualization type not supported!" << armarx::flush;
391  }
392 
393  if (posDistVisu)
394  {
395  // Object orientation shouldn't affect uncertainty, but it does,
396  // since technically attached visu's are child nodes of main object.
397  // Thus apply inverse transformation to compensate for this
398  Eigen::Matrix3f objOrientInv = objOrient->toEigen().inverse();
400  dm.block(0, 0, 3, 3) = objOrientInv;
401  posDistVisu->setGlobalPose(dm);
402 
403  if (moVis)
404  {
405  moVis->attachVisualization(UNCERTAINTY_VISU_NAME, posDistVisu);
406  }
407  else
408  {
409  //ARMARX_WARNING_S << "Manipulation object's visu is null";
410  }
411  }
412  }
413  }
414  }
415 
416  VirtualRobot::VisualizationNodePtr SimoxObjectWrapper::getUncertaintyEllipsesVisu(GaussianMixtureDistributionBasePtr posGM, const Eigen::Vector3f& objPos,
417  const VirtualRobot::ColorMap& cmap, float transparency)
418  {
419  static float COV_SCALE_FACTOR = 1.f; // make cov more realistic
420 
421  std::vector<VirtualRobot::VisualizationNodePtr> compVisuList;
422 
423  for (int i = 0; i < posGM->size(); ++i)
424  {
425  GaussianMixtureComponent comp = posGM->getComponent(i);
426  NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
427 
428  Eigen::Matrix3f cov = gaussian->toEigenCovariance() * COV_SCALE_FACTOR;
429 
430  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> es(cov);
431  Eigen::Vector3f eval = es.eigenvalues();
432  Eigen::Matrix3f evec = es.eigenvectors();
433  // std::cout << "evalues: " << eval << ", evectors: " << evec << std::endl << std::endl;
434 
435  VirtualRobot::VisualizationFactory::Color color = cmap.getColor(comp.weight);
436 
437  SoMaterial* matBody = new SoMaterial;
438  matBody->diffuseColor.setValue(color.r, color.b, color.g);
439  matBody->ambientColor.setValue(color.r, color.b, color.g);
440  matBody->transparency.setValue(transparency);
441 
442  VirtualRobot::VisualizationNodePtr compVisu(new VirtualRobot::CoinVisualizationNode(coinVisFactory->CreateEllipse(
443  ellipseRadiusInSigmas * sqrt(eval(0)) / 1000., // mm -> m
444  ellipseRadiusInSigmas * sqrt(eval(1)) / 1000.,
445  ellipseRadiusInSigmas * sqrt(eval(2)) / 1000.,
446  matBody, // default material
447  false // hide axis
448  )
449  ));
450 
452  dm.block(0, 0, 3, 3) = evec;
453  dm.block(0, 3, 3, 1) = gaussian->toEigenMean() - objPos;
454  compVisu->setGlobalPose(dm);
455 
456  compVisuList.push_back(compVisu);
457  }
458 
459  return coinVisFactory->createUnitedVisualization(compVisuList);
460  }
461 
462  VirtualRobot::VisualizationNodePtr SimoxObjectWrapper::getUncertaintyHeatVisu(GaussianMixtureDistributionBasePtr posGM, const Eigen::Vector3f& objPos, bool heatSurface,
463  const VirtualRobot::ColorMap& cmap)
464  {
465  const float SCALE_FACTOR = sqrt(2 * M_PI * heatmapMinVariance); // scale axes in a way that the height of distribution with sigmaX=sigmaY=1mm is 1.0
466  const float COV_SCALE_FACTOR = 1.f; // make cov more realistic
467  const float LOG_PROB_CUTOFF = -logf(heatmapDensityThreshold);
468  const float MAX_CLUSTER_VARIANCE = 100; // AABB side size in mm
469 
470  std::vector<VirtualRobot::VisualizationNodePtr> clusterVisuList;
471 
472  WestGMMReducer reducer;
473  GaussianMixtureDistributionBasePtr reducedGMM = reducer.reduceByMaxAABB(posGM, MAX_CLUSTER_VARIANCE);
474 
475  // iterate over clusters
476  for (int k = 0; k < reducedGMM->size(); ++k)
477  {
478 
479  // calculate common mean
480  /* float minX = FLT_MAX, minY = FLT_MAX, maxX = FLT_MIN, maxY = FLT_MIN;
481  for (int i = 0; i < posGM->size(); ++i)
482  {
483  GaussianMixtureComponent comp = posGM->getComponent(i);
484  NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
485 
486  Eigen::Vector3f mean = gaussian->toEigenMean();
487  float varX = 6. * sqrt(gaussian->getCovariance(0, 0) * COV_SCALE_FACTOR);
488  float varY = 6. * sqrt(gaussian->getCovariance(1, 1) * COV_SCALE_FACTOR);
489  minX = std::min(minX, mean(0) - varX);
490  minY = std::min(minY, mean(1) - varY);
491  maxX = std::max(maxX, mean(0) + varX);
492  maxY = std::max(maxY, mean(1) + varY);
493  }
494  float sx = maxX - minX;
495  float sy = maxY - minY;*/
496 
497  NormalDistributionPtr compGaussian = NormalDistributionPtr::dynamicCast(reducedGMM->getComponent(k).gaussian);
498  Eigen::Vector3f clusterCenter = compGaussian->toEigenMean();
499  // clusterCenter << minX + sx / 2., minY + sy / 2., objPos(2);
500  std::cout << "Cluster mean: " << clusterCenter << std::endl;
501  // std::cout << "MinX: " << minX << ", MaxX: " << maxX << ", minY: " << minY << ", MaxY: " << maxY << std::endl;
502 
503  clusterCenter /= SCALE_FACTOR; // scale mean
504 
505  float sx = 6. * sqrtf(compGaussian->getCovariance(0, 0));
506  float sy = 6. * sqrtf(compGaussian->getCovariance(1, 1));
507  int GRID_SIZE_X = (int) round(sqrt(sx / sy) * (float) heatmapGridSize);
508  int GRID_SIZE_Y = (int) round(sqrt(sy / sx) * (float) heatmapGridSize);
509  float CELL_SIZE_X = sx / (float) GRID_SIZE_X / SCALE_FACTOR; //
510  float CELL_SIZE_Y = sy / (float) GRID_SIZE_Y / SCALE_FACTOR; //
511 
512  Eigen::MatrixXf grid = Eigen::MatrixXf::Zero(GRID_SIZE_X, GRID_SIZE_Y);
513 
514  for (int i = 0; i < posGM->size(); ++i)
515  {
516  GaussianMixtureComponent comp = posGM->getComponent(i);
517  NormalDistributionPtr gaussian = NormalDistributionPtr::dynamicCast(comp.gaussian);
518 
519  FloatVector meanVec = comp.gaussian->getMean();
520  Eigen::Vector2f mean;
521  mean << meanVec[0], meanVec[1];
522  mean /= SCALE_FACTOR; // scale mean
523 
524  Eigen::Matrix3f cov3D = gaussian->toEigenCovariance();
525  Eigen::Matrix2f cov = cov3D.block(0, 0, 2, 2) * COV_SCALE_FACTOR / (SCALE_FACTOR * SCALE_FACTOR);
526  Eigen::Matrix2f covInv = cov.inverse();
527  float covDet = cov.determinant();
528  float covDetSqrtInv = powf(covDet, -0.5f);
529 
530  Eigen::Vector2f pos;
531  pos(0) = clusterCenter(0) - (float)(GRID_SIZE_X * CELL_SIZE_X) / 2.;
532 
533  for (int xc = 0; xc < GRID_SIZE_X; ++xc)
534  {
535  pos(1) = clusterCenter(1) - (float)(GRID_SIZE_Y * CELL_SIZE_Y) / 2.;
536 
537  for (int yc = 0; yc < GRID_SIZE_Y; ++yc)
538  {
539  float pdfValue = (LOG_PROB_CUTOFF + log(evaluate2DGaussian(pos, mean, covInv, covDetSqrtInv))) / LOG_PROB_CUTOFF * comp.weight;
540 
541  // normalize value to [0, 1]
542  if (pdfValue < 0.)
543  {
544  // probablity density too low -> set it to 0, since we can ignore such small differences for visu
545  // std::cout << "Cutting off value: " << pdfValue << ", new value: 0.0" << std::endl;
546  pdfValue = 0.f;
547  }
548  else if (pdfValue > 1.)
549  {
550  // probablity density too high (i.e. stddev < 1 mm) -> cut off at 1
551  pdfValue = 1.f;
552  }
553 
554  grid(xc, yc) += pdfValue;
555 
556  // std::cout << "Pos: " << pos << ", mean: " << mean << ", value: " << grid(xc, yc) << std::endl;
557  pos(1) += CELL_SIZE_Y; // step y
558  }
559 
560  pos(0) += CELL_SIZE_X; // step x
561  }
562  }
563 
564  VirtualRobot::VisualizationNodePtr clusterVisu;
565 
566  if (heatSurface)
567  {
568  clusterVisu.reset(new VirtualRobot::CoinVisualizationNode(coinVisFactory->Create2DHeightMap(
569  grid, // prob map
570  CELL_SIZE_X * SCALE_FACTOR, // x cell size in mm
571  CELL_SIZE_Y * SCALE_FACTOR, // y cell size in mm
572  100., // height in mm
573  cmap, // color map
574  false, // draw zero cells
575  false // draw lines between cells
576  )
577  ));
578  }
579  else
580  {
581  clusterVisu.reset(new VirtualRobot::CoinVisualizationNode(coinVisFactory->Create2DMap(
582  grid, // prob map
583  CELL_SIZE_X * SCALE_FACTOR, // x cell size in mm
584  CELL_SIZE_Y * SCALE_FACTOR, // y cell size in mm
585  cmap, // color map
586  false, // draw zero cells
587  false // draw lines between cells
588  )
589  ));
590  }
591 
593  dm.block(0, 3, 3, 1) = clusterCenter * SCALE_FACTOR - objPos;
594  clusterVisu->setGlobalPose(dm);
595 
596  clusterVisuList.push_back(clusterVisu);
597  } // end clusters loop
598 
599  return coinVisFactory->createUnitedVisualization(clusterVisuList);;
600  }
601 
602  float SimoxObjectWrapper::evaluate2DGaussian(const Eigen::Vector2f& point, const Eigen::Vector2f& mean, const Eigen::Matrix2f& covInv, float covDetSqrtInv)
603  {
604  Eigen::Matrix<float, 1, 1> inner = -0.5 * (point - mean).transpose() * covInv * (point - mean);
605  float e = exp(inner(0, 0));
606 
607  static const float c = 1. / 2 * M_PI;
608 
609  return c * covDetSqrtInv * e;
610  }
611 
612  float SimoxObjectWrapper::evaluate3DGaussian(const Eigen::Vector3f& point, const Eigen::Vector3f& mean, const Eigen::Matrix3f& covInv, float covDet)
613  {
614  Eigen::Matrix<float, 1, 1> inner = -0.5 * (point - mean).transpose() * covInv * (point - mean);
615  float e = exp(inner(0, 0));
616 
617  return pow(2 * M_PI, -3.0f / 2.0f) * pow(covDet, -0.5) * e;
618  }
619 
620  void SimoxObjectWrapper::clear(bool removeFiles /* = true */)
621  {
622  if (getEntity() && removeFiles)
623  {
624  fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_MANIPULATION_FILE));
625  fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_IV_COLLISION_FILE));
626  fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_IV_FILE));
627  fileManager->removeAttrFiles(getEntity()->getAttribute(ATTR_TEXTURE_FILES));
628  }
629 
630  simoxObject.reset();
631  }
632 
633  /*
634  * private methods
635  */
636  void SimoxObjectWrapper::loadSimoxObject() const
637  {
638  ARMARX_TRACE;
639  if (loadMode != VirtualRobot::RobotIO::eStructure)
640  {
641  ARMARX_TRACE;
642  cacheAttributeFiles(ATTR_TEXTURE_FILES, true);
643  }
644 
645  const std::string ivFName = (loadMode == VirtualRobot::RobotIO::eFull || (loadMode == VirtualRobot::RobotIO::eCollisionModel && !getEntity()->hasAttribute(ATTR_IV_COLLISION_FILE)))
646  ? cacheAttributeFile(ATTR_IV_FILE, true) : "";
647  ARMARX_TRACE;
648  const std::string ivFNameCollision = (loadMode == VirtualRobot::RobotIO::eFull || loadMode == VirtualRobot::RobotIO::eCollisionModel) ? cacheAttributeFile(ATTR_IV_COLLISION_FILE, true) : "";
649  ARMARX_TRACE;
650  const std::string moFName = cacheAttributeFile(ATTR_MANIPULATION_FILE, true);
651 
652  ARMARX_TRACE;
653  if (moFName.empty() && ivFName.empty() && ivFNameCollision.empty())
654  {
655  ARMARX_WARNING_S << "SimoxWrapper was not able to find ManipulationObjectFile or an ivFile an ivCollision of ObjectInstance " << getEntity()->getName() << std::endl;
656  }
657  else if (ivFName.empty() && loadMode == VirtualRobot::RobotIO::eFull)
658  {
659  ARMARX_WARNING_S << "SimoxWrapper was not able to find IvFile Attribute of ObjectInstance " << getEntity()->getName() << std::endl;
660  }
661  else if (ivFNameCollision.empty() && loadMode == VirtualRobot::RobotIO::eCollisionModel)
662  {
663  ARMARX_WARNING_S << "SimoxWrapper was not able to find ivFNameCollision Attribute of ObjectInstance " << getEntity()->getName() << std::endl;
664  }
665 
666  if (!moFName.empty())
667  {
668  ARMARX_TRACE;
669  ARMARX_DEBUG_S << "Loading simox object " << moFName;
670  simoxObject = loadManipulationObjectFile(moFName);
671  }
672  else
673  {
674  ARMARX_TRACE;
675  simoxObject = createManipulationObjectFromIvFiles(getEntity()->getName(), ivFName, ivFNameCollision);
676  }
677  }
678 
679  std::string SimoxObjectWrapper::cacheAttributeFile(const std::string& attrName, bool preserveOriginalFName /* = false */) const
680  {
681  ARMARX_TRACE;
682  std::string result = "";
683 
684  if (getEntity()->hasAttribute(attrName))
685  {
686  ARMARX_TRACE;
687  ARMARX_DEBUG_S << "Caching file(s) from attribute \"" << attrName << "\"";
688 
689  fileManager->ensureFileInCache(getEntity()->getAttribute(attrName), result, preserveOriginalFName);
690  }
691  else
692  {
693  ARMARX_DEBUG_S << "Attribute \"" << attrName << "\" not available";
694  }
695 
696  return result;
697  }
698 
699  bool SimoxObjectWrapper::cacheAttributeFiles(const std::string& attrName, bool preserveOriginalFName /* = false */) const
700  {
701  if (getEntity()->hasAttribute(attrName))
702  {
703  // ARMARX_INFO_S << "Caching file(s) from attribute \"" << attrName << "\"" << armarx::flush;
704 
705  return fileManager->ensureFilesInCache(getEntity()->getAttribute(attrName), preserveOriginalFName);
706  }
707  else
708  {
709  return false;
710  }
711  }
712 
713  VirtualRobot::ManipulationObjectPtr SimoxObjectWrapper::loadManipulationObjectFile(const std::string& xmlFName) const
714  {
715  return VirtualRobot::ObjectIO::loadManipulationObject(xmlFName);
716  }
717 
718  VirtualRobot::ManipulationObjectPtr SimoxObjectWrapper::createManipulationObjectFromIvFiles(const std::string& objName,
719  const std::string& ivFNameVis, const std::string& ivFNameCollision) const
720  {
721  VirtualRobot::ManipulationObjectPtr mo(new VirtualRobot::ManipulationObject(objName));
722 
723  mo->setFilename(objName + ".xml");
724 
725  VirtualRobot::VisualizationNodePtr visFull = (!ivFNameVis.empty()) ? coinVisFactory->getVisualizationFromFile(ivFNameVis) : VirtualRobot::VisualizationNodePtr();
726 
727  if (visFull)
728  {
729  mo->setVisualization(visFull);
730  }
731 
732  VirtualRobot::VisualizationNodePtr visCollision = ivFNameCollision.empty() ? VirtualRobot::VisualizationNodePtr() : coinVisFactory->getVisualizationFromFile(ivFNameCollision);
733 
734  if (visCollision)
735  {
736  VirtualRobot::CollisionModelPtr collisionModel(new VirtualRobot::CollisionModel(visCollision));
737  mo->setCollisionModel(collisionModel);
738  }
739 
740  return mo;
741  }
742 
743 
744  std::string SimoxObjectWrapper::storeManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string& filesDBName)
745  {
746  if (!mo)
747  {
748  return "";
749  }
750 
751  EntityAttributeBasePtr oldMOFileAttr = getEntity()->getAttribute(ATTR_MANIPULATION_FILE);
752 
753 
754  // store .iv files and their textures first
755  fs::path visuFName = mo->getVisualization()->getFilename();
756  fs::path collisionFName = mo->getCollisionModel()->getVisualization()->getFilename();
757  storeEntityIVFiles(visuFName.string(), collisionFName.string(), filesDBName);
758 
759  // we don't want relative filepaths for IVs in MO XML file, since they would become invalid after caching on local system
760  // so create a new XML with IVs truncated to filenames only
761  const std::string onlyFilenameVisu = visuFName.filename().string();
762  mo->getVisualization()->setFilename(onlyFilenameVisu, mo->getVisualization()->usedBoundingBoxVisu());
763 
764  const std::string onlyFilenameCol = collisionFName.filename().string();
765  mo->getCollisionModel()->getVisualization()->setFilename(onlyFilenameCol,
766  mo->getCollisionModel()->getVisualization()->usedBoundingBoxVisu());
767 
768  VirtualRobot::ObjectIO::saveManipulationObject(mo, moTempFile);
769 
770  EntityAttributeBasePtr moAttr = new EntityAttribute(ATTR_MANIPULATION_FILE);
771 
772 
773  std::string fileId = fileManager->storeFileToAttr(filesDBName, moTempFile, moAttr, fs::path(mo->getFilename()).filename().string());
774 
775  cleanUpAttributeFiles(oldMOFileAttr, moAttr);
776  getEntity()->putAttribute(moAttr);
777 
778  fs::remove(moTempFile);
779  return fileId;
780  }
781 
782  void SimoxObjectWrapper::storeEntityIVFiles(const std::string& visuFName, const std::string& collisionFName, const std::string& filesDBName, bool processTextures)
783  {
784  NameList textures;
785  NameList::const_iterator itTex;
786 
787  EntityAttributeBasePtr oldIVColFileAttr = getEntity()->getAttribute(ATTR_IV_COLLISION_FILE);
788  EntityAttributeBasePtr oldIVFileAttr = getEntity()->getAttribute(ATTR_IV_FILE);
789  EntityAttributeBasePtr oldTextureFileAttr = getEntity()->getAttribute(ATTR_TEXTURE_FILES);
790 
791  EntityAttributeBasePtr visuAttr = new EntityAttribute(ATTR_IV_FILE);
792 
793  std::string visu_filename = visuFName;
794  if (!visu_filename.empty())
795  {
796  // Make filename an absolute path if needed
797  if (std::filesystem::path(visu_filename).is_relative())
798  {
799  visu_filename = (cachePath / visu_filename).string();
800  }
801 
802  ARMARX_INFO << " Saving visualization iv file: " << visu_filename;
803  fileManager->storeFileToAttr(filesDBName, visu_filename, visuAttr);
804  cleanUpAttributeFiles(oldIVFileAttr, visuAttr);
805  getEntity()->putAttribute(visuAttr);
806 
807  if (processTextures)
808  {
809  SimoxObjectWrapper::FindIvTextures(visu_filename, textures);
810  }
811  }
812 
813  std::string col_filename = collisionFName;
814  if (!col_filename.empty())
815  {
816  // Make filename an absolute path if needed
817  if (std::filesystem::path(col_filename).is_relative())
818  {
819  col_filename = (cachePath / col_filename).string();
820  }
821 
822  ARMARX_INFO_S << " Saving collision iv file: " << col_filename << armarx::flush;
823  EntityAttributeBasePtr collisionAttr = new EntityAttribute(ATTR_IV_COLLISION_FILE);
824 
825  // if the same .iv-file is used as both visu and collision model, we don't need to store it again,
826  // just copy a reference instead
827  if (col_filename == visu_filename)
828  {
829  collisionAttr->addValue(visuAttr->getValue());
830  }
831  else
832  {
833  fileManager->storeFileToAttr(filesDBName, col_filename, collisionAttr);
834 
835  if (processTextures)
836  {
837  SimoxObjectWrapper::FindIvTextures(col_filename, textures);
838  }
839  }
840 
841  cleanUpAttributeFiles(oldIVColFileAttr, collisionAttr);
842  getEntity()->putAttribute(collisionAttr);
843  }
844 
845 
846  if (!textures.empty())
847  {
848  EntityAttributeBasePtr texAttr = new EntityAttribute(ATTR_TEXTURE_FILES);
849 
850  //fileManager->storeFilesToAttr(filesDBName,localBaseDir,textures,texAttr);
851  for (itTex = textures.begin(); itTex != textures.end(); ++itTex)
852  {
853  ARMARX_INFO_S << " Saving texture file: " << *itTex << armarx::flush;
854  fileManager->storeFileToAttr(filesDBName, *itTex, texAttr);
855  }
856 
857  cleanUpAttributeFiles(oldTextureFileAttr, texAttr);
858  getEntity()->putAttribute(texAttr);
859  }
860  else if (oldTextureFileAttr) // new object has no texture, remove old texture attributes and files
861  {
862  removeAttributeFiles(oldTextureFileAttr);
863  getEntity()->removeAttribute(oldTextureFileAttr->getName());
864  }
865  }
866 
867 
868  void SimoxObjectWrapper::FindIvTextures(const std::string& ivFName, NameList& textures)
869  {
870  // TODO replace with CoinVisualizationFactory call ???
871  SoInput in;
872 
873  if (!in.openFile(ivFName.c_str()))
874  {
875  ARMARX_WARNING_S << "Could not open iv file:" << ivFName << std::endl;
876  return;
877  }
878 
879  SoNode* n = SoDB::readAll(&in);
880  n->ref();
881  SimoxObjectWrapper::GetAllFilenames(n, textures, ivFName);
882  n->unref();
883  }
884 
885 
887  {
888  Eigen::Vector3f result = {0, 0, 0};
889 
890  if (getEntity()->hasAttribute(ATTR_PUTDOWN_ORIENTATION_RPY))
891  {
892  EntityPtr p = EntityPtr::dynamicCast(getEntity());
893  EntityAttributeBasePtr attr = p->getAttribute(ATTR_PUTDOWN_ORIENTATION_RPY);
894 
895  if (attr->size() > 0)
896  {
897  armarx::Vector3BasePtr vecBase = armarx::VariantPtr::dynamicCast(attr->getValueAt(0))->getClass<armarx::Vector3Base>();
898  result(0) = vecBase->x;
899  result(1) = vecBase->y;
900  result(2) = vecBase->z;
901  }
902  }
903 
904  return result;
905  }
906 
907 
909  {
910  EntityAttributeBasePtr attr = new EntityAttribute(ATTR_PUTDOWN_ORIENTATION_RPY);
911  armarx::Vector3 vec(rpy);
912  attr->addValue(new armarx::Variant(vec));
913  getEntity()->putAttribute(attr);
914  }
915 
916 
917  void SimoxObjectWrapper::GetAllFilenames(SoNode* node, std::vector<std::string>& storeFilenames, const std::string& origFile)
918  {
919  if (!node)
920  {
921  return;
922  }
923 
924  if (node->getTypeId() == SoFile::getClassTypeId())
925  {
926  // get filename
927  SoFile* fileNode = (SoFile*)node;
928  SbString fileNodeName = fileNode->getFullName();
929 
930  if (!fileNodeName)
931  {
932  ARMARX_INFO_S << "Empty file?!";
933  SbString s2 = fileNode->name.getValue();
934 
935  if (!s2)
936  {
937  ARMARX_INFO_S << "Empty relative name";
938  }
939  else
940  {
941  storeFilenames.push_back(s2.getString());
942  }
943  }
944  else
945  {
946  storeFilenames.push_back(fileNodeName.getString());
947  }
948 
949  // process file data
950  SoGroup* fileChildren = fileNode->copyChildren();
951  SimoxObjectWrapper::GetAllFilenames(fileChildren, storeFilenames, fileNodeName.getString());
952  }
953  else if (node->getTypeId().isDerivedFrom(SoGroup::getClassTypeId()))
954  {
955  SoGroup* groupNode = (SoGroup*)node;
956 
957  // process group node
958  for (int i = 0; i < groupNode->getNumChildren(); i++)
959  {
960  SimoxObjectWrapper::GetAllFilenames(groupNode->getChild(i), storeFilenames, origFile);
961  }
962  }
963  else if (node->getTypeId() == SoImage::getClassTypeId())
964  {
965  // get image filename
966  SbString imageFilename = ((SoImage*)node)->filename.getValue();
967  storeFilenames.push_back(SimoxObjectWrapper::GetAbsolutePath(imageFilename, origFile));
968  }
969  else if (node->getTypeId() == SoTexture2::getClassTypeId())
970  {
971  // get filename
972  SbString texture2Filename = ((SoTexture2*)node)->filename.getValue();
973  storeFilenames.push_back(SimoxObjectWrapper::GetAbsolutePath(texture2Filename, origFile));
974  }
975  else if (node->getTypeId() == SoTexture3::getClassTypeId())
976  {
977  ARMARX_WARNING_S << "Texture3 nyi..." << std::endl;
978  }
979  else //if (node->getTypeId() == SoVRMLImageTexture::getClassTypeId())
980  {
981  SoSearchAction sa;
982  sa.setType(SoVRMLImageTexture::getClassTypeId());
983  sa.setInterest(SoSearchAction::ALL);
984  sa.setSearchingAll(TRUE);
985  sa.apply(node);
986 
987  SoPathList& pathList = sa.getPaths();
988  if (pathList.getLength() <= 0)
989  {
990  return;
991  }
992  SoFullPath* p = (SoFullPath*)pathList[0];
993  if (!p->getTail()->isOfType(SoVRMLImageTexture::getClassTypeId()))
994  {
995  return;
996  }
997  SoVRMLImageTexture* texture = (SoVRMLImageTexture*) p->getTail();
998  if (texture->url.getNum() <= 0)
999  {
1000  return;
1001  }
1002  for (int i = 0; i < texture->url.getNum(); ++i)
1003  {
1004  auto path = SimoxObjectWrapper::GetAbsolutePath(texture->url[i], origFile);
1005  if (!path.empty() && fs::exists(path))
1006  {
1007  storeFilenames.push_back(path);
1008  break;
1009  }
1010  if (i == texture->url.getNum() - 1)
1011  {
1012  Ice::StringSeq textures;
1013  for (int j = 0; j < texture->url.getNum(); ++j)
1014  {
1015  textures.push_back(texture->url[j].getString());
1016  }
1017  ARMARX_ERROR << "Could not make any of the texture paths absolute: " << simox::alg::join(textures, ", ");
1018  }
1019  }
1020 
1021  // ARMARX_IMPORTANT_S << "VRML ImageTexture of node: " << node->getName().getString() << " : " << texture->url[0].getString() << VAROUT(origFile) << VAROUT(storeFilenames);
1022  }
1023  }
1024 
1025 
1026 
1027  std::string SimoxObjectWrapper::GetAbsolutePath(SbString filename, const std::string& origFile)
1028  {
1029  if (!filename)
1030  {
1031  //ARMARX_INFO_S << "Empty relative name"; // skip
1032  return std::string();
1033  }
1034  else
1035  {
1036  fs::path filepath(armarx::ArmarXDataPath::cleanPath(filename.getString()));
1037  fs::path absOrigFileDirPath;
1038  std::string resultPathStr;
1039  if (!origFile.empty())
1040  {
1041  absOrigFileDirPath = fs::path(armarx::ArmarXDataPath::cleanPath(origFile)).parent_path();
1042  }
1043  if (filepath.is_absolute())
1044  {
1045  try
1046  {
1047  return armarx::ArmarXDataPath::relativeTo(absOrigFileDirPath.string(), filepath.string());
1048  }
1049  catch (...)
1050  {
1051  return std::string();
1052  }
1053  }
1054  else if (!filepath.has_parent_path())
1055  {
1056  return (absOrigFileDirPath / filepath).string();
1057  }
1058  else if (armarx::ArmarXDataPath::mergePaths(absOrigFileDirPath.string(), filepath.string(), resultPathStr))
1059  {
1060  return resultPathStr;
1061  }
1062 
1063  // completeFile = relativePath / fs::path(filename.getString());
1064  }
1065  return std::string();
1066  }
1067 }
armarx::ArmarXDataPath::relativeTo
static std::string relativeTo(const std::string &from, const std::string &to)
Transform an absolute filepath into a relative path of the other absolute filepath.
Definition: ArmarXDataPath.cpp:410
GfxTL::sqrt
VectorXD< D, T > sqrt(const VectorXD< D, T > &a)
Definition: VectorXD.h:662
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
memoryx::EntityWrappers::ATTR_MANIPULATION_FILE
const std::string ATTR_MANIPULATION_FILE
Definition: SimoxObjectWrapper.cpp:68
armarx::eFull
@ eFull
Definition: MorphingItem.h:35
memoryx::EntityWrappers::ATTR_PUTDOWN_ORIENTATION_RPY
const std::string ATTR_PUTDOWN_ORIENTATION_RPY
Definition: SimoxObjectWrapper.cpp:71
memoryx::EntityWrappers::SimoxObjectWrapper::eHeatSurface
@ eHeatSurface
Definition: SimoxObjectWrapper.h:53
LocalException.h
memoryx::EntityWrappers::ELLIPSE_RADIUS_IN_SIGMAS
const float ELLIPSE_RADIUS_IN_SIGMAS
Definition: SimoxObjectWrapper.cpp:76
memoryx::EntityWrappers::SimoxObjectWrapper::refreshVisuPose
void refreshVisuPose()
Update object visualization using the current values of pose and position uncertainty from the wrappe...
Definition: SimoxObjectWrapper.cpp:299
armarx::ArmarXDataPath::cleanPath
static std::string cleanPath(const std::string &filepathStr)
Definition: ArmarXDataPath.cpp:331
memoryx::EntityWrappers::SimoxObjectWrapper::setModelIVFiles
void setModelIVFiles(const std::string &ivFNameVis, const std::string &ivFNameCollision)
Create Simox ManipulationObject from separate .iv models, but do not store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:205
memoryx::EntityWrappers::SimoxObjectWrapper::getPutdownOrientationRPY
Eigen::Vector3f getPutdownOrientationRPY()
Returns the orientation of the object that it should have when it is being put down e....
Definition: SimoxObjectWrapper.cpp:886
memoryx::EntityWrappers::SimoxObjectWrapper::setUncertaintyVisuParams
void setUncertaintyVisuParams(float ellipseRadiusInSigmas, float heatmapMinVariance, float heatmapDensityThreshold, int heatmapGridSize)
Set parameters for uncertainty visualization.
Definition: SimoxObjectWrapper.cpp:216
memoryx::EntityWrappers::AbstractFileEntityWrapper::removeAttributeFiles
void removeAttributeFiles(const memoryx::EntityAttributeBasePtr &attr)
removes all files of an attribute from the gridfs
Definition: AbstractEntityWrapper.cpp:102
memoryx::EntityWrappers::AbstractFileEntityWrapper::cleanUpAttributeFiles
void cleanUpAttributeFiles(EntityAttributeBasePtr oldAttr, EntityAttributeBasePtr newAttr)
cleanUpAttributeFiles compares the files attached to the two given attributes and removes the files o...
Definition: AbstractEntityWrapper.cpp:64
memoryx::NormalDistributionPtr
IceInternal::Handle< NormalDistribution > NormalDistributionPtr
Definition: ProbabilityMeasures.h:125
trace.h
memoryx::EntityWrappers::SimoxObjectWrapper::FindIvTextures
static void FindIvTextures(const std::string &ivFName, NameList &textures)
FindIvTextures scans the given ivFName for texture files and returns them in the textures string list...
Definition: SimoxObjectWrapper.cpp:868
memoryx::EntityWrappers::SimoxObjectWrapper::setManipulationObject
void setManipulationObject(const VirtualRobot::ManipulationObjectPtr mo)
Set Simox ManipulationObject in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:167
memoryx::EntityWrappers::SimoxObjectWrapper::ice_clone
Ice::ObjectPtr ice_clone() const override
Definition: SimoxObjectWrapper.cpp:105
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
memoryx::EntityWrappers::SimoxObjectWrapper::clear
void clear(bool removeFiles=true)
Clears the Simox ManipulationObject and optionally removes the corresponding files from GridFS.
Definition: SimoxObjectWrapper.cpp:620
memoryx::EntityWrappers::SimoxObjectWrapper::eEllipse
@ eEllipse
Definition: SimoxObjectWrapper.h:51
memoryx::EntityWrappers::SimoxObjectWrapper::getManipulationObject
VirtualRobot::ManipulationObjectPtr getManipulationObject() const
Retrieve the complete Simox ManipulationObject.
Definition: SimoxObjectWrapper.cpp:138
memoryx::EntityWrappers::ATTR_TEXTURE_FILES
const std::string ATTR_TEXTURE_FILES
Definition: SimoxObjectWrapper.cpp:65
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreManipulationObject
std::string setAndStoreManipulationObject(const VirtualRobot::ManipulationObjectPtr mo, const std::string &filesDBName)
Store Simox ManipulationObject in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:161
memoryx::EntityWrappers::AbstractEntityWrapper::getEntity
EntityBasePtr getEntity() const
Get the stored name of the stored entity.
Definition: AbstractEntityWrapper.cpp:49
memoryx::EntityWrappers::SimoxObjectWrapper::SimoxObjectWrapper
SimoxObjectWrapper(const GridFileManagerPtr &gfm, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
Constructs new SimoxObjectWrapper.
Definition: SimoxObjectWrapper.cpp:81
memoryx::EntityWrappers::SimoxObjectWrapper::getCollisionModel
VirtualRobot::CollisionModelPtr getCollisionModel() const
Retrieve collision model.
Definition: SimoxObjectWrapper.cpp:124
IceInternal::Handle< ObjectInstance >
memoryx::EntityWrappers::SimoxObjectWrapper::getVisualization
VirtualRobot::VisualizationNodePtr getVisualization() const
Retrieve visualization model.
Definition: SimoxObjectWrapper.cpp:110
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:69
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
Color
uint32_t Color
RGBA color.
Definition: color.h:8
scene3D::ALL
@ ALL
Definition: ManipulatorMode.h:35
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:198
armarx::mean
std::optional< float > mean(const boost::circular_buffer< NameValueMap > &buffer, const std::string &key)
Definition: KinematicUnitGuiPlugin.cpp:1615
GfxTL::Matrix2f
MatrixXX< 2, 2, float > Matrix2f
Definition: MatrixXX.h:599
M_PI
#define M_PI
Definition: MathTools.h:17
if
if(!yyvaluep)
Definition: Grammar.cpp:724
armarx::FloatVector
::std::vector< ::Ice::Float > FloatVector
Definition: KinematicUnitGuiPlugin.h:327
ARMARX_ERROR_S
#define ARMARX_ERROR_S
Definition: Logging.h:209
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
memoryx::EntityWrappers::SimoxObjectWrapper::setPutdownOrientationRPY
void setPutdownOrientationRPY(Eigen::Vector3f &rpy)
Sets the orientation of the object that it should have when it is being put down e....
Definition: SimoxObjectWrapper.cpp:908
memoryx::EntityWrappers::AbstractFileEntityWrapper
Definition: AbstractEntityWrapper.h:82
armarx::Vector3
The Vector3 class.
Definition: Pose.h:112
filename
std::string filename
Definition: VisualizationRobot.cpp:83
memoryx::EntityWrappers::SimoxObjectWrapper::UncertaintyVisuType
UncertaintyVisuType
Definition: SimoxObjectWrapper.h:49
memoryx::EntityWrappers::MO_TEMP_FILE_NAME
const std::string MO_TEMP_FILE_NAME
Definition: SimoxObjectWrapper.cpp:73
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
ARMARX_WARNING_S
#define ARMARX_WARNING_S
Definition: Logging.h:206
memoryx::EntityWrappers::AbstractEntityWrapper::getName
std::string getName() const
Get the name of the stored entity.
Definition: AbstractEntityWrapper.cpp:44
memoryx::EntityWrappers::SimoxObjectWrapper::setUncertaintyVisuType
void setUncertaintyVisuType(UncertaintyVisuType visuType)
Set visualization type for position uncertainty.
Definition: SimoxObjectWrapper.cpp:211
memoryx::GaussianMixtureDistribution::FromProbabilityMeasure
static GaussianMixtureDistributionPtr FromProbabilityMeasure(const ProbabilityMeasureBasePtr &probMeasure)
Convert or approximate given ProbabilityMeasure to a gaussian mixture.
Definition: ProbabilityMeasures.cpp:658
memoryx::EntityWrappers::ATTR_IV_COLLISION_FILE
const std::string ATTR_IV_COLLISION_FILE
Definition: SimoxObjectWrapper.cpp:67
WestGMMReducer.h
SimoxObjectWrapper.h
memoryx::EntityWrappers::HEATMAP_GRID_SIZE
const int HEATMAP_GRID_SIZE
Definition: SimoxObjectWrapper.cpp:79
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreManipulationFile
void setAndStoreManipulationFile(const std::string &xmlFName, const std::string &filesDBName)
Create Simox ManipulationObject from an XML file and store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:173
memoryx::EntityWrappers::ATTR_UNCERTAINTY_COLOR_MAP
const std::string ATTR_UNCERTAINTY_COLOR_MAP
Definition: SimoxObjectWrapper.cpp:69
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:600
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ObjectInstance.h
memoryx::EntityWrappers::SimoxObjectWrapper::~SimoxObjectWrapper
~SimoxObjectWrapper() override
Definition: SimoxObjectWrapper.cpp:101
memoryx::EntityWrappers
Definition: AbstractEntityWrapper.cpp:28
memoryx::EntityWrappers::AbstractEntityWrapper::setEntity
void setEntity(const EntityBasePtr &entity)
Replace the stored entity with a new one.
Definition: AbstractEntityWrapper.cpp:54
memoryx::EntityWrappers::HEATMAP_DENSITY_THRESHOLD
const float HEATMAP_DENSITY_THRESHOLD
Definition: SimoxObjectWrapper.cpp:78
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:32
memoryx::EntityWrappers::SimoxObjectWrapper::eHeatMap
@ eHeatMap
Definition: SimoxObjectWrapper.h:52
memoryx::EntityAttribute
Attribute of MemoryX entities.
Definition: EntityAttribute.h:48
memoryx::EntityWrappers::SimoxObjectWrapper::setAndStoreModelIVFiles
void setAndStoreModelIVFiles(const std::string &ivFNameVis, const std::string &ivFNameCollision, const std::string &filesDBName)
Create Simox ManipulationObject from separate .iv models and store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:197
memoryx::EntityWrappers::SimoxObjectWrapper::updateFromEntity
void updateFromEntity(const EntityBasePtr &entity)
Update the Simox object using attribute values (position, orientation etc) from the entity,...
Definition: SimoxObjectWrapper.cpp:292
Eigen::Matrix
Definition: EigenForwardDeclarations.h:27
memoryx::EntityWrappers::HEATMAP_MIN_VARIANCE
const float HEATMAP_MIN_VARIANCE
Definition: SimoxObjectWrapper.cpp:77
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:195
armarx::aron::type::ObjectPtr
std::shared_ptr< Object > ObjectPtr
Definition: Object.h:36
Logging.h
armarx::transpose
std::vector< std::vector< T > > transpose(const std::vector< std::vector< T >> &src, Thrower thrower)
Definition: SimoxCSpace.cpp:706
memoryx::EntityWrappers::ATTR_UNCERTAINTY_TRANSPARENCY
const std::string ATTR_UNCERTAINTY_TRANSPARENCY
Definition: SimoxObjectWrapper.cpp:70
ArmarXDataPath.h
memoryx::EntityWrappers::SimoxObjectWrapper::GetAllFilenames
static void GetAllFilenames(SoNode *node, std::vector< std::string > &storeFilenames, const std::string &origFile="")
GetAllFilenames walks node and appends absolute paths of all found SoFile, SoImage,...
Definition: SimoxObjectWrapper.cpp:917
armarx::ArmarXDataPath::mergePaths
static bool mergePaths(std::string path, std::string subPath, std::string &result)
Definition: ArmarXDataPath.cpp:456
memoryx::EntityWrappers::SimoxObjectWrapper::getManipulationObjectFileName
std::string getManipulationObjectFileName() const
Retrieve the Simox MainpulationObjects file name in local cache.
Definition: SimoxObjectWrapper.cpp:150
memoryx::EntityWrappers::SimoxObjectWrapper::GetAbsolutePath
static std::string GetAbsolutePath(SbString filename, const std::string &origFile)
GetAbsolutePath prepend filename with origFile and append it to storeFilenames (does nothing if filen...
Definition: SimoxObjectWrapper.cpp:1027
memoryx::EntityWrappers::AbstractFileEntityWrapper::fileManager
GridFileManagerPtr fileManager
Definition: AbstractEntityWrapper.h:106
memoryx::EntityWrappers::ATTR_IV_FILE
const std::string ATTR_IV_FILE
Definition: SimoxObjectWrapper.cpp:66
memoryx::EntityWrappers::SimoxObjectWrapper::setManipulationFile
void setManipulationFile(const std::string &xmlFName)
Create Simox ManipulationObject from an XML file, but do not store it in the wrapped entity.
Definition: SimoxObjectWrapper.cpp:187