def main(): rospy.init_node('moveit_py_place', anonymous=True) #right_arm.set_planner_id("KPIECEkConfigDefault"); scene = PlanningSceneInterface() robot = RobotCommander() #group = MoveGroupCommander("head") right_arm = MoveGroupCommander("right_arm") #right_arm.set_planner_id("KPIECEkConfigDefault"); rospy.logwarn("cleaning world") GRIPPER_FRAME = 'gripper_bracket_f2' #scene.remove_world_object("table") scene.remove_world_object("part") scene.remove_attached_object(GRIPPER_FRAME, "part") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.67 p.pose.position.y = -0. p.pose.position.z = 0.75 scene.add_box("part", p, (0.07, 0.01, 0.2)) # move to a random target #group.set_named_target("ahead") #group.go() #rospy.sleep(1) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part") n_attempts += 1 print "Attempts pickup: ", n_attempts rospy.sleep(0.2) #robot.right_arm.pick("part") #right_arm.go() rospy.sleep(5)
class planning: def __init__(self): # Retrieve params: # if there is no param named 'table_object_name' then use the default # set the scene in rzvi # original initilization is for 2 self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') # table self._grasp_object_name_1 = rospy.get_param('~grasp_object_name', 'Grasp_Object1') # box1 self._grasp_object_name_2 = rospy.get_param( '~grasp_object_name2', 'Wall_behind') # wall_behind as a obstacle self._grasp_object_name_3 = rospy.get_param( '~grasp_object_name3', 'Wall_above') # wall_above as a obstacle self._grasp_object_name_screw = rospy.get_param( '~grasp_object_name5', 'screw') # screw #print self._grasp_object_name_screw self._grasp_object_name_capscrew4 = rospy.get_param( '~grasp_object_name4', 'capscrew4') # capscrew #print self._grasp_object_name_capscrew4 #### Not needed in the model spawn Section #### # Create (debugging) publishers(display in rviz): #self._grasps_pub = rospy.Publisher( # 'grasps', PoseArray, queue_size=1, latch=True) #self._places_pub = rospy.Publisher( # 'places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name_1) self._scene.remove_world_object( self._grasp_object_name_2) #wall_behind #self._scene.remove_world_object(self._grasp_object_name_3) #wall_above self._scene.remove_world_object( self._grasp_object_name_capscrew4) #capscrew4 self._scene.remove_world_object(self._grasp_object_name_screw) #screw ##############3# Add table and Coke can objects to the planning scene:##################################### self._pose_table = self._add_table(self._table_object_name) self._add_grasp_block_1(self._grasp_object_name_1) # ##########################################3Adding obstacle_behind############################# p_wall = PoseStamped() robot = moveit_commander.RobotCommander() p_wall.header.frame_id = robot.get_planning_frame() p_wall.header.stamp = rospy.Time.now() p_wall.pose.position.x = -0.25 p_wall.pose.position.y = 0.0 p_wall.pose.position.z = 1.5 q_wall_temp = quaternion_from_euler(0.0, 80.0, 0.0) #Q number p_wall.pose.orientation = Quaternion(*q_wall_temp) self._scene.add_box(self._grasp_object_name_2, p_wall, (3, 3, 0.05)) ###################################### Adding obstacle wall_above###################################### p_wall2 = PoseStamped() p_wall2.header.frame_id = robot.get_planning_frame() p_wall2.header.stamp = rospy.Time.now() p_wall2.pose.position.x = table_x p_wall2.pose.position.y = table_y p_wall2.pose.position.z = 1.25 q_wall2_temp = quaternion_from_euler(0.0, 0.0, 0.0) #Q number p_wall2.pose.orientation = Quaternion(*q_wall2_temp) #self._scene.add_box(self._grasp_object_name_3, p_wall2, (3, 3, 0.05)) ###################################### Adding obstacle side-1##################################### p_wall3 = PoseStamped() p_wall3.header.frame_id = robot.get_planning_frame() p_wall3.header.stamp = rospy.Time.now() p_wall3.pose.position.x = table_x p_wall3.pose.position.y = table_y - 0.8 / 2 - 0.2 p_wall3.pose.position.z = table_z q_wall3_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number p_wall3.pose.orientation = Quaternion(*q_wall3_temp) self._scene.add_box("wall_side-1", p_wall3, (2, 2, 0.05)) ###################################### Adding obstacle side -2###################################### p_wall4 = PoseStamped() p_wall4.header.frame_id = robot.get_planning_frame() p_wall4.header.stamp = rospy.Time.now() p_wall4.pose.position.x = table_x p_wall4.pose.position.y = table_y + 0.8 / 2 + 0.05 p_wall4.pose.position.z = table_z q_wall4_temp = quaternion_from_euler(80.0, 0.0, 0.0) #Q number p_wall4.pose.orientation = Quaternion(*q_wall4_temp) self._scene.add_box("wall_side-2", p_wall4, (2, 2, 0.05)) ##############################################adding screw########################### screw = PoseStamped() screw.header.frame_id = robot.get_planning_frame() screw.header.stamp = rospy.Time.now() screw.pose.position.x = table_x screw.pose.position.y = table_y - 0.2 screw.pose.position.z = box1_z + 0.02 screw_q = quaternion_from_euler(0.0, 0.0, 0.0) #Q number screw.pose.orientation = Quaternion(*screw_q) print self._scene.add_box(self._grasp_object_name_screw, screw, (0.036, 0.046, 0.032)) ##############################################adding capscrew 0.045########################### capscrew4 = PoseStamped() capscrew4.header.frame_id = robot.get_planning_frame() capscrew4.header.stamp = rospy.Time.now() capscrew4.pose.position.x = table_x capscrew4.pose.position.y = table_y capscrew4.pose.position.z = box1_z + 0.02 q4 = quaternion_from_euler(0.0, 0.0, 0.0) #Q number capscrew4.pose.orientation = Quaternion(*q4) print self._scene.add_box(self._grasp_object_name_capscrew4, capscrew4, (0.05, 0.05, 0.040)) rospy.sleep(1.0) # gripper graspping point setting. # Define target place pose: #########################################Section: Neccessay function for (displaying)Rviz################################### ######this section for adding stuffs in the planning scene.(table, box1, box2, etc...) in RViz############################3 def _add_table(self, name): """ Create and add table to the scene """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = table_x p.pose.position.y = table_y p.pose.position.z = table_z q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. # display in RZvi print self._scene.add_box(name, p, (1, 1, 0.05)) #arg(name, pose, size) return p.pose def _add_grasp_block_1(self, name): """ Create and add block to the scene """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = box1_x + 0.3 p.pose.position.y = box1_y + 0.3 p.pose.position.z = box1_z q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. #self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose
class ObjectManipulationAS: def __init__(self, name): # stuff for grasp planning rospy.loginfo("Getting a TransformListener...") self.tf_listener = tf.TransformListener() rospy.loginfo("Getting a TransformBroadcaster...") self.tf_broadcaster = tf.TransformBroadcaster() rospy.loginfo("Initializing a ClusterBoundingBoxFinder...") self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_clusters = None rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...") self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback) if DEBUG_MODE: self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped) rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...") self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...") self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...") self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction) self.grasps_ac.wait_for_server() rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...") self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty) self.depth_service.wait_for_service() rospy.loginfo("Getting a PlanningSceneInterface instance...") self.scene = PlanningSceneInterface() # blocking action server rospy.loginfo("Creating Action Server '" + name + "'...") self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False) self.as_feedback = ObjectManipulationFeedback() self.as_result = ObjectManipulationActionResult() self.current_goal = None # Take care of left and right arm grasped stuff self.right_hand_object = None self.left_hand_object = None self.current_side = 'right' rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!") self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_clusters = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return # TODO: Check if pose is not empty, if it is, reject # elif len(self.last_clusters.objects) - 1 < goal.get_goal().target_id: # goal.set_rejected() # "No objects to grasp were received on the objects topic." # return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run the corresponding operation if self.current_goal.get_goal().operation == ObjectManipulationGoal.PICK: rospy.loginfo("ObjectManipulation: PICK!") self.pick_operation() elif self.current_goal.get_goal().operation == ObjectManipulationGoal.PLACE: rospy.loginfo("ObjectManipulation: PLACE!") self.place_operation() else: rospy.logerr("ObjectManipulation: Erroneous operation!") #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def pick_operation(self): """Execute pick operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking hand to use") # Check which arm group was requested and if it's currently free of objects if 'right' in goal_message_field.group: if self.right_hand_object: # Something already in the hand self.update_aborted("Right hand already contains an object.") return # necessary? self.current_side = 'right' elif 'left' in goal_message_field.group: if self.left_hand_object: self.update_aborted("Left hand already contains an object.") return self.current_side = 'left' # Publish pose of goal position if DEBUG_MODE: self.to_grasp_object_pose_pub.publish(goal_message_field.target_pose) self.update_feedback("Detecting clusters") if not self.wait_for_recognized_array(wait_time=5, timeout_time=10): # wait until we get clusters published self.update_aborted("Failed detecting clusters") # Search closer cluster # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) object_to_grasp_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: object_to_grasp_pose = goal_message_field.target_pose.pose self.update_feedback("Searching closer cluster while clustering") (closest_cluster_id, (object_points, obj_bbox_dims, object_bounding_box, object_pose)) = self.get_id_of_closest_cluster_to_pose(object_to_grasp_pose) rospy.logdebug("in AS: Closest cluster id is: " + str(closest_cluster_id)) #TODO visualize bbox #TODO publish filtered pointcloud? rospy.logdebug("BBOX: " + str(obj_bbox_dims)) ######## self.update_feedback("Check reachability") ######## self.update_feedback("Generating grasps") rospy.logdebug("Object pose before tf thing is: " + str(object_pose)) #transform pose to base_link, IS THIS NECESSARY?? should be a function in any case self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2] / 2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("No grasps received") return if DEBUG_MODE: # TODO: change to dynamic param publish_grasps_as_poses(grasp_list) self.current_goal.publish_feedback(self.as_feedback) ######## self.update_feedback("Setup planning scene") # Add object to grasp to planning scene self.scene.add_box(self.current_side + "_hand_object", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.as_result.object_scene_name = self.current_side + "_hand_object" ######## self.update_feedback("Execute grasps") pug = createPickupGoal(self.current_side + "_hand_object", grasp_list, group=goal_message_field.group) rospy.loginfo("Sending pickup goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result...") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.as_result.object_pose = object_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: if self.current_side == 'right': self.right_hand_object = self.current_side + "_hand_object" elif self.current_side == 'left': self.left_hand_object = self.current_side + "_hand_object" self.current_goal.set_succeeded(result=self.as_result) else: # Remove object as it has not been picked self.scene.remove_world_object(self.current_side + "_hand_object") self.update_aborted(text="MoveIt pick failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def place_operation(self): """Execute the place operation""" if self.message_fields_ok(): self.as_result = ObjectManipulationResult() goal_message_field = self.current_goal.get_goal() self.update_feedback("Checking arm to use") # Check which arm group was requested and if it currently has an object if 'right' in goal_message_field.group: if not self.right_hand_object: # Something already in the hand self.update_aborted("Right hand does not contain an object.") return # necessary? self.current_side = 'right' current_target = self.right_hand_object elif 'left' in goal_message_field.group: if not self.left_hand_object: self.update_aborted("Left hand does not contain an object.") return self.current_side = 'left' current_target = self.left_hand_object # transform pose to base_link if needed if goal_message_field.target_pose.header.frame_id != "base_link": self.tf_listener.waitForTransform("base_link", goal_message_field.target_pose.header.frame_id, goal_message_field.target_pose.header.stamp, rospy.Duration(5)) placing_pose = self.tf_listener.transformPose("base_link", goal_message_field.target_pose) else: placing_pose = goal_message_field.target_pose.pose #### TODO: ADD HERE LOGIC ABOUT SEARCHING GOOD PLACE POSE #### self.update_feedback("Sending place order to MoveIt!") placing_pose = PoseStamped(header=Header(frame_id="base_link"), pose=placing_pose) goal = createPlaceGoal(placing_pose, group=goal_message_field.group, target=current_target) rospy.loginfo("Sending place goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result...") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) self.as_result.object_pose = placing_pose self.as_result.error_code = result.error_code self.as_result.error_message = str(moveit_error_dict[result.error_code.val]) if result.error_code.val == MoveItErrorCodes.SUCCESS: # Emptying hand self.update_feedback("Emptying hand") if self.current_side == 'right': self.as_result.object_scene_name = current_target self.right_hand_object = None elif self.current_side == 'left': self.as_result.object_scene_name = current_target self.left_hand_object = None # Removing object from the collision world self.scene.remove_world_object(current_target) self.current_goal.set_succeeded(result=self.as_result) else: self.update_aborted(text="MoveIt place failed", manipulation_result=self.as_result) else: self.update_aborted("Goal fields not correctly fulfilled") def message_fields_ok(self): """Check if the minimal fields for the message are fulfilled""" if self.current_goal: goal_message_field = self.current_goal.get_goal() if len(goal_message_field.group) == 0: rospy.logwarn("Group field empty.") return False if goal_message_field.operation != ObjectManipulationGoal.PICK and goal_message_field.operation != ObjectManipulationGoal.PLACE: rospy.logwarn("Asked for an operation different from PICK or PLACE: " + str(goal_message_field.operation)) return False if len(goal_message_field.target_pose.header.frame_id) == 0: rospy.logwarn("Target pose frame_id is empty") return False return True def update_feedback(self, text): """Publish feedback with given text""" self.as_feedback.last_state = text self.current_goal.publish_feedback(self.as_feedback) def update_aborted(self, text="", manipulation_result=None): """Publish feedback and abort with the text give and optionally an ObjectManipulationResult already fulfilled""" self.update_feedback("aborted." + text) if manipulation_result == None: aborted_result = ObjectManipulationResult() aborted_result.error_message = text self.current_goal.set_aborted(result=aborted_result) else: self.current_goal.set_aborted(result=manipulation_result) def generate_grasps(self, pose, width): """Send request to block grasp generator service""" goal = GenerateGraspsGoal() goal.pose = pose.pose goal.width = width grasp_options = GraspGeneratorOptions() grasp_options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Y grasp_options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_DOWN grasp_options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_HALF goal.options.append(grasp_options) self.grasps_ac.send_goal(goal) if DEBUG_MODE: rospy.loginfo("Sent goal, waiting:\n" + str(goal)) self.grasps_ac.wait_for_result() grasp_list = self.grasps_ac.get_result().grasps return grasp_list def get_id_of_closest_cluster_to_pose(self, input_pose): """Returns the id of the closest cluster to the pose provided (in Pose or PoseStamped) and all the associated clustering information""" current_id = 0 closest_pose = None closest_object_points = None closest_id = 0 closest_bbox = None closest_bbox_dims = None closest_distance = 99999.9 for myobject in self.last_clusters.objects: (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(myobject.point_clouds[0]) self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(15)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose rospy.loginfo("id: " + str(current_id) + "\n pose:\n" + str(object_pose)) if closest_pose == None: # first loop iteration closest_object_points = object_points closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_distance = dist_between_poses(closest_pose, input_pose) else: if dist_between_poses(object_pose, input_pose) < closest_distance: closest_object_points = object_points closest_distance = dist_between_poses(object_pose, input_pose) closest_pose = object_pose closest_bbox = object_bounding_box closest_bbox_dims = obj_bbox_dims closest_id = current_id current_id += 1 rospy.loginfo("Closest id is: " + str(closest_id) + " at " + str(closest_pose)) return closest_id, (closest_object_points, closest_bbox_dims, closest_bbox_dims, closest_pose) def wait_for_recognized_array(self, wait_time=6, timeout_time=10): """Ask for depth images until we get a recognized array wait for wait_time between depth throtle calls stop if timeout_time is achieved If we dont find it in the correspondent time return false, true otherwise""" initial_time = rospy.Time.now() self.last_clusters = None count = 0 num_calls = 1 self.depth_service.call(EmptyRequest()) rospy.loginfo("Depth throtle server call #" + str(num_calls)) rospy.loginfo("Waiting for a recognized array...") while rospy.Time.now() - initial_time < rospy.Duration(timeout_time) and self.last_clusters == None: rospy.sleep(0.1) count += 1 if count >= wait_time / 10: self.depth_service.call(EmptyRequest()) num_calls += 1 rospy.loginfo("Depth throtle server call #" + str(num_calls)) if self.last_clusters == None: return False else: return True
class GraspObjectServer: def __init__(self, name): # stuff for grasp planning self.tf_listener = tf.TransformListener() self.tf_broadcaster = tf.TransformBroadcaster() self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link") self.last_objects = RecognizedObjectArray() #rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback) self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback) self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray) rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) #pickup_ac.wait_for_server() # needed? rospy.loginfo("Connecting to grasp generator AS") self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction) #grasps_ac.wait_for_server() # needed? #planning scene for motion planning self.scene = PlanningSceneInterface() # blocking action server self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False) self.feedback = GraspObjectFeedback() self.result = GraspObjectResult() self.current_goal = None self.grasp_obj_as.start() def objects_callback(self, data): rospy.loginfo(rospy.get_name() + ": This message contains %d objects." % len(data.objects)) self.last_objects = data def goal_callback(self, goal): if self.current_goal: goal.set_rejected() # "Server busy" return elif len(self.last_objects.objects) - 1 < goal.get_goal().target_id: goal.set_rejected() # "No objects to grasp were received on the objects topic." return else: #store and accept new goal self.current_goal = goal self.current_goal.set_accepted() #run grasping state machine self.grasping_sm() #finished, get rid of goal self.current_goal = None def cancel_callback(self, goal): #TODO stop motions? self.current_goal.set_canceled() def grasping_sm(self): if self.current_goal: self.update_feedback("running clustering") (object_points, obj_bbox_dims, object_bounding_box, object_pose) = self.cbbf.find_object_frame_and_bounding_box(self.last_objects.objects[self.current_goal.get_goal().target_id].point_clouds[0]) #TODO visualize bbox #TODO publish filtered pointcloud? print obj_bbox_dims ######## self.update_feedback("check reachability") ######## self.update_feedback("generate grasps") #transform pose to base_link self.tf_listener.waitForTransform("base_link", object_pose.header.frame_id, object_pose.header.stamp, rospy.Duration(5)) trans_pose = self.tf_listener.transformPose("base_link", object_pose) object_pose = trans_pose #HACK remove orientation -> pose is aligned with parent(base_link) object_pose.pose.orientation.w = 1.0 object_pose.pose.orientation.x = 0.0 object_pose.pose.orientation.y = 0.0 object_pose.pose.orientation.z = 0.0 # shift object pose up by halfway, clustering code gives obj frame on the bottom because of too much noise on the table cropping (2 1pixel lines behind the objects) # TODO remove this hack, fix it in table filtering object_pose.pose.position.z += obj_bbox_dims[2]/2.0 grasp_list = self.generate_grasps(object_pose, obj_bbox_dims[0]) # width is the bbox size on x # check if there are grasps, if not, abort if len(grasp_list) == 0: self.update_aborted("no grasps received") return self.publish_grasps_as_poses(grasp_list) self.feedback.grasps = grasp_list self.current_goal.publish_feedback(self.feedback) self.result.grasps = grasp_list ######## self.update_feedback("setup planning scene") #remove old objects self.scene.remove_world_object("object_to_grasp") # add object to grasp to planning scene self.scene.add_box("object_to_grasp", object_pose, (obj_bbox_dims[0], obj_bbox_dims[1], obj_bbox_dims[2])) self.result.object_scene_name = "object_to_grasp" ######## if self.current_goal.get_goal().execute_grasp: self.update_feedback("execute grasps") pug = self.createPickupGoal("object_to_grasp", grasp_list) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(pug) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.loginfo("Result is:") print result rospy.loginfo("Human readable error: " + str(moveit_error_dict[result.error_code.val])) ######## self.update_feedback("finished") self.result.object_pose = object_pose #bounding box in a point message self.result.bounding_box = Point() self.result.bounding_box.x = obj_bbox_dims[0] self.result.bounding_box.y = obj_bbox_dims[1] self.result.bounding_box.z = obj_bbox_dims[2] self.current_goal.set_succeeded(result = self.result) #self.current_goal.set_aborted() def update_feedback(self, text): self.feedback.last_state = text self.current_goal.publish_feedback(self.feedback) def update_aborted(self, text = ""): self.update_feedback("aborted." + text) self.current_goal.set_aborted() def generate_grasps(self, pose, width): #send request to block grasp generator service if not self.grasps_ac.wait_for_server(rospy.Duration(3.0)): return [] rospy.loginfo("Successfully connected.") goal = GenerateBlockGraspsGoal() goal.pose = pose.pose goal.width = width self.grasps_ac.send_goal(goal) rospy.loginfo("Sent goal, waiting:\n" + str(goal)) t_start = rospy.Time.now() self.grasps_ac.wait_for_result() t_end = rospy.Time.now() t_total = t_end - t_start rospy.loginfo("Result received in " + str(t_total.to_sec())) grasp_list = self.grasps_ac.get_result().grasps return grasp_list def publish_grasps_as_poses(self, grasps): rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose") graspmsg = Grasp() grasp_PA = PoseArray() header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() grasp_PA.header = header for graspmsg in grasps: p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) grasp_PA.poses.append(p) self.grasp_publisher.publish(grasp_PA) rospy.sleep(0.1) def createPickupGoal(self, target, possible_grasps, group="right_arm_torso_grasping"): """ Create a PickupGoal with the provided data""" pug = PickupGoal() pug.target_name = target pug.group_name = group pug.possible_grasps.extend(possible_grasps) pug.allowed_planning_time = 5.0 pug.planning_options.planning_scene_diff.is_diff = True pug.planning_options.planning_scene_diff.robot_state.is_diff = True pug.planning_options.plan_only = False pug.planning_options.replan = True pug.planning_options.replan_attempts = 10 pug.attached_object_touch_links = ['arm_right_5_link', "arm_right_grasping_frame"] pug.allowed_touch_objects.append(target) #pug.attached_object_touch_links.append('all') return pug
scene = PlanningSceneInterface() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = REFERENCE_LINK p.header.stamp = rospy.Time.now() quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw p.pose.orientation = Quaternion(*quat) p.pose.position.x = 0.6 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # add table scene.add_box(TABLE_OBJECT, p, (0.5, 0.5, 0.2)) i = 1 while i <= 10: gripper_pose = arm.get_current_pose(arm.get_end_effector_link()) # part p.pose.position.x = gripper_pose.pose.position.x p.pose.position.y = gripper_pose.pose.position.y p.pose.position.z = gripper_pose.pose.position.z # add part scene.add_box(PICK_OBJECT, p, (0.07, 0.07, 0.1)) rospy.loginfo("Added object to world") # attach object manually arm.attach_object(PICK_OBJECT, arm.get_end_effector_link(), GRIPPER_JOINTS)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object("right_gripper_link", "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("start1") right_arm.go() right_gripper.set_named_target("open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.15 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.0130178 start_pose.pose.position.y = -0.125155 start_pose.pose.position.z = 0.597653 start_pose.pose.orientation.x = 0.0 start_pose.pose.orientation.y = 0.388109 start_pose.pose.orientation.z = 0.0 start_pose.pose.orientation.w = 0.921613 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps #grasps = self.make_grasps(start_pose) #result = False #n_attempts = 0 # repeat until will succeed #while result == False: #result = robot.right_arm.pick("part", grasps) #n_attempts += 1 #print "Attempts: ", n_attempts #rospy.sleep(0.2) rospy.spin() roscpp_shutdown()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('MotionSequence') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying left wam finger poses self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped) self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped) self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Define move group comander for each moveit group left_wam = moveit_commander.MoveGroupCommander('left_wam') right_wam = moveit_commander.MoveGroupCommander('right_wam') left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1') left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2') left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3') right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1') right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2') right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3') left_wam.set_planner_id("PRMstarkConfigDefault") right_wam.set_planner_id("PRMstarkConfigDefault") #left_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_3.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_3.set_planner_id("RRTstarkConfigDefault") # Get the name of the end-effector link left_end_effector_link = left_wam.get_end_effector_link() right_end_effector_link = right_wam.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link)) rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link)) # Allow some leeway in position (meters) and orientation (radians) right_wam.set_goal_position_tolerance(0.01) right_wam.set_goal_orientation_tolerance(0.05) left_wam.set_goal_position_tolerance(0.01) left_wam.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Allow 5 seconds per planning attempt right_wam.set_planning_time(15) left_wam.set_planning_time(25) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Set the reference frame for wam arms left_wam.set_pose_reference_frame(REFERENCE_FRAME) right_wam.set_pose_reference_frame(REFERENCE_FRAME) # 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 = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' bowl_id = 'bowl' pitcher_id = 'pitcher' spoon_id = 'spoon' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(bowl_id) scene.remove_world_object(pitcher_id) scene.remove_world_object(spoon_id) # Remove leftover objects from a previous run scene.remove_attached_object(right_end_effector_link, 'spoon') #right_wam.set_named_target('right_wam_start') #right_wam.go() #rospy.sleep(2) # Closing the hand first # Closing the hand #right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") #right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") #right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") #right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(5) #right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(5) #right_wam_finger_3.execute(right_wam_finger_3.plan()) #rospy.sleep(5) # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = right_end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.02 # Align the object with the gripper (straight out) p.pose.orientation.x = -0.5 p.pose.orientation.y = 0.5 p.pose.orientation.z = -0.5 p.pose.orientation.w = 0.5 # Attach the tool to the end-effector # Set the height of the table off the ground table_ground = 0.5762625 # Set the length, width and height of the table and boxes table_size = [0.90128, 0.381, 0.0238125] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0 table_pose.pose.position.y = 0.847725 table_pose.pose.position.z = table_ground scene.add_box(table_id, table_pose, table_size) # Set the height of the bowl bowl_ground = 0.57816875 bowl_pose = PoseStamped() bowl_pose.header.frame_id = REFERENCE_FRAME bowl_pose.pose.position.x = 0.015 bowl_pose.pose.position.y = 0.847725 bowl_pose.pose.position.z = bowl_ground scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl') # Set the height of the pitcher #pitcher_ground = 0.57816875 #pitcher_pose = PoseStamped() #pitcher_pose.header.frame_id = REFERENCE_FRAME #pitcher_pose.pose.position.x = 0.25 #pitcher_pose.pose.position.y = 0.847725 #pitcher_pose.pose.position.z = pitcher_ground #pitcher_pose.pose.orientation.w = -0.5 #pitcher_pose.pose.orientation.z = 0.707 #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl') # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0.4, 0, 1.0) self.setColor(bowl_id, 0, 0.4, 0.8, 1.0) #self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0) self.sendColors() rospy.sleep(2) start = input("Start left_wam planning ? ") # Set the support surface name to the table object #left_wam.set_support_surface_name(table_id) #right_wam.set_support_surface_name(table_id) # Set the target pose. target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.40363476287 target_pose.pose.position.y = 0.847725 target_pose.pose.position.z = 0.721472317843 target_pose.pose.orientation.x = 0.707 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = -0.707 target_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(target_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) left_wam.shift_pose_target(5, 0, left_end_effector_link) start = input("Left wam starting position ") # Pause for a second # Closing the hand left_wam_finger_1.set_named_target("left_wam_finger_1_grasp") left_wam_finger_2.set_named_target("left_wam_finger_2_grasp") left_wam_finger_3.set_named_target("left_wam_finger_3_grasp") left_wam_finger_1.execute(left_wam_finger_1.plan()) #rospy.sleep(3) left_wam_finger_2.execute(left_wam_finger_2.plan()) #rospy.sleep(3) left_wam_finger_3.execute(left_wam_finger_3.plan()) #rospy.sleep(3) start = input("Left wam hand closing ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("Hold up the Pitcher ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("left_wam into pouring position ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) back_pose = deepcopy(end_pose) end_pose.pose.orientation.x = 0.97773401145 end_pose.pose.orientation.y = 0 end_pose.pose.orientation.z = -0.209726592658 end_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Pour the water ") # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(back_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) end_pose.pose.position.x = end_pose.pose.position.x + 0.1 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Left_wam back to prep position") target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = -0.600350195463908 target_pose.pose.position.y = 0.80576308041 target_pose.pose.position.z = 0.794775212132 target_pose.pose.orientation.x = 3.01203251908e-05 target_pose.pose.orientation.y = 0.705562870053 target_pose.pose.orientation.z = 4.55236739937e-05 target_pose.pose.orientation.w = 0.708647326547 start = input("Start right_wam planning ? ") right_wam.set_start_state_to_current_state() right_wam.set_pose_target(target_pose, right_end_effector_link) right_wam.execute(right_wam.plan()) start = input("right_wam into position. ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6 plan = self.StraightPath(start_pose, intermidiate_pose, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) rospy.sleep(3) # Closing the hand right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(3) right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(3) right_wam_finger_3.execute(right_wam_finger_3.plan()) rospy.sleep(1) scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl') #create a circle path circles = input("How many circles you want the wam to mix ? ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) plan = self.CircularPath(start_pose, circles, right_wam) #execute the circle path right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("Mix the oatmeal ") #put the right_wam back to preparation pose end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose1 = deepcopy(end_pose) intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1 plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("wait for the execution of straight path ") end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose2 = deepcopy(end_pose) intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25 plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("right_wam back into prep position ") #left_wam.shift_pose_target(5, 0, left_end_effector_link) #left_wam.go() #rospy.sleep(2) #left_wam.shift_pose_target(0, -0.05, left_end_effector_link) #left_wam.go() #rospy.sleep(2) # Initialize the grasp pose to the target pose #grasp_pose = target_pose # Generate a list of grasps #grasps = self.make_grasps(grasp_pose, [pitcher_id]) # Publish the grasp poses so they can be viewed in RViz #for grasp in grasps: # self.left_wam_finger_1_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_2_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_3_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class PickAndPlace: def setColor(self, name, r, g, b, a=0.9): color = ObjectColor() color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a self.colors[name] = color def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def setupSence(self): r_tool_size = [0.03, 0.02, 0.18] l_tool_size = [0.03, 0.02, 0.18] ''' #sim table table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=0.75 table_pose.pose.position.y=0.0 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) ''' #real scene table table_pose = PoseStamped() table_pose.header.frame_id = self.reference_frame table_pose.pose.position.x = -0.184 table_pose.pose.position.y = 0.62 table_pose.pose.position.z = self.table_ground + self.table_size[ 2] / 2.0 table_pose.pose.orientation.w = 1.0 self.scene.add_box(self.table_id, table_pose, self.table_size) #left gripper l_p = PoseStamped() l_p.header.frame_id = self.arm_end_effector_link l_p.pose.position.x = 0.00 l_p.pose.position.y = 0.057 l_p.pose.position.z = 0.09 l_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p, l_tool_size) #right gripper r_p = PoseStamped() r_p.header.frame_id = self.arm_end_effector_link r_p.pose.position.x = 0.00 r_p.pose.position.y = -0.057 r_p.pose.position.z = 0.09 r_p.pose.orientation.w = 1 self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p, r_tool_size) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.gripperCtrl = rospy.ServiceProxy( "/two_finger/gripper/gotoPositionUntilTouch", SetPosition) self.m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint", JointAnglesDuration, queue_size=1, latch=True) self.colors = dict() rospy.sleep(1) arm = MoveGroupCommander('arm') cartesian = rospy.get_param('~cartesian', True) self.arm_end_effector_link = arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) self.reference_frame = 'base_link' arm.set_pose_reference_frame(self.reference_frame) arm.set_planning_time(5) #scene planning self.l_id = 'l_tool' self.r_id = 'r_tool' self.table_id = 'table' self.target_id = 'target_object' self.f_target_id = 'receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.scene.remove_world_object(self.f_target_id) self.table_ground = 0.13 self.table_size = [0.9, 0.6, 0.018] self.setupSence() target_size = [0.058, 0.058, 0.19] ''' #target sim target_pose=PoseStamped() target_pose.header.frame_id=self.reference_frame target_pose.pose.position.x=0.6 target_pose.pose.position.y=0.05 target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 self.scene.add_box(self.target_id,target_pose,target_size) ''' f_target_size = [0.2, 0.2, 0.04] #final target f_target_pose = PoseStamped() f_target_pose.header.frame_id = self.reference_frame f_target_pose.pose.position.x = -0.184 + 0.27 f_target_pose.pose.position.y = 0.62 + 0.1 f_target_pose.pose.position.z = self.table_ground + self.table_size[ 2] + f_target_size[2] / 2.0 f_target_pose.pose.orientation.x = 0 f_target_pose.pose.orientation.y = 0 f_target_pose.pose.orientation.z = 0 f_target_pose.pose.orientation.w = 1 self.scene.add_box(self.f_target_id, f_target_pose, f_target_size) #pouring pose pour_pose = f_target_pose pour_pose.pose.position.x -= 0.06 pour_pose.pose.position.y -= 0.12 pour_pose.pose.position.z += 0.15 #pour_pose.pose.position.y+=0.17 pour_pose.pose.orientation.x = -0.5 pour_pose.pose.orientation.y = -0.5 pour_pose.pose.orientation.z = -0.5 pour_pose.pose.orientation.w = 0.5 #set color self.setColor(self.table_id, 0.8, 0, 0, 1.0) self.setColor(self.f_target_id, 0.8, 0.4, 0, 1.0) self.setColor('r_tool', 0.8, 0, 0) self.setColor('l_tool', 0.8, 0, 0) self.setColor(self.target_id, 0, 1, 0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) arm.set_named_target("initial_arm") arm.go() rospy.sleep(5) #j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14] j_ori_state = [ -1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156 ] #j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536] signal = True arm.set_joint_value_target(j_ori_state) arm.go() rospy.sleep(3) #arm.set_joint_value_target(j_trans_state) #arm.go() #rospy.sleep(5) waypoints_1 = [] waypoints_2 = [] start_pick_pose = arm.get_current_pose(self.arm_end_effector_link).pose if cartesian: msg = rospy.wait_for_message('/aruco_single/pose', PoseStamped) target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.pose.position.x = -(msg.pose.position.y) - 0.28 target_pose.pose.position.y = -msg.pose.position.x + 0.81 - 0.072 target_pose.pose.position.z = 1.36 - msg.pose.position.z + 0.1 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = -0 target_pose.pose.orientation.w = 1 self.scene.add_box(self.target_id, target_pose, target_size) self.setColor(self.target_id, 0, 0.8, 0) self.sendColors() #pre_g_pose pre_grasp_pose = PoseStamped() pre_grasp_pose.header.frame_id = self.reference_frame pre_grasp_pose.pose.position = target_pose.pose.position pre_grasp_pose.pose.position.y -= 0.2 pre_grasp_pose.pose.position.z += 0.01 pre_grasp_pose.pose.orientation.x = -0.5 pre_grasp_pose.pose.orientation.y = -0.5 pre_grasp_pose.pose.orientation.z = -0.5 pre_grasp_pose.pose.orientation.w = 0.5 #grasp_pose grasp_pose = PoseStamped() grasp_pose.header.frame_id = self.reference_frame grasp_pose.pose.position = target_pose.pose.position grasp_pose.pose.position.y += 0.025 grasp_pose.pose.orientation.x = -0.5 grasp_pose.pose.orientation.y = -0.5 grasp_pose.pose.orientation.z = -0.5 grasp_pose.pose.orientation.w = 0.5 g_p = PoseStamped() g_p.header.frame_id = self.arm_end_effector_link g_p.pose.position.x = 0.00 g_p.pose.position.y = -0.00 g_p.pose.position.z = 0.025 g_p.pose.orientation.w = 0.707 g_p.pose.orientation.x = 0 g_p.pose.orientation.y = -0.707 g_p.pose.orientation.z = 0 waypoints_1.append(start_pick_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_1.append(deepcopy(grasp_pose.pose)) fraction = 0.0 attempts = 0 maxtries = 300 while fraction != 1 and attempts < maxtries: (plan_1, fraction) = arm.compute_cartesian_path( waypoints_1, 0.01, 0.0, True) attempts += 1 if (attempts % 300 == 0 and fraction != 1): rospy.loginfo("path planning failed with " + str(fraction * 100) + "% success.") signal_pick = False if fraction == 1: rospy.loginfo("path compute successfully with " + str(attempts) + " sttempts.") rospy.loginfo("Arm moving.") rospy.sleep(3) #arm.execute(plan_1) p_pick_1 = plan_1.joint_trajectory.points[-1] #print p arm.set_joint_value_target(p_pick_1.positions) arm.go() rospy.sleep(7) self.gripperCtrl(0) rospy.sleep(3) self.scene.remove_world_object(self.target_id) #p_pick_2=plan_1.joint_trajectory.points[1] #print p #arm.set_joint_value_target(p_pick_2.positions) #arm.go() #rospy.sleep(4) signal_pick = True #p=plan_1.joint_trajectory.points[-1] #print p #arm.set_joint_value_target(p.positions) #arm.go() if signal_pick: start_trans_pose = arm.get_current_pose( self.arm_end_effector_link).pose waypoints_2.append(start_trans_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_2.append(deepcopy(pour_pose.pose)) fraction_2 = 0.0 attempts_2 = 0 maxtries_2 = 300 while fraction_2 != 1 and attempts_2 < maxtries_2: (plan_2, fraction_2) = arm.compute_cartesian_path( waypoints_2, 0.01, 0.0, True) attempts_2 += 1 if (attempts_2 % 300 == 0 and fraction_2 != 1): rospy.loginfo("path planning failed with " + str(fraction_2 * 100) + "% success.") if fraction_2 == 1: #signal=False rospy.loginfo("path compute successfully with " + str(attempts_2) + " sttempts.") rospy.loginfo("Arm moving.") p_trans = plan_2.joint_trajectory.points[-1] #print p arm.set_joint_value_target(p_trans.positions) arm.go() rospy.sleep(3) j_pre_pour_state = arm.get_current_joint_values() j_pour_state = j_pre_pour_state j_pour_state[6] -= (1.57 + 0.26) arm.set_joint_value_target(j_pour_state) arm.go() print "shaking balabala" rospy.sleep(1) print "shaking balabala" rospy.sleep(1) print "shaking balabala" rospy.sleep(1) ''' arm.set_joint_value_target(j_pre_pour_state) arm.go() rospy.sleep(6) arm.set_joint_value_target(p_pick_1.positions) arm.go() rospy.sleep(7) self.gripperCtrl(255) arm.set_joint_value_target(j_ori_state) arm.go() rospy.sleep(5) ''' ''' #rospy.sleep(2) #arm.set_pose_target(pre_grasp_pose,self.arm_end_effector_link) #arm.go() #rospy.sleep(15) start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose waypoints_1=[] waypoints_2=[] if cartesian: waypoints_1.append(start_pick_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_1.append(deepcopy(grasp_pose.pose)) fraction=0.0 attempts=0 maxtries=300 while fraction!=1 and attempts<maxtries: (plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True) attempts+=1 if (attempts %300==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") signal_1=False if fraction==1: #signal=False rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.") rospy.loginfo("Arm moving.") #print plan_1.joint_trajectory.points arm.execute(plan_1) #self.gripperCtrl(0) rospy.sleep(7) self.gripperCtrl(0) self.scene.attach_box(self.arm_end_effector_link,self.target_id,g_p,target_size) #signal=False start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose waypoints_2.append(start_trans_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_2.append(deepcopy(pour_pose.pose)) fraction_2=0.0 attempts_2=0 maxtries_2=300 while fraction_2!=1 and attempts_2<maxtries_2: (plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True) attempts_2+=1 if (attempts_2 %300==0 and fraction_2!=1): rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.") #signal_1=False if fraction_2==1: #signal=False rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.") rospy.loginfo("Arm moving.") #print plan_1.joint_trajectory.points arm.execute(plan_2) #self.gripperCtrl(0) rospy.sleep(7) j_state=arm.get_current_joint_values() j_state[6]+=(1.57+0.26) arm.set_joint_value_target(j_state) #if fraction==1.0: #rospy.loginfo("path compute successfully. Move the arm.") #self.m_pub_m2j.publish(place) #arm.execute(plan_1) #for i in range(len(plan_1.joint_trajectory.points)): #p=plan_1.joint_trajectory.points[-1] #place=JointAnglesDuration(JointAngles(p.positions[0],p.positions[1],p.positions[2],p.positions[3],p.positions[4],p.positions[5],p.positions[6]),rospy.Duration(5)) #self.m2j.publish(place) #self.gripperCtrl(0) #rospy.sleep(2) #self.gripperCtrl(255) #rospy.sleep(2) ''' #remove and shut down #self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') #self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
kuka = KukaRobot() ur10e = Ur10eRobot() # this amount of sleep is a MUST rospy.sleep(2) # add object and table to the planning scene kuka_table_size = [1.0, 0.4, 1.0] kuka_table_pose_list = [0.0, 0.4, kuka_table_size[2] / 2.0] kuka_table_name = "kuka_table" kuka_table_pose = PoseStamped() kuka_table_pose.header.frame_id = "world" kuka_table_pose.header.stamp = rospy.Time.now() kuka_table_pose.pose.position.x = kuka_table_pose_list[0] kuka_table_pose.pose.position.y = kuka_table_pose_list[1] kuka_table_pose.pose.position.z = kuka_table_pose_list[2] scene.add_box(name=kuka_table_name, pose=kuka_table_pose, size=kuka_table_size) rospy.sleep(1) kuka_object_size = [0.2, 0.2, 0.2] kuka_object_pose_list = [ kuka_table_pose_list[0], kuka_table_pose_list[1], kuka_table_size[2] + kuka_object_size[2] / 2 ] kuka_object_name = "kuka_object" kuka_object_pose = PoseStamped() kuka_object_pose.header.frame_id = "world" kuka_object_pose.header.stamp = rospy.Time.now() kuka_object_pose.pose.position.x = kuka_object_pose_list[0] kuka_object_pose.pose.position.y = kuka_object_pose_list[1] kuka_object_pose.pose.position.z = kuka_object_pose_list[2] scene.add_box(name=kuka_object_name,
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() # 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(5) # 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 = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # 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) # Set the height of the table off the ground table_ground = 0.30 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.30 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.34 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object #right_arm.set_support_surface_name(table_id) # 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 = 0.25 place_pose.pose.position.y = -0.18 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # 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 success = False n_attempts = 0 # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if success: success = False n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_place_attempts: for place in places: success = right_arm.place(target_id, place) if success: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if not success: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to 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) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
group.go() # Add Object to Planning Scene rospy.loginfo("Planning Scene Settings") scene = PlanningSceneInterface() rospy.sleep(2) # Waiting for PlanningSceneInterface box_pose = PoseStamped() box_pose.header.frame_id = group.get_planning_frame() box_pose.pose.position.x = 0.3 box_pose.pose.position.y = -0.3 box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Pose Target 2 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5 pose_target_2.position.z = 0.15 pose_target_2.orientation.x = 0.0 pose_target_2.orientation.y = -0.707 pose_target_2.orientation.z = 0.0 pose_target_2.orientation.w = 0.707
class WorldManagerServer: def __init__(self): rospy.init_node('world_manager_node') moveit_commander.roscpp_initialize(sys.argv) # wait for moveit to come up self.scene = PlanningSceneInterface() self.tf_manager = TFManager() self.clear_planning_scene_service = rospy.Service( "/world_manager/clear_objects", std_srvs.srv.Empty, self.clear_objects_cb) self.add_box = rospy.Service("/world_manager/add_box", world_manager_msgs.srv.AddBox, self.add_box_cb) self.add_mesh = rospy.Service("/world_manager/add_mesh", world_manager_msgs.srv.AddMesh, self.add_mesh_cb) self.add_tf_service = rospy.Service("/world_manager/add_tf", world_manager_msgs.srv.AddTF, self.add_tf_cb) self.add_walls_service = rospy.Service("/world_manager/add_walls", std_srvs.srv.Empty, self.add_walls_cb) rospy.sleep(1.0) self.clear_objects_cb(request=None) self.add_walls_cb(request=None) rospy.loginfo("World Manager Node is Up and Running") def add_mesh_cb(self, request): so = request.scene_object # add the tf self.tf_manager.add_tf(so.object_name, so.pose_stamped) # remove the old completion if it is there self.scene.remove_world_object(so.object_name) # add the new object to the planning scene self.scene.add_mesh(so.object_name, so.pose_stamped, so.mesh_filepath) return [] def add_box_cb(self, request): # type: (world_manager_msgs.srv.AddBoxRequest) -> [] box = request.scene_box # type: box -> world_manager_msgs.msg.SceneBox # add the tf self.tf_manager.add_tf(box.object_name, box.pose_stamped) # remove the old box if it is there self.scene.remove_world_object(box.object_name) # add the new box to the planning scene self.scene.add_box(name=box.object_name, pose=box.pose_stamped, size=(box.edge_length_x, box.edge_length_y, box.edge_length_z)) rospy.loginfo("Added box {}".format(box.object_name)) return [] def add_tf_cb(self, request): self.tf_manager.add_tf(request.frame_name, request.pose_stamped) return [] def clear_objects_cb(self, request): body_names = self.scene.get_known_object_names() walls = rospy.get_param('walls') rospy.loginfo("Clearing objects: {}".format(body_names)) for body_name in body_names: if not body_name in walls: rospy.loginfo("Removing object: {}".format(body_name)) self.scene.remove_world_object(body_name) self.tf_manager.clear_tfs() return [] def add_walls_cb(self, request): walls = rospy.get_param('walls') for wall_params in walls: rospy.loginfo("Adding wall " + str(wall_params)) self._add_wall(wall_params) return [] def _add_wall(self, wall_params): name = wall_params["name"] x_thickness = wall_params["x_thickness"] y_thickness = wall_params["y_thickness"] z_thickness = wall_params["z_thickness"] x = wall_params["x"] y = wall_params["y"] z = wall_params["z"] qx = wall_params["qx"] qy = wall_params["qy"] qz = wall_params["qz"] qw = wall_params["qw"] frame_id = wall_params["frame_id"] back_wall_pose = geometry_msgs.msg.PoseStamped() back_wall_pose.header.frame_id = '/' + frame_id wall_dimensions = [x_thickness, y_thickness, z_thickness] back_wall_pose.pose.position = geometry_msgs.msg.Point(**{ 'x': x, 'y': y, 'z': z }) back_wall_pose.pose.orientation = geometry_msgs.msg.Quaternion(**{ 'x': qx, 'y': qy, 'z': qz, 'w': qw }) self.scene.add_box(name, back_wall_pose, wall_dimensions)
class PickAndPlace: def setColor(self,name,r,g,b,a=0.9): color=ObjectColor() color.id=name color.color.r=r color.color.g=g color.color.b=b color.color.a=a self.colors[name]=color def sendColors(self): p=PlanningScene() p.is_diff=True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def setupSence(self): #box1_size=[0.1,0.05,0.03] box2_size=[0.05,0.05,0.1] r_tool_size=[0.03,0.01,0.06] l_tool_size=[0.03,0.01,0.06] table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=0.75 table_pose.pose.position.y=0.0 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=self.arm_end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.04 l_p.pose.position.z=0.04 l_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=self.arm_end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.04 r_p.pose.position.z=0.04 r_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,'r_tool',r_p,r_tool_size) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) #self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition) #self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True) self.colors=dict() rospy.sleep(1) arm=MoveGroupCommander('arm') cartesian=rospy.get_param('~cartesian',True) #gripper=MoveGroupCommander('gripper') self.arm_end_effector_link=arm.get_end_effector_link() #gripper_end_effector_link=gripper.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) #gripper.set_goal_position_tolerance(0.01) #gripper.set_goal_orientation_tolerance(0.05) #gripper.allow_replanning(True) self.reference_frame='base_link' arm.set_pose_reference_frame(self.reference_frame) #gripper.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #gripper.set_planning_time(5) #scene planning self.table_id='table' self.box2_id='box2' self.target_id='target_object' #scene.remove_world_object(box1_id) self.scene.remove_world_object(self.box2_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target_id) self.table_ground=0.5 self.table_size=[0.5,1,0.01] self.setupSence() target_size=[0.05,0.05,0.1] #target target_pose=PoseStamped() target_pose.header.frame_id=self.reference_frame target_pose.pose.position.x=0.6 target_pose.pose.position.y=0.05 target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=0 target_pose.pose.orientation.w=1 self.scene.add_box(self.target_id,target_pose,target_size) #grasp g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y=-0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 #set color self.setColor(self.table_id,0.8,0,0,1.0) #self.setColor(self.box2_id,0.8,0.4,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor(self.target_id,0,1,0) self.sendColors() #motion planning arm.set_named_target("slight") arm.go() rospy.sleep(2) arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) start_pose=arm.get_current_pose(self.arm_end_effector_link).pose rospy.sleep(2) grasp_pose=target_pose grasp_pose.pose.position.x-=0.15 #grasp_pose.pose.position.z= grasp_pose.pose.orientation.x=0 grasp_pose.pose.orientation.y=0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 waypoints_1=[] waypoints_2=[] if cartesian: waypoints_1.append(start_pose) waypoints_1.append(deepcopy(grasp_pose.pose)) #waypoints_1.append(deepcopy(grasp_pose.pose)) fraction=0.0 #maxtries=300 attempts=0 #arm.set_start_state__1to_current_state() #plan the cartesian path connecting waypoints #while fraction<1.0 and attempts<maxtries: while fraction!=1: (plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True) attempts+=1 if (attempts %300==0 and fraction!=1.0): rospy.loginfo("path planning failed with " + str(attempts)+" attempts") if KeyboardInterrupt: break rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.") rospy.loginfo("Arm moving.") arm.execute(plan_1) rospy.sleep(6) #remove and shut down #self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') #self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class PickAndPlace: def setColor(self,name,r,g,b,a=0.9): color=ObjectColor() color.id=name color.color.r=r color.color.g=g color.color.b=b color.color.a=a self.colors[name]=color def sendColors(self): p=PlanningScene() p.is_diff=True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def add_point(self,traj, time, positions, velocities=None): point= trajectory_msgs.msg.JointTrajectoryPoint() point.positions= copy.deepcopy(positions) if velocities is not None: point.velocities= copy.deepcopy(velocities) point.time_from_start= rospy.Duration(time) traj.points.append(point) def FollowQTraj(self,q_traj, t_traj): assert(len(q_traj)==len(t_traj)) #Insert current position to beginning. if t_traj[0]>1.0e-2: t_traj.insert(0,0.0) q_traj.insert(0,self.Q(arm=arm)) self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj) #self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj #print traj #self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj)) def QTrajToDQTraj(self,q_traj, t_traj): dof= len(q_traj[0]) #Modeling the trajectory with spline. splines= [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj= [] for t in t_traj: dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) #print dq_traj return dq_traj def JointNames(self): #0arm= 0 return self.joint_names[0] def ROSGetJTP(self,q,t,dq=None): jp= trajectory_msgs.msg.JointTrajectoryPoint() jp.positions= q jp.time_from_start= rospy.Duration(t) if dq is not None: jp.velocities= dq return jp def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None): assert(len(q_traj)==len(t_traj)) if dq_traj is not None: (len(dq_traj)==len(t_traj)) #traj= trajectory_msgs.msg.JointTrajectory() self.traj.joint_names= joint_names if dq_traj is not None: self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)] else: self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)] self.traj.header.stamp= rospy.Time.now() #print self.traj return self.traj def SmoothQTraj(self,q_traj): if len(q_traj)==0: return q_prev= np.array(q_traj[0]) q_offset= np.array([0]*len(q_prev)) for q in q_traj: q_diff= np.array(q) - q_prev for d in range(len(q_prev)): if q_diff[d]<-math.pi: q_offset[d]+=1 elif q_diff[d]>math.pi: q_offset[d]-=1 q_prev= copy.deepcopy(q) q[:]= q+q_offset*2.0*math.pi def add_target(self,f_target_pose,f_target_size,frame,x,y,o1,o2,o3,o4): f_target_pose.header.frame_id=frame f_target_pose.pose.position.x=x f_target_pose.pose.position.y=y f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0 f_target_pose.pose.orientation.x=o1 f_target_pose.pose.orientation.y=o2 f_target_pose.pose.orientation.z=o3 f_target_pose.pose.orientation.w=o4 #self.scene.add_box(f_target_id,f_target_pose,f_target_size) def cts(self,start_pose,end_pose,maxtries,exe_signal=False): waypoints=[] fraction=0.0 attempts=0 #maxtries_z=300 waypoints.append(start_pose) waypoints.append(end_pose) while fraction!=1 and attempts<maxtries: (plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True) attempts+=1 if (attempts %maxtries==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") return 0,0,0 continue elif fraction==1: rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.") #print(plan.joint_trajectory.points[-5].velocities) #for i in range(len(plan.joint_trajectory.points)): #plan.joint_trajectory.points[i].velocities=[0,0,0,0,0,0,0] #plan = self.arm.retime_trajectory(self.robot.get_current_state(), plan, 1.0) rospy.sleep(3) if exe_signal: q_traj=[self.arm.get_current_joint_values()] t_traj=[0.0] for i in range(2,len(plan.joint_trajectory.points)): q_traj.append(plan.joint_trajectory.points[i].positions) t_traj.append(t_traj[-1]+0.03) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) #self.sub_jpc.publish(plan) #self.arm.execute(plan) end_joint_state=plan.joint_trajectory.points[-1].positions signal=1 return plan,end_joint_state, signal # shaking function: # freq : shaking freqence # times : shaking time per action def shaking(self,initial_state,start_joint_state,end_joint_state,freq,times): q_traj=[initial_state] t_traj=[0.0] for i in range(times): q_traj.append(end_joint_state) t_traj.append(t_traj[-1]+0.5/freq) q_traj.append(start_joint_state) t_traj.append(t_traj[-1]+0.5/freq) q_traj.append(initial_state) t_traj.append(t_traj[-1]+0.5/freq) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(6) def setupSence(self): r_tool_size=[0.03,0.02,0.18] l_tool_size=[0.03,0.02,0.18] #real scene table table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=-0.0 table_pose.pose.position.y=0.65 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=self.arm_end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.057 l_p.pose.position.z=0.09 l_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=self.arm_end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.057 r_p.pose.position.z=0.09 r_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size) def grasp_pose(self,x,y,z,r,theta): g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=x+r*math.sin(theta) g_p.pose.position.y=y-r*math.cos(theta) g_p.pose.position.z=z+0.05 g_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) g_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta)) g_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) g_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta)) return g_p def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene=PlanningSceneInterface() pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition) #self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True) self.colors=dict() rospy.sleep(1) self.robot=moveit_commander.robot.RobotCommander() self.arm=MoveGroupCommander('arm') self.arm.allow_replanning(True) cartesian=rospy.get_param('~cartesian',True) self.arm_end_effector_link=self.arm.get_end_effector_link() self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.025) self.arm.allow_replanning(True) self.reference_frame='base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.set_planning_time(5) #shaking self.joint_names= [[]] self.joint_names[0]= rospy.get_param('controller_joint_names') self.traj= trajectory_msgs.msg.JointTrajectory() self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10) #scene planning self.l_id='l_tool' self.r_id='r_tool' self.table_id='table' self.target1_id='target1_object' self.target2_id='target2_object' self.target3_id='target3_object' self.target4_id='target4_object' self.f_target_id='receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target1_id) self.scene.remove_world_object(self.target2_id) self.scene.remove_world_object(self.target3_id) self.scene.remove_world_object(self.target4_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.scene.remove_world_object(self.f_target_id) self.table_ground=0.13 self.table_size=[0.9,0.6,0.018] self.setupSence() target1_size=[0.035,0.035,0.19] target2_size=target1_size target3_size=target1_size target4_size=target1_size self.f_target_size=[0.2,0.2,0.04] f_target_pose=PoseStamped() pre_pour_pose=PoseStamped() target1_pose=PoseStamped() target2_pose=PoseStamped() target3_pose=PoseStamped() target4_pose=PoseStamped() joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')] joint_names= rospy.get_param('controller_joint_names') traj= trajectory_msgs.msg.JointTrajectory() traj.joint_names= joint_names #final target #self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1) self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.3,0.6,0,0,0,1) self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size) #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1) #pouring pose pour_pose=f_target_pose pour_pose.pose.position.x-=0.06 pour_pose.pose.position.y-=0.12 pour_pose.pose.position.z+=0.15 pour_pose.pose.orientation.x=-0.5 pour_pose.pose.orientation.y=-0.5 pour_pose.pose.orientation.z=-0.5 pour_pose.pose.orientation.w=0.5 #attach_pose g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y=-0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 #set color self.setColor(self.target1_id,0.8,0,0,1.0) self.setColor(self.target2_id,0.8,0,0,1.0) self.setColor(self.target3_id,0.8,0,0,1.0) self.setColor(self.target4_id,0.8,0,0,1.0) self.setColor(self.f_target_id,0.8,0.3,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) self.arm.set_named_target("initial_arm") self.arm.go() rospy.sleep(5) #j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156] #j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135] j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393] signal= True self.arm.set_joint_value_target(j_ori_state) self.arm.go() rospy.sleep(3) #target localization tar_num=4 maxtries=300 r=0.16 #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) self.add_target(target1_pose,target1_size,self.reference_frame,-0.25,0.8,0,0,0,1) self.scene.add_box(self.target1_id,target1_pose,target1_size) self.add_target(target2_pose,target2_size,self.reference_frame,-0.12,0.87,0,0,0,1) self.scene.add_box(self.target2_id,target2_pose,target2_size) self.add_target(target3_pose,target3_size,self.reference_frame,0.02,0.88,0,0,0,1) self.scene.add_box(self.target3_id,target3_pose,target3_size) self.add_target(target4_pose,target4_size,self.reference_frame,0.12,0.81,0,0,0,1) self.scene.add_box(self.target4_id,target4_pose,target4_size) #path planning #input : target pose #output : policy and execute #cts : output : plan, end_joint_state, signal #m=0 for i in range(tar_num): #grasp target if i==0: target_pose=target1_pose.pose target_id=self.target1_id target_size=target1_size elif i==1: target_pose=target2_pose.pose target_id=self.target2_id target_size=target2_size elif i==2: target_pose=target3_pose.pose target_id=self.target3_id target_size=target3_size elif i==3: target_pose=target4_pose.pose target_id=self.target4_id target_size=target4_size #grasp : #-- if x>0: theta range () initial_angle=int(np.arctan(abs(target_pose.position.x/target_pose.position.y))*100) for theta in range(initial_angle,157,8): #m+=1 if target_pose.position.x>0: theta*=-1 #if m%2==0: theta*=-1 theta/=100.0 start_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose grasp_pose=self.grasp_pose(target_pose.position.x,target_pose.position.y,target_pose.position.z,r,theta) #pick up if cartesian: plan_1, end_joint_state_1,signal_1=self.cts(start_pose,grasp_pose.pose,maxtries,True) if signal_1==0: continue break rospy.sleep(2) self.gripperCtrl(0) rospy.sleep(2) self.gripperCtrl(255) rospy.sleep(2) self.arm.set_start_state_to_current_state() back_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose #rospy.sleep(10) if cartesian: plan_2, end_joint_state_2,signal_2=self.cts(back_pose,start_pose,maxtries,True) #shaking """ amp= freq= r_angle= start_pick_pose=self.arm.get_current_pose(self.arm_end_effector_link).pose start_joint_state=self.arm.get_current_joint_values() shift_x_pose=deepcopy(start_pick_pose) shift_z_pose=deepcopy(start_pick_pose) shift_x_pose.position.x+=0.04 shift_z_pose.position.z-=0.04 #exe_sig=False if cartesian: plan_z,end_joint_state_z,signal_z=self.cts(start_pick_pose,shift_z_pose,300) plan_x,end_joint_state_x,signal_x=self.cts(start_pick_pose,shift_x_pose,300) #shaking process #signal_x=False shake_times=0 #self.freq=3.0 signal=True while signal: if (shake_times%3!=0 and signal_x) or (shake_times%3==0 and not signal_z): self.shaking(start_joint_state,end_joint_state_x,3,3) elif shake_times%3==0 and signal_z: self.shaking(start_joint_state,end_joint_state_z,3,1) shake_times+=1 rospy.loginfo("shaking times : "+str(shake_times)+ " .") if shake_times==10: signal=False """ #remove and shut down self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
# p.pose.position.x = 0.8 # p.pose.position.y = -0.15 # p.pose.position.z = 0.85 # p.pose.orientation.w = 1.0 #scene.add_box("pole", p, (0.3, 0.05, 1.0)) # p.pose.position.x = 0.79 # p.pose.position.y = 0.15 # p.pose.position.z = 0.70 # add_result = scene.add_box("table", p, (0.6, 0.6, 0.15)) # print add_result p.pose.position.x = 0.6 p.pose.position.y = 0.18 p.pose.position.z = 0.7 scene.add_box("part1", p, (0.05, 0.05, 0.1 )) robot.right_arm.set_named_target("right_pregrasp") robot.right_arm.go() rospy.sleep(3) gripper.gripper_open() rospy.sleep(3) # pick an object robot.right_arm.pick("part1") rospy.sleep(1) gripper.gripper_close() rospy.sleep(1) robot.right_arm.set_named_target("right_normal")
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = 0.0 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (9, 9, 0.02)) return p.pose def _add_grasp_block_(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0.05 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.1, 0.1, 0.1)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg) def _add_objects(self): """ Here add all the objects that you want to add to the _scene """ self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
class PigeonPickAndPlace: #Class constructor (parecido com java, inicializa o que foi instanciado) def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016) self._arm_group = rospy.get_param('~arm', 'arm_move_group') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) # Create planning scene where we will add the objects etc. self._scene = PlanningSceneInterface() # Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene (remove the old objects: self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: # TODO get the position of the detected object self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name) rospy.sleep(1.0) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Pick object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') def __del__(self): # Clean the scene self._scene.remove_world_object(self._grasp_object_name) def _add_object_grasp(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() #pose p.header.stamp = rospy.Time.now() rospy.loginfo(self._robot.get_planning_frame()) p.pose.position.x = 0.11 # 0.26599 # antigo = 0.75 - 0.01 p.pose.position.y = -0.31 # -0.030892 #antigo = 0.25 - 0.01 p.pose.position.z = 0.41339 #antigo = 1.00 + (0.3 + 0.03) / 2.0 #p.pose.orientation.x = 0.0028925 #p.pose.orientation.y = -0.0019311 #p.pose.orientation.z = -0.71058 #p.pose.orientation.w = 0.70361 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/, # using the measure tape tool from meshlab. # The box is the bounding box of the lego cylinder. # The values are taken from the cylinder base diameter and height. # the size is length (x),thickness(y),height(z) # I don't know if the detector return this values of object self._scene.add_box(name, p, (0.032, 0.016, 0.020)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps #rospy.logerr('Generated: %f grasps:' % grasps.size) # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) #goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 5.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 10 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_object_grasp, 0.016) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg)
class PickAndPlaceServer(object): def __init__(self): rospy.loginfo("Initalizing PickAndPlaceServer...") self.sg = SphericalGrasps() rospy.loginfo("Connecting to pickup AS") self.pickup_ac = SimpleActionClient('/pickup', PickupAction) self.pickup_ac.wait_for_server() rospy.loginfo("Succesfully connected.") rospy.loginfo("Connecting to place AS") self.place_ac = SimpleActionClient('/place', PlaceAction) self.place_ac.wait_for_server() rospy.loginfo("Succesfully connected.") self.scene = PlanningSceneInterface() rospy.loginfo("Connecting to /get_planning_scene service") self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.scene_srv.wait_for_service() rospy.loginfo("Connected.") rospy.loginfo("Connecting to clear octomap service...") self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty) self.clear_octomap_srv.wait_for_service() rospy.loginfo("Connected!") # Get the object size self.object_height = rospy.get_param('~object_height') self.object_width = rospy.get_param('~object_width') self.object_depth = rospy.get_param('~object_depth') # Get the links of the end effector exclude from collisions self.links_to_allow_contact = rospy.get_param( '~links_to_allow_contact', None) if self.links_to_allow_contact is None: rospy.logwarn( "Didn't find any links to allow contacts... at param ~links_to_allow_contact" ) else: rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact)) self.pick_as = SimpleActionServer('/pickup_pose', PickUpPoseAction, execute_cb=self.pick_cb, auto_start=False) self.pick_as.start() self.place_as = SimpleActionServer('/place_pose', PickUpPoseAction, execute_cb=self.place_cb, auto_start=False) self.place_as.start() def pick_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.grasp_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.pick_as.set_aborted(p_res) else: self.pick_as.set_succeeded(p_res) def place_cb(self, goal): """ :type goal: PickUpPoseGoal """ error_code = self.place_object(goal.object_pose) p_res = PickUpPoseResult() p_res.error_code = error_code if error_code != 1: self.place_as.set_aborted(p_res) else: self.place_as.set_succeeded(p_res) def wait_for_planning_scene_object(self, object_name='part'): rospy.loginfo("Waiting for object '" + object_name + "'' to appear in planning scene...") gps_req = GetPlanningSceneRequest() gps_req.components.components = gps_req.components.WORLD_OBJECT_NAMES part_in_scene = False while not rospy.is_shutdown() and not part_in_scene: # This call takes a while when rgbd sensor is set gps_resp = self.scene_srv.call(gps_req) # check if 'part' is in the answer for collision_obj in gps_resp.scene.world.collision_objects: if collision_obj.id == object_name: part_in_scene = True break else: rospy.sleep(1.0) rospy.loginfo("'" + object_name + "'' is in scene!") def grasp_object(self, object_pose): rospy.loginfo("Removing any previous 'part' object") self.scene.remove_attached_object("arm_tool_link") self.scene.remove_world_object("part") self.scene.remove_world_object("table") rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) rospy.sleep(2.0) # Removing is fast rospy.loginfo("Adding new 'part' object") rospy.loginfo("Object pose: %s", object_pose.pose) #Add object description in scene self.scene.add_box( "part", object_pose, (self.object_depth, self.object_width, self.object_height)) rospy.loginfo("Second%s", object_pose.pose) table_pose = copy.deepcopy(object_pose) #define a virtual table below the object table_height = object_pose.pose.position.z - self.object_width / 2 table_width = 1.8 table_depth = 0.5 table_pose.pose.position.z += -( 2 * self.object_width) / 2 - table_height / 2 table_height -= 0.008 #remove few milimeters to prevent contact between the object and the table self.scene.add_box("table", table_pose, (table_depth, table_width, table_height)) # # We need to wait for the object part to appear self.wait_for_planning_scene_object() self.wait_for_planning_scene_object("table") # compute grasps possible_grasps = self.sg.create_grasps_from_object_pose(object_pose) self.pickup_ac goal = createPickupGoal("arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact) rospy.loginfo("Sending goal") self.pickup_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.pickup_ac.wait_for_result() result = self.pickup_ac.get_result() rospy.logdebug("Using torso result: " + str(result)) rospy.loginfo("Pick result: " + str(moveit_error_dict[result.error_code.val])) return result.error_code.val def place_object(self, object_pose): rospy.loginfo("Clearing octomap") self.clear_octomap_srv.call(EmptyRequest()) possible_placings = self.sg.create_placings_from_object_pose( object_pose) # Try only with arm rospy.loginfo("Trying to place using only arm") goal = createPlaceGoal(object_pose, possible_placings, "arm", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.loginfo(str(moveit_error_dict[result.error_code.val])) if str(moveit_error_dict[result.error_code.val]) != "SUCCESS": rospy.loginfo("Trying to place with arm and torso") # Try with arm and torso goal = createPlaceGoal(object_pose, possible_placings, "arm_torso", "part", self.links_to_allow_contact) rospy.loginfo("Sending goal") self.place_ac.send_goal(goal) rospy.loginfo("Waiting for result") self.place_ac.wait_for_result() result = self.place_ac.get_result() rospy.logerr(str(moveit_error_dict[result.error_code.val])) # print result rospy.loginfo("Result: " + str(moveit_error_dict[result.error_code.val])) rospy.loginfo("Removing previous 'part' object") self.scene.remove_world_object("part") return result.error_code.val
rospy.loginfo("Cleaning world objects") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene p = PoseStamped() p.header.frame_id = '/base_link' p.header.stamp = rospy.Time.now() p.pose.position.x = 0.6 p.pose.position.y = 0.0 p.pose.position.z = 0.65 p.pose.orientation.w = 1.0 scene.add_box("table", p, (0.5, 1.5, 0.9)) p.pose.position.x = 0.4 p.pose.position.y = -0.3 p.pose.position.z = 1.15 angle = radians(80) # angles are expressed in radians quat = quaternion_from_euler(0.0, 0.0, angle) # roll, pitch, yaw p.pose.orientation = Quaternion(*quat.tolist()) #scene.add_box("part", p, (0.04, 0.04, 0.1)) scene.add_box("part", p, (0.03, 0.03, 0.1)) rospy.loginfo("Added object to world") rospy.sleep(1)
#move(right, pr) move(left, pl) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("bowl") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.4 p.pose.position.y = -0.9 p.pose.position.z = -0.4 scene.add_box("table", p, (1.6, 0.8, 0.4)) # p.pose.position.x = 0.8 # p.pose.position.y = 0.0 # p.pose.position.z = -0.02 # scene.add_box("bowl", p, (0.8, 0.01, 0.05)) #scene.remove_world_object("part") rospy.sleep(1) print ("Standby at initial position") # camera # open camera # rh_cam=CameraController("right_hand_camera") # lh_cam=CameraController("left_hand_camera") # h_cam=CameraController("head_camera")
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) # initialise moveit rospy.init_node('lm_move', anonymous=True) # create a node arm = moveit_commander.MoveGroupCommander("right_manipulator") # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'rightbase_link' arm.set_pose_reference_frame(reference_frame) # 控制机械臂先回到初始化位置 # arm.set_named_target('right_up') # arm.go() # rospy.sleep(5) wall_size = [2, 0.05, 2] wall_pose = PoseStamped() wall_pose.header.frame_id = 'world' wall_pose.pose.position.x = 0 wall_pose.pose.position.y = -0.20 wall_pose.pose.position.z = 1 wall_pose.pose.orientation.w = 0 # 将桌子设置成红色,两个box设置成橙色 self.setColor('wall', 255, 255, 255, 1.0) # 将场景中的颜色设置发布 self.sendColors() scene.add_box('wall', wall_pose, wall_size) Pose_goal = PoseStamped() Pose_goal.header.frame_id = 'rightbase_link' Pose_goal.pose.position.x = -0.234666801624 # red line 0.2 0.2 Pose_goal.pose.position.y = 0.463673741637 # green line 0.15 0.15 Pose_goal.pose.position.z = 0.23632011805 # blue line # 0.35 0.6 Pose_goal.pose.orientation.x = -0.223798960073 Pose_goal.pose.orientation.y = -0.726641725375 Pose_goal.pose.orientation.z = -0.586249390912 Pose_goal.pose.orientation.w = 0.279673881636 arm.set_pose_target(Pose_goal) arm.go(True) rospy.sleep(3) arm.set_named_target('right_up') arm.go() rospy.sleep(3) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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() # 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 10 seconds per planning attempt right_arm.set_planning_time(10) # 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 = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # 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_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # 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 = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # 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 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break 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.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveitMoveObstacle(object): """move obstacle """ def __init__(self): # 设置参考坐标系 self.reference_frame = 'base_link' # 初始化场景 self.scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 移除已存在障碍物 self.scene.remove_world_object() # 添加障碍物 self.add_obstacles() def add_obstacles(self): """ 在场景中增加障碍物 """ # 在场景中添加一个立方体 self.box1_id = 'box1' self.box1_size = [0.1, 0.5, 0.2] self.box1_pose = PoseStamped() self.box1_pose.header.frame_id = self.reference_frame self.box1_pose.pose.position.x = 0.5 self.box1_pose.pose.position.y = 0.6 self.box1_pose.pose.position.z = 0.3 self.box1_pose.pose.orientation.w = 1.0 self.scene.add_box(self.box1_id, self.box1_pose, self.box1_size) self.setColor(self.box1_id, 0.8, 0.4, 0, 1.0) # 在场景中添加一个桌子 self.desk1_id = 'desk1' self.desk1_size = [3, 3, 0.1] self.desk1_pose = PoseStamped() self.desk1_pose.header.frame_id = self.reference_frame self.desk1_pose.pose.position.x = 0.0 self.desk1_pose.pose.position.y = 0.0 self.desk1_pose.pose.position.z = -0.25 self.desk1_pose.pose.orientation.w = 1.0 self.scene.add_box(self.desk1_id, self.desk1_pose, self.desk1_size) self.setColor(self.desk1_id, 0.1, 0.5, 0.1, 1.0) # 在桌子上添加一个底座 self.base1_id = 'base1' self.base1_size = [0.2, 0.2, 0.2] self.base1_pose = PoseStamped() self.base1_pose.header.frame_id = self.reference_frame self.base1_pose.pose.position.x = 0.0 self.base1_pose.pose.position.y = 0.0 self.base1_pose.pose.position.z = -0.1 self.base1_pose.pose.orientation.w = 1.0 self.scene.add_box(self.base1_id, self.base1_pose, self.base1_size) self.setColor(self.base1_id, 0.5, 0.5, 0, 1.0) # 发送场景消息 self.sendColors() rospy.sleep(1) def setColor(self, name, r, g, b, a=0.9): """ 设置场景物体的颜色 """ # 初始化moveit颜色对象 color = ObjectColor() # 设置颜色值 color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a # 更新颜色字典 self.colors[name] = color def sendColors(self): """ 将颜色设置发送并应用到moveit场景当中 """ # 初始化规划场景对象 p = PlanningScene() # 需要设置规划场景是否有差异 p.is_diff = True # 从颜色字典中取出颜色设置 for color in self.colors.values(): p.object_colors.append(color) # 发布场景物体颜色设置 self.scene_pub.publish(p) def move_obstacle(self): """移动障碍物 """ # 在场景中增加物体 for i in range(100): self.scene.add_box(self.box1_id, self.box1_pose, self.box1_size) self.scene.add_box(self.desk1_id, self.desk1_pose, self.desk1_size) self.scene.add_box(self.base1_id, self.base1_pose, self.base1_size) self.sendColors() rospy.sleep(0.1) # 连续移动障碍物 def move_box1_continuous(start_pose, end_pose, pose_number, sleep_time): rospy.loginfo('start move box1...') poses = self.get_3d_linspace(start_pose, end_pose, pose_number) for p in poses: rospy.loginfo('new pose:\n {}'.format(p)) self.scene.remove_world_object(self.box1_id) self.scene.add_box(self.box1_id, p, self.box1_size) rospy.sleep(sleep_time) # 把障碍物从pose_0移动到pose_1, 分5步走完,每步停顿0.2秒 #rospy.sleep(7) pose_0 = copy.deepcopy(self.box1_pose) pose_1 = copy.deepcopy(pose_0) pose_1.pose.position.y -= 0.9 move_box1_continuous(pose_0, pose_1, 80, 0.6) # 把障碍物从pose_2移动到pose_3, 分10步走完,每步停顿0.5秒 # pose_1 pose_2 pose_2 = copy.deepcopy(pose_1) # 沿x轴移动 pose_2.pose.position.x += 0.25 pose_2.pose.position.y += 0.45 #pose_2.pose.position.z += 0.3 move_box1_continuous(pose_1, pose_2, 10, 0.6) #rospy.sleep(10) # 如果还需要增加路径,按照上面的方法继续写 # pose_2 pose_3 #pose_2.pose.orientation.x += 0.15 pose_3 = copy.deepcopy(pose_2) #pose_3.pose.position.x += 0.4 pose_3.pose.position.y += 0.3 #pose_3.pose.position.z -= 0.2 move_box1_continuous(pose_2, pose_3, 10, 0.6) #rospy.sleep(20) #pose_3 --pose_4 #pose_3.pose.orientation.x -= 0.6 pose_4 = copy.deepcopy(pose_3) pose_4.pose.position.x -= 0.1 pose_4.pose.position.y -= 0.6 #pose_4.pose.position.x -= 0.4 pose_4.pose.position.z += 0.3 #pose_4.pose.position.z -= 0.8 self.box1_size = [0.3, 0.5, 0.3] self.setColor(self.box1_id, 0.8, 0.4, 0, 0.3) # 发送场景消息 self.sendColors() rospy.sleep(1) move_box1_continuous(pose_3, pose_4, 10, 0.1) #rospy.sleep(10) pose_5 = copy.deepcopy(pose_4) pose_5.pose.position.y += 0.9 #pose_4.pose.position.x -= 0.4 #pose_4.pose.position.z -= 0.8 move_box1_continuous(pose_4, pose_5, 10, 0.6) pose_6 = copy.deepcopy(pose_5) pose_6.pose.position.y -= 0.9 pose_6.pose.position.z -= 0.3 #pose_4.pose.position.x -= 0.4 #pose_4.pose.position.z -= 0.8 move_box1_continuous(pose_5, pose_6, 10, 0.6) #rospy.sleep(10) #pose_4 pose_5 #pose_5 = copy.deepcopy(pose_4) #pose_5.pose.position.y -= 0.4 #pose_5.pose.position.z += 0.4 #move_box1_continuous(pose_4, pose_5, 7, 0.8) #rospy.sleep(10) def get_3d_linspace(self, start_p, end_p, p_number): """calculate 3d linspace between 2 points :param start_p: PoseStamped() :param end_p: PoseStamped() :param p_number: points number :return: points list """ x_new = np.linspace(start_p.pose.position.x, end_p.pose.position.x, p_number) y_new = np.linspace(start_p.pose.position.y, end_p.pose.position.y, p_number) z_new = np.linspace(start_p.pose.position.z, end_p.pose.position.z, p_number) points = [] for i in range(len(x_new)): p = copy.deepcopy(start_p) p.pose.position.x = float(x_new[i]) p.pose.position.y = float(y_new[i]) p.pose.position.z = float(z_new[i]) points.append(p) return points
if __name__ == '__main__': rospy.init_node('scripting_example') while rospy.get_time() == 0.0: pass ### Create a handle for the Planning Scene Interface psi = PlanningSceneInterface() rospy.sleep(1.0) ### Create a handle for the Move Group Commander mgc = MoveGroupCommander("arm") rospy.sleep(1.0) ### Add virtual obstacle pose = gen_pose(pos=[-0.2, -0.1, 1.2]) psi.add_box("box", pose, size=(0.15, 0.15, 0.6)) rospy.sleep(1.0) ### Move to stored joint position mgc.set_named_target("left") mgc.go() ### Move to Cartesian position goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707]) mgc.go(goal_pose.pose) ### Move Cartesian linear goal_pose.pose.position.z -= 0.1 (traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True) mgc.execute(traj)
class Pick_Place: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param( '~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param( '~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface() #未知 self._robot = RobotCommander() #未知 rospy.sleep(0.5) # Clean the scene:清理现场: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景: self._pose_table = self._add_table(self._table_object_name) #增加桌子 self._pose_coke_can = self._add_grasp_block_( self._grasp_object_name) #增加障碍物块 rospy.sleep(0.5) # Define target place pose:定义目标位置姿势 self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y - 0.20 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion( *quaternion_from_euler(0.0, 0.0, 0.0)) self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) self._arm.set_named_target('up') self._arm.go(wait=True) print("up pose") print("第一部分恢复位置初始") # Create grasp generator 'generate' action client: # #创建抓取生成器“生成”动作客户端: print("开始Action通信") self._grasps_ac = SimpleActionClient( '/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown( 'Grasp generator action client not available!') return print("结束Action通信") # Create move group 'pickup' action client: # 创建移动组“抓取”操作客户端: print("开始pickup通信") self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return print("结束pickup通信") # Create move group 'place' action client: # 创建移动组“放置”动作客户端: print("开始place通信") self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(0.5)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return print("结束place通信") # Pick Coke can object:抓取快 while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(0.5) print("抓取物体") rospy.loginfo('Pick up successfully') print("pose_place: ", self._pose_place) # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(0.5) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): """ Create and add table to the scene创建桌子并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.85 p.pose.position.y = 0.0 p.pose.position.z = 0.70 q = quaternion_from_euler( 0.0, 0.0, numpy.deg2rad(90.0)) #numpy.deg2rad(x):度转换为弧度。 #旋转变换,调了 tf.transformations.quaternion_from_euler函数,将(0,0,msg.theta)转换成了quaternion四元数。 p.pose.orientation = Quaternion(*q) #转换成了quaternion四元数 # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (1, 1, 0.05)) return p.pose def _add_grasp_block_(self, name): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.74 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _generate_grasps(self, pose, width): """ 使用抓握生成器生成动作来生成抓握; 基于moveit_simple_grasps pkg上的server_test.py示例。 生成抓取姿势阵列数据以进行可视化, 然后将抓取目标发送到抓取服务器. """ self._grasps_ac.wait_for_server() rospy.loginfo("Successfully connected!") # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: # Sends a goal to the ActionServer, waits for the goal to complete, and preempts goal is necessary. # rospy.loginfo("Sent goal,waiting,目标物体的位置:\n " + str(goal)) #self._grasps_ac.wait_for_result() #发送目标并等待结果: # #将目标发送到ActionServer,等待目标完成,然后抢占目标是必要的。 t_start = rospy.Time.now() state = self._grasps_ac.send_goal_and_wait(goal) t_end = rospy.Time.now() t_toal = t_end - t_start rospy.loginfo("发送时间Result received in " + str(t_toal.to_sec())) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) """ print("---------grasps--start------------") print(grasps) print("-----------test-grasps-----end-----------------") rospy.sleep(2) print("-grasps---sleep----end-----") """ return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp 为该位置的姿势创建姿势数组数据 """ # Generate places: places = [] now = rospy.Time.now() print("---------------test3--------------------------") for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach:生成前置位置方法: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = 0.1 #print("place1---test=====") # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.06 #print("place2---test=====") # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) """ print(places) print("------test4---------------------------------------") """ return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal Create a goal for picking up the grasping object创建一个捡起抓取物体的目标 """ # Create goal: goal = PickupGoal() goal.group_name = group #规划的组 goal.target_name = target #要拾取的对象的名称( #可能使用的抓握列表。 必须至少填写一个掌握 goal.possible_grasps.extend(grasps) #障碍物的可选清单,我们掌握有关语义的信息,可以在抓取过程中触摸/推动/移动这些障碍物; 注意:如果使用对象名称“ all”,则在进近和提升过程中禁止与所有对象发生碰撞。 goal.allowed_touch_objects.append(target) #如果没有可用的名称,则在碰撞图中支撑表面(例如桌子)的名称可以留空 goal.support_surface_name = self._table_object_name # Configure goal planning options:配置目标计划选项: #运动计划者可以规划的最长时间 goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 """ print("----goal-------test-----") print(goal) print("----goal---end------test---") """ rospy.sleep(0.5) print("-goal---sleep----end-----") return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal Create a place goal for MoveIt! """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group使用规划小组选择目标 """ # Obtain possible grasps from the grasp generator server:从掌握生成器服务器获取可能的抓握: grasps = self._generate_grasps(self._pose_coke_can, width) print("--goal--start----") # Create and send Pickup goal:创建并发送zhua取目标: goal = self._create_pickup_goal(group, target, grasps) print("--goal--end----") print("---pick---up---1----") state = self._pickup_ac.send_goal_and_wait(goal) print("-----------------state-------------------") print(state) print("---pick---up---2----") if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() """ print("-----test1--------") print(result) print("------test1--end------") """ # Check for error: err = result.error_code.val print(err) if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False print("pickup-----end-----") rospy.sleep(0.5) return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places:获取可能的位置: places = self._generate_places(place) # Create and send Place goal:创建并发送地方目标: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() """ print("-----test2--------") print(result) print("------test2--end------") """ # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message使用PoseArray消息将抓取发布为姿势 """ print(self._grasps_pub.get_num_connections()) if self._grasps_pub.get_num_connections() != 1: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) print(msg) rospy.loginfo('Publisher ' + str(len(msg.poses)) + ' poses') self._grasps_pub.publish(msg) """ print("11-111") rospy.loginfo('Publisher'+str(len(msg))+'poses') print("11-111") rospy.sleep(2) """ def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
# clean the scene scene.remove_world_object("pole") scene.remove_world_object("ball") scene.remove_world_object("table") scene.remove_world_object("ground_plane") rospy.sleep(5) print ">>>>> add scenes" # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = -1 p.pose.position.z = 0.2 p.pose.orientation.w = 1.0 scene.add_box("pole", p, (0.4, 0.1, 0.4)) p.pose.position.x = 0 p.pose.position.y = 0.8 p.pose.position.z = 0.1 p.pose.orientation.w = 1.0 scene.add_sphere("ball", p, 0.1) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.1 scene.add_box("table", p, (1.0, 2.6, 0.2)) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.4
robot = RobotCommander() rospy.sleep(1) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.7 p.pose.position.y = -0.4 p.pose.position.z = 0.85 p.pose.orientation.w = 1.0 scene.add_box("pole", p, (0.3, 0.1, 1.0)) p.pose.position.y = -0.2 p.pose.position.z = 0.175 scene.add_box("table", p, (0.5, 1.5, 0.35)) p.pose.position.x = 0.6 p.pose.position.y = -0.7 p.pose.position.z = 0.5 scene.add_box("part", p, (0.15, 0.1, 0.3)) rospy.sleep(1) # pick an object robot.right_arm.pick("part")
class CokeCanPickAndPlace: def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~arm', 'arm') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4) # Create (debugging) publishers: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander: self._scene = PlanningSceneInterface() self._robot = RobotCommander() rospy.sleep(1.0) # Clean the scene: self._scene.remove_world_object(self._table_object_name) self._scene.remove_world_object(self._grasp_object_name) # Add table and Coke can objects to the planning scene: self._pose_table = self._add_table(self._table_object_name) self._pose_coke_can = self._add_coke_can(self._grasp_object_name) rospy.sleep(1.0) # Define target place pose: self._pose_place = Pose() self._pose_place.position.x = self._pose_coke_can.position.x self._pose_place.position.y = self._pose_coke_can.position.y + 0.02 self._pose_place.position.z = self._pose_coke_can.position.z self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) # Retrieve groups (arm and gripper): self._arm = self._robot.get_group(self._arm_group) self._gripper = self._robot.get_group(self._gripper_group) # Create grasp generator 'generate' action client: self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction) if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Grasp generator action client not available!') rospy.signal_shutdown('Grasp generator action client not available!') return # Create move group 'pickup' action client: self._pickup_ac = SimpleActionClient('/pickup', PickupAction) if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Pick up action client not available!') rospy.signal_shutdown('Pick up action client not available!') return # Create move group 'place' action client: self._place_ac = SimpleActionClient('/place', PlaceAction) if not self._place_ac.wait_for_server(rospy.Duration(5.0)): rospy.logerr('Place action client not available!') rospy.signal_shutdown('Place action client not available!') return # Pick Coke can object: while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): rospy.logwarn('Pick up failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Pick up successfully') # Place Coke can object on another place on the support surface (table): while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): rospy.logwarn('Place failed! Retrying ...') rospy.sleep(1.0) rospy.loginfo('Place successfully') def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._table_object_name) def _add_table(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.22 q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) p.pose.orientation = Quaternion(*q) # Table size from ~/.gazebo/models/table/model.sdf, using the values # for the surface link. self._scene.add_box(name, p, (0.5, 0.4, 0.02)) return p.pose def _add_coke_can(self, name): p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.25 p.pose.position.y = 0 p.pose.position.z = 0.32 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.01, 0.01, 0.009)) return p.pose def _generate_grasps(self, pose, width): """ Generate grasps by using the grasp generator generate action; based on server_test.py example on moveit_simple_grasps pkg. """ # Create goal: goal = GenerateGraspsGoal() goal.pose = pose goal.width = width options = GraspGeneratorOptions() # simple_graps.cpp doesn't implement GRASP_AXIS_Z! #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z options.grasp_direction = GraspGeneratorOptions.GRASP_DIRECTION_UP options.grasp_rotation = GraspGeneratorOptions.GRASP_ROTATION_FULL # @todo disabled because it works better with the default options #goal.options.append(options) # Send goal and wait for result: state = self._grasps_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) return None grasps = self._grasps_ac.get_result().grasps # Publish grasps (for debugging/visualization purposes): self._publish_grasps(grasps) return grasps def _generate_places(self, target): """ Generate places (place locations), based on https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ baxter_pick_place/src/block_pick_place.cpp """ # Generate places: places = [] now = rospy.Time.now() for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self._robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): q = quaternion_from_euler(0.0, 0.0, angle) place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist place.pre_place_approach.min_distance = self._approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -1 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist place.post_place_retreat.min_distance = self._approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 1 # Add place: places.append(place) # Publish places (for debugging/visualization purposes): self._publish_places(places) return places def _create_pickup_goal(self, group, target, grasps): """ Create a MoveIt! PickupGoal """ # Create goal: goal = PickupGoal() goal.group_name = group goal.target_name = target goal.possible_grasps.extend(grasps) goal.allowed_touch_objects.append(target) goal.support_surface_name = self._table_object_name # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _create_place_goal(self, group, target, places): """ Create a MoveIt! PlaceGoal """ # Create goal: goal = PlaceGoal() goal.group_name = group goal.attached_object_name = target goal.place_locations.extend(places) # Configure goal planning options: goal.allowed_planning_time = 7.0 goal.planning_options.planning_scene_diff.is_diff = True goal.planning_options.planning_scene_diff.robot_state.is_diff = True goal.planning_options.plan_only = False goal.planning_options.replan = True goal.planning_options.replan_attempts = 20 return goal def _pickup(self, group, target, width): """ Pick up a target using the planning group """ # Obtain possible grasps from the grasp generator server: grasps = self._generate_grasps(self._pose_coke_can, width) # Create and send Pickup goal: goal = self._create_pickup_goal(group, target, grasps) state = self._pickup_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) return None result = self._pickup_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _place(self, group, target, place): """ Place a target using the planning group """ # Obtain possible places: places = self._generate_places(place) # Create and send Place goal: goal = self._create_place_goal(group, target, places) state = self._place_ac.send_goal_and_wait(goal) if state != GoalStatus.SUCCEEDED: rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) return None result = self._place_ac.get_result() # Check for error: err = result.error_code.val if err != MoveItErrorCodes.SUCCESS: rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) return False return True def _publish_grasps(self, grasps): """ Publish grasps as poses, using a PoseArray message """ if self._grasps_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for grasp in grasps: p = grasp.grasp_pose.pose msg.poses.append(Pose(p.position, p.orientation)) self._grasps_pub.publish(msg) def _publish_places(self, places): """ Publish places as poses, using a PoseArray message """ if self._places_pub.get_num_connections() > 0: msg = PoseArray() msg.header.frame_id = self._robot.get_planning_frame() msg.header.stamp = rospy.Time.now() for place in places: msg.poses.append(place.place_pose.pose) self._places_pub.publish(msg)
#bring the arm near the object (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], -0.20) # onine_arm.ready() # onine_arm.open_gripper() # onine_arm.go(aim_x, aim_y, aim_z, aim_yaw) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = item_translation[0] p.pose.position.y = item_translation[1] p.pose.position.z = item_translation[2] scene.add_box("target", p, (0.01, 0.01, 0.08)) # rospy.sleep(20) # (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080) grasp_pose = PoseStamped() grasp_pose.header.frame_id = "left_gripper_link" grasp_pose.header.stamp = rospy.Time.now() grasp_pose.pose.position.x = aim_x grasp_pose.pose.position.y = aim_y grasp_pose.pose.position.z = aim_z grasp_pose.pose.orientation = Quaternion( *quaternion_from_euler(0.0, 0, aim_yaw)) g = Grasp() g.pre_grasp_posture = onine_arm.make_gripper_posture(0.09)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(20) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.1) arm.set_goal_orientation_tolerance(0.1) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(3) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(5) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 1.1, 0.01] box1_size = [0.1, 0.05, 0.8] box2_size = [0.05, 0.05, 0.8] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.46 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.41 box1_pose.pose.position.y = -0.3 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.39 box2_pose.pose.position.y = 0.3 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.5 target_pose.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(5) # 设置机械臂的运动目标位置,进行避障规划 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.pose.position.x = 0.4 target_pose2.pose.position.y = 0.5 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.4 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(5) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) robot_arm.set_goal_orientation_tolerance(0.005) robot_arm.set_planning_time(5) robot_arm.set_num_planning_attempts(5) ###eef = robot.get_end_effector_link() #-----error 01 rospy.sleep(2) # Allow replanning to increase the odds of a solution robot_arm.allow_replanning(True) #scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene #scene.remove_world_object("table") #scene.remove_world_object("part") robot_arm.set_named_target("home") #go to goal state. robot_arm.go(wait=True) ''' right_gripper.set_named_target("open") right_gripper.go(wait=True) ''' rospy.sleep(1) print("====== move plan go to home 1 ======") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add an object to be grasped p.pose.position.x = 0.170 p.pose.position.y = 0.04 p.pose.position.z = 0.3 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = -0.00756585784256 start_pose.pose.position.y = -0.225419849157 start_pose.pose.position.z = 0.117192693055 start_pose.pose.orientation.x = 0.95493721962 start_pose.pose.orientation.y = -0.0160209629685 start_pose.pose.orientation.z = -0.00497157918289 start_pose.pose.orientation.w = 0.296333402395 print("going to pick up pose") robot_arm.set_pose_target(start_pose) #right_gripper.set_named_target("close") robot_arm.go(wait=True) #right_gripper.go(wait=True) rospy.sleep(1) print("====== move plan go to up ======") ''' test code 04/20''' pose_zero = PoseStamped() pose_zero.header.frame_id = FIXED_FRAME pose_zero.pose.position.x = 0.0 pose_zero.pose.position.y = 0.194525 pose_zero.pose.position.z = 0.694149 pose_zero.pose.orientation.x = 0.0 pose_zero.pose.orientation.y = 0.0 pose_zero.pose.orientation.x = 0.707062 pose_zero.pose.orientation.x = 0.707062 robot_arm.set_pose_target(pose_zero) robot_arm.go(wait=True) rospy.sleep(3) print("===== move plan test code =====") ''' ''' #robot_arm.set_named_target("up") #robot_arm.go(wait=True) next_pose = PoseStamped() next_pose.header.frame_id = FIXED_FRAME next_pose.pose.position.x = -0.100732862949 next_pose.pose.position.y = -0.210876911879 next_pose.pose.position.z = 0.244678631425 next_pose.pose.orientation.x = 0.784905433655 next_pose.pose.orientation.y = -0.177844554186 next_pose.pose.orientation.z = -0.131161093712 next_pose.pose.orientation.w = 0.578870952129 robot_arm.set_pose_target(next_pose) #right_gripper.set_named_target("open") robot_arm.go(wait=True) #right_gripper.go(wait=True) print("going to pick up pose 2") rospy.sleep(3) print("====== move plan go to home 3 ======") robot_arm.set_named_target("home") robot_arm.go(wait=True) rospy.spin() roscpp_shutdown()
class MoveItDemo: def __init__(self): global obj_att # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # 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('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = PoseStamped() initial_pose.header.frame_id = 'gazebo_world' initial_pose.pose = target_pose.pose print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' self.plan_exec(pre_grasping) #################### GRASPING POSE ######################### M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M3[0,3] = target_pose.pose.position.x M3[1,3] = target_pose.pose.position.y M3[2,3] = target_pose.pose.position.z M4 = transformations.euler_matrix(0, 1.57, 0) M4[0,3] = 0.0 # offset about x M4[1,3] = 0.0 # about y M4[2,3] = 0.18 # about z T2 = np.dot(M3, M4) grasping = deepcopy(target_pose) grasping.pose.position.x = T2[0,3] grasping.pose.position.y = T2[1,3] grasping.pose.position.z = T2[2,3] quat2 = transformations.quaternion_from_matrix(T2) grasping.pose.orientation.x = quat2[0] grasping.pose.orientation.y = quat2[1] grasping.pose.orientation.z = quat2[2] grasping.pose.orientation.w = quat2[3] grasping.header.frame_id = 'gazebo_world' self.plan_exec(grasping) #Close the gripper print "========== Waiting for gazebo to catch up ==========" self.close_gripper() #################### ATTACH OBJECT ###################### touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links) # counter to let the planning scene know when to remove the object obj_att = 1 #self.scene.remove_world_object(target_id) #################### POST-GRASP RETREAT ######################### M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w]) M5[0,3] = initial_pose.pose.position.x M5[1,3] = initial_pose.pose.position.y M5[2,3] = initial_pose.pose.position.z M6 = transformations.euler_matrix(0, 1.57, 0) M6[0,3] = 0.0 # offset about x M6[1,3] = 0.0 # about y M6[2,3] = 0.3 # about z T3 = np.dot(M5, M6) post_grasping = deepcopy(initial_pose) post_grasping.pose.position.x = T3[0,3] post_grasping.pose.position.y = T3[1,3] post_grasping.pose.position.z = T3[2,3] quat3 = transformations.quaternion_from_matrix(T3) post_grasping.pose.orientation.x = quat3[0] post_grasping.pose.orientation.y = quat3[1] post_grasping.pose.orientation.z = quat3[2] post_grasping.pose.orientation.w = quat3[3] post_grasping.header.frame_id = 'gazebo_world' self.plan_exec(post_grasping) # 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 = 0.52 place_pose.pose.position.y = -0.48 place_pose.pose.position.z = 0.48 place_pose.pose.orientation.w = 1.0 n_attempts = 0 max_place_attempts = 2 # Generate valid place poses places = self.make_places(place_pose) success = False # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_place_attempts: for place in places: success = self.right_arm.place(target_id, place) if success: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) self.open_gripper() obj_att = None rospy.sleep(3) ## # Initialize the grasp object ## g = Grasp() ## grasps = [] ## # Set the first grasp pose to the input pose ## g.grasp_pose = pre_grasping ## g.allowed_touch_objects = [target_id] ## grasps.append(deepcopy(g)) ## right_arm.pick(target_id, grasps) # #Change the frame_id for the planning to take place! # #target_pose.header.frame_id = 'gazebo_world' # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################################################################## #Get pose from Gazebo def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg # 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.01, 0.015, -0.005, -0.01, -0.015] # A list of y shifts (meters) to try y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # A list of pitch angles to try #pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02] pitch_vals = [0] # A list of yaw angles to try yaw_vals = [0] # A list to hold the places places = [] # Generate a place pose for each angle and translation for y in yaw_vals: for p in pitch_vals: for y in y_vals: for x in x_vals: place.pose.position.x = init_pose.pose.position.x + x place.pose.position.y = init_pose.pose.position.y + y # Create a quaternion from the Euler angles q = quaternion_from_euler(0, p, y) # 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] # Append this place pose to the list places.append(deepcopy(place)) # Return the list return places def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id global target_size target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene if obj_att is None: self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # 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 = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') # Publish targe's frame #self.object_frames_pub.publish(target_pose) threading.Timer(0.5, self.scene_generator).start() # 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)
class ProbotSorting: def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.arm = moveit_commander.MoveGroupCommander('manipulator') self.end_effector_link = self.arm.get_end_effector_link() self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.allow_replanning(True) self.arm.set_goal_position_tolerance(0.001) self.arm.set_goal_orientation_tolerance(0.01) # 初始化场景对象 self.scene = PlanningSceneInterface() rospy.sleep(1) # Initialize IO self.ioPub = rospy.Publisher('probot_set_output_io', SetOutputIO, queue_size=1) self.arm.set_named_target('home') self.arm.go() rospy.sleep(1) # 移除场景中之前运行残留的物体 #scene.remove_attached_object(end_effector_link, 'tool') self.scene.remove_world_object('table') self.scene.remove_world_object('target') # 设置桌面的高度 self.table_ground = 0.05 # 设置table和tool的三维尺寸 self.table_size = [0.04, 0.04, 1.2] # 将table加入场景当中 self.table_pose = PoseStamped() self.table_pose.header.frame_id = 'base_link' self.table_pose.pose.position.x = -0.2 self.table_pose.pose.position.y = 0.2 self.table_pose.pose.position.z = 0.2 self.table_pose.pose.orientation.w = 1.0 self.scene.add_box('table', self.table_pose, self.table_size) rospy.sleep(2) # 更新当前的位姿 self.arm.set_start_state_to_current_state() ######### def moveToHome(self): self.arm.set_named_target('home') self.arm.go() rospy.sleep(1) def moveTo(self, x, y, z): target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = x target_pose.pose.position.y = y target_pose.pose.position.z = z target_pose.pose.orientation.x = 0.70692 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 0.70729 self.arm.set_start_state_to_current_state() self.arm.set_pose_target(target_pose, self.end_effector_link) traj = self.arm.plan() self.arm.execute(traj) def moveSingleJoint(self, index, value): group_variable_values = self.arm.get_current_joint_values() group_variable_values[index] = value self.arm.set_joint_value_target(group_variable_values) traj = self.arm.plan() self.arm.execute(traj) def pick(self, x, y): self.moveTo(x, y, 0.09) ioOutput0 = SetOutputIO() ioOutput0.ioNumber = 1 ioOutput0.status = SetOutputIO.IO_HIGH self.ioPub.publish(ioOutput0) rospy.sleep(1) ioOutput1 = SetOutputIO() ioOutput1.ioNumber = 2 ioOutput1.status = SetOutputIO.IO_HIGH self.ioPub.publish(ioOutput1) rospy.sleep(1) def place(self, x, y): self.moveTo(x,y,0.15) rospy.sleep(0.3) ioOutput0 = SetOutputIO() ioOutput0.ioNumber = 1 ioOutput0.status = SetOutputIO.IO_LOW self.ioPub.publish(ioOutput0) rospy.sleep(1) ioOutput1 = SetOutputIO() ioOutput1.ioNumber = 2 ioOutput1.status = SetOutputIO.IO_LOW self.ioPub.publish(ioOutput1) rospy.sleep(1) def shutdown(self): print "The sorting is shutting down" moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def ro(self,angle): group_variable_values = self.arm.get_current_joint_values() group_variable_values[0] = angle self.arm.set_joint_value_target(group_variable_values) traj = self.arm.plan() # 按照规划的运动路径控制机械臂运动 self.arm.execute(traj) rospy.sleep(1)
def __init__(self): """ Setup ROS communication between the Kinect and Baxter :return: """ # Initialise variables self.rgb_img = None self.depth_img = None self.marker_box = None self.marker_center_x = None self.marker_center_y = None self.marker_depth = None self.bridge = CvBridge() # First initialize moveit_commander and rospy. print "============ Initialising Baxter" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('can_node', anonymous=True) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander("left_arm") self.right_arm = moveit_commander.MoveGroupCommander("right_arm") self.right_arm_navigator = Navigator('right') # Setup grippers self.right_gripper = baxter_interface.Gripper('right') # Setup the table in the scene scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene) table_id = 'table' scene.remove_world_object(table_id) rospy.sleep(1) table_ground = -0.3 table_size = [4.0, 4.6, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = self.robot.get_planning_frame() table_pose.pose.position.x = 0.7 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Create a dictionary to hold object colors self.colors = dict() self.setColor(table_id, 0.8, 0, 0, 1.0) self.sendColors() self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) # We can get a list of all the groups in the robot print "============ Right Pose:" print self.right_arm.get_current_pose() print "============ Left Pose:" print self.left_arm.get_current_pose() # Setup the subscribers and publishers self.rgb_sub = rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.callback_rgb) self.points_sub = rospy.Subscriber("/camera/depth_registered/points", PointCloud2, self.callback_points) self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) self.screen_pub = rospy.Publisher('robot/xdisplay', Image, latch=True, queue_size=10) self.right_hand_range_pub = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self.callback_range, queue_size=1) self.left_cam_sub = rospy.Subscriber("/cameras/left_hand_camera/image", Image, self.callback_left_hand)
class PickAndPlace: def setColor(self,name,r,g,b,a=0.9): color=ObjectColor() color.id=name color.color.r=r color.color.g=g color.color.b=b color.color.a=a self.colors[name]=color def sendColors(self): p=PlanningScene() p.is_diff=True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def add_point(self,traj, time, positions, velocities=None): point= trajectory_msgs.msg.JointTrajectoryPoint() point.positions= copy.deepcopy(positions) if velocities is not None: point.velocities= copy.deepcopy(velocities) point.time_from_start= rospy.Duration(time) traj.points.append(point) def FollowQTraj(self,q_traj, t_traj): assert(len(q_traj)==len(t_traj)) #Insert current position to beginning. if t_traj[0]>1.0e-2: t_traj.insert(0,0.0) q_traj.insert(0,self.Q(arm=arm)) self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj) #self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj #print traj #self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj)) def QTrajToDQTraj(self,q_traj, t_traj): dof= len(q_traj[0]) #Modeling the trajectory with spline. splines= [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj= [] for t in t_traj: dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) print dq_traj return dq_traj def JointNames(self): #arm= 0 return self.joint_names[0] def ROSGetJTP(self,q,t,dq=None): jp= trajectory_msgs.msg.JointTrajectoryPoint() jp.positions= q jp.time_from_start= rospy.Duration(t) if dq is not None: jp.velocities= dq return jp def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None): assert(len(q_traj)==len(t_traj)) if dq_traj is not None: (len(dq_traj)==len(t_traj)) #traj= trajectory_msgs.msg.JointTrajectory() self.traj.joint_names= joint_names if dq_traj is not None: self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)] else: self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)] self.traj.header.stamp= rospy.Time.now() print self.traj return self.traj def SmoothQTraj(self,q_traj): if len(q_traj)==0: return q_prev= np.array(q_traj[0]) q_offset= np.array([0]*len(q_prev)) for q in q_traj: q_diff= np.array(q) - q_prev for d in range(len(q_prev)): if q_diff[d]<-math.pi: q_offset[d]+=1 elif q_diff[d]>math.pi: q_offset[d]-=1 q_prev= copy.deepcopy(q) q[:]= q+q_offset*2.0*math.pi def setupSence(self): r_tool_size=[0.03,0.02,0.18] l_tool_size=[0.03,0.02,0.18] ''' #sim table table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=0.75 table_pose.pose.position.y=0.0 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) ''' #real scene table table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=-0.184 table_pose.pose.position.y=0.62 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=self.arm_end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.057 l_p.pose.position.z=0.09 l_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=self.arm_end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.057 r_p.pose.position.z=0.09 r_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene=PlanningSceneInterface() pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=1) self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition) self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True) self.colors=dict() rospy.sleep(1) arm=MoveGroupCommander('arm') cartesian=rospy.get_param('~cartesian',True) self.arm_end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.005) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) self.reference_frame='base_link' arm.set_pose_reference_frame(self.reference_frame) arm.set_planning_time(5) #scene planning self.l_id='l_tool' self.r_id='r_tool' self.table_id='table' self.target_id='target_object' self.f_target_id='receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.scene.remove_world_object(self.f_target_id) self.table_ground=0.13 self.table_size=[0.9,0.6,0.018] self.setupSence() joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')] joint_names= rospy.get_param('controller_joint_names') traj= trajectory_msgs.msg.JointTrajectory() traj.joint_names= joint_names target_size=[0.058,0.058,0.19] f_target_size=[0.2,0.2,0.04] #final target f_target_pose=PoseStamped() f_target_pose.header.frame_id=self.reference_frame f_target_pose.pose.position.x=-0.184+0.27 f_target_pose.pose.position.y=0.62+0.1 f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0 f_target_pose.pose.orientation.x=0 f_target_pose.pose.orientation.y=0 f_target_pose.pose.orientation.z=0 f_target_pose.pose.orientation.w=1 self.scene.add_box(self.f_target_id,f_target_pose,f_target_size) #pouring pose pour_pose=f_target_pose pour_pose.pose.position.x-=0.06 pour_pose.pose.position.y-=0.12 pour_pose.pose.position.z+=0.15 #pour_pose.pose.position.y+=0.17 pour_pose.pose.orientation.x=-0.5 pour_pose.pose.orientation.y=-0.5 pour_pose.pose.orientation.z=-0.5 pour_pose.pose.orientation.w=0.5 #set color self.setColor(self.table_id,0.8,0,0,1.0) self.setColor(self.f_target_id,0.8,0.4,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.setColor(self.target_id,0,1,0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) arm.set_named_target("initial_arm") arm.go() rospy.sleep(5) #j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14] j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156] #j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536] signal= True arm.set_joint_value_target(j_ori_state) arm.go() rospy.sleep(3) #arm.set_joint_value_target(j_trans_state) #arm.go() #rospy.sleep(5) waypoints_1=[] waypoints_2=[] start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose if cartesian: msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) target_pose=PoseStamped() target_pose.header.frame_id=self.reference_frame target_pose.pose.position.x=-(msg.pose.position.y)-0.28 target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072 target_pose.pose.position.z=1.36-msg.pose.position.z+0.1 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=-0 target_pose.pose.orientation.w=1 self.scene.add_box(self.target_id,target_pose,target_size) self.setColor(self.target_id,0,0.8,0) self.sendColors() #pre_g_pose pre_grasp_pose=PoseStamped() pre_grasp_pose.header.frame_id=self.reference_frame pre_grasp_pose.pose.position=target_pose.pose.position pre_grasp_pose.pose.position.y-=0.2 pre_grasp_pose.pose.position.z+=0.01 pre_grasp_pose.pose.orientation.x=-0.5 pre_grasp_pose.pose.orientation.y=-0.5 pre_grasp_pose.pose.orientation.z=-0.5 pre_grasp_pose.pose.orientation.w=0.5 #grasp_pose grasp_pose=PoseStamped() grasp_pose.header.frame_id=self.reference_frame grasp_pose.pose.position=target_pose.pose.position grasp_pose.pose.position.y+=0.025 grasp_pose.pose.orientation.x=-0.5 grasp_pose.pose.orientation.y=-0.5 grasp_pose.pose.orientation.z=-0.5 grasp_pose.pose.orientation.w=0.5 g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y= -0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 waypoints_1.append(start_pick_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_1.append(deepcopy(grasp_pose.pose)) fraction=0.0 attempts=0 maxtries=300 while fraction!=1 and attempts<maxtries: (plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True) attempts+=1 if (attempts %300==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") signal_pick=False if fraction==1: rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.") rospy.loginfo("Arm moving.") rospy.sleep(3) #arm.execute(plan_1) p_pick_1=plan_1.joint_trajectory.points[-1] #print p arm.set_joint_value_target(p_pick_1.positions) arm.go() rospy.sleep(7) self.gripperCtrl(0) rospy.sleep(3) self.scene.remove_world_object(self.target_id) #p_pick_2=plan_1.joint_trajectory.points[1] #print p #arm.set_joint_value_target(p_pick_2.positions) #arm.go() #rospy.sleep(4) signal_pick=True #p=plan_1.joint_trajectory.points[-1] #print p #arm.set_joint_value_target(p.positions) #arm.go() if signal_pick: start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose waypoints_2.append(start_trans_pose) #waypoints_1.append(deepcopy(pre_grasp_pose.pose)) waypoints_2.append(deepcopy(pour_pose.pose)) fraction_2=0.0 attempts_2=0 maxtries_2=300 while fraction_2!=1 and attempts_2<maxtries_2: (plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True) attempts_2+=1 if (attempts_2 %300==0 and fraction_2!=1): rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.") if fraction_2==1: #signal=False rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.") rospy.loginfo("Arm moving.") p_trans_shake=plan_2.joint_trajectory.points[-1] p_trans=plan_2.joint_trajectory.points[-2] #print p arm.set_joint_value_target(p_trans.positions) arm.go() rospy.sleep(3) j_pre_pour_state=arm.get_current_joint_values() j_pour_state=j_pre_pour_state j_pour_state[6]-=(1.57+0.26) arm.set_joint_value_target(j_pour_state) arm.go() print "shaking balabala" rospy.sleep(1) print "shaking balabala" rospy.sleep(1) print "shaking balabala" rospy.sleep(1) #shake=True for i in range(10): self.add_point(traj, 0.3, p_trans_shake, [0.0]*7) self.add_point(traj, 0.3, p_trans, [0.0]*7) traj.header.stamp= rospy.Time.now() pub_traj.publish(traj) arm.set_joint_value_target(j_pre_pour_state) arm.go() rospy.sleep(6) arm.set_joint_value_target(p_pick_1.positions) arm.go() rospy.sleep(7) self.gripperCtrl(255) #remove and shut down #self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') #self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) if __name__=="__main__": try: PickAndPlace() except KeyboardInterrupt: raise
rospy.sleep(1) # create a pose p = PoseStamped() p.header.frame_id = "world" #robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.05 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # add a box there scene.add_box("ground", p, (3, 3, 0.02)) # access some meshes rospack = rospkg.RosPack() resourcepath = rospack.get_path('regrasping_app')+"/../resources/" # modify the pose p.pose.position.x = 0.7 p.pose.position.y = 0.7 p.pose.position.z = 0.0 # add the cup scene.add_mesh("cup",p,resourcepath+'objects/cup.dae') # modify the pose p.pose.position.x = 0.72 p.pose.position.z = 0.05
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten") # We need a tf listener to convert poses into arm reference base self.tf_listener = tf.TransformListener() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # 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() # 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.04) #0.04 in original file arm.set_goal_orientation_tolerance(0.1) #0.1 original value # 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(5) # 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 rospy.loginfo("Scaling for MoveIt timeout=" + str( rospy.get_param( '/move_group/trajectory_execution/allowed_execution_duration_scaling' ))) # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "arm_up" pose stored in the SRDF file rospy.loginfo("Set Arm: right_up") arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the closed position rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed)) gripper.set_joint_value_target(self.gripper_closed) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the neutral position rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral)) gripper.set_joint_value_target(self.gripper_neutral) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the open position rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened)) gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.39 # Set the dimensions of the scene objects [l, w, h] table_size = [ 0.7, 0.7, 0.01 ] #set the table coordinates in order to contain the whole scene [first value = 0.2 before] #box1_size = [0.1, 0.05, 0.05] #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.018, 0.018, 0.018] #corresponds roughly to the size of the cube # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.3 table_pose.pose.position.y = -0.19 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) #box1_pose = PoseStamped() #box1_pose.header.frame_id = REFERENCE_FRAME #box1_pose.pose.position.x = table_pose.pose.position.x - 0.04 #box1_pose.pose.position.y = 0.0 #box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 #box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) #box2_pose = PoseStamped() #box2_pose.header.frame_id = REFERENCE_FRAME #box2_pose.pose.position.x = table_pose.pose.position.x - 0.06 #box2_pose.pose.position.y = 0.2 #box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 #box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.34 #table_pose.pose.position.x target_pose.pose.position.y = 0.1 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 #origin position of the target # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) #self.setColor(box1_id, 0.8, 0.4, 0, 1.0) #self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object arm.set_support_surface_name(table_id) # 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 = table_pose.pose.position.x - 0.03 place_pose.pose.position.y = 0.15 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 2.0 #final position of the target # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation result = MoveItErrorCodes.FAILURE n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: rospy.loginfo("Pick attempt") for grasp in grasps: # Publish the grasp poses so they can be viewed in RViz self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) result = arm.pick(target_id, grasps) result = MoveItErrorCodes.SUCCESS break #n_attempts += 1 #rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo(" Pick: Done!") # Generate valid place poses places = self.make_places(place_pose) success = False n_attempts = 0 # Repeat until we succeed or run out of attempts while not success: rospy.loginfo("Place attempt") for place in places: # Publish the place poses so they can be viewed in RViz self.gripper_pose_pub.publish(place) rospy.sleep(0.2) #break success = 1 if success: break #n_attempts += 1 #rospy.sleep(0.2) if not success: rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo(" Place: Done!") else: rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file (passing through right_up) arm.set_named_target('right_up') arm.go() arm.set_named_target('resting') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # 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('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) self.left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.left_gripper = MoveGroupCommander('left_gripper') # eel = len(self.right_arm.get_end_effector_link()) # print eel # Allow 5 seconds per planning attempt # self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ### Attach / Remove Object Flag ### self.aro = None # Run and keep in the BG the scene generator with ctrl^c kill ### timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ## Give some time to ensure the thread starts!! ## rospy.sleep(5) ### GENERATE THE BLACKLIST AND REMOVE ATTACHED OBJECTS FROM PREVIOUS RUNS ### self.idx_list = self.bl() ### GIVE SCENE TIME TO CATCH UP ### rospy.sleep(5) ################################## GRASP EXECUTION ##################################### print "==================== Executing ===========================" start_time = time.time() ### PERSONAL REMINDER!!! WHAT IS WHAT!!! ### # print obj_id[obj_id.index('target')] # print obj_id.index('target') ### MOVE LEFT ARM OUT OF THE WAY ### self.lasp() success = False while success == False and len(self.idx_list)>0: success, pgr_target = self.grasp_attempt() print ("GA Returns:", success) if success is not False: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_target, obj_id.index('target'),'true') self.place_object(obj_id.index('target')) break else: idx = self.idx_list[0] ds, pgr_col_obj = self.declutter_scene(idx) print ("DS Returns:", ds) if ds == True: self.flag = 0 # To let the planning scene know when to remove the object self.post_grasp(pgr_col_obj, obj_id.index(obj_id[idx]),'true') self.place_object(obj_id.index(obj_id[idx])) self.idx_list.pop(0) print "==================== THE END! ======================" print("--- %s seconds ---" % (time.time() - start_time)) rospy.sleep(5) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def grasp_attempt(self): # start_time = time.time() retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): # while obj_id[obj_id.index('target')] is not 'target': # print '!!!!!' # rospy.sleep(0.05) pg = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'pg') gp = self.grasp_pose(obj_pose[obj_id.index('target')], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i = 1 for pg, gr in izip(pre_grasps, grasps): self.gripper_pose_pub.publish(gr) print ("G Attempt: ", i) plp = self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go(wait=True) rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() success = True retreat = gr print "Grasping" break # print("--- %s seconds ---" % (time.time() - start_time)) return success , retreat def declutter_scene(self,index): retreat = None init_poses = [] grasp_poses = [] for axis in range(0,6): pg = self.grasp_pose(obj_pose[index], axis, 'pg') gp = self.grasp_pose(obj_pose[index], axis, 'gp') init_poses.append(pg) grasp_poses.append(gp) pre_grasps = self.grasp_generator(init_poses) grasps = self.grasp_generator(grasp_poses) for grasp in grasps: self.gripper_pose_pub.publish(grasp) rospy.sleep(0.05) success = False i= 1 for pg, gr in izip(pre_grasps, grasps): plp = self.right_arm.plan(pg.pose) print (" DS Attempt: ", i) self.gripper_pose_pub.publish(gr) self.right_arm.plan(pg.pose) if len(plp.joint_trajectory.points) == 0: print "No valid pregrasp Position, continuing on next one" i+=1 continue i+=1 self.right_arm.plan(pg.pose) self.right_arm.go() rospy.sleep(5) plg = self.right_arm.plan(gr.pose) if len(plg.joint_trajectory.points) >= 10: self.right_arm.go() print "Grasping" success = True retreat = gr break return success, retreat def place_object(self, obj_idx): self.aro = obj_idx ### GENERATE PLACE POSES ### places = self.place_generator() ### TRY THESE POSES ### i = 1 for place in places: print (" Place Attempt: ", i) plpl = self.right_arm.plan(place.pose) print len(plpl.joint_trajectory.points) if len(plpl.joint_trajectory.points) == 0: i+=1 continue self.right_arm.plan(plpl) self.right_arm.go(wait=True) ### INFORM SCENE ### # self.open_gripper() # self.aro = None ### RETURN HAND TO STARTING POSITION ### self.post_grasp(place,obj_idx, 'false') self.rasp() break def post_grasp(self,new_pose, obj_idx, fl): ######### GRASP OBJECT/ REMOVE FROM SCENE ######. if fl == 'true': self.close_gripper() self.aro = obj_idx else: self.open_gripper() self.aro = None rospy.sleep(2) ### POST GRASP RETREAT ### M1 = transformations.quaternion_matrix([new_pose.pose.orientation.x, new_pose.pose.orientation.y, new_pose.pose.orientation.z, new_pose.pose.orientation.w]) M1[0,3] = new_pose.pose.position.x M1[1,3] = new_pose.pose.position.y M1[2,3] = new_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T1 = np.dot(M2, M1) npo = deepcopy(new_pose) npo.pose.position.x = T1[0,3] npo.pose.position.y = T1[1,3] npo.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) npo.pose.orientation.x = quat[0] npo.pose.orientation.y = quat[1] npo.pose.orientation.z = quat[2] npo.pose.orientation.w = quat[3] npo.header.frame_id = REFERENCE_FRAME self.right_arm.plan(npo.pose) self.right_arm.go(wait=True) def place_generator(self): place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.57 place_pose.pose.position.y = 0.16 place_pose.pose.position.z = 0.56 place_pose.pose.orientation.w = 1.0 P = transformations.quaternion_matrix([place_pose.pose.orientation.x, place_pose.pose.orientation.y, place_pose.pose.orientation.z, place_pose.pose.orientation.w]) P[0,3] = place_pose.pose.position.x P[1,3] = place_pose.pose.position.y P[2,3] = place_pose.pose.position.z places =[] yaw_angles = [0, 1,57, -1,57 , 3,14] x_vals = [0, 0.05 ,0.1 , 0.15] z_vals = [0.05 ,0.1 , 0.15] for y in yaw_angles: G = transformations.euler_matrix(0, 0, y) G[0,3] = 0.0 # offset about x G[1,3] = 0.0 # about y G[2,3] = 0.0 # about z for z in z_vals: for x in x_vals: TM = np.dot(P, G) pl = deepcopy(place_pose) pl.pose.position.x = TM[0,3] +x pl.pose.position.y = TM[1,3] pl.pose.position.z = TM[2,3] +z quat = transformations.quaternion_from_matrix(TM) pl.pose.orientation.x = quat[0] pl.pose.orientation.y = quat[1] pl.pose.orientation.z = quat[2] pl.pose.orientation.w = quat[3] pl.header.frame_id = REFERENCE_FRAME places.append(deepcopy(pl)) return places def grasp_pose(self, target_pose, axis, stage): ############ TODO : GENERATE AUTOMATED PRE-GRASPING POSITIONS BASED ON THE PRIMITIVE ######### if axis == 0: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 0) if stage == 'pg': M2[0,3] = -0.25 # offset about x elif stage == 'gp': M2[0,3] = -0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 1: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = -0.25 # about y elif stage == 'gp': M2[1,3] = -0.18 # about y M2[2,3] = 0.0 # about z elif axis == 2: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, -1.57) M2[0,3] = 0.0 # offset about x if stage == 'pg': M2[1,3] = 0.25 # about y elif stage == 'gp': M2[1,3] = 0.18 # about y M2[2,3] = 0.0 # about z elif axis == 3: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 0, 3.14) if stage == 'pg': M2[0,3] = 0.25 # offset about x elif stage == 'gp': M2[0,3] = 0.18 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.0 # about z elif axis == 4: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z else: M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(1.57, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y if stage == 'pg': M2[2,3] = 0.30 # about z elif stage == 'gp': M2[2,3] = 0.23 # about z T1 = np.dot(M1, M2) grasp_pose = deepcopy(target_pose) grasp_pose.pose.position.x = T1[0,3] grasp_pose.pose.position.y = T1[1,3] grasp_pose.pose.position.z = T1[2,3] quat = transformations.quaternion_from_matrix(T1) grasp_pose.pose.orientation.x = quat[0] grasp_pose.pose.orientation.y = quat[1] grasp_pose.pose.orientation.z = quat[2] grasp_pose.pose.orientation.w = quat[3] grasp_pose.header.frame_id = REFERENCE_FRAME return grasp_pose def grasp_generator(self, initial_poses): # A list to hold the grasps grasps = [] o = [] # Original Pose of the object (o) O=[] i= 0 while i < len(initial_poses): o.append(initial_poses[i]) i+=1 G = transformations.euler_matrix(0, 0, 0) # Generate a grasps for along z axis (x and y directions) k = 0 while k <= 5: O.append(transformations.quaternion_matrix([o[k].pose.orientation.x, o[k].pose.orientation.y, o[k].pose.orientation.z, o[k].pose.orientation.w])) O[k][0,3] = o[k].pose.position.x O[k][1,3] = o[k].pose.position.y O[k][2,3] = o[k].pose.position.z if k in range(0,4): for z in self.drange(0.005-obj_size[obj_id.index('target')][2]/2,-0.005 + obj_size[obj_id.index('target')][2]/2, 0.02): ### TODO: USE EACH OBJECTS SIZE NOT ONLY THE TARGETS ### # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] +z quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) elif k == 4: for x in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] +x grasp.pose.position.y = T[1,3] grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) else: for y in self.drange(-obj_size[obj_id.index('target')][1]/2, obj_size[obj_id.index('target')][1]/2, 0.02): # print z T = np.dot(O[k], G) grasp = deepcopy(o[k]) grasp.pose.position.x = T[0,3] grasp.pose.position.y = T[1,3] +y grasp.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) grasp.pose.orientation.x = quat[0] grasp.pose.orientation.y = quat[1] grasp.pose.orientation.z = quat[2] grasp.pose.orientation.w = quat[3] grasp.header.frame_id = REFERENCE_FRAME # Append the grasp to the list grasps.append(deepcopy(grasp)) k+=1 print len(grasps) # Return the list return grasps def scene_generator(self): while True: # print "happening" obj_pose =[] obj_id = [] obj_size = [] bl = ['ground_plane','pr2'] global obj_pose, obj_id , obj_size ops = PoseStamped() ops.header.frame_id = REFERENCE_FRAME for model_name in self.pwh.name: if model_name not in bl: obj_id.append(model_name) ops.pose = self.pwh.pose[self.pwh.name.index(model_name)] obj_pose.append(deepcopy(ops)) obj_size.append([0.05, 0.05, 0.15]) # obj_id[obj_id.index('custom_1')] = 'target' obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10] obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05] obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03] if self.aro is None: for i in range(0, len(obj_id)): ### CREATE THE SCENE ### self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i]) self.setColor(obj_id[i], 1, 0.623, 0, 1.0) ### Make the target purple and table green ### self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0) self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0) self.scene.remove_attached_object(GRIPPER_FRAME) # Send the colors to the planning scene self.sendColors() else: if self.flag == 0: touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link'] #print touch_links self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links) ### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ### self.scene.remove_world_object(obj_id[self.aro]) self.flag +=1 time.sleep(0.5) def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def bl(self): blist = ['target','custom_2','custom_3', 'custom_table'] self.blist = [] for name in obj_id: if name not in blist: self.blist.append(obj_id.index(name)) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)]) self.scene.remove_attached_object(GRIPPER_FRAME, 'target') return self.blist def drange(self, start, stop, step): r = start while r < stop: yield r r += step def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.044, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0899, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it # 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 lasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.3665 sp.pose.position.y = 0.74094 sp.pose.position.z = 1.1449 sp.pose.orientation.x = 0.80503 sp.pose.orientation.y = -0.18319 sp.pose.orientation.z = 0.31988 sp.pose.orientation.w = 0.46481 self.left_arm.plan(sp) self.left_arm.go(wait=True) def rasp(self): sp = PoseStamped() sp.header.frame_id = REFERENCE_FRAME sp.pose.position.x = 0.39571 sp.pose.position.y = -0.40201 sp.pose.position.z = 1.1128 sp.pose.orientation.x =0.00044829 sp.pose.orientation.y = 0.57956 sp.pose.orientation.z = 9.4878e-05 sp.pose.orientation.w = 0.81493 self.right_arm.plan(sp) self.right_arm.go(wait=True)
def __init__(self): rospy.init_node('moveit_demo') #self.sub = rospy.Subscriber('/aruco_single/pose', Pose, self.update) #self.sub = rospy.Subscriber('/aruco_single/pose', PoseArray, self.update, queue_size=1) #msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) #grasp msg publisher m_pub_joint_g_1= rospy.Publisher("/ik_solution_g_1", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_2= rospy.Publisher("/ik_solution_g_2", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_3= rospy.Publisher("/ik_solution_g_3", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_4= rospy.Publisher("/ik_solution_g_4", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_g_5= rospy.Publisher("/ik_solution_g_5", JointTrajectoryPoint, queue_size=1, latch=True) #place msg publisher m_pub_joint_p_1= rospy.Publisher("/ik_solution_p_1", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_2= rospy.Publisher("/ik_solution_p_2", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_3= rospy.Publisher("/ik_solution_p_3", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_4= rospy.Publisher("/ik_solution_p_4", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_5= rospy.Publisher("/ik_solution_p_5", JointTrajectoryPoint, queue_size=1, latch=True) m_pub_joint_p_6= rospy.Publisher("/ik_solution_p_6", JointTrajectoryPoint, queue_size=1, latch=True) moveit_commander.roscpp_initialize(sys.argv) cartesian=rospy.get_param('~cartesian',True) scene=PlanningSceneInterface() self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.colors=dict() ''' """set up ros publishers""" self.m_pub_m2j = rospy.Publisher("/two_finger/motoman_control/move_to_joint", JointAnglesDuration, queue_size=1, latch=True); self.m_pub_m2p = rospy.Publisher("/two_finger/motoman_control/move_to_pos", PoseDuration, queue_size=1, latch=True) self.m_pub_m2p_star = rospy.Publisher("/two_finger/motoman_control/move_to_pos_straight", PoseDuration, queue_size=1, latch=True) """ set up ros service """ print("wait gripper service") rospy.wait_for_service("/two_finger/gripper/gotoPosition") rospy.wait_for_service("/two_finger/gripper/gotoPositionUntilTouch") self.m_srv_grpCtrl = rospy.ServiceProxy("/two_finger/gripper/gotoPosition", SetPosition) self.m_srv_grpCtrlUntilTouch = rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch", SetPosition) self.m_srv_grpSpeed = rospy.ServiceProxy("/two_finger/gripper/setSpeed", SetParameter) self.m_srv_grpTorque = rospy.ServiceProxy("/two_finger/gripper/setTorque", SetParameter) self.m_srv_grpTorque(20) self.m_srv_grpSpeed(128) rospy.wait_for_service("/two_finger/motoman_control/trajectory_status") self.m_srv_trajectoryStatus = rospy.ServiceProxy("/two_finger/motoman_control/trajectory_status", Trigger) ''' rospy.sleep(1) arm=MoveGroupCommander('arm') arm.allow_replanning(True) end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.03) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame='base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id='table' #cylinder_id='cylinder' box_id='box' #box2_id='box2' target_id='target_object' scene.remove_world_object(box_id) #scene.remove_world_object(box2_id) scene.remove_world_object(table_id) #scene.remove_world_object(target_id) scene.remove_world_object(target_id) rospy.sleep(2) #table_ground=0.62 table_size=[0.9,0.6,0.018] box_size=[0.15,0.15,0.015] #box2_size=[0.05,0.05,0.1] r_tool_size=[0.03,0.02,0.18] l_tool_size=[0.03,0.02,0.18] target_size=[0.059,0.059,0.12] table_pose=PoseStamped() table_pose.header.frame_id=reference_frame table_pose.pose.position.x=-0.05 table_pose.pose.position.y=0.61 table_pose.pose.position.z=0.194 table_pose.pose.orientation.w=1.0 scene.add_box(table_id,table_pose,table_size) ''' box1_pose=PoseStamped() box1_pose.header.frame_id=reference_frame box1_pose.pose.position.x=0.7 box1_pose.pose.position.y=-0.2 box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0 box1_pose.pose.orientation.w=1.0 scene.add_box(box1_id,box1_pose,box1_size) box2_pose=PoseStamped() box2_pose.header.frame_id=reference_frame box2_pose.pose.position.x=0.6 box2_pose.pose.position.y=-0.05 box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0 box2_pose.pose.orientation.w=1.0 scene.add_box(box2_id,box2_pose,box2_size) ''' #left gripper l_p=PoseStamped() l_p.header.frame_id=end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.057 l_p.pose.position.z=0.09 l_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.057 r_p.pose.position.z=0.09 r_p.pose.orientation.w=1 scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size) #box #place box_pose=PoseStamped() box_pose.header.frame_id=reference_frame box_pose.pose.position.x=-0.30 box_pose.pose.position.y=0.69 box_pose.pose.position.z=0.21 box_pose.pose.orientation.w=1 scene.add_box(box_id,box_pose,box_size) #place p_pose=box_pose p_pose.pose.position.x+=0.01 p_pose.pose.position.y+=0.01 p_pose.pose.position.z+=0.11 p_pose.pose.orientation.w=-0.5 p_pose.pose.orientation.x=-0.5 p_pose.pose.orientation.y=-0.5 p_pose.pose.orientation.z=0.5 #scene.add_box(box1_id,p_pose,box2_size) #grasp g_p=PoseStamped() g_p.header.frame_id=end_effector_link g_p.pose.position.x=0 g_p.pose.position.y=0 g_p.pose.position.z=0.15 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=-0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0.0 #set color self.setColor(table_id,0.8,0,0,1.0) #self.setColor(box1_id,0.8,0.4,0,1.0) #self.setColor(box2_id,0.8,0.4,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.sendColors() #motion planning dev=InputDevice('/dev/input/event4') print "xxx" while not rospy.is_shutdown(): duration=rospy.Duration(4) duration_s = rospy.Duration(2.5) select([dev],[],[]) for event in dev.read(): if (event.value==0) and (event.code==16): print " Path planning" scene.remove_world_object(target_id) rospy.loginfo("Plz move the target") msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped) target_pose=PoseStamped() target_pose.header.frame_id=reference_frame target_pose.pose.position.x=-(msg.pose.position.y)-0.28 target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072 target_pose.pose.position.z=1.36-msg.pose.position.z+0.06 target_pose.pose.orientation.x=0 target_pose.pose.orientation.y=0 target_pose.pose.orientation.z=-0 target_pose.pose.orientation.w=1 scene.add_box(target_id,target_pose,target_size) self.setColor('target_object',0,1,0) #pre-grasp pose pre_grasp_pose=PoseStamped() pre_grasp_pose.header.frame_id=reference_frame pre_grasp_pose.pose.position=target_pose.pose.position pre_grasp_pose.pose.position.y-=0.17 pre_grasp_pose.pose.position.z+=0.02 pre_grasp_pose.pose.orientation.x=-0.5 pre_grasp_pose.pose.orientation.y=-0.5 pre_grasp_pose.pose.orientation.z=-0.5 pre_grasp_pose.pose.orientation.w=0.5 #grasp pose grasp_pose=PoseStamped() grasp_pose.header.frame_id=reference_frame grasp_pose.pose.position=target_pose.pose.position grasp_pose.pose.position.y-=0.12 grasp_pose.pose.position.z+=0.02 ''' #orientation from left grasp_pose.pose.orientation.x=-0.000149 grasp_pose.pose.orientation.y=-0.707 grasp_pose.pose.orientation.z=0 grasp_pose.pose.orientation.w=0.707 ''' #orientation from forward grasp_pose.pose.orientation.x=-0.5 grasp_pose.pose.orientation.y=-0.5 grasp_pose.pose.orientation.z=-0.5 grasp_pose.pose.orientation.w=0.5 arm.set_named_target("initial_arm") arm.go() rospy.sleep(2) start_pose=arm.get_current_pose(end_effector_link).pose #initial waypoints list waypoints_1=[] waypoints_2=[] waypoints_3=[] waypoints_4=[] if cartesian: waypoints_1.append(start_pose) waypoints_1.append(deepcopy(pre_grasp_pose.pose)) fraction=0.0 maxtries=300 attempts=0 #arm.set_start_state__1to_current_state() #plan the cartesian path connecting waypoints while fraction<1.0 and attempts<maxtries: (plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True) attempts+=1 if (attempts %300==0 and fraction!=1.0): rospy.loginfo("path planning failed with only " + str(fraction*100)+ "% success after "+ str(maxtries)+" attempts") continue if fraction==1.0: rospy.loginfo("path compute successfully. Move the arm.") #place = JointAnglesDuration(JointAngles(plan.joint_trajectory.points[len(plan.joint_trajectory)-1].positions), duration) #self.m_pub_m2j.publish(place) print arm.execute(plan_1) m_pub_joint_g_1.publish(plan_1.joint_trajectory.points[-1]) m_pub_joint_g_2.publish(plan_1.joint_trajectory.points[-2]) m_pub_joint_g_3.publish(plan_1.joint_trajectory.points[-3]) m_pub_joint_g_4.publish(plan_1.joint_trajectory.points[-5]) m_pub_joint_g_5.publish(plan_1.joint_trajectory.points[-8]) rospy.loginfo("path execution complete. ") #scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(5) ''' arm.set_start_state_to_current_state() arm.set_pose_target(grasp_pose,end_effector_link) traj=arm.plan() arm.execute(traj) rospy.sleep(2) print arm.get_current_joint_values() #arm.shift_pose_target(4,1.57,end_effector_link) #arm.go() #rospy.sleep(2) arm.shift_pose_target(0,0.11,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() saved_target_pose=arm.get_current_pose(end_effector_link) #arm.set_named_target("initial_arm2") #grasp scene.attach_box(end_effector_link,target_id,g_p,target_size) rospy.sleep(2) #grasping is over , from now is placing arm.shift_pose_target(2,0.15,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(1,-0.2,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() arm.shift_pose_target(2,-0.15,end_effector_link) arm.go() rospy.sleep(2) print arm.get_current_joint_values() scene.remove_attached_object(end_effector_link,target_id) rospy.sleep(2) #arm.set_pose_target(saved_target_pose,end_effector_link) #arm.go() #rospy.sleep(2) arm.set_named_target("initial_arm1") arm.go() rospy.sleep(2) ''' #remove and shut down scene.remove_world_object(target_id) scene.remove_world_object(table_id) scene.remove_attached_object(end_effector_link,'l_tool') scene.remove_attached_object(end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class MoveItDemo: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # 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('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ### Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt self.right_arm.set_planning_time(5) # Prepare Action Controller for gripper self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) ### OPEN THE GRIPPER ### self.open_gripper() self.obj_att = None # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # planning_scene.world.collision_objects.remove('target') # Remove leftover objects from a previous run self.scene.remove_world_object('target') self.scene.remove_world_object('table') # self.scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session self.scene.remove_attached_object(GRIPPER_FRAME, 'target') # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() initial_pose = target_pose initial_pose.header.frame_id = 'gazebo_world' print "==================== Generating Transformations ===========================" #################### PRE GRASPING POSE ######################### # M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) # M1[0,3] = target_pose.pose.position.x # M1[1,3] = target_pose.pose.position.y # M1[2,3] = target_pose.pose.position.z # M2 = transformations.euler_matrix(0, 1.57, 0) # M2[0,3] = 0.0 # offset about x # M2[1,3] = 0.0 # about y # M2[2,3] = 0.25 # about z # T = np.dot(M1, M2) # pre_grasping = deepcopy(target_pose) # pre_grasping.pose.position.x = T[0,3] # pre_grasping.pose.position.y = T[1,3] # pre_grasping.pose.position.z = T[2,3] # quat = transformations.quaternion_from_matrix(T) # pre_grasping.pose.orientation.x = quat[0] # pre_grasping.pose.orientation.y = quat[1] # pre_grasping.pose.orientation.z = quat[2] # pre_grasping.pose.orientation.w = quat[3] # pre_grasping.header.frame_id = 'gazebo_world' # self.plan_exec(pre_grasping) ################# GENERATE GRASPS ################### # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 grasps = self.grasp_generator(initial_pose) possible_grasps = [] for grasp in grasps: self.gripper_pose_pub.publish(grasp) possible_grasps.append(grasp.pose) rospy.sleep(0.2) #print possible_grasps self.right_arm.pick(target_id, grasps) # target_name = target_id # group_name = GROUP_NAME_ARM # end_effector = GROUP_NAME_GRIPPER # attached_object_touch_links = ['r_forearm_link'] # #print (target_name, group_name, end_effector) # PickupGoal(target_name, group_name ,end_effector, possible_grasps ) # # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # # Exit the script moveit_commander.os._exit(0) ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def grasp_generator(self, initial_pose): # Pitch angles to try pitch_vals = [1, 1.57,0, 1,2 ] # Yaw angles to try yaw_vals = [0]#, 1.57, -1.57] # A list to hold the grasps grasps = [] g = PoseStamped() g.header.frame_id = REFERENCE_FRAME g.pose = initial_pose.pose #g.pose.position.z += 0.18 # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = transformations.quaternion_from_euler(0, p, y) # Set the grasp pose orientation accordingly g.pose.orientation.x = q[0] g.pose.orientation.y = q[1] g.pose.orientation.z = q[2] g.pose.orientation.w = q[3] # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps def plan_exec(self, pose): self.right_arm.clear_pose_targets() self.right_arm.set_pose_target(pose, GRIPPER_FRAME) self.right_arm.plan() rospy.sleep(5) self.right_arm.go(wait=True) def close_gripper(self): g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100)) self.ac.send_goal(g_close) self.ac.wait_for_result() rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object def open_gripper(self): g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) self.ac.send_goal(g_open) self.ac.wait_for_result() rospy.sleep(5) # And up to 20 to detach it def scene_generator(self): # print obj_att global target_pose global target_id next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') # obstacle1_id = 'obstacle1' # self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] # obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) # obstacle1_pose = PoseStamped() # obstacle1_pose.header.frame_id = REFERENCE_FRAME # obstacle1_pose.pose = self.pwh.pose[self.o1id] # # Add the target object to the scene # scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # 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)
#move(right, pr) move(left, pl) # clean the scene scene.remove_world_object("pole") scene.remove_world_object("table") scene.remove_world_object("bowl") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.4 p.pose.position.y = -0.9 p.pose.position.z = -0.4 scene.add_box("table", p, (1.6, 0.8, 0.4)) # p.pose.position.x = 0.8 # p.pose.position.y = 0.0 # p.pose.position.z = -0.02 # scene.add_box("bowl", p, (0.8, 0.01, 0.05)) #scene.remove_world_object("part") rospy.sleep(1) print("Standby at initial position") # camera # open camera # rh_cam=CameraController("right_hand_camera") # lh_cam=CameraController("left_hand_camera") # h_cam=CameraController("head_camera") # rh_cam.open()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # 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.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
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() # 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() #move_base action self.fridge = (Pose(Point(X_FRIDGE, Y_FRIDGE, 0.0), Quaternion(0.0, 0.0, 0, 1))) #location of the beer self.person = (Pose(Point(X_PERSON, Y_PERSON, 0.0), Quaternion(0.0, 0.0, 0, 1))) #person requesting the beer self.station = (Pose(Point(0.5, 0.0, 0.0), Quaternion(0.0, 0.0, 0, 1))) #person requesting the beer self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) left_arm = MoveGroupCommander('left_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 10 seconds per planning attempt right_arm.set_planning_time(10) # 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 = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' person1_id = 'person1' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) scene.remove_world_object(person1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # 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_start') right_arm.go() left_arm.set_named_target('left_start') left_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] person1_size = [0.3, 0.7, 0.01] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = X_FRIDGE + 0.55 table_pose.pose.position.y = Y_FRIDGE + 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = X_FRIDGE + 0.55 box1_pose.pose.position.y = Y_FRIDGE + -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = X_FRIDGE + 0.54 box2_pose.pose.position.y = Y_FRIDGE + 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) #add the person to the scene person1_pose = PoseStamped() person1_pose.header.frame_id = REFERENCE_FRAME person1_pose.pose.position.x = X_PERSON + 0.54 person1_pose.pose.position.y = Y_PERSON + 0.13 person1_pose.pose.position.z = table_ground + table_size[ 2] + person1_size[2] / 2.0 person1_pose.pose.orientation.w = 1.0 scene.add_box(person1_id, person1_pose, person1_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = X_FRIDGE + 0.50 target_pose.pose.position.y = Y_FRIDGE + 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) self.setColor(person1_id, 0.8, 0, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # 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 = X_PERSON + 0.50 place_pose.pose.position.y = Y_PERSON + -0.25 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 #move to target#########################################################################################move to target self.move_to(self.fridge) # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # 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 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- right_arm.set_named_target('r_travel') right_arm.go() self.move_to(self.person) #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break 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.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) #move to station self.move_to(self.station) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('SceneSetup') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Set the reference frame for pose targets reference_frame = 'rack1' table_id = 'table' bowl_id = 'bowl' pitcher_id = 'pitcher' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(bowl_id) scene.remove_world_object(pitcher_id) # Set the height of the table off the ground table_ground = 0.5762625 # Set the length, width and height of the table and boxes table_size = [1.0128, 0.481, 0.0238125] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0 table_pose.pose.position.y = 0.847725 table_pose.pose.position.z = table_ground # Set the height of the bowl bowl_ground = 0.57816875 bowl_pose = PoseStamped() bowl_pose.header.frame_id = reference_frame bowl_pose.pose.position.x = 0 bowl_pose.pose.position.y = 0.847725 bowl_pose.pose.position.z = bowl_ground # Set the height of the pitcher pitcher_ground = 0.57816875 pitcher_pose = PoseStamped() pitcher_pose.header.frame_id = reference_frame pitcher_pose.pose.position.x = 0.4 pitcher_pose.pose.position.y = 0.847725 pitcher_pose.pose.position.z = pitcher_ground pitcher_pose.pose.orientation.w = -0.5 pitcher_pose.pose.orientation.z = 0.707 # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0.4, 0, 1.0) self.setColor(bowl_id, 0, 0.4, 0.8, 1.0) self.setColor(pitcher_id, 0, 0.4, 0.8, 1.0) self.sendColors() scene.add_box(table_id, table_pose, table_size) scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl') #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene p = PoseStamped() p.header.frame_id = '/base_link' p.pose.position.x = 0.6 p.pose.position.y = 0.0 p.pose.position.z = 0.45 p.pose.orientation.w = 1.0 #scene.add_box("table", p, (0.5, 1.5, 0.9)) p.pose.position.x = 0.45 p.pose.position.y = -0.4 p.pose.position.z = 0.95 scene.add_box("part", p, (0.04, 0.04, 0.1)) rospy.loginfo("Added object to world") rospy.sleep(1) # p = PoseStamped() # p.header.frame_id = "/base_link" # p.header.stamp = rospy.Time.now() # # p.pose.position.x = 0.61654 # # p.pose.position.y = 0.00954 # # p.pose.position.z = 0.98733 # p.pose.position.x = 0.25 # p.pose.position.y = -0.2 # p.pose.position.z = 1.00 # p.pose.orientation.x = 0.028598
#scene.remove_world_object("pole") #scene.remove_world_object("table") scene.remove_world_object("part") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # p.pose.position.x = 0.7 # p.pose.position.y = -0.4 # p.pose.position.z = 1.15 p.pose.orientation.w = 1.0 #scene.add_box("pole", p, (0.3, 0.1, 1.0)) # p.pose.position.y = -0.2 # p.pose.position.z = 0.175 #scene.add_box("table", p, (0.5, 1.5, 0.8)) # p.pose.position.x = 0.6 # p.pose.position.y = -0.7 # p.pose.position.z = 0.8 p.pose.position.x = 0.15 p.pose.position.y = -0.3 p.pose.position.z = 0.55 scene.add_box("part", p, (0.05, 0.05, 0.05)) rospy.sleep(1) # pick an object robot.right_arm.pick("part") roscpp_shutdown()
def main(): rospy.init_node('Baxter_Env') robot = RobotCommander() scene = PlanningSceneInterface() scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0) rospy.sleep(2) # centre table p1 = PoseStamped() p1.header.frame_id = robot.get_planning_frame() p1.pose.position.x = 1. # position of the table in the world frame p1.pose.position.y = 0. p1.pose.position.z = 0. # left table (from baxter's perspective) p1_l = PoseStamped() p1_l.header.frame_id = robot.get_planning_frame() p1_l.pose.position.x = 0.5 # position of the table in the world frame p1_l.pose.position.y = 1. p1_l.pose.position.z = 0. p1_l.pose.orientation.x = 0.707 p1_l.pose.orientation.y = 0.707 # right table (from baxter's perspective) p1_r = PoseStamped() p1_r.header.frame_id = robot.get_planning_frame() p1_r.pose.position.x = 0.5 # position of the table in the world frame p1_r.pose.position.y = -1. p1_r.pose.position.z = 0. p1_r.pose.orientation.x = 0.707 p1_r.pose.orientation.y = 0.707 # open back shelf p2 = PoseStamped() p2.header.frame_id = robot.get_planning_frame() p2.pose.position.x = 1.2 # position of the table in the world frame p2.pose.position.y = 0.0 p2.pose.position.z = 0.75 p2.pose.orientation.x = 0.5 p2.pose.orientation.y = -0.5 p2.pose.orientation.z = -0.5 p2.pose.orientation.w = 0.5 pw = PoseStamped() pw.header.frame_id = robot.get_planning_frame() # add an object to be grasped p_ob1 = PoseStamped() p_ob1.header.frame_id = robot.get_planning_frame() p_ob1.pose.position.x = .92 p_ob1.pose.position.y = 0.3 p_ob1.pose.position.z = 0.89 # the ole duck p_ob2 = PoseStamped() p_ob2.header.frame_id = robot.get_planning_frame() p_ob2.pose.position.x = 0.87 p_ob2.pose.position.y = -0.4 p_ob2.pose.position.z = 0.24 # clean environment scene.remove_world_object("table_center") scene.remove_world_object("table_side_left") scene.remove_world_object("table_side_right") scene.remove_world_object("shelf") scene.remove_world_object("wall") scene.remove_world_object("part") scene.remove_world_object("duck") rospy.sleep(1) scene.add_box("table_center", p1, size=(.5, 1.5, 0.4)) # dimensions of the table scene.add_box("table_side_left", p1_l, size=(.5, 1.5, 0.4)) scene.add_box("table_side_right", p1_r, size=(.5, 1.5, 0.4)) scene.add_mesh("shelf", p2, "./bookshelf_light.stl", size=(.025, .01, .01)) scene.add_plane("wall", pw, normal=(0, 1, 0)) part_size = (0.07, 0.05, 0.12) scene.add_box("part", p_ob1, size=part_size) scene.add_mesh("duck", p_ob2, "./duck.stl", size=(.001, .001, .001)) rospy.sleep(1) print(scene.get_known_object_names()) ## ---> SET COLOURS table_color = color_norm([105, 105, 105]) # normalize colors to range [0, 1] shelf_color = color_norm([139, 69, 19]) duck_color = color_norm([255, 255, 0]) _colors = {} setColor('table_center', _colors, table_color, 1) setColor('table_side_left', _colors, table_color, 1) setColor('table_side_right', _colors, table_color, 1) setColor('shelf', _colors, shelf_color, 1) setColor('duck', _colors, duck_color, 1) sendColors(_colors, scene)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # 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('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) ## Create a publisher for visualizing direction ### self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # 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 = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) print "==================== Generating Transformations ===========================" M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w]) M1[0,3] = target_pose.pose.position.x M1[1,3] = target_pose.pose.position.y M1[2,3] = target_pose.pose.position.z M2 = transformations.euler_matrix(0, 1.57, 0) M2[0,3] = 0.0 # offset about x M2[1,3] = 0.0 # about y M2[2,3] = 0.25 # about z T = np.dot(M1, M2) pre_grasping = deepcopy(target_pose) pre_grasping.pose.position.x = T[0,3] pre_grasping.pose.position.y = T[1,3] pre_grasping.pose.position.z = T[2,3] quat = transformations.quaternion_from_matrix(T) pre_grasping.pose.orientation.x = quat[0] pre_grasping.pose.orientation.y = quat[1] pre_grasping.pose.orientation.z = quat[2] pre_grasping.pose.orientation.w = quat[3] pre_grasping.header.frame_id = 'gazebo_world' # # Initialize the grasp object # g = Grasp() # grasps = [] # # Set the first grasp pose to the input pose # g.grasp_pose = pre_grasping # g.allowed_touch_objects = [target_id] # grasps.append(deepcopy(g)) # right_arm.pick(target_id, grasps) #Change the frame_id for the planning to take place! #target_pose.header.frame_id = 'gazebo_world' self.p_pub.publish(pre_grasping) right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME) plan = right_arm.plan() rospy.sleep(2) right_arm.go(wait=True) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class add_box: def __init__(self): # Retrieve params: self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') self._grasp_object2_name = rospy.get_param('~grasp_object_name', 'Grasp_Object-2') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~gripper', 'gripper') self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) # Create (debugging) publishers:创建(调试)发布者: self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) # Create planning scene and robot commander:创建规划场景和机器人命令: self._scene = PlanningSceneInterface()#未知 self._robot = RobotCommander()#未知 rospy.sleep(0.5) # Clean the scene:清理现场: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) # Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景: self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)#增加障碍物块 self._pose_coke_can = self._add_grasp_fanfkuai_(self._grasp_object2_name)#增加障碍物块 rospy.sleep(0.5) def __del__(self): # Clean the scene: self._scene.remove_world_object(self._grasp_object_name) self._scene.remove_world_object(self._grasp_object2_name) #----------------------------------1hang--------------------------------- def _add_grasp_block_(self, name): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.6 p.pose.position.y = -0.0 p.pose.position.z = 0.75 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose def _add_grasp_fanfkuai_(self, name): """ Create and add block to the scene创建场景并将其添加到场景 """ p = PoseStamped() p.header.frame_id = self._robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.6 p.pose.position.y = 0.4 p.pose.position.z = 0.75 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, # using the measure tape tool from meshlab. # The box is the bounding box of the coke cylinder. # The values are taken from the cylinder base diameter and height. self._scene.add_box(name, p, (0.045, 0.045, 0.045)) return p.pose
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # 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.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class Place(object): _feedback = moveit_msgs.msg.PlaceActionFeedback().feedback _result = moveit_msgs.msg.PlaceActionResult().result def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, moveit_msgs.msg.PlaceAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo('Action Service Loaded') self.robot = Phantomx_Pincher() rospy.loginfo('Moveit Robot Commander Loaded') self.scene = PlanningSceneInterface() rospy.loginfo('Moveit Planning Scene Loaded') rospy.loginfo('Place action is ok. Awaiting for connections') def get_target(self): target = [0.17, 0.10, 0.028] return target def execute_cb(self, goal): r = rospy.Rate(1) sucess = True if len(self.scene.get_attached_objects()) < 1: rospy.loginfo("No object attached") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self.robot.set_start_state_to_current_state() self._feedback.state = "Planing to place pose" self._as.publish_feedback(self._feedback) target = [ goal.place_locations[0].place_pose.pose.position.x, goal.place_locations[0].place_pose.pose.position.y, goal.place_locations[0].place_pose.pose.position.z ] plan = self.robot.ef_pose(target) if plan is None: rospy.loginfo("Plan to place failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._feedback.state = "Going to place the object" self._as.publish_feedback(self._feedback) self._result.trajectory_descriptions.append( "Going to place the object") self._result.trajectory_stages.append(plan) self.robot.arm_execute(plan) rospy.sleep(7) self._feedback.state = "Removing object to be placed from the planning scene" self._as.publish_feedback(self._feedback) obj = self.scene.get_attached_objects() link = obj[obj.keys()[0]].link_name obj = obj[obj.keys()[0]].object self.scene.remove_attached_object(link, obj.id) self._feedback.state = "Planning to open the gripper" self._as.publish_feedback(self._feedback) plan = self.robot.openGripper() if plan is None: rospy.loginfo("Open Gripper plan failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._result.trajectory_descriptions.append('OpenGripper') self._result.trajectory_stages.append(plan) self._feedback.state = "Openning gripper" print self._feedback self.robot.gripper_execute(plan) rospy.sleep(1) self._as.publish_feedback(self._feedback) self._feedback.state = "Re-adding object" pose = PoseStamped() pose.pose = obj.primitive_poses[0] pose.header = obj.header self.scene.add_box(obj.id, pose, obj.primitives[0].dimensions) self._as.publish_feedback(self._feedback) self._feedback.state = "Planing to retreat after place" self._as.publish_feedback(self._feedback) target[2] += 0.02 plan = self.robot.ef_pose(target) if plan is None: rospy.loginfo("Plan to retreat failed") self._as.set_preempted() self._result.error_code.val = -1 sucess = False return None self._feedback.state = "Retreating" self._as.publish_feedback(self._feedback) self._result.trajectory_descriptions.append("Retreat") self._result.trajectory_stages.append(plan) self.robot.arm_execute(plan) rospy.sleep(7) if sucess: self._result.error_code.val = 1 rospy.loginfo(self._result) self._as.set_succeeded(self._result)
class scene_generator: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('scene_generator') # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) ############## CLEAR THE SCENE ################ # Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c timerThread = threading.Thread(target=self.scene_generator) timerThread.daemon = True timerThread.start() ################################################################# FUNCTIONS ################################################################################# def model_state_callback(self,msg): self.pwh = ModelStates() self.pwh = msg def scene_generator(self): # print obj_att global target_pose global target_id global target_size next_call = time.time() while True: next_call = next_call+1 target_id = 'target' self.taid = self.pwh.name.index('custom_1') table_id = 'table' self.tid = self.pwh.name.index('table') obstacle1_id = 'obstacle1' self.o1id = self.pwh.name.index('wood_cube_5cm') obstacle2_id = 'obstacle2' self.o2id = self.pwh.name.index('custom_2') # Set the sizes [l, w, h] table_size = [1.5, 0.8, 0.03] target_size = [0.05, 0.05, 0.15] obstacle1_size = [0.05, 0.05, 0.05] obstacle2_size = [0.05, 0.05, 0.10] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] if self.obj_att is None: # Add the target object to the scene self.scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 self.scene.add_box(table_id, table_pose, table_size) obstacle1_pose = PoseStamped() obstacle1_pose.header.frame_id = REFERENCE_FRAME obstacle1_pose.pose = self.pwh.pose[self.o1id] obstacle1_pose.pose.position.z += 0.025 # Add the target object to the scene self.scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) obstacle2_pose = PoseStamped() obstacle2_pose.header.frame_id = REFERENCE_FRAME obstacle2_pose.pose = self.pwh.pose[self.o2id] # Add the target object to the scene self.scene.add_box(obstacle2_id, obstacle2_pose, obstacle2_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() else: self.scene.remove_world_object('target') time.sleep(next_call - time.time()) #threading.Timer(0.5, self.scene_generator).start() # 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 __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # 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('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link eef = 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(5) # 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 = 5 # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # 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 = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) # Initialize the grasp pose to the target pose grasp_pose = target_pose #grasp_pose.header.frame_id = 'gazebo_wolrd' # Shift the grasp pose by half the width of the target to center it # grasp_pose.pose.position.y -= target_size[1] / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x -0.07 # grasp_pose.pose.position.z += 0.18 #Allowed touch object list # ato = [target_id, 'r_forearm_link'] # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id] # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: # print grasp.grasp_pose self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 #Allowed touch link list atl = ['r_forearm_link'] # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) if success: self.ac.send_goal(g_close) rospy.sleep(3) ## If the pick was successful, attempt the place operation #if success: #success = False #n_attempts = 0 ## Generate valid place poses #places = self.make_places(place_pose) ## Repeat until we succeed or run out of attempts #while success == False and n_attempts < max_place_attempts: #for place in places: #success = right_arm.place(target_id, place) #if success: #break #n_attempts += 1 #rospy.loginfo("Place attempt: " + str(n_attempts)) #rospy.sleep(0.2) #if not success: #rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") #else: #rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ## Return the arm to 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) #rospy.spin() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
# right_arm.go() # right_gripper.set_named_target("open") # right_gripper.go() rospy.sleep(3) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table p.pose.position.x = 1.0 p.pose.position.y = 0.2 p.pose.position.z = 0.3 scene.add_box("table", p, (0.7, 1, 0.7)) # add an object to be grasped p.pose.position.x = 0.7 p.pose.position.y = -0.2 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) # move to a random target # right_arm.set_named_target("r_start") # right_arm.go() # rospy.sleep(1) # right_arm.set_random_target() # right_arm.go() # rospy.sleep(1)
arm.go() gripper.set_named_target("open") gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table p.pose.position.x = 0.42 p.pose.position.y = -0.2 p.pose.position.z = 0.3 scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.205 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) grasps = [] g = Grasp() g.id = "test" grasp_pose = PoseStamped() grasp_pose.header.frame_id = "world"
if __name__=='__main__': rospy.init_node("part2_attacher") scene = PlanningSceneInterface() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = '/base_link' p.pose.position.x = 0.6 p.pose.position.y = 0.0 p.pose.position.z = 0.45 p.pose.orientation.w = 1.0 #scene.add_box("table", p, (0.5, 1.5, 0.9)) p.pose.position.x = 0.45 p.pose.position.y = -0.1 p.pose.position.z = 1.0 #scene.add_box("part", p, (0.04, 0.04, 0.1)) scene.add_box("part2", p, (0.03, 0.03, 0.1)) rospy.loginfo("Added object to world") rospy.sleep(1)
p.header.frame_id = "base_link" p.pose.position.x = 0.165 p.pose.position.y = 0. p.pose.position.z = 0.066 - 0.115 p.pose.orientation.x = 0.707106 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 0.707106 # scene.add_box("base", p, [0.052, 0.136, 0.526]) p.header.frame_id = "base_link" p.pose.position.x = 0.165 p.pose.position.y = 0. p.pose.position.z = 0.150 - 0.115 p.pose.orientation.w = 1 p.pose.orientation.x = 0 scene.add_box("box", p, [0.03, 0.03, 0.03]) rospy.sleep(1) # ------------------------------ # Configure the planner # ----------------------------- robot = RobotCommander() robot.pincher_arm.set_start_state(RobotState()) #robot.pincher_arm.set_planner_id('RRTConnectkConfigDefault') robot.pincher_arm.set_num_planning_attempts(10000) robot.pincher_arm.set_planning_time(5) robot.pincher_arm.set_goal_position_tolerance(0.01) robot.pincher_arm.set_goal_orientation_tolerance(0.5) robot.pincher_gripper.set_goal_position_tolerance(0.01) robot.pincher_gripper.set_goal_orientation_tolerance(0.5)
# clean the scene #scene.remove_world_object("block1") #scene.remove_world_object("block2") #scene.remove_world_object("block3") #scene.remove_world_object("block4") #scene.remove_world_object("table") #scene.remove_world_object("bowl") #scene.remove_world_object("box") # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.7 p.pose.position.y = -0.4 p.pose.position.z = 0.85 p.pose.orientation.w = 1.57 scene.add_box("block1", p, (0.044, 0.044, 0.044)) p.pose.position.y = -0.2 p.pose.position.z = 0.175 scene.add_box("block2", p, (0.044, 0.044, 0.044)) p.pose.position.x = 0.6 p.pose.position.y = -0.7 p.pose.position.z = 0.5 scene.add_box("block3", p, (0.044, 0.044, 0.044)) p.pose.position.x = 1 p.pose.position.y = -0.7 p.pose.position.z = 0.5 scene.add_box("block4", p, (0.044, 0.044, 0.044)) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = 0 scene.add_mesh("table", p, "table_scaled.stl")
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_attached_object_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) arm.set_planning_time(10) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('target') # 设置桌面的高度 table_ground = 0.6 # 设置table和tool的三维尺寸 table_size = [0.1, 0.7, 0.01] tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = -0.015 p.pose.position.z = 0.0 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) rospy.sleep(2) # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [ 0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858 ] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(1) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)