def pick(self, object_name, grasp, support_name = '', planner = 'BKPIECEkConfigDefault'): goal = PickupGoal() goal.target_name = object_name goal.group_name = self.name goal.end_effector = '{0}_gripper'.format(self.side) goal.allowed_planning_time = 50 goal.support_surface_name = support_name # @TODO Anadir este parametro con info del shelfdetector goal.planner_id = planner goal.allow_gripper_support_collision = True goal.attached_object_touch_links = ['bender/l_int_finger_link','bender/l_ext_finger_link','bender/l_wrist_pitch_link'] goal.allowed_touch_objects = ['pringles'] #goal.path_constraints = path_constraints goal.possible_grasps = grasp self.pick_client.send_goal(goal)
def pickup(self, name, grasps, wait=True, **kwargs): # Check arguments supported_args = ("allow_gripper_support_collision", "allowed_touch_objects", "plan_only", "planner_id", "planning_scene_diff", "planning_time", "support_name") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("pickup: unsupported argument: %s", arg) # Create goal g = PickupGoal() # 1. Fill in target name g.target_name = name # 2. Fill in group name g.group_name = self._group # 3. Fill in end effector g.end_effector = self._effector # 4. List of grasps g.possible_grasps = grasps # 5. Fill in support surface try: g.support_surface_name = kwargs["support_name"] except KeyError: g.support_surface_name = "" # 6. Can gripper contact the support surface (named above) try: g.allow_gripper_support_collision = kwargs[ "allow_gripper_support_collision"] except KeyError: g.allow_gripper_support_collision = True # 7. What links are part of the "gripper" (and can contact the object/surface) try: g.attached_object_touch_links = kwargs[ "attached_object_touch_links"] except KeyError: g.attached_object_touch_links = list( ) # empty list = use all links of end-effector # 8. Fill in minimize_object_distance # 9. Fill in path_constraints # 10. Fill in planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 11. List of obstacles that we are allowed to touch try: g.allowed_touch_objects = kwargs["allowed_touch_objects"] except KeyError: g.allowed_touch_objects = list() # 12. Planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.allowed_planning_time # 13. Planning options try: g.planning_options.planning_scene_diff = kwargs[ "planning_scene_diff"] except KeyError: g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True g.planning_options.plan_only = self._plan_only self._pick_action.send_goal(g) if wait: self._pick_action.wait_for_result() return self._pick_action.get_result() else: return None
def pickup(self, name, grasps, wait=True, **kwargs): # Check arguments supported_args = ("allow_gripper_support_collision", "allowed_touch_objects", "plan_only", "planner_id", "planning_time", "support_name") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("pickup: unsupported argument: %s", arg) # Create goal g = PickupGoal() # 1. Fill in target name g.target_name = name # 2. Fill in group name g.group_name = self._group # 3. Fill in end effector g.end_effector = self._effector # 4. List of grasps g.possible_grasps = grasps # 5. Fill in support surface try: g.support_surface_name = kwargs["support_name"] except KeyError: g.support_surface_name = "" # 6. Can gripper contact the support surface (named above) try: g.allow_gripper_support_collision = kwargs["allow_gripper_support_collision"] except KeyError: g.allow_gripper_support_collision = True # 7. What links are part of the "gripper" (and can contact the object/surface) try: g.attached_object_touch_links = kwargs["attached_object_touch_links"] except KeyError: g.attached_object_touch_links = list() # empty list = use all links of end-effector # 8. Fill in minimize_object_distance # 9. Fill in path_constraints # 10. Fill in planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 11. List of obstacles that we are allowed to touch try: g.allowed_touch_objects = kwargs["allowed_touch_objects"] except KeyError: g.allowed_touch_objects = list() # 12. Planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.allowed_planning_time # 13. Planning options g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True g.planning_options.plan_only = self._plan_only self._pick_action.send_goal(g) if wait: self._pick_action.wait_for_result() return self._pick_action.get_result() else: return None