def main(): rospy.init_node("arm_obstacle_demo") wait_for_time() argv = rospy.myargv() planning_scene = PlanningSceneInterface("base_link") # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 gripper = robot_api.Gripper() arm = robot_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } planning_scene.removeAttachedObject('tray') gripper.open() error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') rospy.sleep(2) frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() gripper.close() rospy.sleep(2) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') gripper.open() planning_scene.removeAttachedObject('tray') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') return
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox( tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface( REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject( disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') ptu_action = FollowTrajectoryClient( "ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"]) ptu_action.move_to([ 0.0, -0.75, ]) # Use the planning scene object to add or remove objects scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() print("End Effector Link", end_effector_link) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 15 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server find_topic = "basic_grasping_perception/find_objects" sub_topic = "handles_position" rospy.Subscriber(sub_topic, target_pose, getHandlesCb) rospy.loginfo( "Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file # right_arm.set_named_target('right_down') # right_arm.go() # Open the gripper to the neutral position # right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # right_gripper.go() # rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(10.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) rospy.loginfo("Found %d Support Surfaces" % len(find_result.support_surfaces)) # print("perception_results_primitives",find_result.objects.primitives[0]) # print("perception_results_primitives_poses",find_result.objects.primitives_poses) # Remove all previous objects from the planning scene # for name in scene.getKnownCollisionObjects(): # scene.removeCollisionObject(name, False) # for name in scene.getKnownAttachedObjects(): # scene.removeAttachedObject(name, False) # scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None # target_position = None the_object = None the_object_dist = 1.0 count = -1 # target_size # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive( "object%d" % count, obj.object.primitives[0], obj.object.primitive_poses[0], ) #wait = False # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions print("target_size %d ", count, target_size) # target_position = obj.object.primitive_poses[0].position # print("target_size %d ", %count, target_size ) # Set the target pose target_pose.pose = obj.object.primitive_poses[0] print("target_pose_before ", target_pose) # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.519617092464 target_pose.pose.orientation.y = -0.510439243162 target_pose.pose.orientation.z = 0.479912175114 target_pose.pose.orientation.w = -0.489013456294 print("target_pose_after ", target_pose) # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d" % the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor # height = obj.primitive_poses[0].position.z # obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], # 2.0, # make table wider # obj.primitives[0].dimensions[2] ] # obj.primitive_poses[0].position.z += -height/1.5 # Add to scene scene.addSolidPrimitive( obj.name, obj.primitives[0], obj.primitive_poses[0], ) #wait = False # Get the table dimensions table_size = obj.primitives[0].dimensions print("table_size", table_size) # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0 / 256.0, 90.0 / 256.0, 12.0 / 256.0) # orange scene.setColor( find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID #support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object #right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up # place_pose = PoseStamped() # place_pose.header.frame_id = REFERENCE_FRAME # place_pose.pose.position.x = target_pose.pose.position.x # place_pose.pose.position.y = 0.03 # place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 # place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation # if result == MoveItErrorCodes.SUCCESS: # result = None # n_attempts = 0 # # Generate valid place poses # places = self.make_places(place_pose) # # Set the start state to the current state # #right_arm.set_start_state_to_current_state() # # Repeat until we succeed or run out of attempts # while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: # for place in places: # result = right_arm.place(target_id, place) # if result == MoveItErrorCodes.SUCCESS: # break # n_attempts += 1 # rospy.loginfo("Place attempt: " + str(n_attempts)) # rospy.sleep(0.2) # if result != MoveItErrorCodes.SUCCESS: # rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position # right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # right_gripper.go() # rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file # right_arm.set_named_target('resting') # right_arm.go() # rospy.sleep(2) # Give the servos a rest # arbotix_relax_all_servos() # rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() # argv = rospy.myargv() planning_scene = PlanningSceneInterface(frame='base_link') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray') # Create table obstacle table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # add orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 # # set torso to max height torso = robot_api.Torso() torso.set_height(robot_api.Torso.MAX_HEIGHT) arm = robot_api.Arm() # register shutdown method def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 20, 'num_planning_attempts': 10, 'replan': True } error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') # attach an object if pose1 is successfully reached frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() # close the gripper # gripper = robot_api.Gripper() # gripper.close() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') # At the end of the program planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') planning_scene.removeAttachedObject('tray')
class MoveItDemo: def __init__(self): rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() self.server = InteractiveMarkerServer("obj_find") # Initialize the move group for the right arm #######arm = MoveGroupCommander(GROUP_NAME_ARM) self.arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper #######gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link #######end_effector_link = arm.get_end_effector_link() self.end_effector_link = self.arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) #######arm.set_goal_position_tolerance(0.05) #######arm.set_goal_orientation_tolerance(0.1) self.arm.set_goal_position_tolerance(0.05) self.arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution #######arm.allow_replanning(True) self.arm.allow_replanning(True) # Set the right arm reference frame #######arm.set_pose_reference_frame(REFERENCE_FRAME) self.arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt #######arm.set_planning_time(15) self.arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing #######max_pick_attempts = 1 self.max_pick_attempts = 1 # Set a limit on the number of place attempts ########max_place_attempts = 3 self.max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) ''' rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") rospy.sleep(1) ''' self.arm.set_named_target('right_up') if self.arm.go() != True: rospy.logwarn(" Go failed") self.gripper.set_joint_value_target(self.gripper_opened) if self.gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) # Initialize the grasping goal #goal = FindGraspableObjectsGoal() #goal.plan_grasps = False ''' find_objects.send_goal(goal) find_objects.wait_for_result(rospy.Duration(5.0)) find_result = find_objects.get_result() rospy.loginfo("Found %d objects" %len(find_result.objects)) ''' for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() self.scene._colors = dict() # Use the nearest object on the table as the target self.target_pose = PoseStamped() self.target_pose.header.frame_id = REFERENCE_FRAME self.target_id = 'target' self.target_size = None self.the_object = None self.the_object_dist = 0.30 self.the_object_dist_min = 0.10 self.count = -1 #for obj in find_result.objects: # count +=1 #dx = obj.object.primitive_poses[0].position.x #dy = obj.object.primitive_poses[0].position.y dx = 0.25 dy = 0.0 d = math.sqrt((dx * dx) + (dy * dy)) m = Marker() m.type = Marker.CUBE m.scale.x = 0.02 m.scale.y = 0.02 m.scale.z = 0.05 m.color.r = 0.0 m.color.g = 0.5 m.color.b = 0.5 m.color.a = 1.0 int_marker = InteractiveMarker() int_marker.header.frame_id = "base_link" int_marker.name = "block_1" int_marker.pose.position.x = dx int_marker.pose.position.y = dy int_marker.pose.position.z = 0.04 int_marker.pose.orientation.x = 0.0 int_marker.pose.orientation.y = 0.0 int_marker.pose.orientation.z = 0.0 int_marker.pose.orientation.w = 1.0 control = InteractiveMarkerControl() control.orientation.w = 1 control.orientation.x = 0 control.orientation.y = 1 control.orientation.z = 0 control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE int_marker.controls.append(control) control.markers.append(m) control.always_visible = True int_marker.controls.append(control) self.server.insert(int_marker,self.processFeedback) self.server.applyChanges() def processFeedback(self,feedback): #p.pose.position = feedback.pose.position #self.dist_xy.pose.position = feedback.pose.position old_pose = Pose() new_pose = Pose() result = None if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_DOWN: old_pose.position.x = feedback.pose.position.x old_pose.position.y = feedback.pose.position.y result = self.pick_pose(old_pose) print "old pose is " + str(old_pose.position.x) + " , " + str(old_pose.position.y) if feedback.event_type == visualization_msgs.msg.InteractiveMarkerFeedback.MOUSE_UP: new_pose.position.x = feedback.pose.position.x new_pose.position.y = feedback.pose.position.y self.pick_and_place_pose(old_pose, new_pose) print "new pose is " + str(new_pose.position.x) + " , " + str(new_pose.position.y) def pick_and_place_pose(self,pick_pose,place_pose): pass def pick_pose(self, pick_pose): moveit_commander.roscpp_initialize(sys.argv) dx = pick_pose.position.x dy = pick_pose.position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < self.the_object_dist and d > self.the_object_dist_min: #if dx > the_object_dist_xmin and dx < the_object_dist_xmax: # if dy > the_object_dist_ymin and dy < the_object_dist_ymax: rospy.loginfo("object is in the working zone") self.the_object_dist = d self.the_object = self.count self.target_size = [0.02, 0.02, 0.05] #target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 #target_pose.pose.position.y = obj.object.primitive_poses[0].position.y self.target_pose.pose.position.x = dx + self.target_size[0] / 2.0 self.target_pose.pose.position.y = dy self.target_pose.pose.position.z = 0.04 self.target_pose.pose.orientation.x = 0.0 self.target_pose.pose.orientation.y = 0.0 self.target_pose.pose.orientation.z = 0.0 self.target_pose.pose.orientation.w = 1.0 # wait for the scene to sync self.scene.waitForSync() self.scene.setColor(self.target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) self.scene.sendColors() grasp_pose = self.target_pose grasp_pose.pose.position.y -= self.target_size[1] / 2.0 grasp_pose.pose.position.x += self.target_size[0] grasps = self.make_grasps(grasp_pose, [self.target_id], [self.target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation ''' for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) ''' # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state self.arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < self.max_pick_attempts: result = self.arm.pick(self.target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) result = self.arm.pick(self.target_id, grasps) else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) self.arm.set_named_target('right_up') self.arm.go() # Open the gripper to the neutral position self.gripper.set_joint_value_target(self.gripper_neutral) self.gripper.go() rospy.sleep(1) return result # Get the gripper posture as a JointTrajectory def make_gripper_posture(self, joint_positions): # Initialize the joint trajectory for the gripper joints t = JointTrajectory() # Set the joint names to the gripper joint names t.header.stamp = rospy.get_rostime() t.joint_names = GRIPPER_JOINT_NAMES # Initialize a joint trajectory point to represent the goal tp = JointTrajectoryPoint() # Assign the trajectory joint positions to the input positions tp.positions = joint_positions # Set the gripper effort tp.effort = GRIPPER_EFFORT tp.time_from_start = rospy.Duration(0.0) # Append the goal point to the trajectory points t.points.append(tp) # Return the joint trajectory return t # Generate a gripper translation in the direction given by vector def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g # Generate a list of possible grasps def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps # Generate a list of possible place poses def make_places(self, target_id, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr("Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places # Set the color of an object def setColor(self, name, r, g, b, a=0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def get_pose(self, co): # We get object's pose from the mesh/primitive poses; try first with the meshes if isinstance(co, CollisionObject): if co.mesh_poses: return co.mesh_poses[0] elif co.primitive_poses: return co.primitive_poses[0] else: rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id) return None elif isinstance(co, AttachedCollisionObject): if co.object.mesh_poses: return co.object.mesh_poses[0] elif co.object.primitive_poses: return co.object.primitive_poses[0] else: rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id) return None else: rospy.logerr("Input parameter is not a collision object") return None def pose_to_mat(self, pose): '''Convert a pose message to a 4x4 numpy matrix. Args: pose (geometry_msgs.msg.Pose): Pose rospy message class. Returns: mat (numpy.matrix): 4x4 numpy matrix ''' quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T mat = np.matrix(quaternion_matrix(quat)) mat[0:3, 3] = pos return mat def mat_to_pose(self, mat, transform=None): '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform. Args: mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform. transform (numpy.ndarray): Optional 4x4 array representing additional transform Returns: pose (geometry_msgs.msg.Pose): Pose message representing transform. ''' if transform != None: mat = np.dot(mat, transform) pose = Pose() pose.position.x = mat[0,3] pose.position.y = mat[1,3] pose.position.z = mat[2,3] quat = quaternion_from_matrix(mat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() planning_scene = PlanningSceneInterface('base_link') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.4 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) #planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 torso = fetch_api.Torso() torso.set_height(fetch_api.Torso.MAX_HEIGHT) arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) # and add an orientation constraint to pose 2: oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs = { 'allowed_planning_time': 100, 'execution_timeout': 100, 'num_planning_attempts': 15, 'replan': True, 'replan_attempts': 10 } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) kwargs['orientation'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject(disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 1 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the simple_grasping find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #right_arm.set_named_target('resting') #right_arm.go() # Open the gripper to the neutral position arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the simple_grasping grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # Clear the virtual object colors self.scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_id = 'target' target_size = None the_object = None the_object_dist = 0.30 the_object_dist_min = 0.10 count = -1 for obj in find_result.objects: count += 1 self.scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist and d > the_object_dist_min: rospy.loginfo("object is in the working zone") the_object_dist = d the_object = count target_size = [0.02, 0.02, 0.05] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 target_pose.pose.position.y = obj.object.primitive_poses[0].position.y target_pose.pose.position.z = 0.04 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] - height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync self.scene.waitForSync() # Set colors of the table and the object we are grabbing self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange self.scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey self.scene.sendColors() if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object arm.set_support_surface_name(support_surface) # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) rospy.sleep(2) # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('right_up') arm.go() rospy.sleep(2) if args.once: moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0) # Get the gripper posture as a JointTrajectory def make_gripper_posture(self, joint_positions): # Initialize the joint trajectory for the gripper joints t = JointTrajectory() # Set the joint names to the gripper joint names t.header.stamp = rospy.get_rostime() t.joint_names = GRIPPER_JOINT_NAMES # Initialize a joint trajectory point to represent the goal tp = JointTrajectoryPoint() # Assign the trajectory joint positions to the input positions tp.positions = joint_positions # Set the gripper effort tp.effort = GRIPPER_EFFORT tp.time_from_start = rospy.Duration(1.0) # Append the goal point to the trajectory points t.points.append(tp) # Return the joint trajectory return t # Generate a gripper translation in the direction given by vector def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g # Generate a list of possible grasps def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, -1.5]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, 0.0, 1.5]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(initial_pose_stamped, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1,-0.1]] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps # Generate a list of possible place poses def make_places(self, init_pose): # Initialize the place location as a PoseStamped message place = PoseStamped() # Start with the input place pose place = init_pose # A list of x shifts (meters) to try x_vals = [0, 0.005, -0.005] #, 0.01, -0.01, 0.015, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, -0.005, 0.01, -0.01] #, 0.015, -0.015] # A list of pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4, 0.5, -0.5, 0.6, -0.6, 0.7, -0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3, -1.3, 1.4, -1.4, 1.5, -1.5] #, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] # A list to hold the places places = [] # Generate a place pose for each angle and translation for pitch in pitch_vals: for dy in y_vals: for dx in x_vals: place.pose.position.x = init_pose.pose.position.x + dx place.pose.position.y = init_pose.pose.position.y + dy # Yaw angle: given the limited dofs of turtlebot_arm, we must calculate the heading from # arm base to the place location (first we must transform its pose to arm base frame) target_pose_arm_ref = self._tf2_buff.transform(place, ARM_BASE_FRAME) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y yaw = atan2(y, x) # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the place pose orientation accordingly place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3] # MoveGroup::place will transform the provided place pose with the attached body pose, so the object retains # the orientation it had when picked. However, with our 4-dofs arm this is infeasible (nor we care about the # objects orientation!), so we cancel this transformation. It is applied here: # https://github.com/ros-planning/moveit_ros/blob/jade-devel/manipulation/pick_place/src/place.cpp#L64 # More details on this issue: https://github.com/ros-planning/moveit_ros/issues/577 acobjs = self.scene.get_attached_objects([target_id]) aco_pose = self.get_pose(acobjs[target_id]) if aco_pose is None: rospy.logerr("Attached collision object '%s' not found" % target_id) return None aco_tf = self.pose_to_mat(aco_pose) place_tf = self.pose_to_mat(place.pose) place.pose = self.mat_to_pose(place_tf, aco_tf) rospy.logdebug("Compensate place pose with the attached object pose [%s]. Results: [%s]" \ % (aco_pose, place.pose)) # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places # Set the color of an object def setColor(self, name, r, g, b, a = 0.9): # Initialize a MoveIt color object color = ObjectColor() # Set the id to the name given as an argument color.id = name # Set the rgb and alpha values given as input color.color.r = r color.color.g = g color.color.b = b color.color.a = a # Update the global color dictionary self.colors[name] = color # Actually send the colors to MoveIt! def sendColors(self): # Initialize a planning scene object p = PlanningScene() # Need to publish a planning scene diff p.is_diff = True # Append the colors from the global color dictionary for color in self.colors.values(): p.object_colors.append(color) # Publish the scene diff self.scene_pub.publish(p) def get_pose(self, co): # We get object's pose from the mesh/primitive poses; try first with the meshes if isinstance(co, CollisionObject): if co.mesh_poses: return co.mesh_poses[0] elif co.primitive_poses: return co.primitive_poses[0] else: rospy.logerr("Collision object '%s' has no mesh/primitive poses" % co.id) return None elif isinstance(co, AttachedCollisionObject): if co.object.mesh_poses: return co.object.mesh_poses[0] elif co.object.primitive_poses: return co.object.primitive_poses[0] else: rospy.logerr("Attached collision object '%s' has no mesh/primitive poses" % co.id) return None else: rospy.logerr("Input parameter is not a collision object") return None def pose_to_mat(self, pose): '''Convert a pose message to a 4x4 numpy matrix. Args: pose (geometry_msgs.msg.Pose): Pose rospy message class. Returns: mat (numpy.matrix): 4x4 numpy matrix ''' quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] pos = np.matrix([pose.position.x, pose.position.y, pose.position.z]).T mat = np.matrix(quaternion_matrix(quat)) mat[0:3, 3] = pos return mat def mat_to_pose(self, mat, transform=None): '''Convert a homogeneous matrix to a Pose message, optionally premultiply by a transform. Args: mat (numpy.ndarray): 4x4 array (or matrix) representing a homogenous transform. transform (numpy.ndarray): Optional 4x4 array representing additional transform Returns: pose (geometry_msgs.msg.Pose): Pose message representing transform. ''' if transform != None: mat = np.dot(mat, transform) pose = Pose() pose.position.x = mat[0,3] pose.position.y = mat[1,3] pose.position.z = mat[2,3] quat = quaternion_from_matrix(mat) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose
def main(): print('We are running the main of arm_obstable_demo') rospy.init_node('arm_obstacle_demo') wait_for_time() print( "If planning scene fails to init, make sure you have launched move_group.launch" ) planning_scene = PlanningSceneInterface("base_link") # Create table object planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider object planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) # Before moving to the first pose planning_scene.removeAttachedObject('tray') pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': False } error = arm.move_to_pose(pose1, **kwargs) # If the robot reaches the first pose successfully, then "attach" an object there # Of course, you would have to close the gripper first and ensure that you grasped the object properly if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray')
def main(): # Create table obstacle rospy.init_node('arm_obstacle_demo') planning_scene = PlanningSceneInterface('base_link') planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # planning_scene.removeCollisionObject('divider') # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) #Orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 # move to pose args kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') # Was working without this but now its needed planning_scene.clear()