GraspingManager.h
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 #pragma once
26 
27 #include <Eigen/Geometry>
28 
31 
32 #include <RobotAPI/interface/core/RobotState.h>
35 
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>
44 
46 #include <MemoryX/interface/components/PriorKnowledgeInterface.h>
47 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
48 #include <MemoryX/interface/gui/EntityDrawerInterface.h>
50 
51 namespace armarx
52 {
53  /**
54  * @class GraspingManagerPropertyDefinitions
55  * @brief
56  */
58  {
59  public:
62  {
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>(
68  "RobotNodeSetNames",
69  "HipYawRightArm;HipYawLeftArm",
70  "Names of the robot node sets to use for IK calculations (';' delimited)");
71  defineOptionalProperty<std::string>(
72  "RobotCollisionNodeSet",
73  "Robot",
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",
91  1.0f,
92  "1.0 is a good value for clear visualization, 0 the "
93  "visualization should not slow down the process",
95  defineOptionalProperty<bool>("EnableVisualization",
96  true,
97  "If false no visualization is done.",
99  defineOptionalProperty<float>(
100  "MaxDistanceForDirectGrasp",
101  1200.0f,
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",
107  false,
108  "If true, the VoxelGridCSpace is used instead of the SimoxCSpace.",
110  defineOptionalProperty<std::string>("VoxelGridProviderName",
111  "VoxelGridProvider",
112  "Name of the Voxel Grid Provider",
114  defineOptionalProperty<bool>("FilterUnnaturalGrasps",
115  false,
116  "If set to true, uses the NaturalGraspFilter to exclude "
117  "unnatural looking grasps and grasp poses.",
119  }
120  };
121 
122  /**
123  * @defgroup Component-GraspingManager GraspingManager
124  * @ingroup RobotComponents-Components
125  * @brief Provides methods for grasp planning purposes.
126  *
127  * The GraspingManager implements a grasping pipeline containing the following stages (each stage is implemented by the respective Component in the parathesis):
128  * \li Grasp generation (SimpleGraspGenerator)
129  * \li Grasp filtering (GraspSelectionManager)
130  * \li Robot placement (SimpleRobotPlacement)
131  * \li Finding a target configuration (RobotIK)
132  * \li Trajectory planning (MotionPlanningServer)
133  *
134  * The stages are executed sequentially.
135  * The Input for the grasping pipeline is a graspable object.
136  * The output contains trajectories in configuration and pose space to execute a grasp for the given object.
137  *
138  * The GraspingManager requires the following properties:
139  * \li a list of robotnodeset names (; delimited) for IK calculations
140  * \li a list of workspace files (; delimited) for reachability considerations
141  *
142  * \note The Armar3Simulation scenario must be active before running the GraspingManager scenario.
143  */
144 
145  /**
146  * @ingroup Component-GraspingManager
147  * @brief The GraspingManager class
148  */
150  virtual public Component,
151  virtual public GraspingManagerInterface,
152  virtual public NaturalIKComponentPluginUser
153  {
154  public:
155  /**
156  * @see armarx::ManagedIceObject::getDefaultName()
157  */
158  std::string
159  getDefaultName() const override
160  {
161  return "GraspingManager";
162  }
163 
164  /**
165  * @brief Runs the grasping pipeline.
166  *
167  * Checks if the robot is close enough to the object to directly grasp it, attempting to calculate the
168  * inverse kinematics directly. In case nothing is found, tries to calculate the movement a robotPlacement
169  * is generated and the calculation of the IKs is repeated for the new position.
170  *
171  * @param objectInstanceEntityId the id for retrieval of the target object from working memory.
172  * @param c
173  * @return A list of grasping trajectories in configuration and pose space.
174  */
175  GraspingTrajectory
176  generateGraspingTrajectory(const std::string& objectInstanceEntityId,
177  const Ice::Current& c = Ice::emptyCurrent) override;
178  GraspingTrajectoryList
179  generateGraspingTrajectoryList(const std::string& objectInstanceEntityId,
180  const Ice::Current& c = Ice::emptyCurrent) override;
181  GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(
182  const GeneratedGraspList& grasps,
183  const Ice::Current& c = Ice::emptyCurrent) override;
184  void visualizeGraspingTrajectory(const GraspingTrajectory& trajectory,
185  float visuSlowdownFactor,
186  const Ice::Current& c = Ice::emptyCurrent) override;
187 
188  MotionPlanningDataList generateIKs(const std::string& objectInstanceEntityId,
189  const Ice::Current& c = Ice::emptyCurrent) override;
190 
191 
192  ///// single step methods /////
193  GeneratedGraspList
194  generateGraspsByObjectName(const std::string& objectName,
195  const Ice::Current& = Ice::emptyCurrent) override;
196 
197 
198  protected:
199  /**
200  * @see armarx::ManagedIceObject::onInitComponent()
201  */
202  void onInitComponent() override;
203 
204  /**
205  * @see armarx::ManagedIceObject::onConnectComponent()
206  */
207  void onConnectComponent() override;
208 
209  /**
210  * @see armarx::ManagedIceObject::onDisconnectComponent()
211  */
212  void onDisconnectComponent() override;
213 
214  /**
215  * @see armarx::ManagedIceObject::onExitComponent()
216  */
217  void onExitComponent() override;
218 
219  /**
220  * @see PropertyUser::createPropertyDefinitions()
221  */
224  {
225  return PropertyDefinitionsPtr(
227  }
228 
229  /**
230  * @brief Visualizes the grasp using the DebugDrawer topic
231  */
232  std::pair<std::string, Ice::StringSeq>
233  visualizeGrasp(const GeneratedGrasp& grasp,
234  int id,
235  const DrawColor& color = DrawColor{1, 1, 1, 1});
236 
237  private:
238  /**
239  * @brief Creates a pointer to the SimoxCSpace::SimoxCSpace() and initializes it.
240  *
241  * Creates the pointer, adds objects from the working memory and sets up the AgentPlanningInformation.
242  * The Agent is then linked to the pointer and the CSpace is initialized.
243  * @return Pointer to a SimoxCSpace
244  */
245  SimoxCSpacePtr createCSpace();
246 
247  /**
248  * @brief Wrapper for the GraspGeneratorInterface::generateGrasps() method of the GraspGenerator.
249  * @param objectInstanceEntityId
250  *
251  * Gets a list of grasps from the GraspGenerator and sorts them depending on their score. If no grasps are
252  * generated, a warning is thrown. Initiates the visualization of the grasps.
253  * @return
254  */
255  GeneratedGraspList generateGrasps(const std::string& objectInstanceEntityId);
256 
257  /**
258  * @brief Wrapper for the GraspSelectionManagerInterface::filterGrasps() method of the GraspSelectionManager.
259  */
260  GeneratedGraspList filterGrasps(const GeneratedGraspList& grasps);
261  /**
262  * @brief Wrapper for the GraspSelectionManagerInterface::filterPlacements() method o
263  * the GraspSelectionManager.
264  */
265  GraspingPlacementList filterPlacements(const GraspingPlacementList& placements);
266 
267  /**
268  * Wrapper for the RobotPlacementInterface::generateRobotPlacements().
269  * @param grasps
270  * @param objectInstanceEntityId
271  * @return List of grasping placements.
272  */
273  GraspingPlacementList generateRobotPlacements(const GeneratedGraspList& grasps);
274 
275  /**
276  * Wrapper for the PlannedMotionProviderInterface::planMotion() method.
277  *
278  * Calculates the center and size of the bounding box and initializes one SimoxCSpace for the arms and one for
279  * the platform.
280  *
281  * @param mpd
282  * @return
283  */
284  GraspingTrajectory planMotion(const MotionPlanningData& mpd);
285 
286  /**
287  * @brief Draws the trajectory in the DebugDrawer topic.
288  * @param t
289  */
290  void drawTrajectory(const GraspingTrajectory& t, float visuSlowdownFactor);
291 
292  /**
293  * @brief Iterates through the list of found graspPlacements to calculate SingleIKs
294  *
295  * Iterates through all found graspPlacements and calls the GraspingManager::calculateSingleIK() method with
296  * the correct parameters. Then calculates the required parameters, like the currentRobotPose, GlobalFrame,
297  * desiredRobotPose, currentConfig, ikSolution, rnsToUse, gp.grasp.eefName and gp.grasp, for the
298  * MotionPlanningData for each non-empty IKSolution.
299  *
300  * @return Vector of of MotionPlanningData
301  */
302  std::vector<MotionPlanningData> calculateIKs(const GraspingPlacementList& graspPlacements);
303 
304  /**
305  * @brief Calculates the inverse kinematics for a single grasp.
306  *
307  * Calculates the inverse kinematics for a target grasp configuration. If checkCollisionFree is true, the arms
308  * and the platform are added as constraints in the calculation. Otherwise, only the position and orientation
309  * of the rns, tcp & target_pose are considered.
310  *
311  * @param robotNodeSetName
312  * @param eef
313  * @param globalRobotPose
314  * @param tcpPose
315  * @param checkCollisionFree
316  * @return NameValueMap of the solved IK
317  */
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);
323 
324  /**
325  * Wrapper for the property JointToLinkSetMapping
326  *
327  * @return Map of each pair in the JointSetCollisionSet
328  */
329  armarx::StringStringDictionary getJointSetCollisionSetMapping();
330 
331  /**
332  * Generate a list of valid GraspingTrajectories given a list of grasps.
333  *
334  * @param grasps The list of grasps to generate trajetories for
335  * @param objId The object ID. Only used for logging.
336  * @return The list of valid generated grasps
337  */
338  GraspingTrajectoryList
339  generateGraspingTrajectoryListForGraspListInternal(const GeneratedGraspList& grasps);
340  MotionPlanningDataList generateIKsInternal(const GeneratedGraspList& grasps);
341 
342  std::vector<std::string> getRobotNodeSetNodesWithoutAllwaysColliding(
343  const std::vector<std::string>& robotColNodeNames,
344  const std::string& armCollisionSet);
345 
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;
353 
354  Vector3fRange planningBoundingBox;
355  GraspGeneratorInterfacePrx gg;
356  GraspSelectionManagerInterfacePrx gsm;
357  GraspSelectionCriterionInterfacePrx gsc;
358  RobotPlacementInterfacePrx rp;
359 
360  PlannedMotionProviderInterfacePrx pmp;
361  SimoxCSpacePtr cacheCSpace;
362 
363  //RobotIKInterfacePrx rik;
365  VirtualRobot::RobotPtr localRobot, ikRobot;
366  SimoxCSpacePtr cspace;
367 
368  memoryx::CommonStorageInterfacePrx cs;
369  memoryx::WorkingMemoryInterfacePrx wm;
370  memoryx::PriorKnowledgeInterfacePrx prior;
371 
372  memoryx::EntityDrawerInterfacePrx entityDrawer;
373  std::string layerName;
374  std::string robotVisuId;
375 
376  int step = 0;
377  Vector3Ptr globalDescriptionPosition;
378 
379  std::vector<CSpaceVisualizerTaskHandle> planningTasks;
380  Mutex graspManagerMutex;
381  armarx::TimeKeeper stepTimer;
382 
383  RobotNameHelperPtr rnh;
384  };
385 } // namespace armarx
armarx::TimeKeeper
The TimeKeeper class tracks the passing of time and allows to stop it, restart it,...
Definition: TimeKeeper.h:41
armarx::GraspingManager::onExitComponent
void onExitComponent() override
Definition: GraspingManager.cpp:195
TimeKeeper.h
armarx::GraspingManager::generateGraspsByObjectName
GeneratedGraspList generateGraspsByObjectName(const std::string &objectName, const Ice::Current &=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1149
armarx::GraspingManager::onConnectComponent
void onConnectComponent() override
Definition: GraspingManager.cpp:121
armarx::GraspingManager::generateGraspingTrajectoryListForGraspList
GraspingTrajectoryList generateGraspingTrajectoryListForGraspList(const GeneratedGraspList &grasps, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1090
armarx::GraspingManager::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: GraspingManager.h:223
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:345
armarx::GraspingManager::onInitComponent
void onInitComponent() override
Definition: GraspingManager.cpp:83
armarx::GraspingManager::visualizeGraspingTrajectory
void visualizeGraspingTrajectory(const GraspingTrajectory &trajectory, float visuSlowdownFactor, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1124
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
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
armarx::RobotStateComponentInterfacePrx
::IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface > RobotStateComponentInterfacePrx
Definition: RobotVisuWidget.h:64
armarx::RobotNameHelperPtr
std::shared_ptr< class RobotNameHelper > RobotNameHelperPtr
Definition: RobotNameHelper.h:32
armarx::GraspingManager
The GraspingManager class.
Definition: GraspingManager.h:149
armarx::NaturalIKComponentPluginUser
Provides a ready-to-use NaturalIK.
Definition: NaturalIKComponentPlugin.h:40
MemoryXCoreObjectFactories.h
armarx::GraspingManager::generateIKs
MotionPlanningDataList generateIKs(const std::string &objectInstanceEntityId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1134
armarx::GraspingManagerPropertyDefinitions
Definition: GraspingManager.h:57
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:91
armarx::control::njoint_controller::platform::platform_follower_controller::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformFollowerController.h:88
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
SimoxCSpace.h
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
armarx::SimoxCSpacePtr
IceInternal::Handle< SimoxCSpace > SimoxCSpacePtr
An ice handle for a SimoxCSpace.
Definition: SimoxCSpace.h:56
IceUtil::Handle
Definition: forward_declarations.h:30
armarx::GraspingManager::generateGraspingTrajectoryList
GraspingTrajectoryList generateGraspingTrajectoryList(const std::string &objectInstanceEntityId, const Ice::Current &c=Ice::emptyCurrent) override
Definition: GraspingManager.cpp:1073
CSpaceVisualizerTask.h
MemoryXTypesObjectFactories.h
armarx::Mutex
boost::mutex Mutex
Definition: Synchronization.h:149
armarx::Vector3Ptr
IceInternal::Handle< Vector3 > Vector3Ptr
Definition: Pose.h:165
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
NaturalIKComponentPlugin.h
armarx::GraspingManager::onDisconnectComponent
void onDisconnectComponent() override
Definition: GraspingManager.cpp:190
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:35
armarx::PropertyDefinitionBase::eModifiable
@ eModifiable
Definition: PropertyDefinitionInterface.h:57
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
RobotNameHelper.h
armarx::GraspingManagerPropertyDefinitions::GraspingManagerPropertyDefinitions
GraspingManagerPropertyDefinitions(std::string prefix)
Definition: GraspingManager.h:60
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19