def init(): #Wake up Baxter baxter_interface.RobotEnable().enable() rospy.sleep(0.25) print "Baxter is enabled" print "Intitializing clients for services" global ik_service_left ik_service_left = rospy.ServiceProxy( "ExternalTools/left/PositionKinematicsNode/IKService", SolvePositionIK) global ik_service_right ik_service_right = rospy.ServiceProxy( "ExternalTools/right/PositionKinematicsNode/IKService", SolvePositionIK) global obj_loc_service obj_loc_service = rospy.ServiceProxy( "object_location_service", ObjLocation) global stopflag stopflag = False #Taken from the MoveIt Tutorials moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() global scene scene = moveit_commander.PlanningSceneInterface() #Activate Left Arm to be used with MoveIt global left_group left_group = MoveGroupCommander("left_arm") left_group.set_goal_position_tolerance(0.01) left_group.set_goal_orientation_tolerance(0.01) global right_group right_group = MoveGroupCommander("right_arm") pose_right = Pose() pose_right.position = Point(0.793, -0.586, 0.329) pose_right.orientation = Quaternion(1.0, 0.0, 0.0, 0.0) request_pose(pose_right, "right", right_group) global left_gripper left_gripper = baxter_interface.Gripper('left') left_gripper.calibrate() print left_gripper.parameters() global end_effector_subs end_effector_subs = rospy.Subscriber("/robot/end_effector/left_gripper/state", EndEffectorState, end_effector_callback) rospy.sleep(1) global pubpic pubpic = rospy.Publisher('/robot/xdisplay', Image, latch=True, queue_size=1) rospy.sleep(1)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('resting') # Get back the planned trajectory traj = right_arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class Robot: def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position() """Initialise the place and pick manager. """ def initialise_move_actions(self): self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG) self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG) """Move robot arm to "start position". """ def start_position(self): self.arm.set_named_target(START_POSITION) self.arm.go() rospy.sleep(2)
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 = 1 # Set a limit on the number of place attempts max_place_attempts = 1 rospy.loginfo("Scaling for MoveIt timeout=" + str(rospy.get_param('/move_group/trajectory_execution/allowed_execution_duration_scaling'))) # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "arm_up" pose stored in the SRDF file rospy.loginfo("Set Arm: right_up") arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the closed position rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed ) ) gripper.set_joint_value_target(self.gripper_closed) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the neutral position rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral) ) gripper.set_joint_value_target(self.gripper_neutral) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the open position rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened)) gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.34 # 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.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() #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) #print arm.pick(target_id, grasps) result = arm.pick(target_id, grasps) print 'hello' #print MoveItErrorCodes.SUCCESS #if result == MoveItErrorCodes.SUCCESS: result = MoveItErrorCodes.SUCCESS break n_attempts += 1 rospy.sleep(0.2) # If the pick was successful, attempt the place operation print arm.pick(target_id, grasps) print 'kill-me' print MoveItErrorCodes.SUCCESS 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: success=1 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 cartesian_move_to(pose_, execute=False): goal_pose = deepcopy(pose_) ######################################## experiment start # for hardcoded aruco pose # some known arm positions # aruco pose in simulation: 0.52680940312, -0.0490591337742, 0.921301848054, 0.504516550338, 0.506271228009, 0.497290765692, 0.49178693403 # in base_footprint: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0 # pick pose: 0.52680940312, -0.0490591337742, 0.871301848054, 0, 0, 0, 0 # final hand pose in simulation: 0.47653800831, -0.396454309047, 1.05656705145, -0.630265833017, -0.318648344327, 0.582106845777, 0.402963810396 # goal_pose = PoseStamped() # goal_pose.header.frame_id = 'base_footprint' # goal_pose.pose.position.x = 0.52680940312 # goal_pose.pose.position.y = -0.0490591337742 # goal_pose.pose.position.z = 0.871301848054 # goal_pose.pose.orientation.x = 0 # goal_pose.pose.orientation.y = 0 # goal_pose.pose.orientation.z = 0 # goal_pose.pose.orientation.w = 1 arm=MoveGroupCommander('arm') arm.allow_replanning(True) end_effector_link=arm.get_end_effector_link() arm.set_goal_position_tolerance(0.03) arm.set_goal_orientation_tolerance(0.025) arm.allow_replanning(True) reference_frame='base_footprint' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) rospy.sleep(2) start_pose=arm.get_current_pose(end_effector_link).pose rospy.loginfo("End effector start pose: " + str(start_pose)) rospy.loginfo("Aruco pose: " + str(goal_pose)) waypoints=[] waypoints.append(start_pose) # goal_pose.pose.orientation.x = -0.5 # goal_pose.pose.orientation.y = -0.5 # goal_pose.pose.orientation.z = -0.5 # goal_pose.pose.orientation.w = 1 # goal_pose.pose.orientation.y -= 0.1*(1.0/2.0) waypoints.append(deepcopy(goal_pose.pose)) fraction=0.0 maxtries=100 attempts=0 while fraction<1.0 and attempts<maxtries: (plan,fraction)=arm.compute_cartesian_path(waypoints,0.02,0.0,True) attempts+=1 if attempts %20==0: if (attempts<100): rospy.loginfo("still trying after"+str(attempts)+" attempts...") else : rospy.loginfo("Finished after "+str(attempts)+" attempts...") if fraction>0.89: rospy.loginfo("path compute successfully. Arm is moving.") print(plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1].positions) print(plan) #for item in plan.joint_trajectory.points[len(plan.joint_trajectory.points)-1] if execute: arm.execute(plan) rospy.loginfo("path execution complete. ") rospy.sleep(2) if (attempts %100==0 and fraction<0.9): rospy.loginfo("path planning failed with only " + str(fraction*100)+ "% success after "+ str(maxtries)+" attempts") return fraction
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) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table_id = 'table' target_id = 'target' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_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() rospy.sleep(1) gripper.set_named_target('finger_home') gripper.go() rospy.sleep(3) # 设置桌面的高度 table_ground = 0.1 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [1, 2, 0.01] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 1.5 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) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) # 设置目标物体的尺寸 target_size = [0.5, 0.05, 0.2] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 1.5 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 # 将抓取的目标物体加入场景中 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 = 1.5 place_pose.pose.position.y = 0.3 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 # 生成抓取姿态 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.set_named_target('finger_home') gripper.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class BaseArmNavigation(object): def __init__(self, node_name): self.node_name = node_name rospy.init_node(node_name) rospy.loginfo("Starting node " + str(node_name)) rospy.on_shutdown(self.shutdown) #Initialize necessary variables self.arm_group_name = rospy.get_param("~arm_group_name", "ur5_arm") self.stow_group_state = rospy.get_param("~stow_group_state", "stow") self.work_group_state = rospy.get_param("work_group_state", "work") self.reference_frame = rospy.get_param("~reference_frame", "map") self.robot_base_frame = rospy.get_param("~robot_base_frame", "odom") self.initial_location_topic = rospy.get_param( "~initial_location_topic", "/initial_locations") self.poi_topic = rospy.get_param("~poi_topic", "/poi") self.error_topic = rospy.get_param("~error_topic", "/error") self.allow_replanning = rospy.get_param("~allow_replanning", True) self.position_tolerance = rospy.get_param("~position_tolerance", .0001) self.orientation_tolerance = rospy.get_param("~orientation_tolerance", .0001) self.desired_orientation = rospy.get_param("~desired_orientation", Quaternion(0, 0, 0, 1)) self.cartestian_path = rospy.get_param("~cartestian_path", False) self.cp_max_attempts = rospy.get_param("~cp_max_attempts", 100) self.workface_offset = rospy.get_param("~workface_offset", 1.0) # Initialize a number of global variables self.poi = None self.error = None # Arm Navigation Initialization # Initialize the move group for the right arm self.arm = MoveGroupCommander(self.arm_group_name) # Set the arm reference frame accordingly self.arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.arm.allow_replanning(self.allow_replanning) # Set a position tolerance in meters self.arm.set_goal_position_tolerance(self.position_tolerance) # Set an orientation tolerance in radians self.arm.set_goal_orientation_tolerance(self.orientation_tolerance) # What is the end effector link? self.end_effector_link = self.arm.get_end_effector_link() # Move Base Initialization # Publisher to manually control the robot self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) # Subscribe to the move_base action server self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") # Wait 60 seconds for the action server to become available self.move_base.wait_for_server(rospy.Duration(60)) rospy.loginfo("Connected to move base server") # Initialize Tf # Create the transform listener self.listener = tf.TransformListener() # Queue Tf data rospy.sleep(3) rospy.loginfo("Listening to Tfs") # Subscribe to necessary topics # Subscribe to the initial locations topic and store them as self.initial_locations self.initial_locations = rospy.wait_for_message( self.initial_location_topic, PoseArray) # Subscribe to the poi topic rospy.wait_for_message(self.poi_topic, PointStamped) # Use queue_size=1 so outdated poi and error messages are not piled up self.poi_subscriber = rospy.Subscriber(self.poi_topic, PointStamped, self.update_poi, queue_size=1) self.error_subscriber = rospy.Subscriber(self.error_topic, PointStamped, self.update_error, queue_size=1) rospy.loginfo("Subscribed to necessary topics") rospy.loginfo("Ready to perform task") # Perform desired task self.perform_task() def perform_task(self): rospy.loginfo("Performing specified task") return def update_poi(self, poi): self.poi = poi def update_error(self, error): self.error = error def convert_point_to_arm_goal(self, point, initial_location): #Create Pose Msgs pose = PoseStamped() pose.header.frame_id = self.reference_frame pose.header.stamp = rospy.Time.now() pose.pose.position.x = initial_location.pose.position.x pose.pose.position.y = point.point.y pose.pose.position.z = point.point.z pose.pose.orientation = initial_location.pose.orientation return pose def convert_pose_to_move_base_goal(self, goal): # Initialize the move_base_goal move_base_goal = MoveBaseGoal() # Use the map frame to define goal poses move_base_goal.target_pose.header.frame_id = self.reference_frame # Set the time stamp to "now" move_base_goal.target_pose.header.stamp = rospy.Time.now() # Set the pose move_base_goal.target_pose.pose = Pose( Point(goal.pose.position.x - self.workface_offset, goal.pose.position.y, 0.0), goal.pose.orientation) return move_base_goal def plan_cartesian_path(self, end_pose): #Initialize necessary variables fraction = 0.0 attempts = 0 start_pose = self.arm.get_current_pose(self.end_effector_link) # Set the internal state to the current state self.arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the start point and goal while fraction < 1.0 and attempts < self.cp_max_attempts: (plan, fraction) = self.arm.compute_cartesian_path( [start_pose, end_pose], # waypoint poses 0.01, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # If we have a complete plan, return that plan if fraction == 1.0: return plan else: return None def plan_regular_path(self, end_pose): # Set the start state to the current state self.arm.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose self.arm.set_pose_target(end_pose, self.end_effector_link) # Plan the trajectory to the goal plan = self.arm.plan() return plan def arm_nav_predicition(self, goal): # Initialize the outcome to False outcome = False if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True return outcome def arm_nav_predicition_with_move_base(self, goal): # Initialize the outcome to False outcome = False if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True else: move_base_goal = self.convert_pose_to_move_base_goal(goal) base_move_outcome = self.base_move(move_base_goal) if base_move_outcome: if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True return outcome def arm_nav_correction(self, goal): # Initialize the outcome to False outcome = False if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True return outcome def arm_nav_correction_with_move_base(self, goal): # Initialize the outcome to False outcome = False if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True else: move_base_goal = self.convert_pose_to_move_base_goal(goal) base_move_outcome = self.base_move(move_base_goal) if base_move_outcome: if self.cartestian_path: plan = self.plan_cartesian_path(goal) else: plan = self.plan_regular_path(goal) if plan is not None: # Execute the planned trajectory self.arm.execute(plan) outcome = True return outcome def base_move(self, goal): # Initialize the outcome to False base_move_outcome = False # Get the current position of the system's base if self.listener.canTransform(self.reference_frame, self.robot_base_frame, rospy.Time.now()): try: start_position = PoseStamped() start_position.header.frame_id = self.robot_base_frame start_position.header.stamp = rospy.Time.now() start_position.pose = Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1)) start_position = self.listener.transformPose( self.reference_frame, start_position) except: print "TF execption in base_arm_nav.base_move" else: return # Send the goal pose to the MoveBaseAction server self.move_base.send_goal(goal) # Allow 1 minute for the system to get there finished_within_time = self.move_base.wait_for_result( rospy.Duration(60)) # If the system doesn't get there in time, abort the goal and return # to the start position if not finished_within_time: # Cancel current goal self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") # Move back to start_position self.move_base.send_goal(start_position) else: # The system made it to the goal position state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: #Set the outcome to True base_move_outcome = True else: self.move_base.send_goal(start_position) return base_move_outcome def shutdown(self): rospy.loginfo("Stopping the robot") # Stop any further target messages from being processed self.poi_subscriber.unregister() self.error_subscriber.unregister() # Stop any current arm movement self.arm.stop() # Cancel any active move_base goals self.move_base.cancel_goal() rospy.sleep(2) # Stop the robot from moving self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) # Move the arm to the stow position self.arm.set_named_target(self.stow_group_state) self.arm.go() #Shut down MoveIt! cleanly rospy.loginfo("Shutting down 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') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0])/2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 1 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") rospy.sleep(1) arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(1) # Initialize the grasping goal goal = FindGraspableObjectsGoal() goal.plan_grasps = False find_objects.send_goal(goal) find_objects.wait_for_result(rospy.Duration(5.0)) find_result = find_objects.get_result() rospy.loginfo("Found %d objects" %len(find_result.objects)) for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() self.scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_id = 'target' target_size = None the_object = None the_object_dist = 0.30 the_object_dist_min = 0.10 count = -1 for obj in find_result.objects: count +=1 dx = obj.object.primitive_poses[0].position.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist and d > the_object_dist_min: #if dx > the_object_dist_xmin and dx < the_object_dist_xmax: # if dy > the_object_dist_ymin and dy < the_object_dist_ymax: rospy.loginfo("object is in the working zone") the_object_dist = d the_object = count target_size = [0.02, 0.02, 0.05] target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0 target_pose.pose.position.y = obj.object.primitive_poses[0].position.y target_pose.pose.position.z = 0.04 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # wait for the scene to sync self.scene.waitForSync() self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) self.scene.sendColors() grasp_pose = target_pose grasp_pose.pose.position.y -= target_size[1] / 2.0 grasp_pose.pose.position.x += target_size[0] grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) else: rospy.loginfo("object is not in the working zone") rospy.sleep(1) arm.set_named_target('right_up') 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 __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander('right_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.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base_footprint' table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) # Update the current state right_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Return the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link, 'tool') 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) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # 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 some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def placeSrvCallback(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 as e: print "Service call failed: %s" % e self.shutdown() print(target.name) scene = PlanningSceneInterface() # 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.1) arm.set_goal_orientation_tolerance(0.3) # 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 place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(0.05) target_id = str(req.object_id) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = req.x target_pose.pose.position.y = req.y target_pose.pose.position.z = req.z # q = quaternion_from_euler(target.rr, target.rp, target.ry) q = quaternion_from_euler(req.roll, req.pitch, req.yaw) 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] print(target_pose.pose.position.x) print(target_pose.pose.position.y) print(target_pose.pose.position.z) print(target_pose.pose.orientation.x) print(target_pose.pose.orientation.y) print(target_pose.pose.orientation.z) print(target_pose.pose.orientation.w) # Initialize the grasp pose to the target pose place_pose = target_pose # Generate a list of grasps places = self.make_places(place_pose) result = None n_attempts = 0 # 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 = arm.place(target_id, place) print(result) if result == MoveItErrorCodes.SUCCESS: break # rospy.sleep(0.2) scene.remove_world_object(str(req.object_id)) ret = rp_placeResponse() # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo("Success the place operation") ret.result = True else: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") ret.result = False return ret
class PickAndPlace: def setColor(self,name,r,g,b,a=0.9): color=ObjectColor() color.id=name color.color.r=r color.color.g=g color.color.b=b color.color.a=a self.colors[name]=color def sendColors(self): p=PlanningScene() p.is_diff=True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p) def add_point(self,traj, time, positions, velocities=None): point= trajectory_msgs.msg.JointTrajectoryPoint() point.positions= copy.deepcopy(positions) if velocities is not None: point.velocities= copy.deepcopy(velocities) point.time_from_start= rospy.Duration(time) traj.points.append(point) def FollowQTraj(self,q_traj, t_traj): assert(len(q_traj)==len(t_traj)) #Insert current position to beginning. if t_traj[0]>1.0e-2: t_traj.insert(0,0.0) q_traj.insert(0,self.Q(arm=arm)) self.dq_traj=self.QTrajToDQTraj(q_traj, t_traj) def QTrajToDQTraj(self,q_traj, t_traj): dof= len(q_traj[0]) #Modeling the trajectory with spline. splines= [TCubicHermiteSpline() for d in range(dof)] for d in range(len(splines)): data_d= [[t,q[d]] for q,t in zip(q_traj,t_traj)] splines[d].Initialize(data_d, tan_method=splines[d].CARDINAL, c=0.0, m=0.0) #NOTE: We don't have to make spline models as we just want velocities at key points. # They can be obtained by computing tan_method, which will be more efficient. with_tan=True dq_traj= [] for t in t_traj: dq= [splines[d].Evaluate(t,with_tan=True)[1] for d in range(dof)] dq_traj.append(dq) #print dq_traj return dq_traj def JointNames(self): #0arm= 0 return self.joint_names[0] def ROSGetJTP(self,q,t,dq=None): jp= trajectory_msgs.msg.JointTrajectoryPoint() jp.positions= q jp.time_from_start= rospy.Duration(t) if dq is not None: jp.velocities= dq return jp def ToROSTrajectory(self,joint_names, q_traj, t_traj, dq_traj=None): assert(len(q_traj)==len(t_traj)) if dq_traj is not None: (len(dq_traj)==len(t_traj)) #traj= trajectory_msgs.msg.JointTrajectory() self.traj.joint_names= joint_names if dq_traj is not None: self.traj.points= [self.ROSGetJTP(q,t,dq) for q,t,dq in zip(q_traj, t_traj, dq_traj)] else: self.traj.points= [self.ROSGetJTP(q,t) for q,t in zip(q_traj, t_traj)] self.traj.header.stamp= rospy.Time.now() #print self.traj return self.traj def SmoothQTraj(self,q_traj): if len(q_traj)==0: return q_prev= np.array(q_traj[0]) q_offset= np.array([0]*len(q_prev)) for q in q_traj: q_diff= np.array(q) - q_prev for d in range(len(q_prev)): if q_diff[d]<-math.pi: q_offset[d]+=1 elif q_diff[d]>math.pi: q_offset[d]-=1 q_prev= copy.deepcopy(q) q[:]= q+q_offset*2.0*math.pi def add_target(self,target_pose,target_size,frame,x,y,o1,o2,o3,o4): target_pose.header.frame_id=frame target_pose.pose.position.x=x target_pose.pose.position.y=y target_pose.pose.position.z=self.table_ground+self.table_size[2]+target_size[2]/2.0 target_pose.pose.orientation.x=o1 target_pose.pose.orientation.y=o2 target_pose.pose.orientation.z=o3 target_pose.pose.orientation.w=o4 #self.scene.add_box(f_target_id,f_target_pose,f_target_size) def cts(self,start_pose,end_pose,maxtries,exe_signal=False): waypoints=[] fraction=0.0 attempts=0 waypoints.append(start_pose.pose) waypoints.append(end_pose.pose) while fraction!=1 and attempts<maxtries: (plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True) attempts+=1 if (attempts %maxtries==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") return 0,0 continue elif fraction==1: rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.") if exe_signal: q_traj=[self.arm.get_current_joint_values()] t_traj=[0.0] for i in range(2,len(plan.joint_trajectory.points)): q_traj.append(plan.joint_trajectory.points[i].positions) t_traj.append(t_traj[-1]+0.08) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) end_joint_state=plan.joint_trajectory.points[-1].positions signal=1 return signal,end_joint_state #move and rotate def cts_rotate(self,start_pose,end_pose,angle_r,maxtries,exe_signal=False): angle=angle_r*3.14/180.0 waypoints=[] fraction=0.0 attempts=0 waypoints.append(start_pose.pose) waypoints.append(end_pose.pose) while fraction!=1 and attempts<maxtries: (plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True) attempts+=1 if (attempts %maxtries==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") return 0,0.0 continue elif fraction==1: rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.") if exe_signal: q_traj=[self.arm.get_current_joint_values()] t_traj=[0.0] per_angle=angle/(len(plan.joint_trajectory.points)-2) for i in range(2,len(plan.joint_trajectory.points)): joint_inc_list = [j for j in plan.joint_trajectory.points[i].positions] #plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1) joint_inc_list[6]-=per_angle*(i-1) q_traj.append(joint_inc_list) t_traj.append(t_traj[-1]+0.05) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(3.5) end_joint_state=plan.joint_trajectory.points[-1].positions signal=1 return signal,end_joint_state def cts_rotate_delta(self,start_pose,end_pose,angle_pre,angle_r,maxtries,exe_signal=False): angle=(angle_r-angle_pre)*3.14/180.0 waypoints=[] fraction=0.0 attempts=0 waypoints.append(start_pose.pose) waypoints.append(end_pose.pose) while fraction!=1 and attempts<maxtries: (plan,fraction)=self.arm.compute_cartesian_path(waypoints,0.005,0.0,True) attempts+=1 if (attempts %maxtries==0 and fraction!=1): rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.") return 0,0.0 continue elif fraction==1: rospy.loginfo("path compute successfully with "+str(attempts)+" attempts.") if exe_signal: end_joint_state=plan.joint_trajectory.points[-1].positions q_traj=[self.arm.get_current_joint_values()] t_traj=[0.0] per_angle=angle/(len(plan.joint_trajectory.points)-2) for i in range(2,len(plan.joint_trajectory.points)): joint_inc_list = [j for j in plan.joint_trajectory.points[i].positions] #~ plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1) #~ if i==len(plan.joint_trajectory.points)-1: #~ joint_inc_list[6]-=angle #~ q_traj.append(joint_inc_list) #~ t_traj.append(1.5) joint_inc_list[6]-=per_angle*(i-1)+(angle_pre*3.14/180.0) q_traj.append(joint_inc_list) t_traj.append(t_traj[-1]+0.1) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) signal=1 return signal,end_joint_state # shaking function: # freq : shaking freqence # times : shaking time per action def shaking(self,initial_state,end_joint_state_f,end_joint_state_b,freq,times): q_traj=[initial_state] t_traj=[0.0] for i in range(times): q_traj.append(end_joint_state_f) t_traj.append(t_traj[-1]+0.5/freq) q_traj.append(end_joint_state_b) t_traj.append(t_traj[-1]+0.5/freq) q_traj.append(initial_state) t_traj.append(t_traj[-1]+0.5/freq) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(5) def shake_a(self,pre_p_angle,r_angle,amp): start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate shift_pose=deepcopy(start_shake_pose) r_angle=(r_angle/180.0)*3.14 pre_p_angle=(pre_p_angle/180.0)*3.14 shift_pose.pose.position.x+=amp*math.sin(r_angle)*math.cos(pre_p_angle)# in verticle direction shift_pose.pose.position.y-=amp*math.sin(r_angle)*math.sin(pre_p_angle) shift_pose.pose.position.z+=amp*math.cos(r_angle)#... signal,end_joint_state=self.cts(start_shake_pose,shift_pose,300) return signal,end_joint_state def shake_axis(self,pre_p_angle,r_angle,amp,axis_angle): start_shake_pose=self.arm.get_current_pose(self.arm_end_effector_link)# for trajectory of shaking start_joint_state=self.arm.get_current_joint_values() # for state[6] to rotate shift_pose_b=deepcopy(start_shake_pose) shift_pose_f=deepcopy(start_shake_pose) r_angle=(r_angle/180.0)*3.14 axis_angle=(axis_angle/180.0)*3.14 pre_p_angle=(pre_p_angle/180.0)*3.14 shift_pose_b.pose.position.x-=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.cos(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.cos(pre_p_angle) shift_pose_b.pose.position.y+=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.sin(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.sin(pre_p_angle) shift_pose_b.pose.position.z+=0.5*amp*math.sin(axis_angle)*math.sin(r_angle)+0.5*amp*math.cos(axis_angle)*math.cos(r_angle)#... signal,end_joint_state_1=self.cts(start_shake_pose,shift_pose_b,300) shift_pose_f.pose.position.x+=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.cos(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.cos(pre_p_angle) shift_pose_f.pose.position.y-=0.5*amp*math.sin(axis_angle)*math.cos(r_angle)*math.sin(pre_p_angle)-0.5*amp*math.cos(axis_angle)*math.sin(r_angle)*math.sin(pre_p_angle) shift_pose_f.pose.position.z-=0.5*amp*math.sin(axis_angle)*math.sin(r_angle)+0.5*amp*math.cos(axis_angle)*math.cos(r_angle)#... signal,end_joint_state_2=self.cts(start_shake_pose,shift_pose_f,300) return signal,end_joint_state_1,end_joint_state_2 def setupSence(self): r_tool_size=[0.03,0.02,0.18] l_tool_size=[0.03,0.02,0.18] #real scene table table_pose=PoseStamped() table_pose.header.frame_id=self.reference_frame table_pose.pose.position.x=-0.052 table_pose.pose.position.y=0.65 table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0 table_pose.pose.orientation.w=1.0 self.scene.add_box(self.table_id,table_pose,self.table_size) #left gripper l_p=PoseStamped() l_p.header.frame_id=self.arm_end_effector_link l_p.pose.position.x=0.00 l_p.pose.position.y=0.057 l_p.pose.position.z=0.09 l_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.l_id,l_p,l_tool_size) #right gripper r_p=PoseStamped() r_p.header.frame_id=self.arm_end_effector_link r_p.pose.position.x=0.00 r_p.pose.position.y=-0.057 r_p.pose.position.z=0.09 r_p.pose.orientation.w=1 self.scene.attach_box(self.arm_end_effector_link,self.r_id,r_p,r_tool_size) #Params: pose of bottle, grasp_radius, grasp_height, grasp_theta def get_grasp_pose(self,pose,r,theta): g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=pose.pose.position.x+r*math.sin(theta) g_p.pose.position.y=pose.pose.position.y-r*math.cos(theta) g_p.pose.position.z=pose.pose.position.z g_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) g_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta)) g_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) g_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta)) return g_p def get_pour_pose(self,pose,h,r,theta): p_p=PoseStamped() p_p.header.frame_id=self.arm_end_effector_link p_p.pose.position.x=pose.pose.position.x-r*math.cos(theta) p_p.pose.position.y=pose.pose.position.y+r*math.sin(theta) p_p.pose.position.z=pose.pose.position.z+h theta*=-1 p_p.pose.orientation.w=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) p_p.pose.orientation.x=-0.5*(math.cos(0.5*theta)+math.sin(0.5*theta)) p_p.pose.orientation.y=0.5*(math.cos(0.5*theta)-math.sin(0.5*theta)) p_p.pose.orientation.z=0.5*(math.sin(0.5*theta)+math.cos(0.5*theta)) return p_p def pour_rotate(self,initial_pose,angle_pre,angle_r,r,maxtries): angle_pre=(angle_pre/180.0)*3.14 angle_r_1=(angle_r/180.0)*3.14 cur_pose=self.arm.get_current_pose(self.arm_end_effector_link) final_pose=deepcopy(initial_pose) final_pose.pose.position.x+=r*(1-math.cos(angle_r_1))*math.cos(angle_pre) final_pose.pose.position.y+=r*(1-math.cos(angle_r_1))*math.sin(angle_pre) final_pose.pose.position.z+=r*math.sin(angle_r_1) #~ signal,e_j_s=self.cts_rotate_delta(cur_pose,final_pose,angle_r_pre,angle_r,maxtries,True) signal,e_j_s=self.cts_rotate(cur_pose,final_pose,angle_r,maxtries,True) return signal def p_r_trail(self,angle_pre,angle_r,r,maxtries): angle_pre=(angle_pre/180.0)*3.14 angle_r_1=(angle_r/180.0)*3.14 initial_pose=self.arm.get_current_pose(self.arm_end_effector_link) final_pose=deepcopy(initial_pose) final_pose.pose.position.x+=r*(1-math.cos(angle_r_1))*math.cos(angle_pre) final_pose.pose.position.y+=r*(1-math.cos(angle_r_1))*math.sin(angle_pre) final_pose.pose.position.z+=r*math.sin(angle_r_1) signal,e_j_s=self.cts_rotate(initial_pose,final_pose,angle_r,maxtries,True) return signal def move_back(self,back_pose): current_pose=self.arm.get_current_pose(self.arm_end_effector_link) signal,end_j=self.cts(current_pose,back_pose,300,True) return signal def pg_g_pp(self,pose,r): start_pose=self.arm.get_current_pose(self.arm_end_effector_link) p_i_radian=np.arctan(abs(pose.pose.position.x/pose.pose.position.y)) p_i_angle=p_i_radian*180.0/3.14 pick_list=[p_i_angle,5.0,25.0,45.0,65.0,15.0,35.0,55.0,75.0,10.0,20.0,30.0,40.0,50.0,60.0] for i in range(len(pick_list)): theta=(pick_list[i]/180.0)*3.14 if pose.pose.position.x>0: theta*=-1.0 grasp_pose=self.get_grasp_pose(pose,r,theta) #pick up signal,e_j_s=self.cts(start_pose,grasp_pose,300,True) if signal==0: continue break rospy.sleep(1) self.gripperCtrl(0) rospy.sleep(2) #move to ori_pose current_pose=self.arm.get_current_pose(self.arm_end_effector_link) signal,e_j_s=self.cts(current_pose,start_pose,300,True) rospy.sleep(2) rospy.loginfo("Grasping done.") return start_pose,grasp_pose def pp_ps_old(self,target_pose,pour_angle,r_pour,h_pour): for i in range(len(pour_angle)): maxtries=300 start_pose=self.arm.get_current_pose(self.arm_end_effector_link) theta=(pour_angle[i]/180.0)*3.14 pour_pose=self.get_pour_pose(target_pose,h_pour,r_pour,theta) #move to pose signal_1,e_j_s=self.cts_rotate(start_pose,pour_pose,90.0,maxtries,True) if signal_1==0: continue pre_pour_angle=pour_angle[i] rospy.loginfo("Ready for pouring.") return pre_pour_angle def pp_ps(self,target_pose,pour_angle,r_pour,h_pour): maxtries=300 start_pose=self.arm.get_current_pose(self.arm_end_effector_link) theta=(pour_angle/180.0)*3.14 pour_pose=self.get_pour_pose(target_pose,h_pour,r_pour,theta) #move to pose signal_1,e_j_s=self.cts_rotate(start_pose,pour_pose,90.0,maxtries,True) return signal_1 def go_back(self,ori_pose,pre_grasp_pose): cur_pose=self.arm.get_current_pose(self.arm_end_effector_link) signal,e1=self.cts(cur_pose,ori_pose,300,True) rospy.loginfo("back to pre_grasp pose, ready for placing bottle..") rospy.sleep(1) signal_1,e2=self.cts(ori_pose,pre_grasp_pose,300,True) rospy.loginfo("back to grasp pose, open gripper..") rospy.sleep(1) self.gripperCtrl(255) rospy.sleep(1) signal_2,e3=self.cts(pre_grasp_pose,ori_pose,300,True) rospy.loginfo("back to pre_grasp pose, ready for next kind of source.") def rotate_back(self,joint_state): q_traj=[self.arm.get_current_joint_values()] t_traj=[0.0] q_traj.append(joint_state) t_traj.append(t_traj[-1]+3) self.FollowQTraj(q_traj,t_traj) self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, self.dq_traj)) rospy.sleep(3) def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.scene=PlanningSceneInterface() pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=10) #pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1) self.scene_pub=rospy.Publisher('planning_scene',PlanningScene) self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition) self.colors=dict() rospy.sleep(1) self.robot=moveit_commander.robot.RobotCommander() self.arm=MoveGroupCommander('arm') self.arm.allow_replanning(True) cartesian=rospy.get_param('~cartesian',True) self.arm_end_effector_link=self.arm.get_end_effector_link() self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.025) self.arm.allow_replanning(True) self.reference_frame='base_link' self.arm.set_pose_reference_frame(self.reference_frame) self.arm.set_planning_time(5) #shaking self.joint_names= [[]] self.joint_names[0]= rospy.get_param('controller_joint_names') self.traj= trajectory_msgs.msg.JointTrajectory() self.sub_jpc= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory,queue_size=10) #scene planning self.l_id='l_tool' self.r_id='r_tool' self.table_id='table' self.target1_id='target1_object' self.target2_id='target2_object' #~ self.target3_id='target3_object' #~ self.target4_id='target4_object' self.f_target_id='receive_container' self.scene.remove_world_object(self.l_id) self.scene.remove_world_object(self.r_id) self.scene.remove_world_object(self.table_id) self.scene.remove_world_object(self.target1_id) self.scene.remove_world_object(self.target2_id) #~ self.scene.remove_world_object(self.target3_id) #~ self.scene.remove_world_object(self.target4_id) self.scene.remove_world_object(self.f_target_id) #self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id) self.table_ground=0.13 self.table_size=[0.9,0.6,0.018] self.setupSence() target1_size=[0.035,0.035,0.19] target2_size=target1_size #~ target3_size=target1_size #~ target4_size=target1_size self.f_target_size=[0.2,0.2,0.04] f_target_pose=PoseStamped() pre_pour_pose=PoseStamped() target1_pose=PoseStamped() target2_pose=PoseStamped() #~ target3_pose=PoseStamped() #~ target4_pose=PoseStamped() joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')] joint_names= rospy.get_param('controller_joint_names') traj= trajectory_msgs.msg.JointTrajectory() traj.joint_names= joint_names #final target l_gripper=0.18 #self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1) #~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1) #~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size) #self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1) #target localization msg1 = rospy.wait_for_message('/aruco_simple/pose',Pose) rospy.sleep(1) msg2 = rospy.wait_for_message('/aruco_simple/pose2',Pose) rospy.sleep(1) self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.1-l_gripper,0,0,0,1) self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size) self.add_target(target1_pose,target1_size,self.reference_frame,-msg1.position.y-0.257,-msg1.position.x+0.81-0.065,0,0,0,1) self.scene.add_box(self.target1_id,target1_pose,target1_size) #~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1) #~ self.scene.add_box(self.target2_id,target2_pose,target2_size) #~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1) #~ self.scene.add_box(self.target3_id,target3_pose,target3_size) #~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1) #~ self.scene.add_box(self.target4_id,target4_pose,target4_size) #attach_pose g_p=PoseStamped() g_p.header.frame_id=self.arm_end_effector_link g_p.pose.position.x=0.00 g_p.pose.position.y=-0.00 g_p.pose.position.z=0.025 g_p.pose.orientation.w=0.707 g_p.pose.orientation.x=0 g_p.pose.orientation.y=-0.707 g_p.pose.orientation.z=0 #set color self.setColor(self.target1_id,0.8,0,0,1.0) self.setColor(self.target2_id,0.8,0,0,1.0) #~ self.setColor(self.target3_id,0.8,0,0,1.0) #self.setColor(self.target4_id,0.8,0,0,1.0) self.setColor(self.f_target_id,0.8,0.3,0,1.0) self.setColor('r_tool',0.8,0,0) self.setColor('l_tool',0.8,0,0) self.sendColors() self.gripperCtrl(255) rospy.sleep(3) self.arm.set_named_target("initial_arm") self.arm.go() rospy.sleep(3) #j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156] #j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135] j_ori_state=[-1.2628753185272217, -0.442996621131897, -0.1326361745595932, 2.333048105239868, -0.15598002076148987, -1.2167049646377563, 3.1414425373077393] #signal= True self.arm.set_joint_value_target(j_ori_state) self.arm.go() rospy.sleep(3) tar_num=1 maxtries=300 r_grasp=0.16 #~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"] #~ save_data=["carrot.txt","pepper.txt","cucumber.txt"] #ratio_table=[0.03,0.025,0.04] for i in range(0,tar_num): #grasp target rospy.loginfo("Choosing source...") if i==0: target_pose=target1_pose target_id=self.target1_id target_size=target1_size #~ amp=0.13 #~ freq=2.75 #~ r_angle=40.0 elif i==1: target_pose=target2_pose target_id=self.target2_id target_size=target2_size #~ amp=0.15 #~ freq=2.5 #~ r_angle=45.0 elif i==2: target_pose=target3_pose target_id=self.target3_id target_size=target3_size amp=0.15 freq=2.75 r_angle=40.0 elif i==3: target_pose=target4_pose target_id=self.target4_id target_size=target4_size amp=0.1 freq=2.75 r_angle=45.0 r_pour=0.14 r_bottle=0.1 #~ h_pour=0.1 pour_angle=[0.0,-15.0,15.0,-30.0,30.0,-45.0,45.0] h_pour_list=[0.12] #~ tip_angle=[30.0,40.0,50,60.0,70.0,80.0] tip_angle=[30.0,40.0,50.0] s_axis=[0.0] shake_freq=[2.5,2.75] #~ shake_freq=[2.50,2.75] shake_amp=[0.15,0.18] shake_per_times=4 shake_times_tgt=2 rospy.loginfo("Params loaded.") rospy.loginfo("Current Source: "+str(i+1)+" ") #grasp and back ori_pose,g_pose=self.pg_g_pp(target_pose,r_grasp) ori_joint_state=self.arm.get_current_joint_values() #move to target position for pouring #~ for m in range(len(pour_angle)): for m in range(len(h_pour_list)): dfile=open("moyashi_0_shake_1","a") #~ for x in range(len(s_axis)): for x in range(len(s_axis)): for j in range(len(tip_angle)): #~ for x in range(len(s_axis)): for k in range(len(shake_freq)): #~ dfile=open("carrot_h_"+str(h_pour_list[m])+"_ta_"+str(tip_angle[j])+"_axis_"+str(s_axis[x])"_shaking_freq_"+str(shake_freq[k])+"_1","a") for n in range(len(shake_amp)): pp_ps_sgn=self.pp_ps(f_target_pose,pour_angle[0],r_pour,h_pour_list[m]) if pp_ps_sgn==0: rospy.loginfo("pre-Pouring angle not correct, back for replanning...") self.rotate_back(ori_joint_state) rospy.sleep(2) continue initial_pose=self.arm.get_current_pose(self.arm_end_effector_link) pour_signal=self.pour_rotate(initial_pose,pour_angle[0],tip_angle[j],r_bottle,maxtries) rospy.sleep(2) if pour_signal !=1: rospy.loginfo("Pouring angle not correct, back for replanning...") self.rotate_back(ori_joint_state) rospy.sleep(2) continue back_signal=0 ratio=[0] ratio_signal=0 shake_times=0 for n_shake in range(10): #~ for x in range(len(s_axis)): if back_signal==1: pp_ps_sgn=self.pp_ps(f_target_pose,pour_angle[0],r_pour,h_pour_list[m]) if pp_ps_sgn==0: rospy.loginfo("pre-Pouring angle not correct, back for replanning...") self.rotate_back(ori_joint_state) rospy.sleep(2) continue initial_pose=self.arm.get_current_pose(self.arm_end_effector_link) pour_signal=self.pour_rotate(initial_pose,pour_angle[0],tip_angle[j],r_bottle,maxtries) rospy.sleep(2) if pour_signal !=1: rospy.loginfo("Pouring angle not correct, back for replanning...") self.rotate_back(ori_joint_state) rospy.sleep(2) continue back_signal=0 amp=shake_amp[n] freq=shake_freq[k] shake_times+=1 rospy.loginfo("shaking degree : "+str(tip_angle[j])+", axis degree: "+str(s_axis[x])+", shaking amp : "+str(amp)+ ", shaking freq : "+str(freq)+", times :"+str(shake_times)) signal=True start_joint_state=self.arm.get_current_joint_values() signal,end_joint_state_b,end_joint_state_f=self.shake_axis(pour_angle[0],tip_angle[j],amp,s_axis[x]) self.shaking(start_joint_state,end_joint_state_f,end_joint_state_b,freq,shake_per_times) rospy.sleep(0.5) area_ratio= rospy.wait_for_message('/ratio_carrot',Float64) dfile.write("%f %f %f %f %f %f \r\n" % (h_pour_list[m],tip_angle[j],s_axis[x],shake_freq[k],shake_amp[n],area_ratio.data)) #~ ratio.append(area_ratio) if (area_ratio.data-ratio[-1]<0.01): ratio_signal+=1 if ratio_signal==2: ratio_signal=0 self.rotate_back(ori_joint_state) back_signal=1 rospy.sleep(1) ratio.append(area_ratio.data) self.rotate_back(ori_joint_state) rospy.sleep(3) #remove and shut down self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool') self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('contract') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.4 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.40 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.40 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() rospy.sleep(2) # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.40 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[ 2] + 0.08 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('contract') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('kinect_ur3_move', anonymous=True) self.listener = tf.TransformListener() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.initialPose = PoseStamped() self.Pose_goal = PoseStamped() self.start_goal = PoseStamped() self.current_goal = PoseStamped() self.next_pose_goal = PoseStamped() self.waypoints = [] self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.allow_replanning(True) self.robot_arm.set_goal_position_tolerance(0.01) self.robot_arm.set_goal_orientation_tolerance(0.1) print("====== move plan go to initial pose ======") self.initialPose.header.frame_id = 'leftee_link' self.initialPose.pose.position.x = 0.103190426853 # red line 0.2 0.2 self.initialPose.pose.position.y = -0.827689781681 # green line 0.15 0.15 self.initialPose.pose.position.z = 0.978219026059 # blue line # 0.35 0.6 # Pose_goal.pose.orientation = start_pose.orientation self.initialPose.pose.orientation.x =-0.556104592788 self.initialPose.pose.orientation.y = 0.448401851544 self.initialPose.pose.orientation.z = -0.495185387101 self.initialPose.pose.orientation.w = 0.494444024955 self.start_goal = self.robot_arm.get_current_pose("leftee_link") self.Pose_goal = PoseStamped() t = rospy.Time(0) self.listener.waitForTransform('/leftbase_link','/world',t,rospy.Duration(5)) if self.listener.canTransform('/leftbase_link','/world',t): self.Pose_goal = self.listener.transformPose('/leftbase_link',self.start_goal) print(self.Pose_goal) else: rospy.logerr('Transformation is not possible!') sys.exit(0) print("Start pose:%s",self.Pose_goal) self.listener.waitForTransform('/leftbase_link','/kinect2_rgb_optical_frame',t,rospy.Duration(2)) self.waypoints.append(self.Pose_goal.pose) # initialJointValues = [4.111435176058169, # 2.715653621728593, # 0.7256647920137681, # 5.983459446005512, # -5.682231515319553, # -6.283185179581844] # self.robot_arm.set_joint_value_target(initialJointValues) # plan = self.robot_arm.plan() # self.robot_arm.execute(plan) # rospy.sleep(3) # wait for robot to move to initial position # # # save these positions on run # self.robot_initialPos = Pose().position # self.robot_initialPos.x = self.robot_arm.get_current_pose().pose.position.x # self.robot_initialPos.y = self.robot_arm.get_current_pose().pose.position.z # self.robot_initialPos.z = self.robot_arm.get_current_pose().pose.position.y # # self.maxLeft = self.robot_initialPos.x - 0.200 # x left # self.maxRight = self.robot_initialPos.x + 0.200 # x right # self.maxHeight = self.robot_initialPos.y + 0.300 # y height # self.maxUp = self.robot_initialPos.z + 0.100 # z up # self.maxDown = self.robot_initialPos.z - 0.100 # z down # # 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.index_tip_variation = Point() # self.indextip = Point() # self.executing = False def move_code(self): # self.robot_arm.set_pose_reference_frame('leftbase_link') # self.wpose = copy.deepcopy(self.Pose_goal.pose) # # self.wpose.position.x = 0.22844899063259538 # # self.wpose.position.y = 0.3232940134968078 # self.wpose.position.z += 0.12581195793171412 # self.waypoints.append(copy.deepcopy(self.wpose)) # wpose.position.x = 0.24706361090659945 # wpose.position.y = 0.399076783202562 # wpose.position.z = 0.11524980903243331 # self.waypoints.append(copy.deepcopy(wpose)) # wpose.position.x = 0.2384431148207265 # wpose.position.y = 0.42584037071081504 # wpose.position.z = 0.11603463304830763 # self.waypoints.append(copy.deepcopy(wpose)) # fraction = 0.0 # attempts = 0 # self.robot_arm.set_start_state_to_current_state() # while fraction < 1.0 and attempts < 100: # (plan, fraction) = self.robot_arm.compute_cartesian_path( # self.waypoints, # waypoints to follow # 0.01, # eef_step # 0.0) # jump_threshold # attempts+=1 # print(attempts) # if fraction == 1.0: # self.robot_arm.execute(plan, wait=True) # else: # print("failed") self.pose_goal1 = PoseStamped() self.pose_goal1.header.frame_id = '/leftbase_link' self.pose_goal1.pose.position.x = 0.22844899063259538 self.pose_goal1.pose.position.y = 0.3232940134968078 self.pose_goal1.pose.position.z = 0.12581195793171412 self.pose_goal1.pose.orientation.x = 0.651452360152262 self.pose_goal1.pose.orientation.y = -0.22289710950191718 self.pose_goal1.pose.orientation.z = 0.29974203617303874 self.pose_goal1.pose.orientation.w = 0.6603646059402177 self.robot_arm.set_pose_target(self.pose_goal1,"leftee_link") self.robot_arm.go() # rospy.sleep(1) # traj = self.robot_arm.plan() # self.robot_arm.execute(traj,wait=True) self.pose_goal2 = PoseStamped() self.pose_goal2.header.frame_id = '/leftbase_link' self.pose_goal2.pose.position.x = 0.24706361090659945 self.pose_goal2.pose.position.y = 0.399076783202562 self.pose_goal2.pose.position.z = 0.11524980903243331 self.pose_goal2.pose.orientation.x = 0.6965603247710086 self.pose_goal2.pose.orientation.y = -0.3587025266077312 self.pose_goal2.pose.orientation.z = 0.24958274959805474 self.pose_goal2.pose.orientation.w = 0.5690735123542583 self.robot_arm.set_pose_target(self.pose_goal2,"leftee_link") self.robot_arm.go() # rospy.sleep(1) # traj = self.robot_arm.plan() # self.robot_arm.execute(traj,wait=True) self.pose_goal3 = PoseStamped() self.pose_goal3.header.frame_id = '/leftbase_link' self.pose_goal3.pose.position.x = 0.2384431148207265 self.pose_goal3.pose.position.y = 0.42584037071081504 self.pose_goal3.pose.position.z = 0.11603463304830763 self.pose_goal3.pose.orientation.x = 0.6611332801180229 self.pose_goal3.pose.orientation.y = -0.25768212027385673 self.pose_goal3.pose.orientation.z = 0.31112563888658357 self.pose_goal3.pose.orientation.w = 0.6322211224239263 self.robot_arm.set_pose_target(self.pose_goal3,"leftee_link") self.robot_arm.go() # rospy.sleep(1) # traj = self.robot_arm.plan() # self.robot_arm.execute(traj,wait=True) self.pose_goal4 = PoseStamped() self.pose_goal4.header.frame_id = '/leftbase_link' self.pose_goal4.pose.position.x = 0.20267968408694 self.pose_goal4.pose.position.y = 0.3807708740746333 self.pose_goal4.pose.position.z = 0.1621807421476515 self.pose_goal4.pose.orientation.x = 0.6500893824643447 self.pose_goal4.pose.orientation.y = -0.17866011127418618 self.pose_goal4.pose.orientation.z = 0.19968845817208114 self.pose_goal4.pose.orientation.w = 0.71104773336217 self.robot_arm.set_pose_target(self.pose_goal4,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) self.pose_goal5 = PoseStamped() self.pose_goal5.header.frame_id = '/leftbase_link' self.pose_goal5.pose.position.x = 0.19261972875892824 self.pose_goal5.pose.position.y = 0.3487254051054294 self.pose_goal5.pose.position.z = 0.17423391692094703 self.pose_goal5.pose.orientation.x = 0.6623443436827807 self.pose_goal5.pose.orientation.y = -0.20253221354720935 self.pose_goal5.pose.orientation.z = 0.17253708109857815 self.pose_goal5.pose.orientation.w = 0.7003653535927353 self.robot_arm.set_pose_target(self.pose_goal5,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) self.pose_goal6 = PoseStamped() self.pose_goal6.header.frame_id = '/leftbase_link' self.pose_goal6.pose.position.x = 0.2157980659367808 self.pose_goal6.pose.position.y = 0.36988878402044156 self.pose_goal6.pose.position.z = 0.1431730427943465 self.pose_goal6.pose.orientation.x = 0.6607398125620346 self.pose_goal6.pose.orientation.y = -0.24398334555829662 self.pose_goal6.pose.orientation.z = 0.2786725065244132 self.pose_goal6.pose.orientation.w = 0.6528680274703835 self.robot_arm.set_pose_target(self.pose_goal6,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) self.pose_goal7 = PoseStamped() self.pose_goal7.header.frame_id = '/leftbase_link' self.pose_goal7.pose.position.x = 0.22114246898738382 self.pose_goal7.pose.position.y = 0.4464193752676767 self.pose_goal7.pose.position.z = 0.13641486607992853 self.pose_goal7.pose.orientation.x = 0.6791504338035828 self.pose_goal7.pose.orientation.y = -0.2962309634157678 self.pose_goal7.pose.orientation.z = 0.2696628694935311 self.pose_goal7.pose.orientation.w = 0.6150478366718206 self.robot_arm.set_pose_target(self.pose_goal7,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) self.pose_goal8 = PoseStamped() self.pose_goal8.header.frame_id = '/leftbase_link' self.pose_goal8.pose.position.x = 0.2045166261139516 self.pose_goal8.pose.position.y = 0.3927153592410454 self.pose_goal8.pose.position.z = 0.15411342380507587 self.pose_goal8.pose.orientation.x = 0.6693368542084698 self.pose_goal8.pose.orientation.y = -0.2647950292869915 self.pose_goal8.pose.orientation.z = 0.2573395449506767 self.pose_goal8.pose.orientation.w = 0.6447077839360945 self.robot_arm.set_pose_target(self.pose_goal8,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) self.pose_goal9 = PoseStamped() self.pose_goal9.header.frame_id = '/leftbase_link' self.pose_goal9.pose.position.x = 0.2213576147649789 self.pose_goal9.pose.position.y = 0.32543277234322004 self.pose_goal9.pose.position.z = 0.1435574570513627 self.pose_goal9.pose.orientation.x = 0.6325814784326014 self.pose_goal9.pose.orientation.y = -0.24063914058631417 self.pose_goal9.pose.orientation.z = 0.38283744641817435 self.pose_goal9.pose.orientation.w = 0.6287837201947233 self.robot_arm.set_pose_target(self.pose_goal9,"leftee_link") traj = self.robot_arm.plan() self.robot_arm.execute(traj,wait=True) # self.pose_goal1 = PoseStamped() # self.pose_goal1.header.frame_id = '/leftbase_link' # self.pose_goal1.pose.position.x = 0.2053894995328921 # self.pose_goal1.pose.position.y = 0.35793405277574963 # self.pose_goal1.pose.position.z = 0.15759221743998164 # self.pose_goal1.pose.orientation.x = 0.6896423630881793 # self.pose_goal1.pose.orientation.y = -0.3133288084434257 # self.pose_goal1.pose.orientation.z = 0.1543914767126632 # self.pose_goal1.pose.orientation.w = 0.6343356688316201 # self.robot_arm.set_pose_target(self.pose_goal1,"leftee_link") # # traj = self.robot_arm.plan() # # self.robot_arm.execute(traj,wait=True) # self.robot_arm.go() # # rospy.sleep(1) def back_initailPose(self): self.robot_arm.set_pose_target(self.Pose_goal,"leftee_link") self.robot_arm.go() rospy.sleep(5) def callback_left(self, data): print("left===========") self.next_pose_goal = data rospy.loginfo(rospy.get_name() + ": kinect data %s" % data) # self.move_code() def callback_right(self, data): print("right===========") rospy.loginfo("I heard: [%s]" % data) self.back_initailPose() def kinectlistener(self): # rospy.Subscriber("leapmotion/raw", leap, callback) rospy.Subscriber("/kinect2/click_point/left", PoseStamped, self.callback_left) rospy.Subscriber("/kinect2/click_point/right", PoseStamped, self.callback_right) rospy.spin()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_attached_object_demo') # 初始化场景对象 scene = PlanningSceneInterface() rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 设置每次运动规划的时间限制:10s arm.set_planning_time(10) # 移除场景中之前运行残留的物体 scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('target') # 设置桌面的高度 table_ground = 0.6 # 设置table和tool的三维尺寸 table_size = [0.1, 0.7, 0.01] tool_size = [0.2, 0.02, 0.02] # 设置tool的位姿 p = PoseStamped() p.header.frame_id = end_effector_link p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = -0.015 p.pose.position.z = 0.0 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # 将tool附着到机器人的终端 scene.attach_box(end_effector_link, 'tool', p, tool_size) # 将table加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = 'base_link' table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) rospy.sleep(2) # 更新当前的位姿 arm.set_start_state_to_current_state() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [ 0.827228546495185, 0.29496592875743577, 1.1185644936946095, -0.7987583317769674, -0.18950024740190782, 0.11752152218233858 ] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(1) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class MoveItObstaclesDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('manipulator') # 设置机械臂工作空间 self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100]) # 设置机械臂最大速度 self.arm.set_max_velocity_scaling_factor(value=0.1) # 获取终端link的名称 self.end_effector_link = self.arm.get_end_effector_link() rospy.loginfo('end effector link: {}'.format(self.end_effector_link)) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 # self.arm.allow_replanning(True) self.arm.set_num_planning_attempts(10) # self.arm.allow_looking(True) # 设置目标位置所使用的参考坐标系 self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(3) # 控制机械臂先回到初始化位置 self.arm.set_named_target('home') self.arm.go() rospy.sleep(2) def planning(self, start_point, end_point): """ 功能:动态避障 :param start_point: 起始点, type: dict :param end_point: 终点, type: dict :return: None """ # 移动机械臂到指定位置,并获取当前位姿数据为机械臂运动的起始位姿 if start_point: self.move_arm(p=start_point) self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose # 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中 # way_points = self.get_way_points(start_point, end_point) while True: print('set planner id: RRT') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('up') self.arm.go() rospy.sleep(5) print('set planner id: PRM') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('home') self.arm.go() rospy.sleep(5) # ------------------------------------------------------------------- def get_way_points(self, a, b): way_points = [] # plan 1 self.arm.set_named_target('up') traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") else: rospy.logerr("Trajectory is empty. Planning false!") self.arm.clear_pose_targets() # plan 2 # traj = self.moveit_planning(p=b) for i, p in enumerate(traj.joint_trajectory.points): # rospy.loginfo('waypoint: {}'.format(i)) if i%2 == 0: point = { 'x': p.positions[0], 'y': p.positions[1], 'z': p.positions[2], 'ox': p.positions[3], 'oy': p.positions[4], 'oz': p.positions[5] } way_points.append(point) rospy.loginfo('waypoint: \n {}'.format(way_points)) rospy.loginfo(len(way_points)) return way_points def moveit_planning(self, p): """ 使用moveit求解器规划路径 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start moveit planning...') target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.pose.position.x = p['x'] target_pose.pose.position.y = p['y'] target_pose.pose.position.z = p['z'] if 'ox' in p.keys() and p['ox']: target_pose.pose.orientation.x = p['ox'] if 'oy' in p.keys() and p['oy']: target_pose.pose.orientation.y = p['oy'] if 'oz' in p.keys() and p['oz']: target_pose.pose.orientation.z = p['oz'] if 'ow' in p.keys() and p['ow']: target_pose.pose.orientation.w = p['ow'] # 传入一个PoseStamped # self.arm.set_pose_target(target_pose, self.end_effector_link) # 尝试直接传入一个列表 self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link) traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") return traj else: rospy.logerr("Trajectory is empty. Planning false!") def move_arm(self, p): """ 移动机械臂到p点 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start arm moving...') traj = self.moveit_planning(p) self.arm.execute(traj) # 设置当前位置为起始位置 self.arm.set_start_state_to_current_state() rospy.sleep(1) def stop_arm(self): """ 急停 :return: """ pass def exist_danger_obstacle(self): """ 环境中是否存在危险的障碍物 :return: True or False """ return False def get_obstacle_octomap(self): """ 获取环境的octomap信息 :return: """ pass
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.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.1) arm.set_max_velocity_scaling_factor(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() print(end_effector_link) # 控制机械臂先回到初始化位置 # 获取当前位姿数据最为机械臂运动的起始位姿 # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 # arm.set_named_target('test5') # arm.go() # rospy.sleep(1) ###### marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot) listener = tf.TransformListener() r = rospy.Rate(100) while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/base_link', '/ar_marker_4', rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue x = euler_from_quaternion(rot) print x quaternion = quaternion_from_euler(x[0] + 1.5707, x[1], x[2] + 3.14) y = euler_from_quaternion(quaternion) print y arm.set_named_target('test5') arm.go() rospy.sleep(1) start_pose = arm.get_current_pose(end_effector_link).pose ###### # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.orientation.x = quaternion[0] wpose.orientation.y = quaternion[1] wpose.orientation.z = quaternion[2] wpose.orientation.w = quaternion[3] wpose.position.x = 0.33 wpose.position.y = -0.134 wpose.position.z = 1.0484 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 10 #最大尝试规划次数 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.005, # 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.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) # 控制机械臂先回到初始化位置 # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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('panda_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 = 'panda_link0' 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) rospy.sleep(1) # 控制机械臂先回到初始化位置 #arm.set_named_target('home') #arm.go() #rospy.sleep(2) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.4, 1.4, 0.02] box1_size = [0.1, 0.1, 0.1] #box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.4 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) #box2_pose = PoseStamped() #box2_pose.header.frame_id = reference_frame #box2_pose.pose.position.x = 0.19 #box2_pose.pose.position.y = 0.15 #box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 #box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) #self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.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.4 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 moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(1) arm.set_max_velocity_scaling_factor(1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() listener = tf.TransformListener() while not rospy.is_shutdown(): try: # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换 (trans, rot) = listener.lookupTransform('/base_link', '/iron_block', rospy.Time(0)) object_detect_x = trans[0] object_detect_y = trans[1] # object_detect_z = trans[2] # if(object_detect_x>0.033 or object_detect_x<0.027): # # print("detect x error!") # continue if (object_detect_y > 0.25 or object_detect_y < 0.2): # print("detect y error!") continue # if(trans[2]>-0.04 or trans[2]<-0.08): # print("detect z error!") # continue print(trans) # print(rot) # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # 姿态使用四元数描述,基于base_link坐标系 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = object_detect_x target_pose.pose.position.y = object_detect_y - 0.05 target_pose.pose.position.z = 0.15 target_pose.pose.orientation.x = -rot[0] target_pose.pose.orientation.y = -rot[1] target_pose.pose.orientation.z = -rot[2] target_pose.pose.orientation.w = -rot[3] # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) # 规划运动路径 traj = arm.plan() num_of_points = len(traj.joint_trajectory.points) d = rospy.Duration.from_sec(1) print traj.joint_trajectory.points[num_of_points - 1].time_from_start for index in range(num_of_points): traj.joint_trajectory.points[index].time_from_start *= \ d/traj.joint_trajectory.points[num_of_points-1].time_from_start # 按照规划的运动路径控制机械臂运动 arm.execute(traj) # # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # # 姿态使用四元数描述,基于base_link坐标系 # target_pose = PoseStamped() # target_pose.header.frame_id = reference_frame # target_pose.header.stamp = rospy.Time.now() # target_pose.pose.position.x = object_detect_x # target_pose.pose.position.y = object_detect_y-0.1 # target_pose.pose.position.z = 0.10 # target_pose.pose.orientation.x = -rot[0] # target_pose.pose.orientation.y = -rot[1] # target_pose.pose.orientation.z = -rot[2] # target_pose.pose.orientation.w = -rot[3] # # 设置机器臂当前的状态作为运动初始状态 # arm.set_start_state_to_current_state() # # 设置机械臂终端运动的目标位姿 # arm.set_pose_target(target_pose, end_effector_link) # # 规划运动路径 # traj = arm.plan() # num_of_points = len(traj.joint_trajectory.points) # d = rospy.Duration.from_sec(0.2) # print traj.joint_trajectory.points[num_of_points-1].time_from_start # for index in range(num_of_points): # traj.joint_trajectory.points[index].time_from_start *= \ # d/traj.joint_trajectory.points[num_of_points-1].time_from_start # # 按照规划的运动路径控制机械臂运动 # arm.execute(traj) # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) wpose = deepcopy(start_pose) wpose.position.y -= 0.3 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 10 #最大尝试规划次数 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.1, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # print plan # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo( "Path computed successfully. Moving the arm.") # num_of_points = len(plan.joint_trajectory.points) # d = rospy.Duration.from_sec(6) # scale_factor = d/plan.joint_trajectory.points[num_of_points-1].time_from_start # print scale_factor # for index in range(num_of_points): # plan.joint_trajectory.points[index].time_from_start *= scale_factor # print plan # Scale the trajectory speed by a factor of 0.25 new_traj = self.scale_trajectory_speed(plan, 0.25) arm.execute(new_traj) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # #rospy.sleep(1) # # os.system("python ~/MQTT/ros_gripper_control.py 1100") # # rospy.sleep(1) # 回起始点 # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # rospy.loginfo("go origin position") # joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # os.system("python ~/MQTT/ros_gripper_control.py 0") # rospy.sleep(1) # #回起始点 # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() rospy.sleep(2) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue # listener = tf.TransformListener() # while not rospy.is_shutdown(): # try: # # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换 # (trans,rot) = listener.lookupTransform('/camera_color_optical_frame', '/iron_block', rospy.Time(0)) # object_detect_x = trans[0] # object_detect_y = trans[1] # if(object_detect_x>0.033 or object_detect_x<0.027): # # print("detect x error!") # continue # if(object_detect_y>0.040 or object_detect_y<0.020): # # print("detect y error!") # continue # # if(trans[2]>-0.04 or trans[2]<-0.08): # # print("detect z error!") # # continue # print(trans) # # print(rot) # # 获取当前位姿数据最为机械臂运动的起始位姿 # start_pose = arm.get_current_pose(end_effector_link).pose # # 初始化路点列表 # waypoints = [] # # 将初始位姿加入路点列表 # waypoints.append(start_pose) # wpose = deepcopy(start_pose) # wpose.position.y -= 0.2 # wpose.position.z -= 0.155 # waypoints.append(deepcopy(wpose)) # wpose.position.y -= 0.1 # waypoints.append(deepcopy(wpose)) # fraction = 0.0 #路径规划覆盖率 # maxtries = 100 #最大尝试规划次数 # 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.001, # eef_step,终端步进值 # 0.0, # jump_threshold,跳跃阈值 # True) # avoid_collisions,避障规划 # # 尝试次数累加 # attempts += 1 # # 打印运动规划进程 # if attempts % 10 == 0: # rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # #print plan # # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 # if fraction == 1.0: # rospy.loginfo("Path computed successfully. Moving the arm.") # num_of_points = len(plan.joint_trajectory.points) # d = rospy.Duration.from_sec(3) # print plan.joint_trajectory.points[num_of_points-1].time_from_start # for index in range(num_of_points): # plan.joint_trajectory.points[index].time_from_start *= \ # d/plan.joint_trajectory.points[num_of_points-1].time_from_start # #print plan # arm.execute(plan) # rospy.loginfo("Path execution complete.") # # 如果路径规划失败,则打印失败信息 # else: # rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # #rospy.sleep(1) # # os.system("python ~/MQTT/ros_gripper_control.py 1100") # # rospy.sleep(1) # # 回起始点 # rospy.loginfo("go origin position") # joint_positions = [-0.007717277486910995, -0.9330994764397906, 0.46196335078534034, 7.155322862129146e-05, -1.08595462478185, -0.007682373472949389] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686] # # arm.set_joint_value_target(joint_positions) # # # 控制机械臂完成运动 # # arm.go() # # os.system("python ~/MQTT/ros_gripper_control.py 0") # # rospy.sleep(1) # # #回起始点 # # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05] # # arm.set_joint_value_target(joint_positions) # # # 控制机械臂完成运动 # # arm.go() # rospy.sleep(2) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue # 关闭并退出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_constraints_demo', anonymous=True) robot = RobotCommander() # Connect to the arm move group arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while arm.set_planning_time(5) # 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 some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.263803774718 target_pose.pose.position.y = 0.295405791959 target_pose.pose.position.z = 0.690438884208 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the start state and target pose, then plan and execute arm.set_start_state(robot.get_current_state()) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # Close the gripper gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() rospy.sleep(1) # Store the current pose start_pose = arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 # q = quaternion_from_euler(0, 0, -1.57079633) # orientation_constraint.orientation.x = q[0] # orientation_constraint.orientation.y = q[1] # orientation_constraint.orientation.z = q[2] # orientation_constraint.orientation.w = q[3] # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the arm arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.39000848183 target_pose.pose.position.y = 0.185900663329 target_pose.pose.position.z = 0.732752341378 target_pose.pose.orientation.w = 1 # Set the start state and target pose, then plan and execute arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) # Clear all path constraints arm.clear_path_constraints() # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
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.008) arm.set_goal_orientation_tolerance(0.04) arm.allow_replanning(True) reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) arm.set_planning_time(5) joint_state = [ -2.419833894224669, 0.06734230828523968, -0.5274768816844496, 2.0819618074484882, -0.4270016439669033, -0.47903255506397024, -2.711311334870862 ] #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.5 table_size = [0.5, 1, 0.01] #box1_size=[0.1,0.05,0.03] box2_size = [0.05, 0.05, 0.16] 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.16] 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.13 #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) #arm.set_joint_value_target(joint_state) 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.18, end_effector_link) arm.go() rospy.sleep(2) 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.18, end_effector_link) arm.go() rospy.sleep(2) 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): # 初始化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() self.trajectory_publisher = rospy.Publisher('/path', path, queue_size=20) self.trajectory = path() # 等待场景准备就绪 rospy.sleep(1) robot_cmd = RobotCommander() # 等待场景准备就绪 rospy.sleep(1) # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 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' # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go()
class MoveItCartesianDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node(NODE_NAME, anonymous=True) self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2) self.rate = rospy.Rate(2) # 是否需要使用笛卡尔运动规划 self.cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的self.arm group self.arm = MoveGroupCommander(ARM_GROUP) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' self.arm.set_pose_reference_frame(REFERENCE_FRAME) # 获取终端link的名称 end_effector_link = 'link_6' # 当运动规划失败后,允许重新规划 self.arm.allow_replanning(True) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # 环境建模 scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) self.colors = dict() rospy.sleep(1) # 场景物体设置 table_id = 'table' scene.remove_world_object(table_id) self.table_ground = 0.592 - 0.148 table_size = [0.5, 1.5, 0.04] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.6 table_pose.pose.position.y = -0.5 table_pose.pose.position.z = self.table_ground - table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) self.setColor(table_id, 0, 0.8, 0, 1.0) self.sendColors() # rospy.loginfo(self.arm.get_current_pose(END_EFFECTOR_LINK).pose) # # 控制机械臂先回到初始化位置 # self.arm.set_named_target(ALL_ZERO_POSE) # self.arm.go() # # rospy.sleep(1) # rospy.loginfo("已到达准备点") # rospy.spin() # 控制机械臂先回到初始化位置 self.arm.set_named_target(HOME_POSE) self.arm.go() # rospy.sleep(1) rospy.loginfo("已到达准备点") # rospy.spin() # 第二段运动:移动到停泊点 anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(anchor_pose) self.arm.go() rospy.loginfo("已移动到停泊点") start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose # 提前规划:停泊点位置 self.anchor_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose self.anchor_pose.position.x = 0.7 self.anchor_pose.position.y = 0 self.anchor_pose.position.z = 0.764 # self.table_ground + 0.10 + HEIGHT_OF_END_EFFECTOR self.anchor_pose.orientation.x = 0 self.anchor_pose.orientation.y = 0.707 self.anchor_pose.orientation.z = 0 self.anchor_pose.orientation.w = 0.707 # 用于确定停泊点的关节角情况 # waypoints = [] # waypoints.append(start_pose) # waypoints.append(self.anchor_pose) # traj, fraction = self.cartesian_plan(self.arm, waypoints) # self.arm.set_start_state_to_current_state() # if fraction == 1.0: # self.arm.execute(traj) # rospy.loginfo("已到达吸取点") # else: # rospy.spin() # def callback(data): # rospy.loginfo(data) # rospy.Subscriber('joint_states', JointState, callback) # rospy.spin() # 提前规划:中间点位置 self.middle_pose = deepcopy(start_pose) self.middle_pose.position.x = 0 self.middle_pose.position.y = 0.7 self.middle_pose.position.z = self.anchor_pose.position.z self.middle_pose.orientation.x = -0.5 self.middle_pose.orientation.y = 0.5 self.middle_pose.orientation.z = 0.5 self.middle_pose.orientation.w = 0.5 # 提前规划:下料点位置 self.place_pose = deepcopy(start_pose) self.place_pose.position.x = 0 self.place_pose.position.y = 0.7 self.place_pose.position.z = 0.05 + HEIGHT_OF_END_EFFECTOR - 0.14 self.place_pose.orientation.x = -0.5 self.place_pose.orientation.y = 0.5 self.place_pose.orientation.z = 0.5 self.place_pose.orientation.w = 0.5 # 用于确定下料点的关节角情况 # waypoints = [] # waypoints.append(start_pose) # waypoints.append(self.place_pose) # traj, fraction = self.cartesian_plan(self.arm, waypoints) # self.arm.set_start_state_to_current_state() # if fraction == 1.0: # self.arm.execute(traj) # rospy.loginfo("已到达吸取点") # else: # rospy.spin() # def callback(data): # rospy.loginfo(data) # rospy.Subscriber('joint_states', JointState, callback) # rospy.spin() # 提前规划:释放点位置 self.release_pose = deepcopy(start_pose) self.release_pose.position.x = 0.7 self.release_pose.position.y = 0 self.release_pose.position.z = 0.704 # self.table_ground + 0.10 + HEIGHT_OF_END_EFFECTOR self.release_pose.orientation.x = 0 self.release_pose.orientation.y = 0.707 self.release_pose.orientation.z = 0 self.release_pose.orientation.w = 0.707 while not rospy.is_shutdown(): # 开启web端action通讯,等待讯号 self.server = actionlib.SimpleActionServer('abb_grasp', AbbGraspAction, self.abb_execute, False) self.server.start() rospy.spin() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # action callback function def abb_execute(self, goal): # 统计参数,与action有关 self.num = 0 # 计算本轮循环中的件数 self.schedule_percent = AbbGraspFeedback() # 统计进度百分比 self.schedule_result = AbbGraspResult() # 统计完成进度数量 self.goal = goal # 设定目标值 self.item_id = 0 # 初始化计件id值 rate = rospy.Rate(25) rospy.loginfo("Get message from web socket and action!") while self.num <= self.goal.grasp_id: self.num += 1 # service 方法 - 接收视觉处理后的数据 rospy.wait_for_service('abb_visual') rospy.loginfo("Start to get coorporation data from visual PC...") try: rospy.loginfo("Service client Start.") abb_get_visual_data = rospy.ServiceProxy('abb_visual', AbbPose) res_coordinate = abb_get_visual_data(GET_DATA_SIGNAL) rospy.loginfo(res_coordinate) if (res_coordinate.item_id - self.item_id): # self.arm.set_start_state_to_current_state() # 执行函数 self.callback(res_coordinate) self.item_id = res_coordinate.item_id except rospy.ServiceException as e: rospy.loginfo("Service call failed: %s" % e) self.server.set_succeeded(self.schedule_result) rospy.loginfo(self.schedule_result) rospy.loginfo("------------Finished------------") # 获取视觉处理的坐标位置后的执行函数 def callback(self, data): # 设置机械臂工作空间中的目标位姿,位置使用x,y,z坐标描述 # 姿态使用四元数描述,基于base_link坐标系 rospy.loginfo("目标点位置:\n") rospy.loginfo(data) ##################### 即时规划 ###################### # 获取当前位姿数据为停泊点位姿 anchor_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose anchor_pose.orientation.x = 0 anchor_pose.orientation.y = 0.707 anchor_pose.orientation.z = 0 anchor_pose.orientation.w = 0.707 self.anchor_pose = anchor_pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if self.cartesian: waypoints.append(anchor_pose) # 初始化路点数据 wpose = deepcopy(anchor_pose) # wpose.orientation.x = 0 # wpose.orientation.y = 0.707 # wpose.orientation.z = 0 # wpose.orientation.w = 0.707 # 第三段路径:停泊点=》吸取点 rospy.loginfo("规划:从停泊点至吸取点") wpose.position.x = data.position_x wpose.position.y = data.position_y #------------------- !!!注意!!! -------------------# # if data.orientation_x: # wpose.position.z = 0.699 # data.position_z # 0.714 # else: # wpose.position.z = 0.692 # data.position_z # 0.714 wpose.position.z = data.position_z #------------------- !!!修改!!! -------------------# rad_x = data.orientation_w rad_y = 1.5708 rad_z = 0 list_orientation = quaternion_from_euler(rad_x, rad_y, rad_z) wpose.orientation.x = list_orientation[0] wpose.orientation.y = list_orientation[1] wpose.orientation.z = list_orientation[2] wpose.orientation.w = list_orientation[3] #------------------- !!!修改!!! -------------------# if self.cartesian: # 气泵控制,气阀操作,吸盘抓取 on_off_signal = 1 self.pub.publish(on_off_signal) rospy.loginfo("Open the gas valve.") self.rate.sleep() waypoints.append(wpose) traj, fraction = self.cartesian_plan(self.arm, waypoints) self.arm.set_start_state_to_current_state() if fraction == 1.0: wait_time = data.time / 1000 rospy.sleep(wait_time) self.arm.set_start_state_to_current_state() self.arm.execute(traj) rospy.loginfo("已到达吸取点") else: rospy.loginfo("Anchor_to_Grasp_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") # def callback(data): # rospy.loginfo(data) # rospy.Subscriber('joint_states', JointState, callback) # rospy.spin() # # 第3.5段路径:吸取点 =》 反弹点 # self.arm.shift_pose_target(2, 0.05, END_EFFECTOR_LINK) # self.arm.go() # rospy.sleep(1) # 第四段路径:反弹点 =》 停泊点 rospy.loginfo("规划:从吸取点返回停泊点") # grasp_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose # waypoints = [] # waypoints.append(grasp_pose) # waypoints.append(anchor_pose) # traj, fraction = self.cartesian_plan(self.arm, waypoints) # self.arm.set_start_state_to_current_state() # if fraction == 1.0: # self.arm.execute(traj) # rospy.loginfo("已返回停泊点") # else: # rospy.loginfo("Grasp_to_Anchor_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] # 停泊点 [0, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(anchor_pose) self.arm.go() rospy.loginfo("已移动到停泊点") ###################### 即时规划结束 #################### # rospy.spin() # anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] # 停泊点 [0, 0.597, -0.665, 0, 1.636, 0] # self.arm.set_joint_value_target(anchor_pose) # self.arm.go() # rospy.loginfo("已移动到停泊点") # rospy.spin() ####################### 提前规划 ###################### if data.orientation_x: # ox != 0, trangle middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0] place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003] else: # ox == 0, circle middle_pose = [2.285, 0.597, -0.665, 0, 1.636, 0] place_pose = [2.285, 1.471, -0.687, -0.002, 0.788, 0.715] # 停泊点 =》 下料点 # self.anchor_point_to_middle_point(True) # middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(middle_pose) self.arm.go() rospy.loginfo("已移动到中间点") # 测试,下料点定点使用 # rospy.spin() # middle_point = self.arm.get_current_pose(END_EFFECTOR_LINK).pose # waypoints = [] # waypoints.append(middle_point) # waypoints.append(self.place_pose) # traj, fraction = self.cartesian_plan(self.arm, waypoints) # self.arm.set_start_state_to_current_state() # if fraction == 1.0: # self.arm.execute(traj) # rospy.loginfo("已到达吸取点") # else: # rospy.spin() # def callback(data): # rospy.loginfo(data) # rospy.Subscriber('joint_states', JointState, callback) # rospy.spin() # self.middle_point_to_place_point(True) # place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003] self.arm.set_joint_value_target(place_pose) self.arm.go() rospy.loginfo("已到达下料点") # rospy.sleep(1) # rospy.spin() # 气阀操作,吸盘释放 on_off_signal = 0 self.pub.publish(on_off_signal) rospy.loginfo("Close the gas valve.") self.rate.sleep() rospy.sleep(1) # 下料点 =》 停泊点 # self.middle_point_to_place_point(False) # middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(middle_pose) self.arm.go() rospy.loginfo("已返回中间点") # self.anchor_point_to_middle_point(False) anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(anchor_pose) self.arm.go() rospy.loginfo("已返回停泊点") rospy.sleep(1) # rospy.spin() # 停泊点 =》 下料点 # self.anchor_point_to_middle_point(True) # middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(middle_pose) self.arm.go() rospy.loginfo("已移动到中间点") # self.middle_point_to_place_point(True) # place_pose = [1.571, 1.297, -0.270, -0.003, 0.544, 0.003] self.arm.set_joint_value_target(place_pose) self.arm.go() rospy.loginfo("已返回下料点") rospy.sleep(1) # 气阀操作,吸盘吸取 on_off_signal = 1 self.pub.publish(on_off_signal) rospy.loginfo("Open the gas valve.") self.rate.sleep() rospy.sleep(1) # 下料点 =》 停泊点 # self.middle_point_to_place_point(False) # middle_pose = [1.571, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(middle_pose) self.arm.go() rospy.loginfo("已返回中间点") # self.anchor_point_to_middle_point(False) anchor_pose = [0, 0.597, -0.665, 0, 1.636, 0] self.arm.set_joint_value_target(anchor_pose) self.arm.go() rospy.loginfo("已返回停泊点") # rospy.spin() # 停泊点 =》 释放点 self.anchor_point_to_release_point(True) rospy.loginfo("已到达释放点") # 气阀操作,吸盘释放 on_off_signal = 0 self.pub.publish(on_off_signal) rospy.loginfo("Close the gas valve.") self.rate.sleep() # 释放点 =》 停泊点 self.anchor_point_to_release_point(False) rospy.loginfo("已返回停泊点") ####################### 提前规划结束 ###################### self.schedule_percent.percent_complete = self.num / float( self.goal.grasp_id) self.server.publish_feedback(self.schedule_percent) self.schedule_result.total_grasped = self.num rospy.loginfo("Schedule: %0.2f %%", self.schedule_percent.percent_complete * 100) rospy.loginfo("Total Grasped: %d ", int(self.schedule_result.total_grasped)) rospy.loginfo("------------- End: Wait For Next Grasp -----------") def cartesian_plan(self, arm_object, point_list): fraction = 0.0 # 路径规划覆盖率 maxtries = 100 # 最大尝试规划次数 attempts = 0 # 已经尝试规划次数 # 设置机械臂当前的状态为运动初始状态 arm_object.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (traj, fraction) = arm_object.compute_cartesian_path( point_list, # 路点列表 0.01, # 终端步进值 0.0, # 跳跃阈值 True # 避障规划 ) attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") return traj, fraction def anchor_point_to_middle_point(self, dirc): # 提前规划运动: # dirc = true: 从停泊点到下料点 # dirc = false: 从下料点到停泊点 waypoints = [] if dirc: start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose waypoints.append(start_pose) # waypoints.append(self.anchor_pose) wpose = deepcopy(self.anchor_pose) # 平面圆弧转动 rospy.loginfo("进入多边形迫近圆形插补方法规划") i = 1 while wpose.position.x > 0: # cos_5 = 0.99619 # sin_5 = 0.08716 # rad_5 = 5.0 / 180.0 * math.pi cos_1 = 0.99985 sin_1 = 0.01745 rad_1 = 1.0 / 180.0 * math.pi wpose.position.x = wpose.position.x * cos_1 - np.sign( self.place_pose.position.y) * wpose.position.y * sin_1 wpose.position.y = np.sign( self.place_pose.position.y ) * wpose.position.x * sin_1 + wpose.position.y * cos_1 # wpose.position.z += delta_z # z轴取消插补 rad_x = -np.sign(self.place_pose.position.y) * rad_1 * i rad_y = 1.5708 rad_z = 0 list_orientation = quaternion_from_euler(rad_x, rad_y, rad_z) wpose.orientation.x = list_orientation[0] wpose.orientation.y = list_orientation[1] wpose.orientation.z = list_orientation[2] wpose.orientation.w = list_orientation[3] i += 1 if self.cartesian: waypoints.append(deepcopy(wpose)) else: self.arm.set_pose_target(wpose) self.arm.go() # waypoints.append(self.middle_pose) if not dirc: start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose waypoints.append(start_pose) waypoints.reverse() traj, fraction = self.cartesian_plan(self.arm, waypoints) # f = open("plan_of_cartisen_demo.txt", "w") # f.write(str(traj)) # f.close() if fraction == 1.0: self.arm.set_start_state_to_current_state() self.arm.execute(traj) rospy.loginfo("停泊点和中间点间运动已执行") else: rospy.loginfo("Anchor_to_Place_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") def middle_point_to_place_point(self, dirc): # 提前规划运动: # dirc = true: 从中间点到下料点 # dirc = false: 从下料点到中间点 waypoints = [] if dirc: start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose waypoints.append(start_pose) waypoints.append(self.place_pose) else: start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose waypoints.append(start_pose) waypoints.append(self.middle_pose) traj, fraction = self.cartesian_plan(self.arm, waypoints) if fraction == 1.0: self.arm.set_start_state_to_current_state() self.arm.execute(traj) # def callback(data): # rospy.loginfo(data) # rospy.Subscriber('joint_states', JointState, callback) # rospy.spin() rospy.loginfo("中间点和下料点间运动已执行") else: rospy.loginfo("Anchor_to_Place_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") def anchor_point_to_release_point(self, dirc): # 提前规划运动: # dirc = true: 从停泊点到释放点 # dirc = false: 从释放点到停泊点 waypoints = [] waypoints.append(self.anchor_pose) waypoints.append(self.release_pose) if not dirc: waypoints.reverse() traj, fraction = self.cartesian_plan(self.arm, waypoints) if fraction == 1.0: self.arm.set_start_state_to_current_state() self.arm.execute(traj) rospy.loginfo("停泊点和释放点间运动已执行") else: rospy.loginfo("Anchor_to_Release_Path Planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") def B_spline(self, degree, l, coeff, knot, dense, points): ''' degree: 阶数 l: l = n - k = point_num - k + 1 coeff: 控制点坐标,list knot: 节点组,list dense: 分辨率,每段点数 points: 轨迹点坐标,list ''' u = 0 point_num = 0 i = degree while i <= l + degree: if knot[i + 1] > knot[i]: for p in range(dense): u = knot[i] + p * (knot[i + 1] - knot[i]) / dense points[point_num] = self.deboor(degree, coeff, knot, u, i, l) point_num += 1 i += 1 # return points # 可能不需要返回值 def deboor(self, degree, coeff, knot, u, i, l): ''' degree: 阶数 coeff: 控制点坐标,list knot: 节点组,list u 和 i: 主要是传参 ''' coeffa = [None] * (degree + 1) j = i - degree while j <= i: coeffa[j] = coeff[j] j += 1 k = 1 while k <= degree: j = i while j >= i - degree + k: t1 = (knot[j + degree - k + 1] - u) / (knot[j + degree - l + 1] - knot[j]) t2 = 1.0 - t1 coeffa[j] = t1 * coeffa[j - 1] + t2 * coeffa[j] j -= 1 k += 1 return coeffa[i] def middle_ctrl_point(self, start_pose, target_pose): middle_point_X = abs(start_pose.position.x - target_pose.linear.x) / 2 middle_point_Y = abs(start_pose.position.y - target_pose.linear.y) / 2 theta = math.atan(middle_point_Y / middle_point_X) cos_theta = math.cos(theta) sin_theta = math.sin(theta) print(middle_point_X, middle_point_Y) print(theta, cos_theta, sin_theta) middle_ctrl_point_X = start_pose.position.x * cos_theta middle_ctrl_point_Y = start_pose.position.x * sin_theta return middle_ctrl_point_X, middle_ctrl_point_Y def setColor(self, name, r, g, b, a=0.9): color = ObjectColor() color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a self.colors[name] = color def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = True # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('left_manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('leftbase_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).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 = 100 #最大尝试规划次数 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 # 打印运动规划进程 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('home') # 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("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_drawing', anonymous=True) #订阅stop话题 rospy.Subscriber("/probot_controller_ctrl", ControllerCtrl, callback) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.01) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.5) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) star_pose = PoseStamped() star_pose.header.frame_id = reference_frame star_pose.header.stamp = rospy.Time.now() star_pose.pose.position.x = 0.30 star_pose.pose.position.y = 0.0 star_pose.pose.position.z = 0.30 star_pose.pose.orientation.w = 1.0 # 设置机械臂终端运动的目标位姿 arm.set_pose_target(star_pose, end_effector_link) arm.go() radius = 0.05 centerY = 0.0 centerX = 0.40 - radius # 初始化路点列表 waypoints = [] starPoints = [] pose_temp = star_pose for th in numpy.arange(0, 6.2831855, 1.2566371): pose_temp.pose.position.y = -(centerY + radius * math.sin(th)) pose_temp.pose.position.x = centerX + radius * math.cos(th) pose_temp.pose.position.z = 0.30 wpose = deepcopy(pose_temp.pose) starPoints.append(deepcopy(wpose)) # 将圆弧上的路径点加入列表 waypoints.append(starPoints[0]) waypoints.append(starPoints[2]) waypoints.append(starPoints[4]) waypoints.append(starPoints[1]) waypoints.append(starPoints[3]) waypoints.append(starPoints[0]) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 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 # 打印运动规划进程 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.") rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('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) # 设置每次运动规划的时间限制:20s arm.set_planning_time(20) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table1_id = 'table1' table2_id = 'table2' target_id = 'cube_marker' # 移除场景中之前运行残留的物体 scene.remove_world_object(table1_id) scene.remove_world_object(table2_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的三维尺寸[长, 宽, 高] table1_size = [0.8, 1.5, 0.03] table2_size = [1.5, 0.8, 0.03] # 将两张桌子加入场景当中 table1_pose = PoseStamped() table1_pose.header.frame_id = 'base_link' table1_pose.pose.position.x = 0 table1_pose.pose.position.y = 1.05 table1_pose.pose.position.z = -0.05 table1_pose.pose.orientation.w = 1.0 scene.add_box(table1_id, table1_pose, table1_size) table2_pose = PoseStamped() table2_pose.header.frame_id = 'base_link' table2_pose.pose.position.x = -1.05 table2_pose.pose.position.y = -0.1 table2_pose.pose.position.z = -0.05 table2_pose.pose.orientation.w = 1.0 scene.add_box(table2_id, table2_pose, table2_size) # 将桌子设置成红色 self.setColor(table1_id, 0.8, 0.4, 0, 1.0) self.setColor(table2_id, 0.8, 0.4, 0, 1.0) #监听目标到'base_link'的tf变换 listener = tf.TransformListener() while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('base_link', 'ar_marker_0', rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo( "Waiting for transform between 'base_link' and 'ar_marker_0'" ) rospy.sleep(1) rospy.loginfo("Found transform between 'base_link' and 'ar_marker_0'") # 设置目标物体的尺寸 target_size = [0.05, 0.05, 0.2] # 设置目标物体的位置 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = trans[0] target_pose.pose.position.y = trans[1] target_pose.pose.position.z = 0.065 target_pose.pose.orientation.x = rot[0] target_pose.pose.orientation.y = rot[1] target_pose.pose.orientation.z = rot[2] target_pose.pose.orientation.w = rot[3] # 将抓取的目标物体加入场景中 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(table2_id) # 设置一个place阶段需要放置物体的目标位置 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = -0.5 place_pose.pose.position.y = 0 place_pose.pose.position.z = 0.065 while not rospy.is_shutdown(): try: listener.waitForTransform('ar_marker_0', 'ee_link', rospy.Time(0), rospy.Duration(4.0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.loginfo( "Waiting for transform between 'ar_marker_0' and 'ee_link'" ) rospy.sleep(1) rospy.loginfo("Found transform between 'ar_marker_0' and 'ee_link'") # 将目标位置设置为机器人的抓取目标位置 grasp_pose = PoseStamped() grasp_pose.header.frame_id = 'ar_marker_0' grasp_pose.pose.position.x = 0 grasp_pose.pose.position.y = -0.15 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) rospy.init_node('moveit_demo') # We need a tf2 listener to convert poses into arm reference base try: self._tf2_buff = tf2_ros.Buffer() self._tf2_list = tf2_ros.TransformListener(self._tf2_buff) except rospy.ROSException as err: rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err)) raise err self.gripper_opened = [ rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01 ] self.gripper_closed = [ rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01 ] self.gripper_neutral = [ rospy.get_param(GRIPPER_PARAM + "/neutral", (self.gripper_opened[0] + self.gripper_closed[0]) / 2.0) ] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0) # Use the planning scene object to add or remove objects self.scene = PlanningSceneInterface() # 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.01) # 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 each of the scene objects a unique name table_id = 'table' target_id = 'target' # Remove leftover objects from a previous run self.scene.remove_world_object(table_id) self.scene.remove_world_object(target_id) # Remove any attached objects from a previous session self.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") # 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") # Set the height of the table off the ground table_ground = 0.0 # Set the dimensions of the scene objects [l, w, h] # Set the target size [l, w, h] # target_size = [0.02, 0.005, 0.12] target_size = [0.02, 0.02, 0.05] #target pose array target_pose_array = PoseArray() target_pose_array.header.frame_id = REFERENCE_FRAME pose1 = Pose() pose1.position.x = 0.20 pose1.position.y = -0.10 pose1.position.z = 0.025 pose1.orientation.w = 1.0 target_pose_array.poses.append(pose1) pose2 = Pose() pose2.position.x = 0.20 pose2.position.y = 0.10 pose2.position.z = 0.025 pose2.orientation.w = 1.0 target_pose_array.poses.append(pose2) # 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.29 target_pose.pose.position.y = -0.10 target_pose.pose.position.z = 0.025 target_pose.pose.orientation.w = 1.0 ''' target_pose.pose.position.x = target_pose_array.poses[1].position.x target_pose.pose.position.y = target_pose_array.poses[1].position.y target_pose.pose.position.z = target_pose_array.poses[1].position.z target_pose.pose.orientation.w = target_pose_array.poses[ 1].orientation.w # Add the target object to the scene self.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) # 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 # 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!") # 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 __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=10 ) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' 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('hey_there') 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.75 # 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.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.19 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 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.18 place_pose.pose.position.y = -0.18 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation 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 # 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('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
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 right_arm move group right_arm = MoveGroupCommander('right_arm') # 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_footprint') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "straight_forward" configuration stored in the SRDF file right_arm.set_named_target('straight_forward') # Plan and execute a trajectory to the goal configuration right_arm.go() # Get the current pose so we can add it as a waypoint start_pose = right_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) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.2 wpose.position.y -= 0.2 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.05 wpose.position.y += 0.15 wpose.position.z -= 0.15 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state right_arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = right_arm.compute_cartesian_path( waypoints, # waypoint poses 0.01, # 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.") right_arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Move normally back to the 'resting' position right_arm.set_named_target('resting') right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = 0.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the left arm left_arm = MoveGroupCommander('left_arm') # Initialize the MoveIt! commander for the gripper left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link end_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_eef, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.3 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_eef scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_eef, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base' table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) # Update the current state left_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position left_arm.set_named_target('left_ready') left_arm.go() rospy.sleep(2) # Return the arm in the "left_ready" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() rospy.sleep(2) scene.remove_attached_object(end_eef, 'tool') 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): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface('world') 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.01) arm.set_goal_orientation_tolerance(0.05) arm.allow_replanning(True) arm.set_planning_time(5) table_id = 'table' box1_id = 'box1' box2_id = 'box2' scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) rospy.sleep(1) reference_frame = 'root' table_ground = 0.55 tabel_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.7 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + tabel_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, tabel_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.65 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + tabel_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.63 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + tabel_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, 1.0) self.setColor(box2_id, 0.8, 0.4, 1.0) self.sendColors() gripper = MoveGroupCommander('gripper') gripper.set_named_target('Close') gripper.go() rospy.sleep(1) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.64 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + tabel_size[ 2] + 0.05 target_quaternion = quaternion_from_euler(-1.6745735920568166, -1.2113336804234238, -1.718476017088519) target_pose.pose.orientation.x = target_quaternion[0] target_pose.pose.orientation.y = target_quaternion[1] target_pose.pose.orientation.z = target_quaternion[2] target_pose.pose.orientation.w = target_quaternion[3] # arm.set_goal_position_tolerance(0.02) # arm.set_goal_orientation_tolerance(0.01) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) 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)
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 right_arm move group right_arm = MoveGroupCommander('right_arm') # 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_footprint') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "straight_forward" configuration stored in the SRDF file right_arm.set_named_target('straight_forward') # Plan and execute a trajectory to the goal configuration right_arm.go() # Get the current pose so we can add it as a waypoint start_pose = right_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) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.2 wpose.position.y -= 0.2 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.05 wpose.position.y += 0.15 wpose.position.z -= 0.15 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state right_arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = right_arm.compute_cartesian_path ( waypoints, # waypoint poses 0.01, # 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.") right_arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Move normally back to the 'resting' position right_arm.set_named_target('resting') right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
class MoveItCartesianDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node(NODE_NAME, anonymous=True) self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2) self.rate = rospy.Rate(2) # 是否需要使用笛卡尔运动规划 self.cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的self.arm group self.arm = MoveGroupCommander(ARM_GROUP) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' self.arm.set_pose_reference_frame(REFERENCE_FRAME) # 获取终端link的名称 end_effector_link = 'link_6' # 当运动规划失败后,允许重新规划 self.arm.allow_replanning(True) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) # 环境建模 scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) self.colors = dict() rospy.sleep(1) # 场景物体设置 table_id = 'table' scene.remove_world_object(table_id) # table_groud = 0.5 - HEIGHT_OF_END_EFFECTOR table_size = [0.5, 1.5, 0.04] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.6 table_pose.pose.position.y = -0.5 table_pose.pose.position.z = table_groud - table_size[2] / 2.0 - 0.02 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) self.setColor(table_id, 0, 0.8, 0, 1.0) self.sendColors() # 第五轴的关节角度旋转,从水平到竖直 joint_positions = [0, 0, 0, 0, 1.571, 0] self.arm.set_joint_value_target(joint_positions) self.arm.go() rospy.sleep(1) # 全局变量,对比目标点位置 global forward_pose forward_pose = Twist() # 全局变量,计数 global item_num item_num = 0 while not rospy.is_shutdown(): # 开启web端action通讯,等待讯号 self.server = actionlib.SimpleActionServer('abb_grasp', AbbGraspAction, self.abb_execute, False) self.server.start() rospy.spin() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) def abb_execute(self, goal): # 统计参数,与action有关 self.num = 0 self.schedule_percent = AbbGraspFeedback() self.schedule_result = AbbGraspResult() self.goal = goal rate = rospy.Rate(25) while self.num < self.goal.grasp_id: self.num += 1 # 接收视觉处理后的坐标位置 - topic 方法 rospy.Subscriber(TOPIC_NAME, VISUAL_TOPIC_TYPE, self.callback) # rospy.spin() rospy.wait_for_message(TOPIC_NAME, VISUAL_TOPIC_TYPE, timeout=None) rospy.loginfo(self.num) rospy.loginfo(self.goal.grasp_id) self.server.set_succeeded(self.schedule_result) rospy.loginfo(self.schedule_result) rospy.loginfo("------------finished------------") # 获取视觉处理的坐标位置后的处理函数 def callback(self, data): # 设置机械臂工作空间中的目标位姿,位置使用x,y,z坐标描述 # 姿态使用四元数描述,基于base_link坐标系 # 检测本次目标位置和上次是否一致 global forward_pose if (data.linear.x == forward_pose.linear.x and \ data.linear.y == forward_pose.linear.y and \ data.linear.z == forward_pose.linear.z): return 0 else: rospy.loginfo("目标点位置:") rospy.loginfo(data) # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose start_pose.orientation.x = 0 start_pose.orientation.y = 0.707 start_pose.orientation.z = 0 start_pose.orientation.w = 0.707 # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if self.cartesian: waypoints.append(start_pose) # 初始化路点数据 wpose = deepcopy(start_pose) wpose.orientation.x = 0 wpose.orientation.y = 0.707 wpose.orientation.z = 0 wpose.orientation.w = 0.707 # 较远距离路径,多边形迫近圆形插补方法 if data.linear.x <= 0: # print(np.sign(data.linear.y)) # delta_z = (data.linear.z - start_pose.position.z) / 18 / 2 i = 1 while wpose.position.x > 0: cos_5 = 0.99619 sin_5 = 0.08716 rad_5 = 5.0 / 180.0 * math.pi wpose.position.x = wpose.position.x * cos_5 - np.sign( data.linear.y) * wpose.position.y * sin_5 wpose.position.y = np.sign( data.linear.y ) * wpose.position.x * sin_5 + wpose.position.y * cos_5 # wpose.position.z += delta_z # z轴取消插补 rad_x = -np.sign(data.linear.y) * rad_5 * i rad_y = 1.5708 rad_z = 0 list_orientation = quaternion_from_euler( rad_x, rad_y, rad_z) wpose.orientation.x = list_orientation[0] wpose.orientation.y = list_orientation[1] wpose.orientation.z = list_orientation[2] wpose.orientation.w = list_orientation[3] i += 1 if self.cartesian: waypoints.append(deepcopy(wpose)) else: self.arm.set_pose_target(wpose) self.arm.go() self.arm.set_start_state_to_current_state() plan_0, fraction = self.cartesian_plan(self.arm, waypoints) # 如果路径规划成功,则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo( "Path to start pose computed successfully. Moving the self.arm." ) self.arm.execute(plan_0) rospy.loginfo("Path to start pose execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") else: rospy.loginfo("进入常规规划") # 常规路径,先水平面移动 wpose.position.x = data.linear.x wpose.position.y = data.linear.y wpose.position.z = data.linear.z + GAP_TO_ITEM wpose.orientation.x = 0 wpose.orientation.y = 0.707 wpose.orientation.z = 0 wpose.orientation.w = 0.707 if self.cartesian: waypoints.append(wpose) traj, fraction = self.cartesian_plan(self.arm, waypoints) self.arm.set_start_state_to_current_state() # rospy.loginfo(traj) self.arm.execute(traj) # 垂直笛卡尔轨迹规划 rospy.loginfo("进入垂直路径规划") start_pose = self.arm.get_current_pose(END_EFFECTOR_LINK).pose start_pose.orientation.x = 0 start_pose.orientation.y = 0.707 start_pose.orientation.z = 0 start_pose.orientation.w = 0.707 # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if self.cartesian: waypoints.append(start_pose) # 初始化路点数据 wpose = deepcopy(start_pose) wpose.orientation.x = 0 wpose.orientation.y = 0.707 wpose.orientation.z = 0 wpose.orientation.w = 0.707 # 设置目标点的路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.position.x = data.linear.x wpose.position.y = data.linear.y wpose.position.z = data.linear.z # 目标点四元数计算 # wpose.orientation.x = - np.sign(data.linear.y) * 0.5 # wpose.orientation.y = 0.5 # wpose.orientation.z = np.sign(data.linear.y) * 0.5 # wpose.orientation.w = 0.5 if self.cartesian: waypoints.append(deepcopy(wpose)) else: self.arm.set_pose_target(wpose) self.arm.go() # 设置当前的状态为运动初始状态 self.arm.set_start_state_to_current_state() # 气阀操作,吸盘抓取 on_off_signal = 1 self.pub.publish(on_off_signal) rospy.loginfo("Open the gas valve.") self.rate.sleep() plan_1, fraction = self.cartesian_plan(self.arm, waypoints) # 第一段路程规划和执行 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") now1 = rospy.get_time() rospy.loginfo(now1) self.arm.execute(plan_1) # rospy.sleep(1.499) now3 = rospy.get_time() rospy.loginfo(now3 - now1) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") waypoints.reverse() plan_2, fraction = self.cartesian_plan(self.arm, waypoints) self.arm.set_start_state_to_current_state() # 第二段路程规划和执行,并紧接着第三段和第四段路程 if fraction == 1.0: rospy.loginfo( "Backpath computed successfully. Moving the arm.") now1 = rospy.get_time() rospy.loginfo(now1) self.arm.execute(plan_2) now2 = rospy.get_time() rospy.loginfo(now2) # rospy.sleep(1.998 - now2 + now1) now3 = rospy.get_time() rospy.loginfo(now3 - now1) rospy.loginfo("Backpath execution complete.") # 放回板材 rospy.loginfo("Path 2: Moving the arm.") self.arm.set_start_state_to_current_state() self.arm.execute(plan_1) rospy.loginfo("Path execution complete.") # 气阀操作,吸盘释放 on_off_signal = 0 self.pub.publish(on_off_signal) rospy.loginfo("Close the gas valve.") self.rate.sleep() # rospy.sleep(1) rospy.loginfo("Backpath 2: Moving the arm.") self.arm.set_start_state_to_current_state() self.arm.execute(plan_2) rospy.loginfo("Backpath execution complete.") # global item_num # item_num += 1 # rospy.loginfo("Finished " + str(item_num) + " grasp.") else: rospy.loginfo("Backpath planning failed with only " + str(fraction) + "success after " + str(100) + " attempts.") # rospy.sleep(5) # # 控制机械臂回到初始化位置 # self.arm.set_named_target(HOME_POSE) # self.arm.go() # rospy.sleep(1) # 记录本次目标位置 global forward_pose forward_pose.linear.x = data.linear.x forward_pose.linear.y = data.linear.y forward_pose.linear.z = data.linear.z rospy.loginfo(forward_pose) self.schedule_percent.percent_complete = self.num / float( self.goal.grasp_id) self.server.publish_feedback(self.schedule_percent) self.schedule_result.total_grasped = self.num rospy.loginfo("schedule: %0.2f %%", self.schedule_percent.percent_complete * 100) rospy.loginfo("total grasped: %d ", int(self.schedule_result.total_grasped)) rospy.loginfo("-----------wait for next response-----------") def cartesian_plan(self, arm_object, point_list): fraction = 0.0 # 路径规划覆盖率 maxtries = 100 # 最大尝试规划次数 attempts = 0 # 已经尝试规划次数 # 设置机械臂当前的状态为运动初始状态 arm_object.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm_object.compute_cartesian_path( point_list, # 路点列表 0.01, # 终端步进值 0.0, # 跳跃阈值 True # 避障规划 ) attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") return plan, fraction def B_spline(self, degree, l, coeff, knot, dense, points): ''' degree: 阶数 l: l = n - k = point_num - k + 1 coeff: 控制点坐标,list knot: 节点组,list dense: 分辨率,每段点数 points: 轨迹点坐标,list ''' u = 0 point_num = 0 i = degree while i <= l + degree: if knot[i + 1] > knot[i]: for p in range(dense): u = knot[i] + p * (knot[i + 1] - knot[i]) / dense points[point_num] = self.deboor(degree, coeff, knot, u, i, l) point_num += 1 i += 1 # return points # 可能不需要返回值 def deboor(self, degree, coeff, knot, u, i, l): ''' degree: 阶数 coeff: 控制点坐标,list knot: 节点组,list u 和 i: 主要是传参 ''' coeffa = [None] * (degree + 1) j = i - degree while j <= i: coeffa[j] = coeff[j] j += 1 k = 1 while k <= degree: j = i while j >= i - degree + k: t1 = (knot[j + degree - k + 1] - u) / (knot[j + degree - l + 1] - knot[j]) t2 = 1.0 - t1 coeffa[j] = t1 * coeffa[j - 1] + t2 * coeffa[j] j -= 1 k += 1 return coeffa[i] def middle_ctrl_point(self, start_pose, target_pose): middle_point_X = abs(start_pose.position.x - target_pose.linear.x) / 2 middle_point_Y = abs(start_pose.position.y - target_pose.linear.y) / 2 theta = math.atan(middle_point_Y / middle_point_X) cos_theta = math.cos(theta) sin_theta = math.sin(theta) print(middle_point_X, middle_point_Y) print(theta, cos_theta, sin_theta) middle_ctrl_point_X = start_pose.position.x * cos_theta middle_ctrl_point_Y = start_pose.position.x * sin_theta return middle_ctrl_point_X, middle_ctrl_point_Y def setColor(self, name, r, g, b, a=0.9): color = ObjectColor() color.id = name color.color.r = r color.color.g = g color.color.b = b color.color.a = a self.colors[name] = color def sendColors(self): p = PlanningScene() p.is_diff = True for color in self.colors.values(): p.object_colors.append(color) self.scene_pub.publish(p)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.72 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the right arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the right arm reference frame accordingly self.right_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.right_arm.allow_replanning(False) # Set a position tolerance in meters self.right_arm.set_goal_position_tolerance(0.1) #0.05 # Set an orientation tolerance in radians self.right_arm.set_goal_orientation_tolerance(0.5) #0.2 # What is the end effector link? self.ee_link = self.right_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target right_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion right_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the right_arm shoulder lift frame #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('right_shoulder_tilt', target) # Convert the position values to a Python list p0 = [ target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z ] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.right_arm.set_named_target('right_start') self.right_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [ target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z ] p2 = [ self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z ] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.right_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z ] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.right_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.right_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.right_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.right_arm.stop() # Move to the resting position self.right_arm.set_named_target('right_start') self.right_arm.go() # Relax the servos self.relax_all_servos() os._exit(0)
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.4 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the right arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the right arm reference frame accordingly self.right_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.right_arm.allow_replanning(False) # Set a position tolerance in meters self.right_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.right_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.right_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target right_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion right_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the right_arm shoulder lift frame #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('right_arm_base_link', target) # Convert the position values to a Python list p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.right_arm.set_named_target('resting') self.right_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z] p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.right_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.right_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.right_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.right_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.right_arm.stop() # Move to the resting position self.right_arm.set_named_target('resting') self.right_arm.go() # Relax the servos self.relax_all_servos() os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_cartesian', anonymous=True) cartesian = rospy.get_param('~cartesian', True) #set cartesian parameters ur5_manipulator = MoveGroupCommander('ur5_manipulator') ur5_manipulator.allow_replanning(True) ur5_manipulator.set_pose_reference_frame('base_link') ur5_manipulator.set_goal_position_tolerance(0.01) ur5_manipulator.set_goal_orientation_tolerance(0.1) end_effector_link = ur5_manipulator.get_end_effector_link() #set the ur5 initial pose ur5_manipulator.set_named_target('ready') ur5_manipulator.go() #get the end effort information start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose print("The first waypoint:") print(start_pose) #define waypoints waypoints = [] waypoints.append(start_pose) wpose = deepcopy(start_pose) wpose.position.z -= 0.3 print("The second waypoint:") print(wpose) waypoints.append(deepcopy(wpose)) print(" ") print(waypoints) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = ur5_manipulator.compute_cartesian_path( waypoints, 0.01, 0.0, True) attempts += 1 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") ur5_manipulator.execute(plan) rospy.sleep(2) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") #ur5_manipulator.set_named_target('home') #ur5_manipulator.go() rospy.sleep(3) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class PickAndPlace: def __init__(self, robot_name="panda_arm", frame="panda_link0"): try: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node(name="pick_place_test") self.scene = PlanningSceneInterface() self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # region Robot initial self.robot = MoveGroupCommander(robot_name) self.robot.set_goal_joint_tolerance(0.00001) self.robot.set_goal_position_tolerance(0.00001) self.robot.set_goal_orientation_tolerance(0.01) self.robot.set_goal_tolerance(0.00001) self.robot.allow_replanning(True) self.robot.set_pose_reference_frame(frame) self.robot.set_planning_time(3) # endregion self.gripper = MoveGroupCommander("hand") self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_OPEN) self.gripper.go() self.gripper.set_joint_value_target(GRIPPER_CLOSED) self.gripper.go() # Robot go home self.robot.set_named_target("home") self.robot.go() # clear all object in world self.clear_all_object() table_pose = un.Pose(0, 0, -10, 0, 0, 0) table_color = un.Color(255, 255, 0, 100) self.add_object_box("table", table_pose, table_color, frame, (2000, 2000, 10)) bearing_pose = un.Pose(250, 250, 500, -90, 45, -90) bearing_color = un.Color(255, 0, 255, 255) bearing_file_name = "../stl/bearing.stl" self.add_object_mesh("bearing", bearing_pose, bearing_color, frame, bearing_file_name) obpose = self.scene.get_object_poses(["bearing"]) # self.robot.set_support_surface_name("table") g = Grasp() # Create gripper position open or close g.pre_grasp_posture = self.open_gripper() g.grasp_posture = self.close_gripper() g.pre_grasp_approach = self.make_gripper_translation( 0.01, 0.1, [0, 1.0, 0]) g.post_grasp_retreat = self.make_gripper_translation( 0.01, 0.9, [0, 1.0, 0]) p = PoseStamped() p.header.frame_id = "panda_link0" p.pose.orientation = obpose["bearing"].orientation p.pose.position = obpose["bearing"].position g.grasp_pose = p g.allowed_touch_objects = ["bearing"] a = [] a.append(g) result = self.robot.pick(object_name="bearing", grasp=a) print(result) except Exception as ex: print(ex) moveit_commander.roscpp_shutdown() moveit_commander.roscpp_shutdown() def make_gripper_translation(self, min_dist, desired, vector): g = GripperTranslation() g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] g.direction.header.frame_id = "panda_link0" g.min_distance = min_dist g.desired_distance = desired return g def open_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.04, 0.04] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def close_gripper(self): t = JointTrajectory() t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2'] tp = JointTrajectoryPoint() tp.positions = [0.0, 0.0] tp.time_from_start = rospy.Duration(secs=1) t.points.append(tp) return t def clear_all_object(self): for world_object in (self.scene.get_known_object_names()): self.scene.remove_world_object(world_object) for attached_object in (self.scene.get_attached_objects()): self.scene.remove_attached_object(attached_object) def add_object_box(self, object_name, pose, color, frame, size): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param size: size of object(mm) :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_box(name=object_name, pose=object_pose, size=(size[0] / 1000.0, size[1] / 1000.0, size[2] / 1000.0)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p) def add_object_mesh(self, object_name, pose, color, frame, file_name): """ add object in rviz :param object_name: name of object :param pose: pose of object. (xyz) in mm,(abc) in degree :param color: color of object.(RGBA) :param frame: reference_frame :param file_name: mesh file name :return: None """ # Add object object_pose = PoseStamped() object_pose.header.frame_id = frame object_pose.pose.position.x = pose.X / 1000.0 object_pose.pose.position.y = pose.Y / 1000.0 object_pose.pose.position.z = pose.Z / 1000.0 quaternion = quaternion_from_euler(np.deg2rad(pose.A), np.deg2rad(pose.B), np.deg2rad(pose.C)) object_pose.pose.orientation.x = quaternion[0] object_pose.pose.orientation.y = quaternion[1] object_pose.pose.orientation.z = quaternion[2] object_pose.pose.orientation.w = quaternion[3] self.scene.add_mesh(name=object_name, pose=object_pose, filename=file_name, size=(0.001, 0.001, 0.001)) # Add object color object_color = ObjectColor() object_color.id = object_name object_color.color.r = color.R / 255.00 object_color.color.g = color.G / 255.00 object_color.color.b = color.B / 255.00 object_color.color.a = color.A / 255.00 p = PlanningScene() p.is_diff = True p.object_colors.append(object_color) self.scene_pub.publish(p)
class Arm_Controller: def __init__(self): # Give the launch a chance to catch up # rospy.sleep(5) # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Arm_Controller') rospy.loginfo("Launched Arm Controller") # constants self.GROUP_NAME_ARM = 'arm' self.GRIPPER_FRAME = 'gripper_link' self.REFERENCE_FRAME = 'base_link' self.ARM_BASE_FRAME = 'arm_base_link' self.done = True self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped) rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1) self.robot_name = "gatlin" move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm) # We need a tf listener to convert poses into arm reference base self.tfl = tf.TransformListener() # Initialize the move group for the right arm self.arm = MoveGroupCommander(self.GROUP_NAME_ARM) self.gripper = Gripper() self.robot = moveit_commander.RobotCommander() # Allow replanning to increase the odds of a solution self.arm.allow_replanning(True) # Set the planner self.arm.set_planner_id("RRTConnectkConfigDefault") # Set the right arm reference frame self.arm.set_pose_reference_frame(self.REFERENCE_FRAME) # Give the scene a chance to catch up rospy.sleep(1) # Allow some leeway in position (meters) and orientation (radians) # USELESS; do not work on pick and place! Explained on this issue: # https://github.com/ros-planning/moveit_ros/issues/577 self.arm.set_goal_position_tolerance(0.005) self.arm.set_goal_orientation_tolerance(0.05) # Allow 2 seconds per planning attempt self.arm.set_planning_time(2.0) # Create a quaternion from the Euler angles #q = quaternion_from_euler(0, pitch, yaw) # horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555) # deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) # back_pos = Point(-0.03, 0.0313, 0.476) # init rest pose self.rest_pose = PoseStamped() self.rest_pose.header.frame_id = self.REFERENCE_FRAME self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35) self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981) # init place_upper pose self.place_upper_pose = PoseStamped() self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998) self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889) # init current pose self.current_pose = PoseStamped() self.current_pose.header.frame_id = self.REFERENCE_FRAME self.current_pose.pose.position = Point(0,0,0) self.current_pose.pose.orientation = down # Open the gripper rospy.loginfo("Set Gripper: open") self.gripper.set(1.0) # self.arm.set_pose_target(self.rest_pose) # self.arm.go() # rospy.sleep(1) # self.ar = ArbotixROS() # rate = rospy.Rate(30) # while not rospy.is_shutdown(): # # rospy.logerr(self.ar.getVoltage(4)) # # rospy.logerr(self.ar.getSpeed(5)) # rospy.logerr(self.ar.getPosition(1)) # rate.sleep() rospy.spin() def MoveToPoseWithIntermediate(self, ps, offsets) : success = False for offset in offsets: # interpose = getOffsetPose(hand_pose, offset) interpose = getOffsetPose(ps, offset) success = self.MoveToPose(interpose, "MoveToIntermediatePose") rospy.sleep(1) success = self.MoveToPose(ps, "MoveToPose") return success def MoveToPose(self, ps, name) : newpose = self.transform_pose(self.REFERENCE_FRAME, ps) down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052) newpose.pose.orientation = down if self.move_arm_to_pose(newpose) : rospy.loginfo("SUCCEEDED: %s" % name) return True else : rospy.logerr("FAILED %s" % name) return False def move_arm_to_pose(self, ps): arm_target_pose = deepcopy(ps) arm_target_pose.header.stamp = rospy.Time.now() self.test_pose_publisher.publish(arm_target_pose) self.arm.set_pose_target(arm_target_pose) success = self.arm.go() return success def handle_move_arm(self, req): success = True gripper = self.gripper if req.action == "OPEN_GRIPPER": rospy.loginfo("Beginning to open gripper") gripper.open(block=True) rospy.loginfo("Opened Gripper") elif req.action == "CLOSE_GRIPPER" : rospy.loginfo("Beginning to close Gripper") gripper.close(block=True) rospy.loginfo("Closed Gripper") elif req.action == "MOVE_TO_POSE_INTERMEDIATE" : rospy.loginfo("Trying to Move To Pose With Intermediate") offsets = [Vector3(0,0,.07)] success = self.MoveToPoseWithIntermediate(req.ps, offsets) elif req.action == "MOVE_TO_POSE" : rospy.loginfo("Trying to Move To Pose") success = self.MoveToPose(req.ps, "FAILED MoveToPose") elif req.action == "RESET_ARM" : rospy.loginfo("Trying to Move To Rest Pose") success = self.move_arm_to_pose(self.rest_pose) elif req.action == "PLACE_UPPER" : rospy.loginfo("Trying to Move To Place Upper Pose") success = self.move_arm_to_pose(self.place_upper_pose) # success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose") # elif req.action == "MOVE_TO_POS" : # rospy.loginfo("Trying to Move To Pos") # new_pose = Pose() # if req.limb == 'left': # try: # self.initial_left # new_pose = deepcopy(self.initial_left) # except AttributeError: # new_pose = deepcopy(self.hand_pose_left) # self.initial_left = deepcopy(self.hand_pose_left) # elif req.limb == 'right': # try: # self.initial_right # new_pose = deepcopy(self.initial_right) # except AttributeError: # new_pose = deepcopy(self.hand_pose_right) # self.initial_right = deepcopy(self.hand_pose_right) # new_pose.position = deepcopy(req.pose.position) # # success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose") # success = self.MoveToPoseWithIntermediate(req.limb, new_pose) # rospy.loginfo("Moved to pos: %r" % success) else : success = False rospy.logerr("invalid action") return MoveRobotResponse(success) def orientation_cb(self, data): if(data.x == -1.0 and data.y == -2.0): print "################################################" print "################### Orientation! #############" print "################################################" deg = data.z*15 rad = -deg * pi/180 + pi/2 print rad q = quaternion_from_euler(0,rad,0) self.current_pose.pose.orientation.x = q[0] self.current_pose.pose.orientation.y = q[1] self.current_pose.pose.orientation.z = q[2] self.current_pose.pose.orientation.w = q[3] else: return # transform the pose stamped to the new frame def transform_pose(self, new_frame, pose): if pose.header.frame_id == new_frame: return pose try: ps = deepcopy(pose) ps.header.stamp = rospy.Time(0) self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0)) new_pose = self.tfl.transformPose(new_frame, ps) new_pose.header.stamp = deepcopy(pose.header.stamp) return new_pose except Exception as e: rospy.logerr(e) rospy.logerr("no transform") return None