class SubTaskGrasp: def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('subtask_grasp') rospy.on_shutdown(self.shutdown) self.grasp_srv = rospy.Service('subtask_grasp', rp_grasp, self.graspSrvCallback) def graspSrvCallback(self, req): rospy.loginfo("Received the service call!") rospy.loginfo(req) temp_dbdata = Tmsdb() target = Tmsdb() temp_dbdata.id = req.object_id rospy.wait_for_service('tms_db_reader') try: tms_db_reader = rospy.ServiceProxy('tms_db_reader', TmsdbGetData) res = tms_db_reader(temp_dbdata) target = res.tmsdb[0] except rospy.ServiceException, e: print "Service call failed: %s" % e self.shutdown() print(target.name) # 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 arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(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) target_id = 'chipstar_red' scene.remove_world_object(target_id) scene.remove_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) arm.set_named_target('l_arm_init') arm.go() gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # target_size = [(target.offset_x*2), (target.offset_y*2), (target.offset_z*2)] target_size = [0.03, 0.03, 0.12] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target.x target_pose.pose.position.y = target.y target_pose.pose.position.z = target.z + target.offset_z # q = quaternion_from_euler(target.rr, target.rp, target.ry) q = quaternion_from_euler(0, 0, 0) 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] scene.add_box(target_id, target_pose, target_size) rospy.sleep(2) print('test2-1') # 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.x -= target_size[0] / 2.0 - 0.01 grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) print('test3') # 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 print('test4') # 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 = arm.pick(target_id, grasps) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: scene.remove_attached_object(GRIPPER_FRAME, target_id) ret = rp_graspResponse() # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo("Success the pick operation") ret.result = True else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ret.result = False return ret
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) self.colors = dict() rospy.sleep(1) arm = MoveGroupCommander('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) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) #scene planning table_id = 'table' #cylinder_id='cylinder' #box1_id='box1' box2_id = 'box2' target_id = 'target_object' #scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) rospy.sleep(2) table_ground = 0.68 table_size = [0.5, 1, 0.01] #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] target_size = [0.05, 0.05, 0.1] table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.75 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.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) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.6 target_pose.pose.position.y = 0.05 target_pose.pose.position.z = table_ground + 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 scene.add_box(target_id, target_pose, target_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.04 l_p.pose.position.z = 0.04 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.04 r_p.pose.position.z = 0.04 r_p.pose.orientation.w = 1 scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size) #grasp g_p = PoseStamped() g_p.header.frame_id = 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(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.setColor('target_object', 0, 1, 0) self.sendColors() #motion planning arm.set_named_target("initial_arm1") arm.go() 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 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_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)
def __init__(self, pickPos, placePos): # 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 pick_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the momitive group for the right gripper pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = pick_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) pick_arm.set_goal_position_tolerance(0.05) pick_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution pick_arm.allow_replanning(True) # Set the right arm reference frame pick_arm.set_pose_reference_frame(REFERENCE_FRAME) #pick_arm.set_planner_id("RRTConnectkConfigDefault") #pick_arm.set_planner_id("RRTstarkConfigDefault") pick_arm.set_planner_id("RRTstarkConfigDefault") # Allow 5 seconds per planning attempt pick_arm.set_planning_time(3) # 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 base_table_id = 'base_table' target_id = 'target' limit_table_id = 'limit_table' #tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(base_table_id) #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(limit_table_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) # Set the height of the table off the ground # table_ground = 0.6 table_ground = 0.0 # Set the dimensions of the scene objects [l, w, h] base_table_size = [2, 2, 0.04] #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.055, 0.055, 0.12] target_radius = 0.03305 target_height = 0.12310 limit_table_size = [0.6, 1.8, 0.04] # Add a base table to the scene base_table_pose = PoseStamped() base_table_pose.header.frame_id = REFERENCE_FRAME base_table_pose.pose.position.x = 0.0 base_table_pose.pose.position.y = 0.0 base_table_pose.pose.position.z = -0.3 base_table_pose.pose.orientation.w = 1.0 scene.add_box(base_table_id, base_table_pose, base_table_size) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() rospy.sleep(1) # 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.5 #table_pose.pose.position.y = -0.4 #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) # 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 = pickPos.pose.position.x target_pose.pose.position.y = pickPos.pose.position.y target_pose.pose.position.z = pickPos.pose.position.z target_pose.pose.orientation.w = pickPos.pose.orientation.w # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) #scene.add_cylinder(target_id,target_pose,target_height,target_radius) #Add the limit_table object to the scene limit_table_pose = PoseStamped() limit_table_pose.header.frame_id = REFERENCE_FRAME limit_table_pose.pose.position.x = 0.58 limit_table_pose.pose.position.y = -0.4 limit_table_pose.pose.position.z = 0.18 limit_table_pose.pose.orientation.w = 1.0 #scene.add_box(limit_table_id, limit_table_pose, limit_table_size) self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0) # 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 #pick_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 = placePos.pose.position.x place_pose.pose.position.y = placePos.pose.position.y place_pose.pose.position.z = placePos.pose.position.z place_pose.pose.orientation.w = placePos.pose.orientation.w place_pose.pose.orientation.x = placePos.pose.orientation.x place_pose.pose.orientation.y = placePos.pose.orientation.y place_pose.pose.orientation.z = placePos.pose.orientation.z #设置机器人运行最大速度位百分之~ pick_arm.set_max_velocity_scaling_factor(0.5) # 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 # this way is just used by PR2 robot #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 RV 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: while result != 1 and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) #moveit中的pick接口,target_id为moveit添加场景的id,此处为目标物体,grasps为可尝试抓取的点位序列 result = pick_arm.pick(target_id, grasps) #打印信息 rospy.logerr("pick_arm.pick: " + str(result)) rospy.sleep(0.2) # If the pick was successful, attempt the place operation #如果抓取成功,尝试放置操作 result_g = None n_attempts = 2 #if result == MoveItErrorCodes.SUCCESS: while result_g != True and n_attempts < max_pick_attempts: #and result == 1: # Generate valid place poses #places = self.make_places(place_pose) n_attempts += 1 print("-------------------") print(place_pose) #更新当前的机械臂状态 pick_arm.set_start_state_to_current_state() #设置moveit运动的目标点位 pick_arm.set_pose_target(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 = pick_arm.place(target_id, place) # if result == MoveItErrorCodes.SUCCESS: # break # rospy.sleep(0.2) #moveit的运动接口,wait=True表示等到执行完成才返回 result_g = pick_arm.go(wait=True) rospy.logerr("pick_arm.go: " + str(result_g)) #打开夹爪 open_client(500) rospy.sleep(0.2) #if result == "False" # rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") #else: # rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "home" pose stored in the SRDF file #将机械臂返回到SRDF中的“home”姿态 #scene.remove_world_object(target_id) #scene.remove_attached_object(GRIPPER_FRAME, target_id) #scene.remove_world_object(target_id) rospy.sleep(1) open_client(500) pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) rospy.logerr("pick_server over ")
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, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() print(end_effector_link) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.1) 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 = 2 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, "part") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('default') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(1) place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.240 place_pose.pose.position.y = 0.01 place_pose.pose.position.z = 0.3 scene.add_box("part", place_pose, (0.07, 0.01, 0.2)) # Specify a pose to place the target after being picked up target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME # start the gripper in a neutral pose part way to the target target_pose.pose.position.x = 0.0733923763037 target_pose.pose.position.y = 0.0129100652412 target_pose.pose.position.z = 0.3097191751 target_pose.pose.orientation.x = -0.524236500263 target_pose.pose.orientation.y = 0.440069645643 target_pose.pose.orientation.z = -0.468739062548 target_pose.pose.orientation.w = 0.558389186859 # Initialize the grasp pose to the target pose grasp_pose = target_pose print("going to start pose") right_arm.set_pose_target(target_pose) right_arm.go() rospy.sleep(2) # 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 = 0.12792118579 #grasp_pose.pose.position.y = -0.285290879999 #grasp_pose.pose.position.z = 0.120301181892 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, 'part') # Publish the grasp poses so they can be viewed in RViz print "Publishing grasps" 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('part', grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # 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('part', 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: # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_arm_rest') right_arm.go() # Open the gripper to the open position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self, handeye_client, mode): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) if mode == "sim": arm.set_max_velocity_scaling_factor(1.0) elif mode == "real": arm.set_max_velocity_scaling_factor(0.5) arm.set_planning_time(0.05) # 规划时间限制 # 获取终端link的名称 eef_link = arm.get_end_effector_link() # print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('calib1') # arm.go() # self.go_to_joint_state([2.5228, 0.8964, 0.9746, 0.8614, 2.7515, -1.8821, 0.1712]) self.mode = mode self.listener = tf.TransformListener() self.handeye_client = handeye_client self.im_converter = image_converter() self.aruco_pix_listener = aruco_pix_listener() self.data_num = 0 if mode == "sim": self.camera_link = "camera_link_optical" self.states = [ [2.5230, 0.8961, 0.9750, 0.8609, 2.7510, -1.8819, 0.1710], [2.4861, 0.9171, 0.9988, 0.8576, 2.8072, -1.8743, 0.1717], [2.5231, 0.8961, 0.9738, 0.8604, 2.7517, -1.8818, 0.1700], [2.4003, 0.7911, 0.9820, 1.0503, 2.8352, -1.6524, 0.3760], [2.5049, 0.9006, 0.9405, 0.8542, 2.8436, -1.8724, 0.4127], [2.6273, 0.8107, 0.9139, 0.9618, 2.7511, -1.7930, 0.5062], [2.7162, 0.8069, 0.8263, 0.9200, 2.7147, -1.8470, 0.7433], [2.7065, 0.8050, 0.8584, 0.9477, 2.4444, -1.8806, 0.1071], [2.2901, 0.7806, 1.0289, 1.2211, 2.7996, -1.7092, 0.1444], [2.1136, 0.7398, 1.0706, 1.2892, 2.7842, -1.5650, -0.0094], [2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462], [2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462], [2.6515, 0.6861, 1.0594, 1.4848, 2.5607, -1.5177, 0.8066], [2.7958, 0.8070, 0.9759, 1.2940, 2.4786, -1.6845, 0.7778], [2.8772, 0.7243, 0.9153, 1.4943, 2.3478, -1.4724, 1.0759], [2.3152, 0.8412, 1.1005, 1.1974, 2.5480, -1.6883, 0.2808], [2.0790, 0.8522, 1.0670, 1.6003, 2.6245, -1.2628, 0.5669], [2.1475, 1.0192, 1.0940, 1.3640, 2.6444, -1.3627, 0.3612], [0.2200, -0.5008, -2.8969, 1.7277, 2.6600, -1.1654, 1.5059], [0.4877, -0.7181, -0.2344, -1.3239, -0.1559, -1.4678, 0.5835], [0.3773, -0.6314, -0.2302, -1.4444, -0.1600, -1.3351, 0.4067], [-2.2371, 1.3616, 1.5669, -1.2129, 0.9923, -1.8344, -0.1351], [-2.3273, 1.3885, 1.5713, -0.9358, 1.0748, -1.9300, 0.5635], [-2.2465, 1.2267, 1.8483, -1.0450, 0.7416, -1.6672, 0.3275], [-1.7879, 1.2150, 1.6576, -1.5896, 0.7221, -1.4276, 0.6940], [-2.2530, 1.0939, 1.8129, -1.3149, 0.7196, -1.6466, 0.4342], [-2.1126, 1.1457, 1.7960, -1.2610, 0.6560, -1.6326, 0.4547], [-2.1270, 1.2311, 1.8561, -1.0124, 0.5739, -1.8181, 0.4290], ] elif mode == "real": self.camera_link = "camera_color_optical_frame" self.states = [ [-0.1629, -0.1209, -0.0899, -0.6650, 0.1250, -1.9180, -1.7391], [0.1768, -0.2480, -0.0637, -0.6867, -0.2229, -1.8912, -1.2967], [0.1064, -0.0347, -0.0530, -1.1423, -0.0809, -1.5052, -1.6230], [0.4235, -0.0861, -0.1177, -1.1818, -0.2675, -1.4303, -1.5148], [0.4016, -0.0026, -0.0992, -1.2851, -0.3538, -1.4122, -1.5428], [0.2777, -0.0294, -0.1911, -1.2395, -0.0141, -1.3563, -2.0564], [2.9142, 0.1833, -2.7987, -1.0661, -0.0965, -1.4191, -1.8634], [2.8310, -0.1007, -2.8406, -1.3918, 0.0263, -1.2798, -1.8718], [2.6899, -0.0448, -2.9686, -1.3379, 0.2609, -1.3600, -2.1981], [2.7554, -0.2997, -2.5511, -1.5645, -0.2672, -1.2493, -1.5705], [2.7252, 0.2366, -2.6045, -0.9946, -0.2552, -1.5880, -1.2153], [2.3309, 0.2733, -2.6384, -1.1497, 0.2391, -1.3979, -1.6642], [2.1696, 0.5283, -2.6540, -0.8638, 0.3913, -1.5963, -1.8724], [2.2318, 0.5134, -2.7120, -0.8413, 0.3381, -1.6116, -1.6351], [2.3469, 0.5802, -2.7714, -0.6768, 0.2250, -1.7614, -1.4197], [2.3452, 0.4840, -2.7232, -0.8959, 0.2437, -1.5798, -1.5464], [2.3888, 0.3196, -2.7301, -1.1076, 0.1213, -1.4436, -1.5656], [2.5365, 0.7079, -2.6854, -0.5944, 0.1202, -1.7089, -1.5256], [2.3243, 0.5048, -2.5923, -1.0267, 0.1985, -1.3861, -1.6988], [2.6772, 0.3100, -2.6433, -1.0222, -0.1752, -1.4295, -1.1358], [2.5951, 0.3157, -2.5408, -1.0630, -0.2109, -1.3997, -1.1196], [2.5800, 0.3364, -2.4705, -1.0478, -0.2944, -1.4289, -1.6568], [2.7948, 0.3616, -2.5705, -1.0294, -0.4474, -1.5064, -0.9428], [2.7595, 0.2951, -2.5461, -1.1454, -0.4809, -1.3790, -1.1017], ]
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂运动到之前设置的“up”姿态 arm.set_named_target('up') arm.go() # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print "start pos", start_pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置第二个路点数据,并加入路点列表 # 第二个路点需要向后运动0.2米,向右运动0.2米 wpose = deepcopy(start_pose) wpose.position.x -= 0.2 wpose.position.y -= 0.2 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第三个路点数据,并加入路点列表 wpose.position.x += 0.05 wpose.position.y += 0.15 #wpose.position.z -= 0.15 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第四个路点数据,回到初始位置,并加入路点列表 if cartesian: waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 #路径规划覆盖率 maxtries = 10000 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 print "fraction rate", fraction print "\n" # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('up') arm.go() rospy.sleep(1) # 关闭并退出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)
def __init__(self): rospy.init_node('commander_example', anonymous=True) self.group_l = MoveGroupCommander("left_arm") self.group_r = MoveGroupCommander("right_arm") self.group_l.set_planner_id('RRTConnectkConfigDefault') self.group_r.set_planner_id('RRTConnectkConfigDefault')
def main(): # moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('get_position_client') scene = PlanningSceneInterface() objects = scene.get_known_object_names() # print("objects", objects) # print("scene", scene) left_arm = MoveGroupCommander("left_arm") # left_gripper_group = moveit_commander.MoveGroupCommander("left_gripper") right_arm = MoveGroupCommander("right_arm") # right_gripper = moveit_commander.MoveGroupCommander("right_gripper") dual_arm_group = MoveGroupCommander("dual_arm") grippers = MoveGroupCommander("grippers") get_position = rospy.ServiceProxy('get_position', target_position) rate = rospy.Rate(2.0) done_l = False done_r = False # dual_joints = dual_arm_group.get_joints() # print("dual_joints" , dual_joints) # dual_joint_values = dual_arm_group.get_current_joint_values() # print("dual_joint_values", dual_joint_values) # left_arm_group.set_named_target("left_home") # plan1 = left_arm_group.plan() # rospy.sleep(2) # left_arm_group.go() # print "...Left Done..." # rospy.sleep(1) dual_arm_group.set_named_target("both_home") plan1 = dual_arm_group.plan() rospy.sleep(2) # print("dual_arm plan1", plan1) dual_arm_group.go() print "...Both Done..." rospy.sleep(1) # print("scene", scene) while not rospy.is_shutdown(): try: rospy.wait_for_service('get_position', timeout=5) res = get_position() left_handle = res.points[0] right_handle = res.points[1] # scene.remove_world_object() # print "right_handle" , right_handle # pose_target_l = [] # pose_target_l.append(( left_handle.x - 0.03)) # pose_target_l.append(left_handle.y) # pose_target_l.append(left_handle.z) # pose_target_r = [] # pose_target_r.append(right_handle.x) # pose_target_r.append(right_handle.y) # pose_target_r.append(right_handle.z) # print(pose_target_l) print('left_handle', left_handle, 'right_handle', right_handle) # left_arm_group.set_position_target(pose_target_l) pose_target_l = Pose() pose_target_r = Pose() # pose_target_l.position = left_handle; # pose_target_l.position.x -= 0.06 # pose_target_l.position.y -= 0.005 # pose_target_l.position.z -= 0.005 # q = quaternion_from_euler(-1.57, 0, -1.57) # Horizental Gripper : paraller to x-axis # # q = quaternion_from_euler(-2.432, -1.57, -0.709) # Vertical Gripper: Vertical to x-axis # pose_target_l.orientation.x = q[0] # pose_target_l.orientation.y = q[1] # pose_target_l.orientation.z = q[2] # pose_target_l.orientation.w = q[3] # print ("pose_target_l",pose_target_l ) # left_arm_group.set_pose_target(pose_target_l) # left_arm_group.set_planning_time(10) # plan1 = left_arm_group.plan() # rospy.sleep(2) # left_arm_group.go() # done_l = True # pose_target_r.position.x = 0.5232 # pose_target_r.position.y = -0.2743 # pose_target_r.position.z = 0.6846 # right_gripper.set_named_target("right_open") # right_gripper.plan() # right_gripper.go() pose_target_r.position = right_handle translation = get_translations(right_handle) # pose_target_r.position.x -= 0.065 pose_target_r.position.x -= translation.x print("translation.x", translation.x) pose_target_r.position.y -= translation.y pose_target_r.position.z -= translation.z # pose_target_r.position.z = 0.7235 # q = quaternion_from_euler(-1.57, 0, -1.57) # This works from 2.35 : Gripper paraller to x-axis q = quaternion_from_euler( -1.57, 1.57, -1.57 ) # This work from 2.35 : Gripper Vertical to x-axis (-2.36, 1.5708, -2.36) pose_target_r.orientation.x = q[0] pose_target_r.orientation.y = q[1] pose_target_r.orientation.z = q[2] pose_target_r.orientation.w = q[3] # print ("pose_target_r",pose_target_r ) right_arm.set_pose_target(pose_target_r) right_arm.set_planning_time(10) plan1 = right_arm.plan() # rospy.sleep(1) # right_arm.go() if done_r == False: # print ("target pose",pose_target_r) if (plan1.joint_trajectory.points ): # True if trajectory contains points # last = (len(plan1.joint_trajectory.points) - 1) # print("joint_trajectory", (plan1.joint_trajectory.points[-1].positions) ) # getting the last position of trajectory r_joints = plan1.joint_trajectory.points[-1].positions d_joints = get_dual_trajectory(r_joints) # print("l_joints", l_joints) # d_joints = l_joints + list(r_joints) # print ("d_joints", d_joints) dual_arm_group.set_joint_value_target(d_joints) plan2 = dual_arm_group.plan() if (plan2.joint_trajectory.points): move_success = dual_arm_group.execute(plan2, wait=True) # eef_pose = right_arm.get_current_pose() # print("eef_pose", eef_pose) # move_success = right_arm.execute(plan1, wait = True) if move_success == True: rospy.sleep(2) right_arm.set_start_state_to_current_state() left_arm.set_start_state_to_current_state() waypoints_l = [] waypoints = [] wpose = right_arm.get_current_pose().pose wpose_l = left_arm.get_current_pose().pose print("wpose", wpose) # Open the gripper to the full position grippers.set_named_target("both_open") grippers.plan() grippers.go() # Create Cartesian Path to move forward mainting the end-effector pose # waypoints = [] # rospy.sleep(5) # wpose = right_arm.get_current_pose().pose if (translation.x >= 0.0505): wpose.position.x += (translation.x + 0.00 ) # move forward in (x) wpose_l.position.x += (translation.x + 0.002) wpose_l.position.z += 0.001 else: wpose.position.x += 0.051 # move forward in (x) wpose_l.position.x += 0.053 wpose_l.position.z += 0.001 waypoints.append(deepcopy(wpose)) (plan1, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) waypoints_l.append(deepcopy(wpose_l)) (plan_l, fraction) = left_arm.compute_cartesian_path( waypoints_l, # waypoints to follow 0.01, # eef_step 0.0) # print("plan1 len", len(plan1.joint_trajectory.points)) # waypoints.append(copy.deepcopy(wpose)) # (plan2, fraction) = dual_arm_group.compute_cartesian_path( # waypoints, # waypoints to follow # 0.005, # eef_step # 0.0) rospy.sleep(1) if plan1.joint_trajectory.points: move_success = right_arm.execute(plan1, wait=True) if move_success == True: rospy.loginfo( "Right Move forward successful") # done_r = True # break; if plan_l.joint_trajectory.points: move_success_l = left_arm.execute(plan_l, wait=True) if move_success_l == True: rospy.loginfo( "Left Move forward successful") # done_r = True if move_success == True and move_success_l == True: rospy.sleep(1) grippers.set_named_target("both_close") grippers.plan() grippers.go() # rospy.sleep(1) waypoints = [] rospy.sleep(1) wpose = right_arm.get_current_pose().pose # q = quaternion_from_euler(-1.57, 1.57, -1.57) # wrist up 5 degrees = 1.66 10deg = 1.75 # wpose.orientation.x = q[0] # wpose.orientation.y = q[1] # wpose.orientation.z = q[2] # wpose.orientation.w = q[3] wpose.position.z += 0.07 # move up in (z) waypoints.append(deepcopy(wpose)) (plan1, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) if plan1.joint_trajectory.points: # move_success = right_arm.execute(plan1, wait=True) # for testing right arm trajectory # done_r = True r_joints = plan1.joint_trajectory.points[ -1].positions # print ("r_joints", r_joints) d_joints = get_dual_trajectory(r_joints) # print ("d_joints", d_joints) dual_arm_group.set_joint_value_target( d_joints) # plan2 = 0 plan2 = dual_arm_group.plan() # print("planed plan len", len(plan2.joint_trajectory.points)) # print("dual arm trajectory", (plan2.joint_trajectory.points)) # done_r = True if (plan2.joint_trajectory.points): move_success = dual_arm_group.go() print("move_success", move_success) done_r = True # traj_points = plan2.joint_trajectory.points[0:-3] # plan2.joint_trajectory.points = traj_points # print("eddited plan len", len(plan2.joint_trajectory.points)) # move_success = dual_arm_group.execute(plan2, wait = True) # # done_r = True # move_success = right_arm.execute(plan1, wait=True) # if move_success == True: # rospy.loginfo ("Lift Successful") # done_r = True # r_joints = plan1.joint_trajectory.points[-1].positions # d_joints = get_dual_trajectory(r_joints) # # print("l_joints", l_joints) # # d_joints = l_joints + list(r_joints) # # print ("d_joints", d_joints) # dual_arm_group.set_joint_value_target(d_joints) # plan2 = dual_arm_group.plan() # if (plan2.joint_trajectory.points) : # move_success = dual_arm_group.execute(plan2, wait = True) # if move_success == True: # rospy.loginfo("Move forward successful") # rospy.sleep(1) # grippers.set_named_target("both_close") # grippers.plan() # grippers.go() # rospy.sleep(1) # waypoints = [] # rospy.sleep(2) # wpose = right_arm.get_current_pose().pose # wpose.position.z += 0.1 # move up in (z) # waypoints.append(copy.deepcopy(wpose)) # (plan1, fraction) = right_arm.compute_cartesian_path( # waypoints, # waypoints to follow # 0.01, # eef_step # 0.0) # if plan1.joint_trajectory.points: # r_joints = plan1.joint_trajectory.points[-1].positions # d_joints = get_dual_trajectory(r_joints) # # print ("d_joints", d_joints) # dual_arm_group.set_joint_value_target(d_joints) # plan2 = dual_arm_group.plan() # if (plan2.joint_trajectory.points) : # move_success = dual_arm_group.execute(plan2, wait = True) # done_r = True # move_success = right_arm.execute(plan1, wait=True) # if move_success == True: # rospy.loginfo ("Lift Successful") # done_r = True # else: # rospy.logwarn("Cartesian Paht Planning Failed for forward movement") else: print("Execution Completed") except rospy.ROSException: rospy.logerr('get_position_server did not respond in 5 sec') return rate.sleep()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) # arm.set_max_velocity_scaling_factor(0.01) arm.set_named_target("home") arm.go() def traj_pack(plan, joint_num=6, save_name=None): traj_len = len(plan.joint_trajectory.points) time_ls, pos_ls, vel_ls, acc_ls = [], [], [], [] # print plan.joint_trajectory.points for point in plan.joint_trajectory.points: time_ls.append(point.time_from_start.secs + point.time_from_start.nsecs / 1e9) pos_ls.append(point.positions) vel_ls.append(point.velocities) acc_ls.append(point.accelerations) pos, vel, acc = [], [], [] for i in range(traj_len): pos.append(pos_ls[i][joint_num]) vel.append(vel_ls[i][joint_num]) acc.append(acc_ls[i][joint_num]) # 保存为CSV if save_name is not None: np.savetxt("%s.csv" % save_name, np.array([time_ls, pos, vel, acc]).transpose(), delimiter=',', header="time,pos,vel,acc", comments="") return time_ls, pos, vel, acc # ************************ 测试普通轨迹规划 ******************************* # arm.set_start_state_to_current_state() # arm.set_named_target("test_1") # plan = arm.plan() # ************************ 测试连续路径轨迹规划(零点->中间点->零点) ******************************* # 重新计算轨迹时间 def retime_trajectory(plan, scale): ref_state = robot.get_current_state() retimed_plan = arm.retime_trajectory(ref_state, plan, velocity_scaling_factor=scale) return retimed_plan # 轨迹点拼接 def stitch_trajectory(plan_list): new_traj = RobotTrajectory() new_traj.joint_trajectory.joint_names = plan_list[0].joint_trajectory.joint_names # 轨迹点拼接 new_points = [] for plan in plan_list: new_points += list(plan.joint_trajectory.points) new_traj.joint_trajectory.points = new_points # 重新计算轨迹时间 new_traj = retime_trajectory(new_traj, scale=1.0) return new_traj # 轨迹列表 plan_list = [] # 设置初始状态 state = robot.get_current_state() arm.set_start_state(state) # 设置目标状态 aim_position1 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57] arm.set_joint_value_target(aim_position1) plan1 = arm.plan() plan_list.append(plan1) # 保存轨迹 traj_pack(plan1, save_name = "trajectory1") # 设置初始状态 state.joint_state.position = aim_position1 arm.set_start_state(state) # 设置目标状态 aim_position2 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] arm.set_joint_value_target(aim_position2) plan2 = arm.plan() plan_list.append(plan2) plan2 = stitch_trajectory(plan_list) # 拼接轨迹1和2 traj_pack(plan2, save_name = "trajectory2") # 设置初始状态 state.joint_state.position = aim_position2 arm.set_start_state(state) # 设置目标状态 aim_position3 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.57] arm.set_joint_value_target(aim_position3) plan3 = arm.plan() plan_list.append(plan3) plan3 = stitch_trajectory(plan_list) # 拼接轨迹1、2和3 traj_pack(plan3, save_name = "trajectory3") # 执行轨迹 arm.execute(plan3) # exit(0) ####################################轨迹可视化############################################# plan = plan3 # 用于可视化的轨迹 traj_len = len(plan.joint_trajectory.points) print "traj_len:", traj_len time_ls, pos, vel, acc = traj_pack(plan) # 显示原始轨迹 print "\npos", pos print "\nvel", vel print "\nacc", acc # plt.plot(time_ls, pos, 'ro') # 轨迹点 # plt.plot(time_ls, pos, 'k') # 连接轨迹点 # plt.show() plt.figure(figsize=(6, 10)) plt.subplot(3,1,1) plt.plot(time_ls, pos, 'ro') # plt.plot(time_ls, pos, 'k') plt.grid('on') plt.title('Origin trajectory') plt.xlabel('time (s)') plt.ylabel('position (rad)') plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5) plt.ylim(min(pos) - 0.1, max(pos) + 0.1) plt.subplot(3,1,2) plt.plot(time_ls, vel, 'ro') # plt.plot(time_ls, vel, 'k') plt.grid('on') plt.xlabel('time (s)') plt.ylabel('velocity (rad / s)') plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5) plt.subplot(3,1,3) plt.plot(time_ls, acc, 'ro') # plt.plot(time_ls, acc, 'k') plt.grid('on') plt.xlabel('time (s)') plt.ylabel('acceleration (rad / s$^{2}$)') plt.xlim(time_ls[0]-0.5, time_ls[-1]+0.5) plt.savefig('Origin trajectory.svg', dpi=600, bbox_inches='tight') # plt.savefig('Origin trajectory.pdf', dpi=600, bbox_inches='tight') plt.show() # 插值测试 # Interpolation(time_ls, pos, vel, acc) # 关闭并退出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') 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) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 3 # 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' These boxes are commented as we do not need them #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) These boxes are commented as we do not need them #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.4 # 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] commented for the same reasons as previously #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.005, 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.36 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() These two blocks of code are commented as they assign postion to unwanted boxes #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 = table_pose.pose.position.x - 0.03 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 # 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 = 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], [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 #" + str(n_attempts)) 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) if 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 and n_attempts < max_place_attempts: rospy.loginfo("Place attempt #" + str(n_attempts)) 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) success = arm.place(target_id, place) 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)
def execute_generic_command(self, cmd): if os.path.isfile("cmd/" + cmd): cmd = "load cmd/" + cmd cmdlow = cmd.lower() if cmdlow.startswith("use"): if cmdlow == "use": return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups())) clist = cmd.split() clist[0] = clist[0].lower() if len(clist) == 2: if clist[0] == "use": if clist[1] == "previous": clist[1] = self._prev_group_name if len(clist[1]) == 0: return (MoveGroupInfoLevel.DEBUG, "OK") if self._gdict.has_key(clist[1]): self._prev_group_name = self._group_name self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") else: try: mg = MoveGroupCommander(clist[1]) self._gdict[clist[1]] = mg self._group_name = clist[1] return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: return (MoveGroupInfoLevel.FAIL, "Initializing " + clist[1] + ": " + str(e)) except: return (MoveGroupInfoLevel.FAIL, "Unable to initialize " + clist[1]) elif cmdlow.startswith("trace"): if cmdlow == "trace": return (MoveGroupInfoLevel.INFO, "trace is on" if self._trace else "trace is off") clist = cmdlow.split() if clist[0] == "trace" and clist[1] == "on": self._trace = True return (MoveGroupInfoLevel.DEBUG, "OK") if clist[0] == "trace" and clist[1] == "off": self._trace = False return (MoveGroupInfoLevel.DEBUG, "OK") elif cmdlow.startswith("load"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse load command") if len(clist) == 2: filename = clist[1] if not os.path.isfile(filename): filename = "cmd/" + filename line_num = 0 line_content = "" try: f = open(filename, 'r') for line in f: line_num += 1 line = line.rstrip() line_content = line if self._trace: print("%s:%d: %s" % (filename, line_num, line_content)) comment = line.find("#") if comment != -1: line = line[0:comment].rstrip() if line != "": self.execute(line.rstrip()) line_content = "" return (MoveGroupInfoLevel.DEBUG, "OK") except MoveItCommanderException as e: if line_num > 0: return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s\n%s" % (filename, line_num, line_content, str(e))) else: return (MoveGroupInfoLevel.WARN, "Processing " + filename + ": " + str(e)) except: if line_num > 0: return (MoveGroupInfoLevel.WARN, "Error at %s:%d: %s" % (filename, line_num, line_content)) else: return (MoveGroupInfoLevel.WARN, "Unable to load " + filename) elif cmdlow.startswith("save"): filename = self.DEFAULT_FILENAME clist = cmd.split() if len(clist) > 2: return (MoveGroupInfoLevel.WARN, "Unable to parse save command") if len(clist) == 2: filename = clist[1] try: f = open(filename, 'w') for gr in self._gdict.keys(): f.write("use " + gr + "\n") known = self._gdict[gr].get_remembered_joint_values() for v in known.keys(): f.write(v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n") if self._db_host != None: f.write("database " + self._db_host + " " + str(self._db_port) + "\n") return (MoveGroupInfoLevel.DEBUG, "OK") except: return (MoveGroupInfoLevel.WARN, "Unable to save " + filename) else: return None
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') ptu_action = FollowTrajectoryClient( "ptu_controller", ["ptu_pan_joint", "ptu_tilt_joint"]) ptu_action.move_to([ 0.0, -0.75, ]) # Use the planning scene object to add or remove objects scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() print("End Effector Link", end_effector_link) # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 15 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server find_topic = "basic_grasping_perception/find_objects" sub_topic = "handles_position" rospy.Subscriber(sub_topic, target_pose, getHandlesCb) rospy.loginfo( "Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file # right_arm.set_named_target('right_down') # right_arm.go() # Open the gripper to the neutral position # right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # right_gripper.go() # rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(10.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) rospy.loginfo("Found %d Support Surfaces" % len(find_result.support_surfaces)) # print("perception_results_primitives",find_result.objects.primitives[0]) # print("perception_results_primitives_poses",find_result.objects.primitives_poses) # Remove all previous objects from the planning scene # for name in scene.getKnownCollisionObjects(): # scene.removeCollisionObject(name, False) # for name in scene.getKnownAttachedObjects(): # scene.removeAttachedObject(name, False) # scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None # target_position = None the_object = None the_object_dist = 1.0 count = -1 # target_size # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive( "object%d" % count, obj.object.primitives[0], obj.object.primitive_poses[0], ) #wait = False # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions print("target_size %d ", count, target_size) # target_position = obj.object.primitive_poses[0].position # print("target_size %d ", %count, target_size ) # Set the target pose target_pose.pose = obj.object.primitive_poses[0] print("target_pose_before ", target_pose) # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.519617092464 target_pose.pose.orientation.y = -0.510439243162 target_pose.pose.orientation.z = 0.479912175114 target_pose.pose.orientation.w = -0.489013456294 print("target_pose_after ", target_pose) # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d" % the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor # height = obj.primitive_poses[0].position.z # obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], # 2.0, # make table wider # obj.primitives[0].dimensions[2] ] # obj.primitive_poses[0].position.z += -height/1.5 # Add to scene scene.addSolidPrimitive( obj.name, obj.primitives[0], obj.primitive_poses[0], ) #wait = False # Get the table dimensions table_size = obj.primitives[0].dimensions print("table_size", table_size) # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0 / 256.0, 90.0 / 256.0, 12.0 / 256.0) # orange scene.setColor( find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID #support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object #right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up # place_pose = PoseStamped() # place_pose.header.frame_id = REFERENCE_FRAME # place_pose.pose.position.x = target_pose.pose.position.x # place_pose.pose.position.y = 0.03 # place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 # place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation # if result == MoveItErrorCodes.SUCCESS: # result = None # n_attempts = 0 # # Generate valid place poses # places = self.make_places(place_pose) # # Set the start state to the current state # #right_arm.set_start_state_to_current_state() # # Repeat until we succeed or run out of attempts # while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: # for place in places: # result = right_arm.place(target_id, place) # if result == MoveItErrorCodes.SUCCESS: # break # n_attempts += 1 # rospy.loginfo("Place attempt: " + str(n_attempts)) # rospy.sleep(0.2) # if result != MoveItErrorCodes.SUCCESS: # rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position # right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # right_gripper.go() # rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file # right_arm.set_named_target('resting') # right_arm.go() # rospy.sleep(2) # Give the servos a rest # arbotix_relax_all_servos() # rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
#!/usr/bin/env python import sys import rospy from moveit_commander import MoveGroupCommander, roscpp_initialize import moveit_commander import copy joint_state_topic = ['joint_states:=/robot/joint_states'] moveit_commander.roscpp_initialize(joint_state_topic) rospy.init_node('foo', anonymous=False) roscpp_initialize(sys.argv) both_arms = MoveGroupCommander('both_arms') right_arm = MoveGroupCommander('right_arm') pose1 = right_arm.get_current_pose().pose pose2 = copy.deepcopy(pose1) pose2.position.z -= 0.1 print pose1 print pose2 waypoints = [pose1, pose2] (plan, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) print plan print fraction right_arm.execute(plan, wait=True) rospy.sleep(0.5) raw_input("please input") # [00000000] print right_arm.get_joint_value_target()
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(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 获取终端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 = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 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) # 控制机械臂先回到初始化位置 arm.set_named_target("home") arm.go() rospy.sleep(2) # 设置桌面的高度 table_groud = 0.25 # 设置table,box1,box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.26 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_groud + 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_groud + 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.15 box2_pose.pose.position.z = table_groud + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个盒子设置成橙色 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.2 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 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # 设置机械臂的运动目标位置,进行避障规划 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.pose.position.x = 0.2 target_pose2.pose.position.y = -0.25 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.05 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(2) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') gripper = robot_gripper.Gripper('left') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): #Construct the request request1 = GetPositionIKRequest() request1.ik_request.group_name = "left_arm" request1.ik_request.ik_link_name = "left_gripper" request1.ik_request.attempts = 20 request1.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request1.ik_request.pose_stamped.pose.position.x = .663 request1.ik_request.pose_stamped.pose.position.y = .341 request1.ik_request.pose_stamped.pose.position.z = 0 request1.ik_request.pose_stamped.pose.orientation.x = 0.0 request1.ik_request.pose_stamped.pose.orientation.y = 1.0 request1.ik_request.pose_stamped.pose.orientation.z = 0.0 request1.ik_request.pose_stamped.pose.orientation.w = 0.0 #Request 2 request2 = GetPositionIKRequest() request2.ik_request.group_name = "left_arm" request2.ik_request.ik_link_name = "left_gripper" request2.ik_request.attempts = 50 request2.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request2.ik_request.pose_stamped.pose.position.x = .802 request2.ik_request.pose_stamped.pose.position.y = .048 request2.ik_request.pose_stamped.pose.position.z = 0 request2.ik_request.pose_stamped.pose.orientation.x = 0.0 request2.ik_request.pose_stamped.pose.orientation.y = 1.0 request2.ik_request.pose_stamped.pose.orientation.z = 0.0 request2.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request1) #Print the response HERE print(response) group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request1.ik_request.pose_stamped) # Plan IK and execute group.go() #Calibration print('Calibrating...') gripper.calibrate() rospy.sleep(2.0) #Close the gripper print('Closing...') gripper.close() rospy.sleep(1.0) response2 = compute_ik(request2) #Print the response HERE print(response2) group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request2.ik_request.pose_stamped) # Plan IK and execute group.go() #Open the gripper print('Opening...') gripper.open() rospy.sleep(1.0) print('Done!') except rospy.ServiceException, e: print "Service call failed: %s" % e
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.01) # arm.set_goal_orientation_tolerance(0.1) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('cali_2') # arm.set_named_target('test_1') arm.set_joint_value_target( [0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575]) arm.go() # exit() pose = cal_end_pose_by_quat(arm.get_current_pose().pose, -0.2, 2) arm.set_pose_target(pose) arm.go() # raw_input() # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose().pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) # 按末端坐标系方向向量平移, 计算终点位姿 wpose = cal_end_pose_by_quat(start_pose, 0.35, 2) waypoints.append(deepcopy(wpose)) def scale_trajectory_speed(traj, scale): new_traj = RobotTrajectory() new_traj.joint_trajectory = traj.joint_trajectory n_joints = len(traj.joint_trajectory.joint_names) n_points = len(traj.joint_trajectory.points) points = list(traj.joint_trajectory.points) for i in range(n_points): point = JointTrajectoryPoint() point.positions = traj.joint_trajectory.points[i].positions point.time_from_start = traj.joint_trajectory.points[ i].time_from_start / scale point.velocities = list( traj.joint_trajectory.points[i].velocities) point.accelerations = list( traj.joint_trajectory.points[i].accelerations) for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[ j] = point.accelerations[j] * scale * scale points[i] = point new_traj.joint_trajectory.points = points return new_traj def move_cartesian(waypoints, scale): group = arm # 开始笛卡尔空间轨迹规划 fraction = 0.0 # 路径规划覆盖率 maxtries = 10 # 最大尝试规划次数 attempts = 0 # 已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 group.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") plan = scale_trajectory_speed(plan, scale) group.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") move_cartesian(waypoints, 0.5) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
#!/usr/bin/env python from moveit_commander import MoveGroupCommander import rospy if __name__ == '__main__': group = MoveGroupCommander("manipulator") rospy.init_node("vs060_demo_wy") temp_pose = group.get_current_pose() temp_pose.pose.position.z = temp_pose.pose.position.z - 0.1 group.set_pose_target(temp_pose) group.go()
def __init__(self): # Retrieve params: self._table_object_name = rospy.get_param('~table_object_name', 'table') self._grasp_object_name = rospy.get_param('~grasp_object_name', 'coke_can') self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.06) self._arm_group = rospy.get_param('~manipulator', 'manipulator') self._gripper_group = rospy.get_param('~endeffector', 'endeffector') 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() self._manipulator = MoveGroupCommander("manipulator") 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.5 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 graspobject(): # Connect to the right_arm move group right_arm = MoveGroupCommander('manipulator') right_arm.set_planner_id('RRTConnectkConfigDefault') # 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('base_link') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() #right_arm.set_end_effector_link(end_effector_link) #start_pose = Pose() #start_pose.position.x = 0.587 #start_pose.position.y = 0.034 #start_pose.position.z = -0.010 #start_pose.orientation.x =0.508 #start_pose.orientation.y = 0.508 #start_pose.orientation.z = -0.491 #start_pose.orientation.w = 0.491 #right_arm.set_pose_target(start_pose) #traj = right_arm.plan() #rospy.sleep(2) #right_arm.execute(traj) #rospy.sleep(2) global getkey print '回到初始位置结束' startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy) startclient(0) print 'call start over' sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback) rate = rospy.Rate(200) while not rospy.is_shutdown(): if getkey==1: break rate.sleep() getkey=0 global grasps print '检测到抓取位置,开始执行' global ser ser.write(('0\n').encode()) rospy.sleep(2) print "-start movingA------------------------------------" pose = [] currentcamera=current_cameralinkpose() grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15) start_pose = current_pose() print 'start_pose',start_pose pose.append(start_pose) pose.append(grasppose1) pose.append(grasppose2) #end_pose = copy.deepcopy(start_pose) # end_pose.position.x = grasps.approach.x # end_pose.position.y = grasps.approach.y # end_pose.position.z = grasps.approach.z catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态 print "-start movingB------------------------------------" print '抓' ser.write(('1\n').encode()) rospy.sleep(2) ############################################################## group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(1) right_arm.execute(plan2)#先移到观测姿态 group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里 group_variable_values[1] = -1.493981663380758 group_variable_values[2] = -1.7679532209979456 group_variable_values[3] =-1.3932693640338343 group_variable_values[4] = 1.5719022750854492 group_variable_values[5] = 1.5657243728637695 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(2) right_arm.execute(plan2)#先移到观测姿态 print "-start movingB------------------------------------" print '笛卡尔插值到放置位置完成' ser.write(('0\n').encode()) rospy.sleep(2) # pose = [] #posecurrent=current_pose()#获取现有的机器人末端姿态 # pose.append(posecurrent) #start_pose = Pose()#似乎这里不能接受相对的移动 #start_pose= objectpose#获取目标末端姿态 # posecurrent.position.z =posecurrent.position.z -0.1 # pose.append(posecurrent) # print start_pose print "-------------------------------------" # print pose print "-------------------------------------" # catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态 #回到初始位置 group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(1) right_arm.execute(plan2)#先移到观测姿态 rospy.sleep(3) print '回到初始位置结束' startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy) startclient2(0) print 'call start over'
def __init__(self, manip_name="manipulator", eef_name="endeffector"): self.robot = RobotCommander() self.man = MoveGroupCommander(manip_name) self.eef = MoveGroupCommander(eef_name) self.target_poses = [] self.picked = []
def __init__(self): # Initialize the move_group API print 'starting' moveit_commander.roscpp_initialize(sys.argv) # Connect to the right_arm move group right_arm = MoveGroupCommander('manipulator') right_arm.set_planner_id('RRTConnectkConfigDefault') # 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('base_link') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() #right_arm.set_end_effector_link(end_effector_link) # Get the current pose so we can add it as a waypoint group_variable_values = right_arm.get_current_joint_values() print "============ Joint values: ", group_variable_values group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里 group_variable_values[1] = -1.5415833632098597 group_variable_values[2] = -1.2562201658831995 group_variable_values[3] = -1.8575199286090296 group_variable_values[4] = 1.572489857673645 group_variable_values[5] = 1.5713907480239868 right_arm.set_joint_value_target(group_variable_values) plan2 = right_arm.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(2) right_arm.execute(plan2)#先移到观测姿态 try: rate = rospy.Rate(10) while not rospy.is_shutdown(): print '.' graspobject( ) rate.sleep() except rospy.ROSInterruptException: pass
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个发布抓取姿态的发布者 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME) # 设置每次运动规划的时间限制:10s arm.set_planning_time(10) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'test' # 移除场景中之前运行残留的物体 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_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 设置桌面的高度 table_ground = 0.5 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # # 将三个物体加入场景当中 # table_pose = PoseStamped() # table_pose.header.frame_id = REFERENCE_FRAME # table_pose.pose.position.x = 0.80 # 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.76 # 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.74 # 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) # # # 将桌子设置成红色,两个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) # 设置目标物体的尺寸 target_size = [0.02, 0.01, 0.12] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.72 target_pose.pose.position.y = 0.22 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # 将抓取的目标物体加入场景中 scene.add_box(target_id, target_pose, target_size) # 将目标物体设置为黄色 self.setColor(target_id, 0.9, 0.9, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置支持的外观 arm.set_support_surface_name(table_id) # 设置一个place阶段需要放置物体的目标位置 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.62 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # 将目标位置设置为机器人的抓取目标位置 grasp_pose = target_pose # grasp_pose.pose.position.z -= 0.1 # 生成抓取姿态 grasps = self.make_grasps(grasp_pose, [target_id]) # 将抓取姿态发布,可以在rviz中显示 for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # 追踪抓取成功与否,以及抓取的尝试次数 result = None n_attempts = 0 # 重复尝试抓取,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # 如果pick成功,则进入place阶段 if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # 生成放置姿态 places = self.make_places(place_pose) # 重复尝试放置,直道成功或者超多最大尝试次数 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 = 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.") # 控制机械臂回到初始化位置 # arm.set_named_target('home') # arm.go() # # # 控制夹爪回到张开的状态 # gripper.set_joint_value_target(GRIPPER_OPEN) # gripper.go() # rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) cartesian = rospy.get_param('~cartesian', True) # Connect to the arm move group arm = MoveGroupCommander('arm') # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame('base_link') # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Set an initial position for the arm start_position = [ 0.0, 0.5, -0.0074579719079, -1.67822729461, -3.1415174069, -1.1, 3.1415174069 ] # Set the goal pose of the end effector to the stored pose arm.set_joint_value_target(start_position) # Plan and execute a trajectory to the goal configuration arm.go() # Get the current pose so we can add it as a waypoint start_pose = arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose if cartesian: # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) # Move end effector to the right 0.3 meters wpose.position.y -= 0.3 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Move end effector up and back wpose.position.x -= 0.2 wpose.position.z += 0.3 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses 0.025, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
#!/usr/bin/env python # coding: utf-8 import sys import rospy import copy import geometry_msgs.msg from moveit_commander import MoveGroupCommander from moveit_commander import roscpp_initialize, roscpp_shutdown from math import sin, copysign if __name__ == '__main__': print "--- Straight line gesture ---" rospy.init_node('straight_line', anonymous=True) right_arm = MoveGroupCommander("right_arm") """ start_pose = geometry_msgs.msg.Pose() start_pose.position.x = -0.0206330384032 start_pose.position.y = 0.077582282778 start_pose.position.z = 1.39283677496 start_pose.orientation.x = 0.5 start_pose.orientation.y = 0.5 start_pose.orientation.z = 0.5 start_pose.orientation.w = 0.5 right_arm.set_pose_target(start_pose) plan_start = right_arm.plan() print "Plan start" rospy.sleep(5) right_arm.execute(plan_start)
#Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = position[0] request.ik_request.pose_stamped.pose.position.y = position[1] request.ik_request.pose_stamped.pose.position.z = position[2] request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service # response = compute_ik(request) # #Print the response HERE # print(response) group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation ###group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() # group.close() except rospy.ServiceException, e: print "Service call failed: %s" % e position_index -= 1
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER" roscpp_initialize(sys.argv) rospy.init_node('control_Husky_UR3', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() ##모바일 파트 관련 변수 선언 x = 0.0 y = 0.0 theta = 0.0 ## 매니퓰레이터 변수 선언 group_name = "ur3_manipulator" move_group = MoveGroupCommander(group_name) FIXED_FRAME = 'world' display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=20) def newOdom(msg): global x global y global theta x = msg.pose.pose.position.x y = msg.pose.pose.position.y rot_q = msg.pose.pose.orientation
def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) #position_list = [[0.861, 0.139, -0.152]] #position_list = [[0.5, 0.5,0.0]] position_list = [[0.618, 0.424, -0.169]] position_index = 0 while not rospy.is_shutdown(): # raw_input('Press [ Enter ]: ') if position_index >= len(position_list): break position = position_list[position_index] position_index += 1 # while True: # position = raw_input('Press enter to compute an IK solution:(x,y,z)\n') # position = position.split() # if len(position) != 3: # print "error: Please enter 3 floats" # else: # position = [float(i) for i in position] # break #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = position[0] request.ik_request.pose_stamped.pose.position.y = position[1] request.ik_request.pose_stamped.pose.position.z = position[2] request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service # response = compute_ik(request) # #Print the response HERE # print(response) group = MoveGroupCommander("left_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation ###group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() # group.close() except rospy.ServiceException, e: print "Service call failed: %s" % e position_index -= 1
#!/usr/bin/env python # coding: utf-8 import sys import rospy import copy import geometry_msgs.msg import moveit_msgs from std_msgs.msg import Float32MultiArray from moveit_commander import MoveGroupCommander from moveit_commander import roscpp_initialize, roscpp_shutdown from math import sin, copysign if __name__ == '__main__': rospy.init_node('mimic', anonymous=True) right_shoulder_to_wrist = MoveGroupCommander("right_arm") start_state = right_shoulder_to_wrist.getCurrentState() print(start_state) print "---------------------------------------End"
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap()