SimpleRobotPlacement.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2015-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotComponents::SimpleRobotPlacement
19  * @author Harry Arnst (harry dot arnst at student dot kit dot edu),
20  * Valerij Wittenbeck (valerij dot wittenbeck at student dot kit dot edu)
21  * @date 2016
22  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
23  * GNU General Public License
24  */
25 
26 #include "SimpleRobotPlacement.h"
27 
28 #include <VirtualRobot/Grasping/Grasp.h>
29 #include <VirtualRobot/Grasping/GraspSet.h>
30 #include <VirtualRobot/ManipulationObject.h>
31 #include <VirtualRobot/MathTools.h>
32 #include <VirtualRobot/Workspace/Manipulability.h>
33 
34 #include <ArmarXCore/core/system/ArmarXDataPath.h>
35 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
36 #include <ArmarXCore/core/util/algorithm.h>
37 
38 #include <RobotAPI/libraries/core/FramedPose.h>
39 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
40 
41 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
42 #include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
43 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
44 #include <MemoryX/libraries/memorytypes/entity/AgentInstance.h>
45 #include <MemoryX/libraries/memorytypes/entity/ObjectInstance.h>
46 //#include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
47 #include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
48 #include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
49 //#include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
50 #include <math.h>
51 
52 #include <IceUtil/UUID.h>
53 
54 #include <SimoxUtility/algorithm/string/string_tools.h>
55 #include <VirtualRobot/RobotConfig.h>
56 #include <VirtualRobot/math/Helpers.h>
57 
58 #include <VisionX/interface/components/VoxelGridProviderInterface.h>
59 
60 #include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
61 
62 using namespace armarx;
63 using namespace memoryx;
64 using namespace VirtualRobot;
65 
66 static const DrawColor COLOR_ROBOT{1.0f, 1.0f, 0.5f, 1.0f};
67 
68 SimpleRobotPlacementPropertyDefinitions::SimpleRobotPlacementPropertyDefinitions(
69  std::string prefix) :
70  ComponentPropertyDefinitions(prefix)
71 {
72  defineOptionalProperty<std::string>("RobotName", "Armar3", "Name of the robot to use");
73  defineOptionalProperty<std::string>(
74  "RobotFilePath", "RobotAPI/robots/Armar3/ArmarIII.xml", "File path of the robot to use");
75  defineOptionalProperty<std::string>(
76  "CollisionModel", "PlatformTorsoHeadColModel", "Collisionmodel of the robot");
77  defineOptionalProperty<std::string>(
78  "WorkspaceFilePaths",
79  "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
80  "reachability_left_hand_smoothened.bin",
81  "Paths to manipulability and reachability files (';' delimited)");
82  // defineOptionalProperty<bool>("VisualizeCollisionSpace",
83  // false,
84  // "If true adds cspace task to MotionPlanning Server");
85  defineOptionalProperty<float>("MinimumDistanceToEnvironment",
86  0.0f,
87  "Minimum distance to the environment for all robot placements. "
88  "Much faster if set to zero.");
89  defineOptionalProperty<float>("VisualizationSlowdownFactor",
90  1.0f,
91  "1.0 is a good value for clear visualization, 0 the "
92  "visualization should not slow down the process",
93  PropertyDefinitionBase::eModifiable);
94  defineOptionalProperty<bool>("EnableVisualization",
95  true,
96  "If false no visualization is done.",
97  PropertyDefinitionBase::eModifiable);
98  defineOptionalProperty<int>("PlacmentsPerGrasp",
99  3,
100  "Number of robot placement that will be generated for each grasp",
101  PropertyDefinitionBase::eModifiable);
102  defineOptionalProperty<float>(
103  "MinManipulabilityDecreaseFactor",
104  0.9f,
105  "Min initial manipulability in relation to max manipulabity value and factor by which this "
106  "threshold is decreased each trial",
107  PropertyDefinitionBase::eModifiable);
108 
109  defineOptionalProperty<bool>("UseVoxelGridCSpace",
110  false,
111  "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
112  PropertyDefinitionBase::eModifiable);
113  defineOptionalProperty<std::string>("VoxelGridProviderName",
114  "VoxelGridProvider",
115  "Name of the Voxel Grid Provider",
116  PropertyDefinitionBase::eModifiable);
117 
118  defineOptionalProperty<std::string>(
119  "EefNamePreferenceFilter",
120  "R",
121  "Prefer grasps where eef name contains this name by setting grasp success probability to "
122  "1. Set to empty string to disable.");
123  defineOptionalProperty<float>("MinPlacementDistance",
124  400,
125  "Minimum Distance for a Placement",
126  PropertyDefinitionBase::eModifiable);
127  defineOptionalProperty<float>("MaxPlacementDistance",
128  1000,
129  "Maximum Distance for a Placement",
130  PropertyDefinitionBase::eModifiable);
131 }
132 
133 void
134 SimpleRobotPlacement::onInitComponent()
135 {
136  drawRobotId = "local_robot_";
137  visuLayerName = "SimpleRobotPlacement";
138 
139  robotName = getProperty<std::string>("RobotName").getValue();
140  robotFilePath = getProperty<std::string>("RobotFilePath").getValue();
141  // retrieve absolute robot file path
142  if (!ArmarXDataPath::getAbsolutePath(robotFilePath, robotFilePath))
143  {
144  ARMARX_ERROR << "Could not find robot file: " << robotFilePath;
145  }
146 
147  workspaceFilePaths =
148  armarx::Split(getProperty<std::string>("WorkspaceFilePaths").getValue(), ";");
149  // retrieve absolute workspace file paths
150  for (std::string& path : workspaceFilePaths)
151  {
152  std::string packageName = std::filesystem::path{path}.begin()->string();
153  ARMARX_CHECK_EXPRESSION(!packageName.empty())
154  << "Workspace file path '" << path
155  << "' could not be parsed correctly, because package name is empty";
156  armarx::CMakePackageFinder project(packageName);
157  path = project.getDataDir() + "/" + path;
158  if (!std::filesystem::exists(path))
159  {
160  throw LocalException("File not found at ") << path;
161  }
162  }
163 
164  colModel = getProperty<std::string>("CollisionModel").getValue();
165 
166  usingProxy("WorkingMemory");
167  usingProxy("PriorKnowledge");
168 
169  usingProxy("RobotIK");
170  usingProxy("RobotStateComponent");
171  offeringTopic("DebugDrawerUpdates");
172 }
173 
174 void
175 SimpleRobotPlacement::onConnectComponent()
176 {
177  srand(IceUtil::Time::now().toSeconds());
178  getProxy(wm, "WorkingMemory");
179  getProxy(prior, "PriorKnowledge");
180 
181  getProxy(rik, "RobotIK");
182  getProxy(robotStateComponentPrx, "RobotStateComponent");
183 
184  objectInstances = wm->getObjectInstancesSegment();
185  agentInstances = wm->getAgentInstancesSegment();
186  objectClasses = prior->getObjectClassesSegment();
187 
188  fileManager =
189  memoryx::GridFileManagerPtr(new memoryx::GridFileManager(prior->getCommonStorage()));
190 
191  debugDrawerPrx = getTopic<armarx::DebugDrawerInterfacePrx>("DebugDrawerUpdates");
192  entityDrawerPrx = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
193 
194  cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(
195  wm, prior->getCommonStorage(), robotStateComponentPrx);
196  loadRobot();
197  loadWorkspaces();
198 }
199 
200 void
201 SimpleRobotPlacement::onDisconnectComponent()
202 {
203 }
204 
205 void
206 SimpleRobotPlacement::onExitComponent()
207 {
208 }
209 
210 GraspingPlacementList
211 SimpleRobotPlacement::generateRobotPlacements(const GeneratedGraspList& grasps, const Ice::Current&)
212 {
213  // TIMING_START(RobotPlacement);
214  planningTasks.clear();
215  GraspingPlacementList result;
216  AgentInstancePtr agent =
217  AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
218  std::shared_ptr<RemoteRobot> remoteRobot(
219  new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
220  robot->setGlobalPose(remoteRobot->getGlobalPose());
221 
222  // first, we get rid of all generated grasps, whose tcp is not given in any of the preloaded workspaces
223  GeneratedGraspList filteredGrasps = filterGrasps(grasps);
224  visualizedGrid.reset();
225  entityDrawerPrx->clearLayer(visuLayerName);
226 
227  // init collision space
228  RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
229  if (getProperty<bool>("UseVoxelGridCSpace").getValue())
230  {
231  cspace =
232  new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
233  getProperty<std::string>("VoxelGridProviderName").getValue()),
234  prior->getCommonStorage());
235  }
236  else
237  {
238  cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
239  }
240  cspace->addObjectsFromWorkingMemory(wm);
241  AgentPlanningInformation agentData;
242  agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
243  agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
244  // agentData.kinemaicChainNames = robotNodeSetNames;
245  agentData.kinemaicChainNames = {};
246  agentData.collisionSetNames = {colModel};
247  agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
248  cspace->setAgent(agentData);
249  cspace->setStationaryObjectMargin(
250  getProperty<float>("MinimumDistanceToEnvironment").getValue());
251  cspace->initCollisionTest();
252 
253  int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
254  ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
255  << " grasps";
256  for (const auto& g : filteredGrasps)
257  {
258  Eigen::Matrix4f graspPose = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
259  /**
260  * Goes through each grasp and creates an empty map with PlacementCandidates. These placementCandidates are
261  * filled with placementsPerGrasp pairs of score <-> GraspingPlacement. When enough pairs have been created,
262  * the candidate with the highest score is added to result.
263  **/
264  std::map<float, GraspingPlacement> placementCandidates;
265  for (int i = 0; i < placementsPerGrasp; ++i)
266  {
267  // std::transform(filteredGrasps.begin(), filteredGrasps.end(), std::back_inserter(placementCandidates), [&](const GeneratedGrasp & g)
268 
269  FramedPosePtr newPose = new FramedPose(robot->getGlobalPose(), GlobalFrame, "");
270 
271  // create a workspace grid and find a suitable position
272  TIMING_START(CreateWorksspaceGrid);
273  VirtualRobot::WorkspaceGridPtr reachGrid = createWorkspaceGrid(g, graspPose);
274  TIMING_END(CreateWorksspaceGrid);
275  TIMING_START(DrawWorksspaceGrid);
276  drawWorkspaceGrid(visualizedGrid);
277  // sleep(2);
278  TIMING_END(DrawWorksspaceGrid);
279 
280 
281  // bool return value is ignored. if placement fails -> random pose???
282  float xGoal, yGoal, platformRotation;
283  int score;
284  getSuitablePosition(g, reachGrid, graspPose, xGoal, yGoal, platformRotation, score);
285 
286  newPose->position->x = xGoal;
287  newPose->position->y = yGoal;
288  Eigen::Matrix4f newPoseEigen = newPose->toGlobalEigen(robot);
289 
290  // we rotate the pose around its local z axis
291  float x = newPoseEigen(0, 3);
292  float y = newPoseEigen(1, 3);
293  float z = newPoseEigen(2, 3);
294  newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
295  Eigen::Affine3f transform;
296  transform = Eigen::Translation<float, 3>(x, y, z) *
297  Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
298  newPoseEigen = transform * newPoseEigen;
299 
300  // drawRobot(newPoseEigen);
301 
302  armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
303  ARMARX_INFO << "Inserting robot placement: " << resultPose->output();
304  placementCandidates[score] = GraspingPlacement{g, resultPose, 0};
305  }
306  if (!placementCandidates.empty())
307  {
308  /*
309  * TODO: Is it the expected behavior that only the best placementCandidate is returned for each grasp??
310  * Should it rather return placementPerGrasp candidates for each grasp?
311  **/
312  result.push_back(placementCandidates.rbegin()->second);
313  }
314  }
315  Eigen::Matrix4f inverseRobotPose = robot->getGlobalPose().inverse();
316  std::map<std::pair<float, float>, GraspingPlacement> orderedMap;
317  std::string EefNamePreferenceFilter = getProperty<std::string>("EefNamePreferenceFilter");
318  for (GraspingPlacement& gp : result)
319  {
320  Eigen::Matrix4f mat =
321  inverseRobotPose * FramedPosePtr::dynamicCast(gp.robotPose)->toEigen();
322  Eigen::AngleAxisf aa(mat.block<3, 3>(0, 0));
323  float distanceLhs = mat.block<3, 1>(0, 3).norm() * aa.angle() * 50;
324 
325  // secondhands demo...
326  float handPreferenceScore =
327  Contains(gp.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
328  orderedMap[{handPreferenceScore, distanceLhs}] = gp;
329  }
330 
331  // TIMING_END(RobotPlacement);
332  entityDrawerPrx->removeLayer(visuLayerName);
333  return getMapValues<>(orderedMap);
334 }
335 
336 struct PlacementInfo
337 {
338  GeneratedGrasp grasp;
339  Eigen::Matrix4f graspPose;
340  std::vector<SimpleRobotPlacement::RobotPlacement> placements;
341  VirtualRobot::WorkspaceGridPtr reachGrid;
342 
343  void
344  sortplacements()
345  {
346  std::sort(placements.begin(),
347  placements.end(),
348  [](auto& a, auto& b) { return a.score > b.score; });
349  }
350 
351  std::pair<float, float> sortingInfo{0, 0};
352  armarx::FramedPosePtr resultPose;
353 };
354 
355 GraspingPlacementList
356 SimpleRobotPlacement::generateRobotPlacementsEx(const GeneratedGraspList& grasps,
357  const Ice::Current&)
358 {
359  // TIMING_START(RobotPlacement);
360  planningTasks.clear();
361  GraspingPlacementList result;
362  AgentInstancePtr agent =
363  AgentInstancePtr::dynamicCast(agentInstances->getAgentInstanceByName(robotName));
364  RemoteRobotPtr remoteRobot(new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
365  robot->setGlobalPose(remoteRobot->getGlobalPose());
366 
367  // first, we get rid of all generated grasps, whose tcp is not given in any of the preloaded workspaces
368  GeneratedGraspList filteredGrasps = filterGrasps(grasps);
369  visualizedGrid.reset();
370  entityDrawerPrx->clearLayer(visuLayerName);
371 
372  // init collision space
373  RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
374  if (getProperty<bool>("UseVoxelGridCSpace").getValue())
375  {
376  cspace =
377  new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
378  getProperty<std::string>("VoxelGridProviderName").getValue()),
379  prior->getCommonStorage());
380  }
381  else
382  {
383  cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
384  }
385  cspace->addObjectsFromWorkingMemory(wm);
386  AgentPlanningInformation agentData;
387  agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
388  agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
389  // agentData.kinemaicChainNames = robotNodeSetNames;
390  agentData.kinemaicChainNames = {};
391  agentData.collisionSetNames = {colModel};
392  agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
393  cspace->setAgent(agentData);
394  cspace->setStationaryObjectMargin(
395  getProperty<float>("MinimumDistanceToEnvironment").getValue());
396  cspace->initCollisionTest();
397 
398  int placementsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
399  ARMARX_VERBOSE << "Searching " << placementsPerGrasp << " poses for " << filteredGrasps.size()
400  << " grasps";
401 
402  std::vector<PlacementInfo> pis;
403  for (const auto& g : filteredGrasps)
404  {
405  PlacementInfo pi;
406  pi.grasp = g;
407  pi.graspPose = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
408  pi.reachGrid = createWorkspaceGrid(g, pi.graspPose);
409  drawWorkspaceGrid(visualizedGrid);
410  pis.push_back(pi);
411  }
412 
413  int maxIterations = 1;
414  for (int n = 0; n < maxIterations; n++)
415  {
416  for (PlacementInfo& pi : pis)
417  {
418  std::vector<SimpleRobotPlacement::RobotPlacement> placements =
419  getSuitablePositions(pi.grasp, pi.reachGrid, pi.graspPose, 0.1, 10, 100);
420  pi.placements.insert(pi.placements.end(), placements.begin(), placements.end());
421  }
422  //requiredManipulabilityFraction *= minManipulabilityDecreaseFactor;
423  }
424 
425  std::string EefNamePreferenceFilter = getProperty<std::string>("EefNamePreferenceFilter");
426 
427  for (PlacementInfo& pi : pis)
428  {
429  if (pi.placements.size() == 0)
430  {
431  continue;
432  }
433  pi.sortplacements();
434  for (RobotPlacement rp : pi.placements)
435  {
436  ARMARX_IMPORTANT << VAROUT(rp.score);
437  }
438  RobotPlacement rp = pi.placements.at(0);
439  Eigen::Matrix4f robotpose = ::math::Helpers::CreatePose(
440  Eigen::Vector3f(rp.x, rp.y, 0),
441  Eigen::AngleAxisf(rp.z, Eigen::Vector3f::UnitZ()).toRotationMatrix());
442  pi.resultPose = new FramedPose(robotpose, GlobalFrame, "");
443  pi.sortingInfo.first =
444  Contains(pi.grasp.eefName, EefNamePreferenceFilter, true) ? 0.f : 1.f;
445  pi.sortingInfo.second = -rp.score;
446  }
447  entityDrawerPrx->removeLayer(visuLayerName);
448  std::sort(
449  pis.begin(), pis.end(), [](auto& a, auto& b) { return a.sortingInfo < b.sortingInfo; });
450  for (PlacementInfo& pi : pis)
451  {
452  if (pi.placements.size() > 0)
453  {
454  GraspingPlacement gp;
455  gp.grasp = pi.grasp;
456  gp.score = pi.placements.at(0).score;
457  gp.robotPose = pi.resultPose;
458  //ARMARX_IMPORTANT
459  result.push_back(gp);
460  }
461  }
462  return result;
463 }
464 
465 GraspingPlacementList
466 SimpleRobotPlacement::generateRobotPlacementsForGraspPose(const std::string& endEffectorName,
467  const FramedPoseBasePtr& target,
468  const PlanarObstacleList& planarObstacles,
469  const ConvexHull& placementArea,
470  const Ice::Current& c)
471 {
472  planningTasks.clear();
473  GraspingPlacementList result;
474 
475  if (!hasWorkspace(endEffectorName))
476  {
477  ARMARX_ERROR << "No pre-loaded workspace found for EEF '" << endEffectorName << "'";
478  return result;
479  }
480 
481  FramedPosePtr target_pose = FramedPosePtr::dynamicCast(target);
482  target_pose->changeToGlobal(robotStateComponentPrx->getSynchronizedRobot());
483 
484  std::shared_ptr<RemoteRobot> remoteRobot(
485  new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
486  robot->setGlobalPose(remoteRobot->getGlobalPose());
487  RemoteRobot::synchronizeLocalClone(robot, robotStateComponentPrx);
488 
489  AgentPlanningInformation agentData;
490  agentData.agentProjectNames = robotStateComponentPrx->getArmarXPackages();
491  agentData.agentRelativeFilePath = robotStateComponentPrx->getRobotFilename();
492  agentData.kinemaicChainNames = {};
493  agentData.collisionSetNames = {colModel};
494  agentData.initialJointValues = robot->getConfig()->getRobotNodeJointValueMap();
495 
496  visualizedGrid.reset();
497  entityDrawerPrx->clearLayer(visuLayerName);
498 
499  // Initialize Simox collision space
500  if (getProperty<bool>("UseVoxelGridCSpace").getValue())
501  {
502  cspace =
503  new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
504  getProperty<std::string>("VoxelGridProviderName").getValue()),
505  prior->getCommonStorage());
506  }
507  else
508  {
509  cspace = new SimoxCSpace(prior->getCommonStorage(), false, 30);
510  cspace->addObjectsFromWorkingMemory(wm);
511  }
512  cspace->setAgent(agentData);
513  cspace->setStationaryObjectMargin(
514  getProperty<float>("MinimumDistanceToEnvironment").getValue());
515 
516 
517  for (auto& obstacle : planarObstacles)
518  {
519  std::vector<Eigen::Vector3f> plane;
520 
521  for (auto& p : obstacle)
522  {
523  plane.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(
524  robotStateComponentPrx->getSynchronizedRobot()));
525  }
526 
527  cspace->addPlanarObject(plane);
528  }
529 
530  cspace->initCollisionTest();
531 
532  GeneratedGrasp g;
533  g.score = 1;
534  g.eefName = endEffectorName;
535  g.framedPose = target_pose;
536 
537  int placmentsPerGrasp = getProperty<int>("PlacmentsPerGrasp");
538  ARMARX_INFO << "Searching " << placmentsPerGrasp << " poses for EEF pose "
539  << target_pose->toEigen();
540 
541  // Construct placement area as convex hull
542  VirtualRobot::MathTools::ConvexHull2DPtr placementArea_ch;
543  if (placementArea.size() > 2)
544  {
545  std::vector<Eigen::Vector2f> area;
546  for (auto& p : placementArea)
547  {
548  area.push_back(FramedPositionPtr::dynamicCast(p)->toGlobalEigen(robot).head(2));
549  }
550  placementArea_ch = VirtualRobot::MathTools::createConvexHull2D(area);
551 
552  ARMARX_INFO << "Suitable placement area:";
553  for (auto& p : placementArea_ch->vertices)
554  {
555  ARMARX_INFO << p;
556  }
557  }
558 
559  for (int i = 0; i < placmentsPerGrasp; ++i)
560  {
561  // Create Simox workspace grid
562  VirtualRobot::WorkspaceGridPtr grid = createWorkspaceGrid(g, target_pose->toEigen());
563  drawWorkspaceGrid(visualizedGrid);
564 
565  float xGoal, yGoal, platformRotation;
566  int score;
567  getSuitablePosition(g,
568  grid,
569  target_pose->toEigen(),
570  xGoal,
571  yGoal,
572  platformRotation,
573  score,
574  placementArea_ch);
575 
576  if (xGoal == 0 && yGoal == 0 && platformRotation == 0)
577  {
578  // This indicates that no suitable pose has been found
579  continue;
580  }
581 
582  FramedPosePtr newPose = new FramedPose(robot->getGlobalPose(), GlobalFrame, "");
583  newPose->position->x = xGoal;
584  newPose->position->y = yGoal;
585  Eigen::Matrix4f newPoseEigen = newPose->toGlobalEigen(robot);
586 
587  // we rotate the pose around its local z axis
588  float x = newPoseEigen(0, 3);
589  float y = newPoseEigen(1, 3);
590  float z = newPoseEigen(2, 3);
591  newPoseEigen.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
592  Eigen::Affine3f transform;
593  transform = Eigen::Translation<float, 3>(x, y, z) *
594  Eigen::AngleAxisf(platformRotation, Eigen::Vector3f(0, 0, 1));
595  newPoseEigen = transform * newPoseEigen;
596 
597  armarx::FramedPosePtr resultPose(new FramedPose(newPoseEigen, GlobalFrame, ""));
598  result.push_back(GraspingPlacement{g, resultPose, score});
599  }
600 
601  entityDrawerPrx->removeLayer(visuLayerName);
602  return result;
603 }
604 
605 VirtualRobot::WorkspaceRepresentationPtr
606 SimpleRobotPlacement::getWorkspaceRepresentation(GeneratedGrasp const& g)
607 {
608 
609  for (VirtualRobot::WorkspaceRepresentationPtr workspace : workspaces)
610  {
611  if (workspace->getTCP()->getName() == robot->getEndEffector(g.eefName)->getTcp()->getName())
612  {
613  return workspace;
614  break;
615  }
616  }
617  return VirtualRobot::WorkspaceRepresentationPtr();
618 }
619 
620 VirtualRobot::WorkspaceGridPtr
621 SimpleRobotPlacement::createWorkspaceGrid(GeneratedGrasp g, const Eigen::Matrix4f& globalObjectPose)
622 {
623  static int counter = 0;
624  std::string graspName = "some_random_grasp_" + to_string(counter++);
625  std::string robotType = robotName;
626  std::string eef = g.eefName;
627 
628  // dummy manipulation object
629  VirtualRobot::ManipulationObjectPtr dummyObject(new ManipulationObject("dummyObject"));
630  dummyObject->setGlobalPose(globalObjectPose);
631 
632  // dummy grasp
633  Eigen::Matrix4f tcpPoseGlobal = FramedPosePtr::dynamicCast(g.framedPose)->toGlobalEigen(robot);
634  Eigen::Matrix4f tcpPrePoseGlobal =
635  FramedPosePtr::dynamicCast(g.framedPrePose)->toGlobalEigen(robot);
636  Eigen::Matrix4f objectPoseInTcpFrame = tcpPoseGlobal.inverse() * globalObjectPose;
637  VirtualRobot::GraspPtr dummyGrasp(new Grasp(graspName, robotType, eef, objectPoseInTcpFrame));
638  VirtualRobot::GraspPtr dummyPrepose(
639  new Grasp(graspName, robotType, eef, tcpPrePoseGlobal.inverse() * globalObjectPose));
640 
641  VirtualRobot::WorkspaceRepresentationPtr ws;
642  // find a workspace whose tcp is equal to the tcp of the generated grasp
643  ws = getWorkspaceRepresentation(g);
644 
645  // create workspace grid and fill it
646  Eigen::Vector3f minBB, maxBB;
647  ws->getWorkspaceExtends(minBB, maxBB);
648  VirtualRobot::WorkspaceGridPtr reachGridPrepose;
649  reachGridPrepose.reset(new WorkspaceGrid(
650  minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
651  reachGridPrepose->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
652  reachGridPrepose->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
653 
654  // reachGrid->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
655  VirtualRobot::WorkspaceGridPtr reachGridGrasp;
656  reachGridGrasp.reset(new WorkspaceGrid(
657  minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
658  reachGridGrasp->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
659  ARMARX_INFO << " grasp pose: " << dummyGrasp->getTcpPoseGlobal(globalObjectPose);
660  ARMARX_INFO << " prepose pose: " << dummyPrepose->getTcpPoseGlobal(globalObjectPose);
661  reachGridGrasp->fillGridData(ws, dummyObject, dummyGrasp, robot->getRootNode());
662  // drawWorkspaceGrid(reachGridGrasp);
663  // debugDrawerPrx->setTextVisu(visuLayerName, "GridLabel", "GraspGrid", new Vector3(dummyObject->getGlobalPose()(0, 3), dummyObject->getGlobalPose()(1, 3), 500), DrawColor {0, 0, 1, 1}, 30);
664  // sleep(2);
665  // // if (!visualizedGrid)
666  // {
667 
668  // visualizedGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), ws->getDiscretizeParameterTranslation()));
669  TIMING_START(GridMerge);
670  visualizedGrid =
671  reachGridGrasp; //VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
672  TIMING_END(GridMerge);
673  // visualizedGrid->setGridPosition(globalObjectPose(0, 3), globalObjectPose(1, 3));
674  // }
675  // visualizedGrid->fillGridData(ws, dummyObject, dummyGrasp, robot->getRootNode());
676 
677  // drawWorkspaceGrid(visualizedGrid);
678  // debugDrawerPrx->setTextVisu(visuLayerName, "GridLabel", "MergedGrid", new Vector3(dummyObject->getGlobalPose()(0, 3), dummyObject->getGlobalPose()(1, 3), 500), DrawColor {0, 0, 1, 1}, 30);
679  // sleep(2);
680  // visualizedGrid->fillGridData(ws, dummyObject, dummyPrepose, robot->getRootNode());
681  // drawWorkspaceGrid(visualizedGrid);
682  // sleep(2);
683  return reachGridGrasp;
684  // return VirtualRobot::WorkspaceGrid::MergeWorkspaceGrids({reachGridGrasp, reachGridPrepose});
685 }
686 
687 bool
688 SimpleRobotPlacement::getSuitablePosition(
689  const GeneratedGrasp& g,
690  VirtualRobot::WorkspaceGridPtr reachGrid,
691  const Eigen::Matrix4f& globalObjectPose,
692  float& storeGlobalX,
693  float& storeGlobalY,
694  float& storeGlobalYaw,
695  int& score,
696  const VirtualRobot::MathTools::ConvexHull2DPtr& placementArea)
697 {
698 
699  // robot pose
700  Eigen::Matrix4f originalRobotPoseGlobal = robot->getGlobalPose();
701  Eigen::Matrix4f tmpRobotPoseGlobal = originalRobotPoseGlobal;
702 
703  // workspace grid params, workspace is a grid filled with reachability scores for each x,y position
704  float minX, maxX, minY, maxY;
705  reachGrid->getExtends(minX, maxX, minY, maxY);
706  int nX = 0;
707  int nY = 0;
708  reachGrid->getCells(nX, nY);
709  int maxEntry = reachGrid->getMaxEntry();
710  int minRequiredEntry = maxEntry;
711 
712  bool collision = true;
713  std::vector<GraspPtr> dummyGrasps;
714  float minCollisionDistance = getProperty<float>("MinimumDistanceToEnvironment").getValue();
715  int counter = 0;
716  auto collisionCheckVisu = "collisionCheckRobotVisu";
717  if (getProperty<bool>("EnableVisualization"))
718  {
719  entityDrawerPrx->setRobotVisu(
720  visuLayerName,
721  collisionCheckVisu,
722  robotStateComponentPrx->getRobotFilename(),
723  simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
724  FullModel);
725  auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
726  ARMARX_INFO << "Using config: " << config;
727  entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
728  }
729 
730  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
731  auto wsr = getWorkspaceRepresentation(g);
732  float minManipulabilityDecreaseFactor =
733  getProperty<float>("MinManipulabilityDecreaseFactor").getValue();
734  int maxTrials = 1000;
735  bool success = false;
736 
737  const float minDistance2D = getProperty<float>("MinPlacementDistance").getValue();
738  const float maxDistance2D = getProperty<float>("MaxPlacementDistance").getValue();
739 
740  while (true)
741  {
742  counter++;
743  if (counter >= maxTrials)
744  {
745  ARMARX_ERROR << "Could not find a collision free robot placement.";
746  storeGlobalX = 0;
747  storeGlobalY = 0;
748  storeGlobalYaw = 0;
749  break;
750  }
751 
752  minRequiredEntry *= minManipulabilityDecreaseFactor;
753  minRequiredEntry = std::max<int>(minRequiredEntry, maxEntry * 0.1f);
754  int entries;
755  if (!reachGrid->getRandomPos(
756  minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
757  {
758  continue;
759  }
760  float distance2D =
761  (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
762  .norm();
763  if (distance2D > maxDistance2D)
764  {
765  // ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
766  ARMARX_INFO << "Placement too far away: " << distance2D;
767  continue;
768  }
769  if (distance2D < minDistance2D)
770  {
771  ARMARX_INFO << "Placement too close: " << distance2D;
772  continue;
773  }
774 
775  if (placementArea != nullptr &&
776  !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
777  placementArea))
778  {
779  ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
780  << ") lies outside the permitted area => Retry.";
781  continue;
782  }
783 
784  // update robot position
785  tmpRobotPoseGlobal = originalRobotPoseGlobal;
786  tmpRobotPoseGlobal(0, 3) = storeGlobalX;
787  tmpRobotPoseGlobal(1, 3) = storeGlobalY;
788 
789  storeGlobalYaw = getPlatformRotation(tmpRobotPoseGlobal, globalObjectPose);
790  score = reachGrid->getEntry(storeGlobalX, storeGlobalY);
791 
792  // we rotate the pose around its local z axis
793  float x = tmpRobotPoseGlobal(0, 3);
794  float y = tmpRobotPoseGlobal(1, 3);
795  float z = tmpRobotPoseGlobal(2, 3);
796  if (std::isnan(x) || std::isnan(y) || std::isnan(z))
797  {
798  ARMARX_INFO << "Position contains NaN";
799  continue;
800  }
801  tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
802  Eigen::Affine3f transform;
803  transform = Eigen::Translation<float, 3>(x, y, z) *
804  Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
805  tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
806 
807  robot->setGlobalPose(tmpRobotPoseGlobal);
808  cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
809  int max = wsr->getMaxEntry();
810  ARMARX_VERBOSE
811  << "Candidate pose: "
812  << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
813  << "% min. required: " << minRequiredEntry << " trial: " << counter * 100 / maxTrials
814  << "%";
815  if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
816  {
817  ARMARX_INFO << "not reachable";
818  continue;
819  }
820 
821  collision = cspace->getCD().isInCollision();
822  if (minCollisionDistance > 0)
823  {
824  float distance = cspace->getCD().getDistance();
825  if (distance < minCollisionDistance)
826  {
827  collision = true;
828  }
829  ARMARX_DEBUG << "distance to objects for placement: " << distance;
830  }
831 
832  //if (!collision)
833  if (getProperty<bool>("EnableVisualization"))
834  {
835  updateRobot(collisionCheckVisu,
836  tmpRobotPoseGlobal,
837  collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
838  usleep(500000 * visuSlowdownFactor);
839  }
840 
841 
842  if (!collision)
843  {
844  ARMARX_IMPORTANT << "Found collision free placement";
845  success = true;
846 
847  // if (getProperty<bool>("VisualizeCollisionSpace").getValue())
848  // {
849  // Eigen::Vector3f rpy;
850  // VirtualRobot::MathTools::eigen4f2rpy(tmpRobotPoseGlobal, rpy);
851  // armarx::VectorXf startPos {tmpRobotPoseGlobal(0, 3), tmpRobotPoseGlobal(1, 3), rpy(2)};
852  // MotionPlanningServerInterfacePrx mps = getProxy<MotionPlanningServerInterfacePrx>("MotionPlanningServer", false, "", false);
853  // if (mps)
854  // {
855  // SimoxCSpaceWith2DPosePtr tmpCSpace = SimoxCSpaceWith2DPosePtr::dynamicCast(cspace->clone());
856  // auto agent = tmpCSpace->getAgent();
857  // agent.agentPose = new Pose(tmpRobotPoseGlobal);
858  // tmpCSpace->setAgent(agent);
859 
860  // CSpaceVisualizerTaskHandle taskHandle = mps->enqueueTask(new CSpaceVisualizerTask(tmpCSpace, startPos, getDefaultName() + "Visu" + IceUtil::generateUUID()));
861  // planningTasks.push_back(taskHandle);
862  // }
863  // }
864 
865  break;
866  }
867  else
868  {
869  ARMARX_INFO << "In collision";
870  }
871  }
872  ARMARX_INFO << "Returning result" << VAROUT(storeGlobalX) << VAROUT(storeGlobalY);
873  //entityDrawerPrx->removeRobotVisu(visuLayerName, collisionCheckVisu);
874  robot->setGlobalPose(originalRobotPoseGlobal);
875  return success;
876 }
877 
878 std::vector<SimpleRobotPlacement::RobotPlacement>
879 SimpleRobotPlacement::getSuitablePositions(const GeneratedGrasp& g,
880  WorkspaceGridPtr reachGrid,
881  const Eigen::Matrix4f& globalObjectPose,
882  float requiredReachabilityFraction,
883  int requiredCount,
884  int maxIterations,
885  const MathTools::ConvexHull2DPtr& placementArea)
886 {
887 
888  std::vector<SimpleRobotPlacement::RobotPlacement> placements;
889  // robot pose
890  Eigen::Matrix4f originalRobotPoseGlobal = robot->getGlobalPose();
891  Eigen::Matrix4f tmpRobotPoseGlobal = originalRobotPoseGlobal;
892 
893  // workspace grid params, workspace is a grid filled with reachability scores for each x,y position
894  float minX, maxX, minY, maxY;
895  reachGrid->getExtends(minX, maxX, minY, maxY);
896  int nX = 0;
897  int nY = 0;
898  reachGrid->getCells(nX, nY);
899  int maxEntry = reachGrid->getMaxEntry();
900  int minRequiredEntry = maxEntry * requiredReachabilityFraction;
901 
902  bool collision = true;
903  std::vector<GraspPtr> dummyGrasps;
904  float minCollisionDistance = getProperty<float>("MinimumDistanceToEnvironment").getValue();
905  auto collisionCheckVisu = "collisionCheckRobotVisu";
906  if (getProperty<bool>("EnableVisualization"))
907  {
908  entityDrawerPrx->setRobotVisu(
909  visuLayerName,
910  collisionCheckVisu,
911  robotStateComponentPrx->getRobotFilename(),
912  simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
913  FullModel);
914  auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
915  ARMARX_INFO << "Using config: " << config;
916  entityDrawerPrx->updateRobotConfig(visuLayerName, collisionCheckVisu, config);
917  }
918 
919  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
920  auto wsr = getWorkspaceRepresentation(g);
921 
922  for (int n = 0; n < maxIterations; n++)
923  {
924 
925  int entries;
926  float storeGlobalX, storeGlobalY;
927  if (!reachGrid->getRandomPos(
928  minRequiredEntry, storeGlobalX, storeGlobalY, dummyGrasps, 100, &entries))
929  {
930  continue;
931  }
932  float distance2D =
933  (Eigen::Vector2f(storeGlobalX, storeGlobalY) - globalObjectPose.block<2, 1>(0, 3))
934  .norm();
935  if (distance2D > 1000)
936  {
937  // ARMARX_INFO << VAROUT(globalObjectPose) << " 2d: " << globalObjectPose.block<2, 1>(0, 3) << " candidate: " << Eigen::Vector2f(storeGlobalX, storeGlobalY);
938  ARMARX_INFO << "Placement too far away: " << distance2D;
939  continue;
940  }
941  if (distance2D < 400)
942  {
943  ARMARX_INFO << "Placement too close: " << distance2D;
944  continue;
945  }
946 
947  if (placementArea != nullptr &&
948  !VirtualRobot::MathTools::isInside(Eigen::Vector2f(storeGlobalX, storeGlobalY),
949  placementArea))
950  {
951  ARMARX_INFO << "Generated placement (" << storeGlobalX << ", " << storeGlobalY
952  << ") lies outside the permitted area => Retry.";
953  continue;
954  }
955 
956  // update robot position
957  tmpRobotPoseGlobal = originalRobotPoseGlobal;
958  tmpRobotPoseGlobal(0, 3) = storeGlobalX;
959  tmpRobotPoseGlobal(1, 3) = storeGlobalY;
960 
961  float storeGlobalYaw = getPlatformRotation(tmpRobotPoseGlobal, globalObjectPose);
962  float score = reachGrid->getEntry(storeGlobalX, storeGlobalY);
963 
964  // we rotate the pose around its local z axis
965  float x = tmpRobotPoseGlobal(0, 3);
966  float y = tmpRobotPoseGlobal(1, 3);
967  float z = tmpRobotPoseGlobal(2, 3);
968  if (std::isnan(x) || std::isnan(y) || std::isnan(z))
969  {
970  ARMARX_INFO << "Position contains NaN";
971  continue;
972  }
973  tmpRobotPoseGlobal.block<3, 1>(0, 3) = Eigen::Vector3f::Zero();
974  Eigen::Affine3f transform;
975  transform = Eigen::Translation<float, 3>(x, y, z) *
976  Eigen::AngleAxisf(storeGlobalYaw, Eigen::Vector3f(0, 0, 1));
977  tmpRobotPoseGlobal = transform * tmpRobotPoseGlobal;
978 
979  robot->setGlobalPose(tmpRobotPoseGlobal);
980  cspace->getAgentSceneObj()->setGlobalPose(tmpRobotPoseGlobal);
981  int max = wsr->getMaxEntry();
982  ARMARX_VERBOSE
983  << "Candidate pose: "
984  << 100. * wsr->getEntry(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()) / max
985  << "% min. required: " << minRequiredEntry;
986  if (!wsr->isCovered(FramedPosePtr::dynamicCast(g.framedPrePose)->toEigen()))
987  {
988  ARMARX_INFO << "not reachable";
989  continue;
990  }
991 
992  collision = cspace->getCD().isInCollision();
993  if (minCollisionDistance > 0)
994  {
995  float distance = cspace->getCD().getDistance();
996  if (distance < minCollisionDistance)
997  {
998  collision = true;
999  }
1000  ARMARX_DEBUG << "distance to objects for placement: " << distance;
1001  }
1002 
1003  //if (!collision)
1004  if (getProperty<bool>("EnableVisualization"))
1005  {
1006  updateRobot(collisionCheckVisu,
1007  tmpRobotPoseGlobal,
1008  collision ? DrawColor{1.0, 0.0, 0.0, 1} : DrawColor{0.0, 1.0, 0.0, 1});
1009  usleep(500000 * visuSlowdownFactor);
1010  }
1011 
1012 
1013  if (!collision)
1014  {
1015  ARMARX_IMPORTANT << "Found collision free placement";
1016  ARMARX_IMPORTANT << VAROUT(dummyGrasps.size());
1017 
1018  RobotPlacement pl;
1019  pl.x = storeGlobalX;
1020  pl.y = storeGlobalY;
1021  pl.z = storeGlobalYaw;
1022  pl.score = score;
1023  placements.push_back(pl);
1024  }
1025  else
1026  {
1027  ARMARX_INFO << "In collision";
1028  }
1029  if ((int)placements.size() > requiredCount)
1030  {
1031  break;
1032  }
1033  }
1034 
1035  ARMARX_INFO << "Found " << placements.size() << " collision free placements";
1036  robot->setGlobalPose(originalRobotPoseGlobal);
1037  return placements;
1038 }
1039 
1040 float
1041 SimpleRobotPlacement::getPlatformRotation(const Eigen::Matrix4f& frameGlobal,
1042  const Eigen::Matrix4f& globalTarget)
1043 {
1044  Eigen::Matrix4f localTarget = frameGlobal.inverse() * globalTarget;
1045  float x = localTarget(0, 3);
1046  float y = localTarget(1, 3);
1047 
1048  float alpha = std::atan2(y, x);
1049  alpha -= M_PI / 2; // armars face direction is the positive y-axis, therefore -pi/2
1050  return alpha;
1051 }
1052 
1053 void
1054 SimpleRobotPlacement::loadRobot()
1055 {
1056  robot = VirtualRobot::RobotIO::loadRobot(robotFilePath);
1057  if (!robot)
1058  {
1059  ARMARX_ERROR << "Failed to load robot: " << robotFilePath;
1060  return;
1061  }
1062 
1063  std::shared_ptr<RemoteRobot> remoteRobot(
1064  new RemoteRobot(robotStateComponentPrx->getSynchronizedRobot()));
1065  robot->setGlobalPose(remoteRobot->getGlobalPose());
1066 }
1067 
1068 void
1069 SimpleRobotPlacement::loadWorkspaces()
1070 {
1071  for (std::string wsFile : workspaceFilePaths)
1072  {
1073  VirtualRobot::WorkspaceRepresentationPtr newSpace;
1074  bool success = false;
1075 
1076  // 1st try to load as manipulability file
1077  try
1078  {
1079  newSpace.reset(new Manipulability(robot));
1080  newSpace->load(wsFile);
1081  success = true;
1082 
1083  ARMARX_INFO << "Map '" << wsFile << "' loaded as Manipulability map";
1084  }
1085  catch (...)
1086  {
1087  }
1088 
1089  // 2nd try to load as reachability file
1090  if (!success)
1091  {
1092  try
1093  {
1094  newSpace.reset(new Reachability(robot));
1095  newSpace->load(wsFile);
1096  success = true;
1097 
1098  ARMARX_INFO << "Map '" << wsFile << "' loaded as Reachability map";
1099  }
1100  catch (...)
1101  {
1102  armarx::handleExceptions();
1103  }
1104  }
1105 
1106  if (success)
1107  {
1108  workspaces.push_back(newSpace);
1109  }
1110  else
1111  {
1112  ARMARX_ERROR << "Failed to load map '" << wsFile << "'";
1113  }
1114  }
1115 }
1116 
1117 bool
1118 SimpleRobotPlacement::hasWorkspace(std::string tcp)
1119 {
1120  for (VirtualRobot::WorkspaceRepresentationPtr ws : workspaces)
1121  {
1122  if (ws->getTCP()->getName() == tcp)
1123  {
1124  return true;
1125  }
1126  }
1127  return false;
1128 }
1129 
1130 GeneratedGraspList
1131 SimpleRobotPlacement::filterGrasps(const GeneratedGraspList grasps)
1132 {
1133  GeneratedGraspList filteredGrasps = grasps;
1134  GeneratedGraspList::iterator it = filteredGrasps.begin();
1135  while (it != filteredGrasps.end())
1136  {
1137  GeneratedGrasp g = (*it);
1138  auto tcpName = robot->getEndEffector(g.eefName)->getTcp()->getName();
1139  if (!hasWorkspace(tcpName))
1140  {
1141  ARMARX_VERBOSE << "Removing grasp because tcp " << tcpName
1142  << " is not available in workspace";
1143  it = filteredGrasps.erase(it);
1144  }
1145  else
1146  {
1147  ++it;
1148  }
1149  }
1150  return filteredGrasps;
1151 }
1152 
1153 void
1154 SimpleRobotPlacement::drawNewRobot(Eigen::Matrix4f globalPose)
1155 {
1156  if (!getProperty<bool>("EnableVisualization"))
1157  {
1158  return;
1159  }
1160  static int suffix = 0;
1161  std::string id = drawRobotId + to_string(suffix++);
1162  entityDrawerPrx->setRobotVisu(
1163  visuLayerName,
1164  id,
1165  robotStateComponentPrx->getRobotFilename(),
1166  simox::alg::join(robotStateComponentPrx->getArmarXPackages(), ","),
1167  FullModel);
1168 
1169  auto config = robotStateComponentPrx->getSynchronizedRobot()->getConfig();
1170  ARMARX_INFO << "Using config: " << config;
1171  entityDrawerPrx->updateRobotConfig(visuLayerName, id, config);
1172  updateRobot(id, globalPose, COLOR_ROBOT);
1173 }
1174 
1175 void
1176 SimpleRobotPlacement::updateRobot(std::string id, Eigen::Matrix4f globalPose, DrawColor color)
1177 {
1178  if (!getProperty<bool>("EnableVisualization"))
1179  {
1180  return;
1181  }
1182  entityDrawerPrx->updateRobotColor(visuLayerName, id, color);
1183  entityDrawerPrx->updateRobotPose(visuLayerName, id, new Pose(globalPose));
1184 }
1185 
1186 void
1187 SimpleRobotPlacement::drawWorkspaceGrid(const GeneratedGrasp& grasp,
1188  const std::string& objectInstanceEntityId)
1189 {
1190  if (!getProperty<bool>("EnableVisualization"))
1191  {
1192  return;
1193  }
1194  ObjectInstancePtr instance =
1195  ObjectInstancePtr::dynamicCast(objectInstances->getEntityById(objectInstanceEntityId));
1196  Eigen::Matrix4f objectPose = instance->getPose()->toGlobalEigen(robot);
1197 
1198  drawWorkspaceGrid(createWorkspaceGrid(grasp, objectPose));
1199 }
1200 
1201 void
1202 SimpleRobotPlacement::drawWorkspaceGrid(VirtualRobot::WorkspaceGridPtr reachGrid)
1203 {
1204  if (!getProperty<bool>("EnableVisualization"))
1205  {
1206  return;
1207  }
1208  int counter = 0;
1209  Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
1210  float minX, maxX, minY, maxY;
1211  reachGrid->getExtends(minX, maxX, minY, maxY);
1212  pose(0, 3) = minX;
1213  pose(1, 3) = minY;
1214 
1215  int nX = 0;
1216  int nY = 0;
1217  reachGrid->getCells(nX, nY);
1218 
1219  int maxEntry = reachGrid->getMaxEntry();
1220 
1221  float sizeX = (maxX - minX) / (float)nX;
1222  float sizeY = (maxY - minY) / (float)nY;
1223  auto batch = debugDrawerPrx->ice_batchOneway();
1224  // iterate through the workspace grid
1225  for (int x = 0; x < nX; x++)
1226  {
1227  float xPos = minX + (float)x * sizeX + 0.5f * sizeX; // x-center of voxel
1228 
1229  for (int y = 0; y < nY; y++)
1230  {
1231  int cellEntry = reachGrid->getCellEntry(x, y);
1232 
1233  if (cellEntry > 0)
1234  {
1235  float intensity = (float)cellEntry / (float)maxEntry;
1236 
1237  float yPos = minY + (float)y * sizeY + 0.5f * sizeY; // y-center of voxel
1238  pose(0, 3) = xPos;
1239  pose(1, 3) = yPos;
1240 
1241  armarx::Vector3Ptr dimensions = new armarx::Vector3(30, 30, 1);
1242  armarx::PosePtr tmpPose = new armarx::Pose(pose);
1243 
1244  VirtualRobot::ColorMap cm = VirtualRobot::ColorMap::eHot;
1245  VirtualRobot::CoinVisualizationFactory::Color color = cm.getColor(intensity);
1246 
1247  armarx::DrawColor voxelColor;
1248  voxelColor.r = color.r;
1249  voxelColor.g = color.g;
1250  voxelColor.b = color.b;
1251  voxelColor.a = 0.5;
1252 
1253  batch->setBoxVisu(visuLayerName,
1254  "reachGridVoxel_" + to_string(counter++),
1255  tmpPose,
1256  dimensions,
1257  voxelColor);
1258  }
1259  }
1260  }
1261  batch->ice_flushBatchRequests();
1262 }
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
algorithm.h
PlacementInfo::placements
std::vector< SimpleRobotPlacement::RobotPlacement > placements
Definition: SimpleRobotPlacement.cpp:340
armarx::SimpleRobotPlacement::RobotPlacement::score
float score
Definition: SimpleRobotPlacement.h:104
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:306
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:522
PlacementInfo
Definition: SimpleRobotPlacement.cpp:336
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
VirtualRobot
Definition: FramedPose.h:42
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::Contains
bool Contains(const ContainerType &container, const ElementType &searchElement)
Definition: algorithm.h:330
PlacementInfo::sortplacements
void sortplacements()
Definition: SimpleRobotPlacement.cpp:344
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
PlacementInfo::grasp
GeneratedGrasp grasp
Definition: SimpleRobotPlacement.cpp:338
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:36
boost::target
Vertex target(const detail::edge_base< Directed, Vertex > &e, const PCG &)
Definition: point_cloud_graph.h:668
armarx::max
std::vector< T > max(const std::vector< T > &v1, const std::vector< T > &v2)
Definition: VectorHelpers.h:297
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::SimpleRobotPlacement::onExitComponent
void onExitComponent() override
Definition: SimpleRobotPlacement.cpp:206
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::SimpleRobotPlacement::RobotPlacement::z
float z
Definition: SimpleRobotPlacement.h:103
armarx::SimpleRobotPlacement::RobotPlacement
Definition: SimpleRobotPlacement.h:99
armarx::SimpleRobotPlacement::RobotPlacement::y
float y
Definition: SimpleRobotPlacement.h:102
project
std::string project
Definition: VisualizationRobot.cpp:85
pi
#define pi
Definition: Transition.cpp:38
PlacementInfo::resultPose
armarx::FramedPosePtr resultPose
Definition: SimpleRobotPlacement.cpp:352
armarx::SimpleRobotPlacement::getWorkspaceRepresentation
VirtualRobot::WorkspaceRepresentationPtr getWorkspaceRepresentation(const GeneratedGrasp &g)
Tries to get a workspace with the same TCP as the EEF of the robot.
Definition: SimpleRobotPlacement.cpp:606
IceInternal::Handle< AgentInstance >
VoxelGridCSpace.h
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
Color
uint32_t Color
RGBA color.
Definition: color.h:8
armarx::SimpleRobotPlacement::onDisconnectComponent
void onDisconnectComponent() override
Definition: SimpleRobotPlacement.cpp:201
M_PI
#define M_PI
Definition: MathTools.h:17
FramedPose.h
armarx::VoxelGridCSpace
Definition: VoxelGridCSpace.h:34
armarx::SimpleRobotPlacement::generateRobotPlacements
GraspingPlacementList generateRobotPlacements(const GeneratedGraspList &grasps, const Ice::Current &c=Ice::emptyCurrent) override
Computes a list of candidate robot placements for a list of grasps.
Definition: SimpleRobotPlacement.cpp:211
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
PlacementInfo::reachGrid
VirtualRobot::WorkspaceGridPtr reachGrid
Definition: SimpleRobotPlacement.cpp:341
MemoryXCoreObjectFactories.h
armarx::SimoxCSpace::PrefetchWorkingMemoryObjects
static SimoxCSpacePtr PrefetchWorkingMemoryObjects(memoryx::WorkingMemoryInterfacePrx workingMemoryPrx, memoryx::CommonStorageInterfacePrx commonStoragePrx, RobotStateComponentInterfacePrx rsc)
Load objects from WorkingMemory and puts them into the mesh cache.
Definition: SimoxCSpace.cpp:734
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:196
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
armarx::SimpleRobotPlacement::onInitComponent
void onInitComponent() override
Definition: SimpleRobotPlacement.cpp:134
armarx::transform
auto transform(const Container< InputT, Alloc > &in, OutputT(*func)(InputT const &)) -> Container< OutputT, typename std::allocator_traits< Alloc >::template rebind_alloc< OutputT >>
Convenience function (with less typing) to transform a container of type InputT into the same contain...
Definition: algorithm.h:351
armarx::SimpleRobotPlacement::generateRobotPlacementsEx
GraspingPlacementList generateRobotPlacementsEx(const GeneratedGraspList &grasps, const Ice::Current &c=Ice::emptyCurrent) override
Definition: SimpleRobotPlacement.cpp:356
armarx::RemoteRobotPtr
std::shared_ptr< RemoteRobot > RemoteRobotPtr
Definition: RemoteRobot.h:291
CMakePackageFinder.h
SimoxObjectWrapper.h
SimoxCSpace.h
armarx::RemoteRobot
Mimics the behaviour of the VirtualRobot::Robot class while redirecting everything to an Ice proxy.
Definition: RemoteRobot.h:144
PlacementInfo::graspPose
Eigen::Matrix4f graspPose
Definition: SimpleRobotPlacement.cpp:339
ObjectInstance.h
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
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
float
#define float
Definition: 16_Level.h:22
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
ConvexHull
Definition: convexHull.hpp:376
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:300
SimpleRobotPlacement.h
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:41
MemoryXTypesObjectFactories.h
AgentInstance.h
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:95
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:109
armarx::SimpleRobotPlacementPropertyDefinitions::SimpleRobotPlacementPropertyDefinitions
SimpleRobotPlacementPropertyDefinitions(std::string prefix)
Definition: SimpleRobotPlacement.cpp:68
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:157
armarx::SimoxCSpace
The SimoxCSpace contains a set of stationary obstacles and an agent.
Definition: SimoxCSpace.h:66
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition: ManagedIceObject.cpp:407
armarx::SimpleRobotPlacement::onConnectComponent
void onConnectComponent() override
Definition: SimpleRobotPlacement.cpp:175
armarx::SimpleRobotPlacement::generateRobotPlacementsForGraspPose
GraspingPlacementList generateRobotPlacementsForGraspPose(const std::string &endEffectorName, const FramedPoseBasePtr &target, const PlanarObstacleList &planarObstacles, const ConvexHull &placementArea, const Ice::Current &c=Ice::emptyCurrent) override
Computes a list of candidate robot placements for a grasp pose.
Definition: SimpleRobotPlacement.cpp:466
ArmarXDataPath.h
armarx::PropertyDefinitionBase::eModifiable
@ eModifiable
Definition: PropertyDefinitionInterface.h:57
armarx::RobotPlacement
Definition: RobotPlacement.h:35
armarx::SimpleRobotPlacement::RobotPlacement::x
float x
Definition: SimpleRobotPlacement.h:101
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
armarx::status::success
@ success
norm
double norm(const Point &a)
Definition: point.hpp:102