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