GraspingManager.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::GraspingManager
19  * @author Valerij Wittenbeck (valerij dot wittenbeck at student dot kit dot edu)
20  * @date 2016
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "GraspingManager.h"
26 
27 #include <atomic>
28 #include <tuple>
29 
30 #include <IceUtil/UUID.h>
31 
32 #include <SimoxUtility/algorithm/string/string_tools.h>
33 #include <VirtualRobot/IK/ConstrainedOptimizationIK.h>
34 #include <VirtualRobot/IK/constraints/CollisionCheckConstraint.h>
35 #include <VirtualRobot/IK/constraints/OrientationConstraint.h>
36 #include <VirtualRobot/IK/constraints/PoseConstraint.h>
37 #include <VirtualRobot/IK/constraints/PositionConstraint.h>
38 #include <VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.h>
39 #include <VirtualRobot/MathTools.h>
40 #include <VirtualRobot/RobotConfig.h>
41 #include <VirtualRobot/RobotNodeSet.h>
42 
43 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
44 #include <ArmarXCore/core/util/OnScopeExit.h>
45 #include <ArmarXCore/core/util/StringHelperTemplates.h>
46 
47 #include <RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h>
48 #include <RobotAPI/libraries/core/CartesianPositionController.h>
49 #include <RobotAPI/libraries/core/FramedPose.h>
50 #include <RobotAPI/libraries/core/Pose.h>
51 #include <RobotAPI/libraries/core/Trajectory.h>
52 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
53 
54 #include <RobotComponents/components/GraspSelectionManager/selectionCriteria/NaturalGraspFilter.h>
55 #include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
56 #include <RobotComponents/components/MotionPlanning/CSpace/VoxelGridCSpace.h>
57 #include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
58 
59 using namespace armarx;
60 
61 static const DrawColor COLOR_POSE_LINE{0.5f, 1.0f, 0.5f, 0.5f};
62 static const DrawColor COLOR_POSE_POINT{0.0f, 1.0f, 0.0f, 0.5f};
63 static const DrawColor COLOR_CONFIG_LINE{1.0f, 0.5f, 0.5f, 0.5f};
64 static const DrawColor COLOR_CONFIG_POINT{1.0f, 0.0f, 0.0f, 0.5f};
65 static const DrawColor COLOR_ROBOT{0.0f, 0.586f, 0.508f, 1.0f};
66 
67 static const float LINE_WIDTH = 5.f;
68 static const float SPHERE_SIZE = 6.f;
69 
70 static const std::map<std::string, std::string> TCP_HAND_MAPPING{{"TCP R", "handright3a"},
71  {"TCP L", "handleft3a"},
72  {"Hand L TCP", "left_hand"},
73  {"Hand R TCP", "right_hand"}};
74 
75 
76 auto newId = []() mutable
77 {
78  static std::atomic<int> i{0};
79  return to_string(i++);
80 };
81 
82 void
83 GraspingManager::onInitComponent()
84 {
85  graspGeneratorName = getProperty<std::string>("GraspGeneratorName").getValue();
86  robotPlacementName = getProperty<std::string>("RobotPlacementName").getValue();
87  robotNodeSetNames =
88  armarx::Split(getProperty<std::string>("RobotNodeSetNames").getValue(), ";");
89  reachabilitySpaceFilePaths =
90  armarx::Split(getProperty<std::string>("ReachabilitySpaceFilePaths").getValue(), ";");
91 
92  globalDescriptionPosition = new Vector3(0, 0, 0);
93 
94  //@TODO still not sure if this is the way to go
95  for (auto& path : reachabilitySpaceFilePaths)
96  {
97  std::string packageName = std::filesystem::path{path}.begin()->string();
98  ARMARX_CHECK_EXPRESSION(!packageName.empty())
99  << "Path '" << path << "' could not be parsed correctly";
100  armarx::CMakePackageFinder project(packageName);
101  path = project.getDataDir() + "/" + path;
102  }
103 
104  offeringTopic("DebugDrawerUpdates");
105 
106  usingProxy(graspGeneratorName);
107  usingProxy("GraspSelectionManager");
108  usingProxy("NaturalGraspFilter");
109  usingProxy(robotPlacementName);
110  usingProxy("PlannedMotionProvider");
111 
112  usingProxy("RobotIK");
113  usingProxy("RobotStateComponent");
114 
115  usingProxy("CommonStorage");
116  usingProxy("WorkingMemory");
117  usingProxy("PriorKnowledge");
118 }
119 
120 void
121 GraspingManager::onConnectComponent()
122 {
123  entityDrawer = getTopic<memoryx::EntityDrawerInterfacePrx>("DebugDrawerUpdates");
124  layerName = getDefaultName();
125  getProxy(gg, graspGeneratorName);
126  getProxy(gsm, "GraspSelectionManager");
127  getProxy(rp, robotPlacementName);
128  getProxy(pmp, "PlannedMotionProvider");
129 
130  //getProxy(rik, "RobotIK");
131  getProxy(rsc, "RobotStateComponent");
132  localRobot =
133  armarx::RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eCollisionModel);
134  ikRobot = localRobot->clone();
135  // localRobot->print();
136 
137  getProxy(cs, "CommonStorage");
138  getProxy(wm, "WorkingMemory");
139  getProxy(prior, "PriorKnowledge");
140 
141  // register the SimplePlacementFilter with the GraspSelectionManager
142  if (getProperty<bool>("FilterUnnaturalGrasps").getValue())
143  {
144  getProxy(gsc, "NaturalGraspFilter");
145  gsm->registerAsGraspSelectionCriterion(gsc);
146  }
147  cacheCSpace = SimoxCSpace::PrefetchWorkingMemoryObjects(wm, cs, rsc);
148 
149  Ice::FloatSeq boundsStrings =
150  Split<float>(getProperty<std::string>("PlanningBoundingBox").getValue(), ",");
151  ARMARX_INFO << VAROUT(boundsStrings);
152  planningBoundingBox.min.e0 = boundsStrings.at(0);
153  planningBoundingBox.min.e1 = boundsStrings.at(1);
154  planningBoundingBox.min.e2 = boundsStrings.at(2);
155  planningBoundingBox.max.e0 = boundsStrings.at(3);
156  planningBoundingBox.max.e1 = boundsStrings.at(4);
157  planningBoundingBox.max.e2 = boundsStrings.at(5);
158 
159  // bool hasUnloadedRNS = false;
160  // for (const auto& rnsName : robotNodeSetNames)
161  // {
162  // if (!rik->hasReachabilitySpace(rnsName))
163  // {
164  // ARMARX_INFO << "RNS '" << rnsName << "' has no reachability space defined.";
165  // hasUnloadedRNS = true;
166  // break;
167  // }
168  // }
169 
170  // if (hasUnloadedRNS)
171  // {
172  // ARMARX_INFO << "at least one RNS has no reachability space defined; loading reachability files...";
173  // for (const auto& path : reachabilitySpaceFilePaths)
174  // {
175  // ARMARX_INFO << "trying to load from path '" << path << "'";
176  // bool rsLoaded = rik->loadReachabilitySpace(path);
177  // ARMARX_CHECK_EXPRESSION(rsLoaded) << "Could not load reachability space from path '" << path << "'";
178  // ARMARX_INFO << "reachability file successfully loaded";
179  // }
180  // }
181 
182  // for (const auto& rnsName : robotNodeSetNames)
183  // {
184  // ARMARX_CHECK_EXPRESSION(rik->hasReachabilitySpace(rnsName)) <<
185  // "RNS '" << rnsName << "' still has no reachability space defined";
186  // }
187 }
188 
189 void
190 GraspingManager::onDisconnectComponent()
191 {
192 }
193 
194 void
195 GraspingManager::onExitComponent()
196 {
197  cacheCSpace = NULL;
198 }
199 
200 GeneratedGraspList
201 GraspingManager::generateGrasps(const std::string& objectInstanceEntityId)
202 {
203  setNextStepDescription("Generating grasps");
204  ARMARX_VERBOSE << "Step: generate grasps";
205  GeneratedGraspList grasps = gg->generateGrasps(objectInstanceEntityId);
206  std::sort(grasps.begin(),
207  grasps.end(),
208  [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
209  if (grasps.empty())
210  {
211  ARMARX_WARNING << " Step 'generate grasps' generated no grasps";
212  }
213  // auto obj = wm->getObjectInstancesSegment()->getObjectInstanceById(objectInstanceEntityId);
214  // auto classes = prior->getObjectClassesSegment();
215  return grasps;
216 }
217 
218 std::pair<std::string, Ice::StringSeq>
219 GraspingManager::visualizeGrasp(const GeneratedGrasp& grasp, int id, const DrawColor& color)
220 {
221  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
222  std::pair<std::string, Ice::StringSeq> result;
223  if (visuSlowdownFactor <= 0 || !getProperty<bool>("EnableVisualization"))
224  {
225  return result;
226  }
227  auto tcpName = localRobot->getEndEffector(grasp.eefName)->getTcp()->getName();
228  if (!TCP_HAND_MAPPING.count(tcpName))
229  {
230  return result;
231  }
232  auto handName = TCP_HAND_MAPPING.at(tcpName);
233  auto objClass = prior->getObjectClassesSegment()->getObjectClassByName(handName);
234  if (objClass)
235  {
236  usleep(500000 * visuSlowdownFactor);
237  // entityDrawer->setPoseVisu("GeneratedGrasps", "GraspCandidate" + handName + to_string(id), grasp.framedPose);
238  result.first = "GeneratedGrasps";
239  result.second = {"GraspCandidate" + handName + to_string(id),
240  "GraspCandidatePrepose" + handName + to_string(id)};
241  entityDrawer->setObjectVisu(
242  "GeneratedGrasps", result.second.at(0), objClass, grasp.framedPose);
243  entityDrawer->setObjectVisu(
244  "GeneratedGrasps", result.second.at(1), objClass, grasp.framedPrePose);
245 
246  entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(0), color);
247  auto darkerColor = color;
248  darkerColor.r *= 0.6f;
249  darkerColor.g *= 0.6f;
250  darkerColor.b *= 0.6f;
251  entityDrawer->updateObjectColor("GeneratedGrasps", result.second.at(1), darkerColor);
252  }
253  else
254  {
255  ARMARX_INFO << "Could not find hand with name " << handName << " in priorknowledge";
256  }
257  return result;
258 }
259 
260 SimoxCSpacePtr
261 GraspingManager::createCSpace()
262 {
263  TIMING_START(CSpaceCreation);
264 
265  SimoxCSpacePtr cspace;
266  if (getProperty<bool>("UseVoxelGridCSpace").getValue())
267  {
268  cspace =
269  new VoxelGridCSpace(getProxy<visionx::VoxelGridProviderInterfacePrx>(
270  getProperty<std::string>("VoxelGridProviderName").getValue()),
271  cs);
272  }
273  else
274  {
275  cspace = new SimoxCSpace(cs, false);
276  }
277  cspace->addObjectsFromWorkingMemory(wm);
278  cspace->setStationaryObjectMargin(50);
279  AgentPlanningInformation agentData;
280  agentData.agentProjectNames = rsc->getArmarXPackages();
281  agentData.agentRelativeFilePath = rsc->getRobotFilename();
282  // agentData.kinemaicChainNames = robotNodeSetNames;
283  agentData.collisionSetNames = {
284  getProperty<std::string>("RobotCollisionNodeSet")
285  .getValue()}; // TODO: Make a mapping between jointset and link set
286  cspace->setAgent(agentData);
287  cspace->initCollisionTest();
288  TIMING_END(CSpaceCreation);
289  return cspace;
290 }
291 
292 GeneratedGraspList
293 GraspingManager::filterGrasps(const GeneratedGraspList& grasps)
294 {
295  setNextStepDescription("Filtering grasps");
296  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
297 
298 
299  ARMARX_VERBOSE << "Step: filter grasps";
300  GeneratedGraspList filteredGrasps = gsm->filterGrasps(grasps);
301  if (filteredGrasps.empty())
302  {
303  ARMARX_WARNING << " Step 'filter generated grasps' filtered out all grasps";
304  }
305  int i = 0;
306  if (getProperty<bool>("EnableVisualization"))
307  for (GeneratedGrasp& grasp : filteredGrasps)
308  {
309  visualizeGrasp(grasp, i);
310  i++;
311  }
312  if (visuSlowdownFactor > 0 && getProperty<bool>("EnableVisualization"))
313  {
314  usleep(2000000 * visuSlowdownFactor);
315  }
316 
317  entityDrawer->removeLayer("GeneratedGrasps");
318 
319  return filteredGrasps;
320 }
321 
322 GraspingPlacementList
323 GraspingManager::filterPlacements(const GraspingPlacementList& placements)
324 {
325  ARMARX_VERBOSE << "Step: filter placements";
326  GraspingPlacementList filteredPlacements = gsm->filterPlacements(placements);
327  if (filteredPlacements.empty())
328  {
329  ARMARX_WARNING << " Step 'filter generated grasps' filtered out all grasps";
330  }
331 
332  return filteredPlacements;
333 }
334 
335 GraspingPlacementList
336 GraspingManager::generateRobotPlacements(const GeneratedGraspList& grasps)
337 {
338  // TODO: Why is there a variable number of robot placements for each run??
339  ARMARX_VERBOSE << "Step: generate robot placements";
340  GraspingPlacementList graspPlacements = rp->generateRobotPlacementsEx(grasps);
341  ARMARX_CHECK_EXPRESSION(!graspPlacements.empty())
342  << "No placements for the robot platform were found.";
343  return graspPlacements;
344 }
345 
346 GraspingTrajectory
347 GraspingManager::planMotion(const MotionPlanningData& mpd)
348 {
349  ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
350  << VAROUT(mpd.globalPoseGoal->output());
351 
352  if (getProperty<bool>("EnableVisualization"))
353  {
354  entityDrawer->setPoseVisu(
355  layerName, "MotionPlanningPlatformTargetPose", mpd.globalPoseGoal);
356  }
357  Eigen::Vector3f bbcenter;
358  bbcenter << (planningBoundingBox.max.e0 + planningBoundingBox.min.e0) / 2,
359  (planningBoundingBox.max.e1 + planningBoundingBox.min.e1) / 2,
360  (planningBoundingBox.max.e2 + planningBoundingBox.min.e2) / 2;
361  Eigen::Vector3f bbSize;
362  bbSize << planningBoundingBox.max.e0 - planningBoundingBox.min.e0,
363  planningBoundingBox.max.e1 - planningBoundingBox.min.e1,
364  planningBoundingBox.max.e2 - planningBoundingBox.min.e2;
365  // entityDrawer->setBoxVisu(layerName, "PlanningBoundingBox", new Pose(Eigen::Matrix3f::Identity(), Eigen::Vector3f(bbcenter)), new Vector3(bbSize), DrawColor {1, 0, 0, 0.1});
366  ARMARX_VERBOSE << "Step: Motion Planning";
367 
368  if (getProperty<bool>("EnableVisualization"))
369  {
370  entityDrawer->setPoseVisu("Poses", "StartPoseAgent", mpd.globalPoseStart);
371  }
372 
373  auto robotFileName = rsc->getRobotFilename();
374  auto packageName = std::filesystem::path{robotFileName}.begin()->string();
375  auto axPackages = rsc->getArmarXPackages();
376  ARMARX_CHECK_EXPRESSION(std::find(axPackages.cbegin(), axPackages.cend(), packageName) !=
377  axPackages.cend())
378  << "Could not determine package name from path '" << robotFileName << "', "
379  << "because the determined package name '" << packageName
380  << "' is not in the following list: " << axPackages;
381  // SimoxCSpacePtr cspace = SimoxCSpacePtr::dynamicCast(this->cspace->clone(false));
382  SimoxCSpacePtr armCSpace = SimoxCSpacePtr::dynamicCast(cspace->clone());
383  // armCSpace->addObjectsFromWorkingMemory(wm);
384  // for (auto& obj : this->cspace->getStationaryObjects())
385  // {
386  // armCSpace->addStationaryObject(obj);
387  // }
388  AgentPlanningInformation agentData;
389  agentData.agentPose = mpd.globalPoseGoal;
390  agentData.agentProjectNames = rsc->getArmarXPackages();
391  agentData.agentRelativeFilePath = robotFileName;
392  // agentData.kinemaicChainNames = robotNodeSetNames;
393  agentData.kinemaicChainNames = {mpd.rnsToUse};
394  auto collisionSetMapping = getJointSetCollisionSetMapping();
395  std::string armCollisionSet = collisionSetMapping.at(mpd.rnsToUse);
396 
397  auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
398  getProperty<std::string>("RobotCollisionNodeSet").getValue());
399  auto robotColNodeNames = robotCol->getNodeNames();
400  agentData.collisionObjectNames =
401  getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
402  agentData.collisionSetNames = {armCollisionSet};
403 
404  agentData.initialJointValues = localRobot->getConfig()->getRobotNodeJointValueMap();
405  for (auto& pair : mpd.configStart)
406  {
407  agentData.initialJointValues[pair.first] = pair.second;
408  }
409  armCSpace->setAgent(agentData);
410 
411 
412  SimoxCSpacePtr cspacePlatform = SimoxCSpacePtr::dynamicCast(cspace->clone());
413  agentData.agentPose = mpd.globalPoseStart;
414  agentData.kinemaicChainNames = {"PlatformPlanning"};
415 
416  agentData.collisionObjectNames = robotColNodeNames;
417  agentData.collisionSetNames = {};
418  for (auto& pair : collisionSetMapping)
419  {
420  if (std::find(agentData.collisionSetNames.begin(),
421  agentData.collisionSetNames.end(),
422  pair.second) == agentData.collisionSetNames.end())
423  {
424  agentData.collisionSetNames.push_back(pair.second);
425  agentData.collisionObjectNames = getRobotNodeSetNodesWithoutAllwaysColliding(
426  agentData.collisionObjectNames, pair.second);
427  }
428  }
429 
430 
431  cspacePlatform->setAgent(agentData);
432  // cspacePlatform->addObjectsFromWorkingMemory(wm);
433  // for (auto& obj : armCSpace->getStationaryObjects())
434  // {
435  // cspacePlatform->addStationaryObject(obj);
436  // }
437 
438 
439  // cspacePlatform->setPoseBounds(planningBoundingBox);
440 
441  ARMARX_VERBOSE << "CSpace created";
442 
443  auto planningResult = pmp->planMotion(armCSpace, cspacePlatform, mpd);
444  return planningResult;
445 }
446 
447 void
448 GraspingManager::drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor)
449 {
450  try
451  {
452  if (visuSlowdownFactor < 0)
453  {
454  return;
455  }
456  ARMARX_VERBOSE << "Step: Draw trajectory";
457 
458  TrajectoryPtr poseTrajectory = TrajectoryPtr::dynamicCast(t.poseTrajectory);
459  TrajectoryPtr configTrajectory = TrajectoryPtr::dynamicCast(t.configTrajectory);
460  ARMARX_CHECK_EXPRESSION(poseTrajectory->size() != 0);
461  ARMARX_CHECK_EXPRESSION(configTrajectory->size() != 0);
462 
463 
464  auto robotId = newId();
465 
466  auto robotConfig = rsc->getSynchronizedRobot()->getConfig();
467  localRobot->setJointValues(robotConfig);
468 
469  entityDrawer->setRobotVisu(
470  layerName, robotId, rsc->getRobotFilename(), "RobotAPI", FullModel);
471  entityDrawer->updateRobotColor(layerName, robotId, COLOR_ROBOT);
472  entityDrawer->updateRobotPose(layerName, robotId, new Pose(localRobot->getGlobalPose()));
473  entityDrawer->updateRobotConfig(
474  layerName, robotId, localRobot->getConfig()->getRobotNodeJointValueMap());
475 
476  ARMARX_INFO << VAROUT(poseTrajectory->output());
477 
478 
479  std::vector<PosePtr> poseData;
480  ARMARX_CHECK_EXPRESSION(poseTrajectory->dim() >= 3) << "dim: " << poseTrajectory->dim();
481 
482  std::transform(poseTrajectory->begin(),
483  poseTrajectory->end(),
484  std::back_inserter(poseData),
485  [](const Trajectory::TrajData& data) -> PosePtr
486  {
487  Eigen::Matrix4f pose = Eigen::Matrix4f::Identity();
488  VirtualRobot::MathTools::rpy2eigen4f(0, 0, data.getPosition(2), pose);
489  pose(0, 3) = data.getPosition(0);
490  pose(1, 3) = data.getPosition(1);
491  pose(2, 3) = 1;
492  return new Pose(pose);
493  });
494  ARMARX_INFO << __LINE__;
495  int stepSize = 1; //std::max<int>(1, poseData.size() / 20);
496  for (auto it = poseData.cbegin(); it != poseData.cend(); it += 1)
497  {
498  Vector3Ptr currPos = new Vector3((*it)->toEigen());
499  auto nextIt = std::next(it);
500  entityDrawer->setSphereVisu(layerName, newId(), currPos, COLOR_POSE_POINT, SPHERE_SIZE);
501  usleep(30000 * visuSlowdownFactor);
502  if (nextIt != poseData.cend())
503  {
504  Vector3Ptr nextPos = new Vector3((*nextIt)->toEigen());
505  entityDrawer->setLineVisu(
506  layerName, newId(), currPos, nextPos, LINE_WIDTH, COLOR_POSE_LINE);
507  Eigen::Matrix4f pose = localRobot->getGlobalPose();
508  pose.block<3, 1>(0, 3) = nextPos->toEigen();
509  entityDrawer->updateRobotPose(layerName, robotId, *nextIt);
510  usleep(1000000 * visuSlowdownFactor / poseData.size());
511  }
512  }
513  ARMARX_INFO << VAROUT(configTrajectory->output());
514  auto targetPose = poseData.back()->toEigen();
515  localRobot->setGlobalPose(targetPose);
516 
517  std::vector<NameValueMap> configData;
518  ARMARX_CHECK_EXPRESSION(configTrajectory->dim() > 0);
519  auto jointLabels = configTrajectory->getDimensionNames();
520  std::transform(configTrajectory->begin(),
521  configTrajectory->end(),
522  std::back_inserter(configData),
523  [&](const Trajectory::TrajData& data) -> NameValueMap
524  {
525  NameValueMap result;
526  for (size_t i = 0; i < jointLabels.size(); ++i)
527  {
528  result.insert({jointLabels[i], data.getPosition(i)});
529  }
530  return result;
531  });
532 
533 
534  VirtualRobot::RobotNodeSetPtr rns = localRobot->getRobotNodeSet(t.rnsToUse);
535  ARMARX_CHECK_EXPRESSION(rns && rns->getTCP());
536  const auto tcpName = rns->getTCP()->getName();
537 
538  std::vector<Vector3Ptr> tcpPoseList;
539  std::transform(configData.cbegin(),
540  configData.cend(),
541  std::back_inserter(tcpPoseList),
542  [&](const NameValueMap& config)
543  {
544  localRobot->setJointValues(config);
545  return new Vector3(localRobot->getRobotNode(tcpName)->getGlobalPose());
546  });
547 
548  // stepSize = std::max<int>(1, tcpPoseList.size() / 20);
549  int i = 0;
550  for (auto it = tcpPoseList.cbegin(); it != tcpPoseList.cend();
551  it += stepSize, i += stepSize)
552  {
553  auto nextIt = std::next(it);
554  auto currPose = *it;
555  entityDrawer->setSphereVisu(
556  layerName, newId(), currPose, COLOR_CONFIG_POINT, SPHERE_SIZE);
557 
558  if (nextIt != tcpPoseList.cend())
559  {
560  auto nextPose = *nextIt;
561  entityDrawer->updateRobotConfig(layerName, robotId, configData.at(i));
562  entityDrawer->setLineVisu(
563  layerName, newId(), currPose, nextPose, SPHERE_SIZE, COLOR_CONFIG_LINE);
564  usleep(1000000 * visuSlowdownFactor / tcpPoseList.size());
565  }
566  }
567 
568  entityDrawer->updateRobotConfig(layerName, robotId, configData.back());
569 
570  ARMARX_CHECK_EXPRESSION(TCP_HAND_MAPPING.find(tcpName) != TCP_HAND_MAPPING.end())
571  << "Unknown TCP '" << tcpName << "'";
572  auto handObjectClass =
573  prior->getObjectClassesSegment()->getObjectClassByName(TCP_HAND_MAPPING.at(tcpName));
574  ARMARX_CHECK_EXPRESSION(handObjectClass) << TCP_HAND_MAPPING.at(tcpName);
575 
576  localRobot->setJointValues(configData.back());
577  entityDrawer->setObjectVisu(layerName,
578  handObjectClass->getName(),
579  handObjectClass,
580  new Pose(localRobot->getRobotNode(tcpName)->getGlobalPose()));
581  entityDrawer->updateObjectColor(layerName, handObjectClass->getName(), COLOR_ROBOT);
582  }
583  catch (...)
584  {
585  armarx::handleExceptions();
586  }
587 }
588 
589 void
590 GraspingManager::setDescriptionPositionForObject(const std::string& objId)
591 {
592  if (!objId.empty())
593  {
594  auto objInstance = wm->getObjectInstancesSegment()->getObjectInstanceById(objId);
595  FramedPositionPtr p =
596  armarx::FramedPositionPtr::dynamicCast(objInstance->getPositionBase());
597  p->changeToGlobal(localRobot);
598  p->z += 400;
599  globalDescriptionPosition = p;
600  }
601 }
602 
603 void
604 GraspingManager::setNextStepDescription(const std::string& description)
605 {
606  step++;
607  if (getProperty<bool>("EnableVisualization"))
608  entityDrawer->setTextVisu(layerName,
609  "stepDescription",
610  "Step " + to_string(step) + ": " + description,
611  globalDescriptionPosition,
612  DrawColor{0, 1, 0, 1},
613  15);
614 }
615 
616 void
617 GraspingManager::resetStepDescription()
618 {
619  step = 0;
620  globalDescriptionPosition = new Vector3(0, 0, 0);
621  entityDrawer->removeTextVisu(layerName, "stepDescription");
622 }
623 
624 std::vector<MotionPlanningData>
625 GraspingManager::calculateIKs(const GraspingPlacementList& graspPlacements)
626 {
627  // TODO: Why is the left hand grasp always discarded for the IK?
628  // Eigen::Vector3f rpy;
629  // VirtualRobot::MathTools::eigen4f2rpy(localRobot->getGlobalPose(), rpy);
630  // armarx::VectorXf startPos {localRobot->getGlobalPose()(0, 3), localRobot->getGlobalPose()(1, 3), rpy(2)};
631  // MotionPlanningServerInterfacePrx mps = this->getProxy<MotionPlanningServerInterfacePrx>("MotionPlanningServer", false, "", false);
632  // if (mps)
633  // {
634  // cspace = new SimoxCSpaceWith2DPose(prior->getCommonStorage(), false, 50);
635  // AgentPlanningInformation agentData;
636  // agentData.agentProjectNames = rsc->getArmarXPackages();
637  // agentData.agentRelativeFilePath = rsc->getRobotFilename();
638  // // agentData.kinemaicChainNames = robotNodeSetNames;
639  // agentData.kinemaicChainNames = {};
640  // agentData.collisionSetNames = {getProperty<std::string>("RobotCollisionNodeSet").getValue()};
641  // agentData.initialJointValues = localRobot->getConfig()->getRobotNodeJointValueMap();
642  // cspace->setAgent(agentData);
643  // cspace->addObjectsFromWorkingMemory(wm);
644  // // cspace->setStationaryObjectMargin(getProperty<float>("MinimumDistanceToEnvironment").getValue());
645  // // cspace->initCollisionTest();
646  // SimoxCSpaceWith2DPosePtr tmpCSpace = SimoxCSpaceWith2DPosePtr::dynamicCast(cspace->clone());
647  // auto agent = tmpCSpace->getAgent();
648  // agent.agentPose = new Pose(localRobot->getGlobalPose());
649  // tmpCSpace->setAgent(agent);
650 
651  // CSpaceVisualizerTaskHandle taskHandle = mps->enqueueTask(new CSpaceVisualizerTask(tmpCSpace, startPos, getDefaultName() + "Visu" + IceUtil::generateUUID()));
652  // planningTasks.push_back(taskHandle);
653  // }
654 
655  std::vector<MotionPlanningData> mpdList;
656  // VirtualRobot::RobotPtr localRobot = RemoteRobot::createLocalCloneFromFile(rsc, VirtualRobot::RobotIO::eStructure);
657 
658  Eigen::Vector3f robotPos = localRobot->getGlobalPose().block<3, 1>(0, 3);
659 
660  ARMARX_IMPORTANT << "Robot position: " << VAROUT(robotPos);
661 
662  // auto disableGraspVisu = [&](int id)
663  // {
664  // entityDrawer->updateObjectColor("GeneratedGrasps", "GraspCandidate" + to_string(id), DrawColor {1.0, 0.0, 0.0, 0.5});
665  // };
666  robotVisuId = ""; // if empty, robot visu will be created
667  int i = 0;
668 
669  for (const GraspingPlacement& gp : graspPlacements)
670  {
671  NameValueMap currentConfig = rsc->getSynchronizedRobot()->getConfig();
672  ARMARX_CHECK_EXPRESSION(!currentConfig.empty());
673  auto graspVisuId = visualizeGrasp(gp.grasp, i);
674  Eigen::Matrix4f currentRobotPose = localRobot->getGlobalPose();
675  auto desiredRobotPose =
676  PosePtr::dynamicCast(FramedPosePtr::dynamicCast(gp.robotPose)->toGlobal(localRobot));
677  auto desiredRobotPoseEigen = desiredRobotPose->toEigen();
678  // objectPose is actually the tcp pose...
679  // FramedPosePtr desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose);
680  // TODO: move along gcp z axis
681  // desiredTCPPose->changeFrame(localRobot, localRobot->getEndEffector(gp.grasp.eefName)->getGCP()->getName());
682  // desiredTCPPose = FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobal(localRobot);
683  auto desiredTCPPose =
684  FramedPosePtr::dynamicCast(gp.grasp.framedPose)->toGlobalEigen(localRobot);
685  auto desiredTCPPoseRelativeToRobotEigen = desiredRobotPoseEigen.inverse() * desiredTCPPose;
686  FramedPosePtr desiredTCPPoseRelativeToRobot{
687  new FramedPose(desiredTCPPoseRelativeToRobotEigen,
688  localRobot->getRootNode()->getName(),
689  localRobot->getName())};
690  auto desiredTCPPrepose =
691  FramedPosePtr::dynamicCast(gp.grasp.framedPrePose)->toGlobalEigen(localRobot);
692  auto desiredTCPPreposeRelativeToRobotEigen =
693  desiredRobotPoseEigen.inverse() * desiredTCPPrepose;
694  FramedPosePtr desiredTCPPreposeRelativeToRobot{
695  new FramedPose(desiredTCPPreposeRelativeToRobotEigen,
696  localRobot->getRootNode()->getName(),
697  localRobot->getName())};
698 
699  Ice::StringSeq potentialRNs;
700  auto tcpName = localRobot->getEndEffector(gp.grasp.eefName)->getTcp()->getName();
701  for (const auto& rnsName : robotNodeSetNames)
702  {
703  ARMARX_CHECK_EXPRESSION(localRobot->hasRobotNodeSet(rnsName))
704  << "Could not find RNS '" << rnsName << "' in RNS list of robot "
705  << localRobot->getName();
706  if (localRobot->getRobotNodeSet(rnsName)->getTCP()->getName() == tcpName)
707  {
708  potentialRNs.push_back(rnsName);
709  }
710  }
711 
712  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor").getValue();
713 
714 
715  if (potentialRNs.empty())
716  {
717  ARMARX_WARNING << "Could not find RNS with tcp '" << tcpName
718  << "'; will not process the corresponding generated grasp...";
719  continue;
720  }
721  std::string rnsToUse;
722  if (/*rik->isFramedPoseReachable(rnsToUse, objectPoseRelativeToRobot) || */ true)
723  {
724  if (getProperty<bool>("EnableVisualization"))
725  {
726  if (robotVisuId.empty())
727  {
728  robotVisuId = newId();
729  entityDrawer->setRobotVisu(layerName,
730  robotVisuId,
731  rsc->getRobotFilename(),
732  simox::alg::join(rsc->getArmarXPackages(), ","),
733  FullModel);
734  entityDrawer->updateRobotColor(layerName, robotVisuId, COLOR_ROBOT);
735  }
736  entityDrawer->updateRobotColor(
737  layerName, robotVisuId, DrawColor{1.0, 1.0f, 1.0, 0.5});
738  entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
739  entityDrawer->updateRobotConfig(
740  layerName, robotVisuId, localRobot->getConfig()->getRobotNodeJointValueMap());
741  }
742  ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
743  << potentialRNs << "' is reachable";
744  NameValueMap ikSolution;
745  for (auto& rns : potentialRNs)
746  {
747  ikSolution = calculateSingleIK(
748  rns, gp.grasp.eefName, gp.robotPose, desiredTCPPoseRelativeToRobot, false);
749  if (ikSolution.empty())
750  {
751  continue;
752  }
753  ikSolution = calculateSingleIK(
754  rns, gp.grasp.eefName, gp.robotPose, desiredTCPPreposeRelativeToRobot);
755  if (!ikSolution.empty())
756  {
757  rnsToUse = rns;
758  break;
759  }
760  else
761  {
762  ARMARX_INFO << "Could not find collision free IK for prepose!";
763  }
764  }
765  if (ikSolution.empty())
766  {
767  if (getProperty<bool>("EnableVisualization"))
768  {
769  usleep(1000000 * visuSlowdownFactor);
770  auto graspVisuId = visualizeGrasp(gp.grasp, i, DrawColor{1.0, 0, 0, 1});
771  entityDrawer->updateRobotColor(
772  layerName, robotVisuId, DrawColor{1.0, 0.0f, 0.0, 1});
773  ARMARX_VERBOSE << "...but has no IK solution";
774  usleep(1000000 * visuSlowdownFactor);
775  for (auto id : graspVisuId.second)
776  {
777  entityDrawer->removeObjectVisu(graspVisuId.first, id);
778  }
779  }
780  else
781  {
782  ARMARX_VERBOSE << "...but has no IK solution";
783  }
784  continue;
785  }
786 
787  ARMARX_VERBOSE << "... and has an IK solution";
788  if (getProperty<bool>("EnableVisualization"))
789  {
790  usleep(1000000 * visuSlowdownFactor);
791  entityDrawer->updateRobotColor(
792  layerName, robotVisuId, DrawColor{0.0, 1.0f, 0.0, 1});
793  entityDrawer->updateRobotPose(layerName, robotVisuId, desiredRobotPose);
794  entityDrawer->updateRobotConfig(layerName, robotVisuId, ikSolution);
795  usleep(2000000 * visuSlowdownFactor);
796  }
797  // usleep(500000);
798 
799  for (auto it = currentConfig.begin(); it != currentConfig.end();)
800  {
801  if (ikSolution.find(it->first) == ikSolution.end())
802  {
803  it = currentConfig.erase(it);
804  }
805  else
806  {
807  ++it;
808  }
809  }
810 
811  for (const auto& entry : ikSolution)
812  {
813  if (currentConfig.find(entry.first) == currentConfig.end())
814  {
815  ARMARX_INFO << "Current config: " << currentConfig;
816  ARMARX_INFO << "Desired config: " << ikSolution;
817  ARMARX_CHECK_EXPRESSION(false)
818  << "calculated configuration contains a joint '" << entry.first
819  << "' whose current value is unknown";
820  }
821  }
822  ARMARX_CHECK_EXPRESSION(currentConfig.size() == ikSolution.size());
823  ARMARX_CHECK_EXPRESSION(!ikSolution.empty());
824  // entityDrawer->setPoseVisu("Poses" , "TCPTargetPose" + to_string(i), objectPoseRelativeToRobot->toGlobal(rsc->getSynchronizedRobot()));
825  ARMARX_INFO << VAROUT(currentRobotPose) << VAROUT(desiredRobotPose->output());
826  mpdList.push_back({new FramedPose(currentRobotPose, GlobalFrame, ""),
827  desiredRobotPose,
828  currentConfig,
829  ikSolution,
830  rnsToUse,
831  gp.grasp.eefName,
832  gp.grasp});
833  }
834  else
835  {
836  ARMARX_VERBOSE << "Pose " << VAROUT(*desiredTCPPoseRelativeToRobot) << " with RNS '"
837  << potentialRNs << "' not reachable";
838  }
839  if (getProperty<bool>("EnableVisualization"))
840  for (auto id : graspVisuId.second)
841  {
842  entityDrawer->removeObjectVisu(graspVisuId.first, id);
843  }
844  i++;
845  }
846  if (getProperty<bool>("EnableVisualization"))
847  {
848  entityDrawer->removeRobotVisu(layerName, robotVisuId);
849  }
850  return mpdList;
851 }
852 
853 NameValueMap
854 GraspingManager::calculateSingleIK(const std::string& robotNodeSetName,
855  const std::string& eef,
856  const PoseBasePtr& globalRobotPose,
857  const FramedPoseBasePtr& tcpPose,
858  bool checkCollisionFree)
859 {
860  TIMING_START(SingleIK);
861  auto ikRobot = cspace->getAgentSceneObj();
862  ikRobot->setConfig(localRobot->getConfig());
863  auto rns = ikRobot->getRobotNodeSet(robotNodeSetName);
864  ikRobot->setGlobalPose(PosePtr::dynamicCast(globalRobotPose)->toEigen());
865  auto tcp = ikRobot->getEndEffector(eef)->getTcp();
866  Eigen::Matrix4f targetPose = FramedPosePtr::dynamicCast(tcpPose)->toGlobalEigen(ikRobot);
867  VirtualRobot::ConstrainedOptimizationIK ik(ikRobot, rns);
868  VirtualRobot::PoseConstraintPtr poseConstraint(
869  new VirtualRobot::PoseConstraint(ikRobot, rns, tcp, targetPose));
870  if (checkCollisionFree)
871  {
872  auto colSetEnv = cspace->getStationaryObjectSet();
873  VirtualRobot::CDManagerPtr cdm(
874  new VirtualRobot::CDManager(cspace->getCD().getCollisionChecker()));
875  VirtualRobot::SceneObjectSetPtr sosRns(
876  new VirtualRobot::SceneObjectSet("RNS", cspace->getCD().getCollisionChecker()));
877  std::string armCollisionSet = getJointSetCollisionSetMapping().at(robotNodeSetName);
878  ARMARX_INFO << "Using Arm Collision Set '" << armCollisionSet << "' and platform set '"
879  << getProperty<std::string>("RobotCollisionNodeSet").getValue() << "'";
880  sosRns->addSceneObjects(cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet));
881  cdm->addCollisionModelPair(colSetEnv, sosRns);
882 
883  VirtualRobot::SceneObjectSetPtr sosPlatform(
884  new VirtualRobot::SceneObjectSet("platform", cspace->getCD().getCollisionChecker()));
885 
886 
887  auto robotCol = cspace->getAgentSceneObj()->getRobotNodeSet(
888  getProperty<std::string>("RobotCollisionNodeSet").getValue());
889  auto robotColNodeNames = robotCol->getNodeNames();
890  std::vector<std::string> remainingNodeNames =
891  getRobotNodeSetNodesWithoutAllwaysColliding(robotColNodeNames, armCollisionSet);
892  for (const auto& colNodeName : remainingNodeNames)
893  {
894  sosPlatform->addSceneObject(cspace->getAgentSceneObj()->getRobotNode(colNodeName));
895  }
896  ARMARX_INFO << "RobotCollisionNodeSet contains the following nodes after filtering allways "
897  "colliding nodes:"
898  << std::endl
899  << VAROUT(remainingNodeNames);
900 
901  cdm->addCollisionModelPair(sosPlatform, sosRns);
902  // ik.addConstraint(poseConstraint);
903  ik.addConstraint(
904  VirtualRobot::ConstraintPtr(new VirtualRobot::CollisionCheckConstraint(rns, cdm)));
905  }
906 
907  VirtualRobot::ConstraintPtr posConstraint(
908  new VirtualRobot::PositionConstraint(ikRobot,
909  rns,
910  tcp,
911  targetPose.block<3, 1>(0, 3),
912  VirtualRobot::IKSolver::CartesianSelection::All));
913  posConstraint->setOptimizationFunctionFactor(1);
914 
915  VirtualRobot::ConstraintPtr oriConstraint(
916  new VirtualRobot::OrientationConstraint(ikRobot,
917  rns,
918  tcp,
919  targetPose.block<3, 3>(0, 0),
920  VirtualRobot::IKSolver::CartesianSelection::All,
921  VirtualRobot::MathTools::deg2rad(2)));
922  oriConstraint->setOptimizationFunctionFactor(1000);
923  ik.addConstraint(posConstraint);
924  ik.addConstraint(oriConstraint);
925 
926 
927  NameValueMap result;
928  NaturalIKResult natIKResult =
929  getNaturalIK()->solveIK(eef,
930  FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot),
931  true,
932  new armarx::aron::data::dto::Dict());
933  if (!natIKResult.reached)
934  {
935  ARMARX_WARNING << "natik failed!";
936  //return result;
937  }
938  else
939  {
940  VirtualRobot::ReferenceConfigurationConstraintPtr natIKConstraint(
941  new VirtualRobot::ReferenceConfigurationConstraint(ikRobot, rns));
942  ARMARX_IMPORTANT << VAROUT(natIKResult.jointValues);
943  Eigen::VectorXf referenceConfig(natIKResult.jointValues.size());
944  for (size_t i = 0; i < natIKResult.jointValues.size(); i++)
945  {
946  referenceConfig(i) = natIKResult.jointValues.at(i);
947  }
948  ARMARX_IMPORTANT << VAROUT(referenceConfig);
949  natIKConstraint->setReferenceConfiguration(referenceConfig);
950  ik.addConstraint(natIKConstraint);
951  }
952 
953  if (!ik.initialize())
954  {
955  ARMARX_WARNING << "IK Initialization failed!";
956  return result;
957  }
958  bool success = ik.solve();
959  if (success)
960  {
961 
962  for (size_t i = 0; i < rns->getSize(); ++i)
963  {
964  result[rns->getNode((int)i)->getName()] = rns->getNode(i)->getJointValue();
965  }
966  //if (PositionControllerHelper::OptimizeNullspace(tcp, rns, FramedPosePtr::dynamicCast(tcpPose)->toRootEigen(ikRobot)))
967  //{
968  // for (size_t i = 0; i < rns->getSize(); ++i)
969  // {
970  // result[rns->getNode((int)i)->getName()] = rns->getNode(i)->getJointValue();
971  // }
972  //}
973  }
974  else if (natIKResult.reached)
975  {
976  ARMARX_WARNING << "constrained ik failed. using natik instead.";
977  for (size_t i = 0; i < rns->getSize(); ++i)
978  {
979  result[rns->getNode((int)i)->getName()] = natIKResult.jointValues.at(i);
980  }
981  }
982  TIMING_END(SingleIK);
983 
984 
985  return result;
986 }
987 
988 StringStringDictionary
989 GraspingManager::getJointSetCollisionSetMapping()
990 {
991  StringStringDictionary result;
992  auto propString = getProperty<std::string>("JointToLinkSetMapping").getValue();
993  auto pairs = armarx::Split(propString, ";", true, true);
994  for (auto& pairStr : pairs)
995  {
996  auto pair = armarx::Split(pairStr, ":", true, true);
997  ARMARX_CHECK_EXPRESSION(pair.size() == 2);
998  result[pair.at(0)] = pair.at(1);
999  }
1000  return result;
1001 }
1002 
1003 GraspingTrajectoryList
1004 GraspingManager::generateGraspingTrajectoryListForGraspListInternal(
1005  const GeneratedGraspList& grasps)
1006 {
1007  std::vector<MotionPlanningData> mpdList = generateIKsInternal(grasps);
1008  if (mpdList.empty())
1009  {
1010  ARMARX_WARNING << "Step 'check reachability / solve IK' produced no valid results";
1011  return GraspingTrajectoryList{};
1012  }
1013 
1014  // std::vector<std::tuple<Eigen::Matrix4f, Eigen::Matrix4f, NameValueMap, NameValueMap, std::string>> sourceTargetConfigs;
1015 
1016 
1017  GraspingTrajectoryList result;
1018  setNextStepDescription("Planning motion");
1019 
1020  for (auto& graspingData : mpdList)
1021  {
1022  try
1023  {
1024  GraspingTrajectory gt = planMotion(graspingData);
1025 
1026  result.push_back(gt);
1027  }
1028  catch (...)
1029  {
1030  handleExceptions();
1031  }
1032  if (!result.empty())
1033  {
1034  break;
1035  }
1036  }
1037  return result;
1038 }
1039 
1040 GraspingTrajectory
1041 GraspingManager::generateGraspingTrajectory(const std::string& objectInstanceEntityId,
1042  const Ice::Current&)
1043 {
1044  ScopedLock lock(graspManagerMutex);
1045  ARMARX_ON_SCOPE_EXIT
1046  {
1047  resetStepDescription();
1048  entityDrawer->clearLayer(layerName);
1049  };
1050  RemoteRobot::synchronizeLocalClone(localRobot, rsc);
1051  setDescriptionPositionForObject(objectInstanceEntityId);
1052 
1053  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
1054 
1055  auto grasps = generateGrasps(objectInstanceEntityId);
1056 
1057  auto result = generateGraspingTrajectoryListForGraspListInternal(grasps);
1058  if (result.empty())
1059  {
1060  throw LocalException("Could not find any valid solution");
1061  }
1062 
1063  if (getProperty<bool>("EnableVisualization"))
1064  {
1065  drawTrajectory(result.front(), visuSlowdownFactor);
1066  sleep(2 * visuSlowdownFactor);
1067  entityDrawer->removeLayer(layerName);
1068  }
1069  return result.front();
1070 }
1071 
1072 GraspingTrajectoryList
1073 GraspingManager::generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
1074  const Ice::Current&)
1075 {
1076  ScopedLock lock(graspManagerMutex);
1077  ARMARX_ON_SCOPE_EXIT
1078  {
1079  resetStepDescription();
1080  entityDrawer->clearLayer(layerName);
1081  };
1082  RemoteRobot::synchronizeLocalClone(localRobot, rsc);
1083  setDescriptionPositionForObject(objectInstanceEntityId);
1084 
1085  auto grasps = generateGrasps(objectInstanceEntityId);
1086  return generateGraspingTrajectoryListForGraspListInternal(grasps);
1087 }
1088 
1089 GraspingTrajectoryList
1090 GraspingManager::generateGraspingTrajectoryListForGraspList(const GeneratedGraspList& grasps,
1091  const Ice::Current&)
1092 {
1093  if (grasps.empty())
1094  return GraspingTrajectoryList{};
1095  ScopedLock lock(graspManagerMutex);
1096  ARMARX_ON_SCOPE_EXIT
1097  {
1098  resetStepDescription();
1099  entityDrawer->clearLayer(layerName);
1100  };
1101  RemoteRobot::synchronizeLocalClone(localRobot, rsc);
1102  FramedPosePtr pose =
1103  FramedPosePtr::dynamicCast(grasps.front().framedPose)->toGlobal(localRobot);
1104  pose->position->z += 400;
1105  globalDescriptionPosition = Vector3Ptr::dynamicCast(pose->position);
1106 
1107  auto result = generateGraspingTrajectoryListForGraspListInternal(grasps);
1108  if (result.empty())
1109  {
1110  throw LocalException("Could not find any valid solution");
1111  }
1112 
1113  if (getProperty<bool>("EnableVisualization"))
1114  {
1115  float visuSlowdownFactor = getProperty<float>("VisualizationSlowdownFactor");
1116  drawTrajectory(result.front(), visuSlowdownFactor);
1117  sleep(2 * visuSlowdownFactor);
1118  entityDrawer->removeLayer(layerName);
1119  }
1120  return result;
1121 }
1122 
1123 void
1124 GraspingManager::visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
1125  float visuSlowdownFactor,
1126  const Ice::Current&)
1127 {
1128  drawTrajectory(trajectory, visuSlowdownFactor);
1129  sleep(2 * visuSlowdownFactor);
1130  entityDrawer->removeLayer(layerName);
1131 }
1132 
1133 MotionPlanningDataList
1134 GraspingManager::generateIKs(const std::string& objectInstanceEntityId, const Ice::Current&)
1135 {
1136  ScopedLock lock(graspManagerMutex);
1137  ARMARX_ON_SCOPE_EXIT
1138  {
1139  resetStepDescription();
1140  entityDrawer->clearLayer(layerName);
1141  };
1142  RemoteRobot::synchronizeLocalClone(localRobot, rsc);
1143  setDescriptionPositionForObject(objectInstanceEntityId);
1144  auto grasps = generateGrasps(objectInstanceEntityId);
1145  return generateIKsInternal(grasps);
1146 }
1147 
1148 GeneratedGraspList
1149 GraspingManager::generateGraspsByObjectName(const std::string& objectName, const Ice::Current&)
1150 {
1151  GeneratedGraspList grasps = gg->generateGraspsByObjectName(objectName);
1152  std::sort(grasps.begin(),
1153  grasps.end(),
1154  [](const GeneratedGrasp& l, const GeneratedGrasp& r) { return l.score < r.score; });
1155  return grasps;
1156 }
1157 
1158 MotionPlanningDataList
1159 GraspingManager::generateIKsInternal(const GeneratedGraspList& grasps)
1160 {
1161  ikRobot->setConfig(localRobot->getConfig());
1162  auto filteredGrasps = filterGrasps(grasps);
1163 
1164  GraspingPlacementList graspPlacements;
1165 
1166  cspace = createCSpace();
1167 
1168  setNextStepDescription("Robot placement");
1169  const float maxDistance2D = getProperty<float>("MaxDistanceForDirectGrasp").getValue();
1170  GeneratedGraspList notDirectGrasps;
1171  for (const auto& g : filteredGrasps)
1172  {
1173  const Eigen::Vector3f position =
1174  FramedPosePtr::dynamicCast(g.framedPose)->getPosition()->toGlobalEigen(localRobot);
1175  const float distance2D =
1176  (position.head(2) - localRobot->getGlobalPose().block<2, 1>(0, 3)).norm();
1177  ARMARX_VERBOSE << "2D Distance to object: " << distance2D;
1178  // if already close to object, try to grasp directly without platform movement
1179  if (distance2D < maxDistance2D)
1180  {
1181  GraspingPlacement pl;
1182  pl.grasp = g;
1183  pl.robotPose = new FramedPose(localRobot->getGlobalPose(), armarx::GlobalFrame, "");
1184  graspPlacements.push_back(pl);
1185  }
1186  else
1187  {
1188  notDirectGrasps.push_back(g);
1189  }
1190  }
1191  if (!notDirectGrasps.empty())
1192  {
1193  auto graspPlacementsNotDirect = generateRobotPlacements(notDirectGrasps);
1194  graspPlacements.insert(graspPlacements.end(),
1195  graspPlacementsNotDirect.begin(),
1196  graspPlacementsNotDirect.end());
1197  }
1198  GraspingPlacementList filteredGraspPlacements = filterPlacements(graspPlacements);
1199 
1200  ARMARX_VERBOSE << "Step: check reachability / solve IK from current pose";
1201  setNextStepDescription("Calculating IK");
1202  return calculateIKs(filteredGraspPlacements);
1203 }
1204 
1205 std::vector<std::string>
1206 GraspingManager::getRobotNodeSetNodesWithoutAllwaysColliding(
1207  const std::vector<std::string>& robotColNodeNames,
1208  const std::string& armCollisionSet)
1209 {
1210  const auto rnsColNames =
1211  cspace->getAgentSceneObj()->getRobotNodeSet(armCollisionSet)->getNodeNames();
1212  std::set<std::string> rnsColNamesIncludingIgnore;
1213  rnsColNamesIncludingIgnore.insert(rnsColNames.begin(), rnsColNames.end());
1214  for (const auto& rnsNodeName : rnsColNames)
1215  {
1216  const auto rn = cspace->getAgentSceneObj()->getRobotNode(rnsNodeName);
1217  const auto ignored = rn->getIgnoredCollisionModels();
1218  rnsColNamesIncludingIgnore.insert(ignored.begin(), ignored.end());
1219  rnsColNamesIncludingIgnore.insert(rn->getParent()->getName());
1220  }
1221 
1222  std::vector<std::string> remainingNodeNames;
1223  for (const auto& colNodeName : robotColNodeNames)
1224  {
1225  const auto it = rnsColNamesIncludingIgnore.find(colNodeName);
1226  if (it == rnsColNamesIncludingIgnore.end())
1227  {
1228  remainingNodeNames.push_back(colNodeName);
1229  }
1230  }
1231  return remainingNodeNames;
1232 }
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
TIMING_START
#define TIMING_START(name)
Definition: TimeUtil.h:289
armarx::GraspingManager::onExitComponent
void onExitComponent() override
Definition: GraspingManager.cpp:195
TIMING_END
#define TIMING_END(name)
Definition: TimeUtil.h:306
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::GraspingManager::generateGraspsByObjectName
GeneratedGraspList generateGraspsByObjectName(const std::string &objectName, const Ice::Current &=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1149
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:48
OnScopeExit.h
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
armarx::GraspingManager::onConnectComponent
void onConnectComponent() override
Definition: GraspingManager.cpp:121
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:522
armarx::GraspingManager::generateGraspingTrajectoryListForGraspList
GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(const GeneratedGraspList &grasps, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1090
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:512
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
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
armarx::Trajectory::TrajData
Definition: Trajectory.h:84
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:52
armarx::GraspingManager::visualizeGraspingTrajectory
void visualizeGraspingTrajectory(const GraspingTrajectory &trajectory, float visuSlowdownFactor, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1124
armarx::GraspingManager::onInitComponent
void onInitComponent() override
Definition: GraspingManager.cpp:83
GraspingManager.h
ARMARX_ON_SCOPE_EXIT
#define ARMARX_ON_SCOPE_EXIT
Executes given code when the enclosing scope is left.
Definition: OnScopeExit.h:120
armarx::ScopedLock
Mutex::scoped_lock ScopedLock
Definition: Synchronization.h:150
NaturalGraspFilter.h
project
std::string project
Definition: VisualizationRobot.cpp:85
armarx::GraspingManager::getDefaultName
std::string getDefaultName() const override
Definition: GraspingManager.h:159
armarx::GraspingManager::generateGraspingTrajectory
GraspingTrajectory generateGraspingTrajectory(const std::string &objectInstanceEntityId, const Ice::Current &c=Ice::emptyCurrent) override
Runs the grasping pipeline.
Definition: GraspingManager.cpp:1041
IceInternal::Handle< SimoxCSpace >
VoxelGridCSpace.h
newId
auto newId
Definition: GraspingManager.cpp:76
FramedPose.h
armarx::VoxelGridCSpace
Definition: VoxelGridCSpace.h:34
data
uint8_t data[1]
Definition: EtherCATFrame.h:68
armarx::NaturalIKComponentPluginUser::getNaturalIK
NaturalIKInterfacePrx getNaturalIK()
Definition: NaturalIKComponentPlugin.cpp:52
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::GraspingManager::generateIKs
MotionPlanningDataList generateIKs(const std::string &objectInstanceEntityId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1134
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
PositionControllerHelper.h
CartesianPositionController.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:351
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:71
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
Trajectory.h
CMakePackageFinder.h
armarx::Pose
The Pose class.
Definition: Pose.h:242
SimoxCSpace.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_INFO
#define ARMARX_INFO
Definition: Logging.h:181
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
armarx::GraspingManager::generateGraspingTrajectoryList
GraspingTrajectoryList generateGraspingTrajectoryList(const std::string &objectInstanceEntityId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1073
memoryx::KBM::Vector3
Eigen::Vector3d Vector3
Definition: kbm.h:43
armarx::GraspingManager::visualizeGrasp
std::pair< std::string, Ice::StringSeq > visualizeGrasp(const GeneratedGrasp &grasp, int id, const DrawColor &color=DrawColor{1, 1, 1, 1})
Visualizes the grasp using the DebugDrawer topic.
Definition: GraspingManager.cpp:219
StringHelperTemplates.h
armarx::GraspingManager::onDisconnectComponent
void onDisconnectComponent() override
Definition: GraspingManager.cpp:190
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:157
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
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::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
armarx::control::njoint_mp_controller::task_space::ConstraintPtr
std::shared_ptr< Constraint1 > ConstraintPtr
Definition: KVILImpedanceController.h:56