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