def pickup_object( self, graspable, graspable_name, table_name, which_arm, execute=True, lift_desired_distance=0.1, lift_min_distance=0.05, allow_support_collision=True, ignore_collisions=False, desired_grasps=None, wait=rospy.Duration(), ): """Picks up a previously detected object. Parameters: graspable: an object_manipulation_msgs/GraspableObject msg instance. This usually comes from a Detector.call_collision_map_processing call. graspable_name: the name of the object to graps. It is provided by Detector.call_collision_map_processing. table_name: the name of the table. Again provided by Detector.call_collision_map_processing. which_arm: left_arm or right_arm exectute: True|False, whether to execute the grasp or just check for feasibility lift_desired_distance: the desired lift distance lift_min_distance: the min distance that must be considered feasible before the grasp is even attempted allow_support_collision: whether collisions between the gripper and the support surface should be acceptable during move from pre-grasp to grasp and during lift. Collisions when moving to the pre-grasp location are still not allowed even if this is set to true. ignore_collisions: set this to true if you want to ignore all collisions throughout the pickup and also move directly to the pre-grasp using Cartesian controllers desired_grasps: a list of custom grasps. If not set, pickup will automatically generate grasps. wait: how long to wait for the action to be successful Return: a object_manipulation_msgs.PickupResult msg """ rospy.loginfo("Calling the pickup action") pickup_goal = PickupGoal() pickup_goal.target = graspable pickup_goal.collision_object_name = graspable_name pickup_goal.collision_support_surface_name = table_name pickup_goal.only_perform_feasibility_test = not execute pickup_goal.arm_name = which_arm pickup_goal.lift.direction.header.frame_id = "base_link" pickup_goal.lift.direction.vector.x = 0 pickup_goal.lift.direction.vector.y = 0 pickup_goal.lift.direction.vector.z = 1 pickup_goal.lift.desired_distance = lift_desired_distance pickup_goal.lift.min_distance = lift_min_distance # do not use tactile-based grasping or tactile-based lift pickup_goal.use_reactive_lift = False pickup_goal.use_reactive_execution = False pickup_goal.allow_gripper_support_collision = allow_support_collision pickup_goal.ignore_collisions = ignore_collisions if desired_grasps: pickup_goal.desired_grasps = desired_grasps self.pickup_client.send_goal_and_wait(pickup_goal, wait) res = self.pickup_client.get_result() assert isinstance(res, PickupResult) manipulation_result = res.manipulation_result if manipulation_result.value != manipulation_result.SUCCESS: rospy.logerr("Error during pickup") return None self.last_pickup_result = res return res