27 #include <Eigen/Geometry>
32 #include <RobotAPI/interface/core/RobotState.h>
38 #include <RobotComponents/interface/components/GraspingManager/GraspGeneratorInterface.h>
39 #include <RobotComponents/interface/components/GraspingManager/GraspSelectionManagerInterface.h>
40 #include <RobotComponents/interface/components/GraspingManager/GraspingManagerInterface.h>
41 #include <RobotComponents/interface/components/GraspingManager/RobotPlacementInterface.h>
42 #include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
43 #include <RobotComponents/interface/components/PlannedMotionProviderInterface.h>
46 #include <MemoryX/interface/components/PriorKnowledgeInterface.h>
47 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
48 #include <MemoryX/interface/gui/EntityDrawerInterface.h>
63 defineOptionalProperty<std::string>(
64 "GraspGeneratorName",
"SimpleGraspGenerator",
"Name of the GraspGenerator proxy");
65 defineOptionalProperty<std::string>(
66 "RobotPlacementName",
"SimpleRobotPlacement",
"Name of the RobotPlacement proxy");
67 defineOptionalProperty<std::string>(
69 "HipYawRightArm;HipYawLeftArm",
70 "Names of the robot node sets to use for IK calculations (';' delimited)");
71 defineOptionalProperty<std::string>(
72 "RobotCollisionNodeSet",
74 "Name of the collision nodeset used for motion planning",
76 defineRequiredProperty<std::string>(
77 "JointToLinkSetMapping",
78 "Mapping from joint set to link set, i.e. which collision set to use for which "
79 "kinematic chain. Format: JointSet1:CollisionSet1;JointSet2:CollisionSet2",
81 defineOptionalProperty<std::string>(
82 "ReachabilitySpaceFilePaths",
83 "Armar3/reachability/reachability_right_hand_smoothened.bin;Armar3/reachability/"
84 "reachability_left_hand_smoothened.bin",
85 "Paths to the reachability space files (needed only if no reachability space is "
86 "loaded for the chosen RobotNodeSets) (';' delimited)");
87 defineOptionalProperty<std::string>(
"PlanningBoundingBox",
88 "-15000, -4000, -3.1416, 15000, 15000, 3.1416",
89 "x_min, y_min, alpha_min, x_max, y_max, alpha_max");
90 defineOptionalProperty<float>(
"VisualizationSlowdownFactor",
92 "1.0 is a good value for clear visualization, 0 the "
93 "visualization should not slow down the process",
95 defineOptionalProperty<bool>(
"EnableVisualization",
97 "If false no visualization is done.",
99 defineOptionalProperty<float>(
100 "MaxDistanceForDirectGrasp",
102 "x-y-Distance from robot base to object for which the grasping manager tries to "
103 "find a solution without platform movement",
105 defineOptionalProperty<bool>(
106 "UseVoxelGridCSpace",
108 "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
110 defineOptionalProperty<std::string>(
"VoxelGridProviderName",
112 "Name of the Voxel Grid Provider",
114 defineOptionalProperty<bool>(
"FilterUnnaturalGrasps",
116 "If set to true, uses the NaturalGraspFilter to exclude "
117 "unnatural looking grasps and grasp poses.",
151 virtual public GraspingManagerInterface,
161 return "GraspingManager";
177 const Ice::Current&
c = Ice::emptyCurrent)
override;
178 GraspingTrajectoryList
180 const Ice::Current&
c = Ice::emptyCurrent)
override;
182 const GeneratedGraspList& grasps,
183 const Ice::Current&
c = Ice::emptyCurrent)
override;
185 float visuSlowdownFactor,
186 const Ice::Current&
c = Ice::emptyCurrent)
override;
188 MotionPlanningDataList
generateIKs(
const std::string& objectInstanceEntityId,
189 const Ice::Current&
c = Ice::emptyCurrent)
override;
195 const Ice::Current& = Ice::emptyCurrent)
override;
232 std::pair<std::string, Ice::StringSeq>
235 const DrawColor& color = DrawColor{1, 1, 1, 1});
255 GeneratedGraspList generateGrasps(
const std::string& objectInstanceEntityId);
260 GeneratedGraspList filterGrasps(
const GeneratedGraspList& grasps);
265 GraspingPlacementList filterPlacements(
const GraspingPlacementList& placements);
273 GraspingPlacementList generateRobotPlacements(
const GeneratedGraspList& grasps);
284 GraspingTrajectory planMotion(
const MotionPlanningData& mpd);
290 void drawTrajectory(
const GraspingTrajectory& t,
float visuSlowdownFactor);
302 std::vector<MotionPlanningData> calculateIKs(
const GraspingPlacementList& graspPlacements);
318 NameValueMap calculateSingleIK(const ::std::string& robotNodeSetName,
319 const std::string& eef,
320 const PoseBasePtr& globalRobotPose,
321 const ::armarx::FramedPoseBasePtr& tcpPose,
322 bool checkCollisionFree =
true);
329 armarx::StringStringDictionary getJointSetCollisionSetMapping();
338 GraspingTrajectoryList
339 generateGraspingTrajectoryListForGraspListInternal(
const GeneratedGraspList& grasps);
340 MotionPlanningDataList generateIKsInternal(
const GeneratedGraspList& grasps);
342 std::vector<std::string> getRobotNodeSetNodesWithoutAllwaysColliding(
343 const std::vector<std::string>& robotColNodeNames,
344 const std::string& armCollisionSet);
346 void setDescriptionPositionForObject(
const std::string& objId);
347 void setNextStepDescription(
const std::string& description);
348 void resetStepDescription();
349 std::string graspGeneratorName;
350 std::string robotPlacementName;
351 std::vector<std::string> robotNodeSetNames;
352 std::vector<std::string> reachabilitySpaceFilePaths;
354 Vector3fRange planningBoundingBox;
355 GraspGeneratorInterfacePrx gg;
356 GraspSelectionManagerInterfacePrx gsm;
357 GraspSelectionCriterionInterfacePrx gsc;
358 RobotPlacementInterfacePrx rp;
360 PlannedMotionProviderInterfacePrx pmp;
368 memoryx::CommonStorageInterfacePrx cs;
369 memoryx::WorkingMemoryInterfacePrx wm;
370 memoryx::PriorKnowledgeInterfacePrx prior;
372 memoryx::EntityDrawerInterfacePrx entityDrawer;
373 std::string layerName;
374 std::string robotVisuId;
379 std::vector<CSpaceVisualizerTaskHandle> planningTasks;
380 Mutex graspManagerMutex;