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('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.00001) # arm.set_goal_orientation_tolerance(0.00001) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(1.0) arm.set_planning_time(0.5) # 规划时间限制为2秒 arm.allow_replanning(True) # 当运动规划失败后,是否允许重新规划 # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener()
def __init__(self, pose): self.check_collision = False # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 将场景中的颜色设置发布 self.sendColors() rospy.sleep(5) joint_positions = pose arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 if arm.go(): pass else: self.check_collision = True 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('get_camera_pose_image_auto') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) arm.set_max_velocity_scaling_factor(0.8) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.5) # 规划时间限制为2秒 arm.allow_replanning(False) # 当运动规划失败后,是否允许重新规划 # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() # 开始图像接收 image_receiver = ImageReceiver() self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.listener = tf.TransformListener() self.save_cnt = 0 print "\n\n[INFO] Pose image saver started."
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) print "[INFO] Current pose:\n", arm.get_current_pose().pose self.scene = scene self.scene_pub = scene_pub self.colors = dict() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('start_pos') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # 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) # 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) # 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_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.08) # 规划时间限制为2秒 # arm.set_num_planning_attempts(1) # 规划1次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) print "[INFO] Current pose:\n", arm.get_current_pose().pose self.scene = scene self.scene_pub = scene_pub self.colors = dict() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.broadcaster = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.gripper_len = 0.095 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.065=0.095m self.approach_distance = 0.06 self.back_distance = 0.05 # sub and pub point cloud self.point_cloud = None self.update_cloud_flag = False rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.callback_pointcloud) thread.start_new_thread(self.publish_pointcloud, ())
def arm_pose(): 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) curr_pose = arm.get_current_pose(end_effector_link).pose.position return curr_pose.x, curr_pose.y, curr_pose.z
def markerPub(): # Create a marker publisher. marker_puber = rospy.Publisher('end_effector_trail', Marker, queue_size=10) rospy.init_node('markerPub', anonymous=True) rate = rospy.Rate(10) # Get the trails of the end effector from moveit API. moveit_commander.roscpp_initialize(sys.argv) right_arm = MoveGroupCommander('arm') right_arm.set_pose_reference_frame('base_link') # Init the Marker and clear the trails created before. marker = Marker() marker.points = [] marker.ns = "my_namespace" marker.header.frame_id = 'base_link' marker.id = 0 marker.type = 4 marker.action = Marker.ADD marker.scale.x = 0.005 marker.scale.y = 0 marker.scale.z = 0 marker.color.a = 1.0 marker.color.r = 0 marker.color.g = 0 marker.color.b = 1 marker_puber.publish(marker) # Publish trails contantly. while not rospy.is_shutdown(): marker.header.stamp = rospy.Time().now() # points and line type use marker.point and arrow et. use pose marker.points.append(right_arm.get_current_pose().pose.position) #marker.pose = right_arm.get_current_pose().pose.position #marker.pose.orientation.x = 0 #marker.pose.orientation.y = 0 #marker.pose.orientation.z = 0 #marker.pose.orientation.w = 1 marker_puber.publish(marker) rate.sleep() # Exit the relevant process. 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) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('pick_6') # arm.set_named_target('home') # arm.go() self.box_name = '' self.scene = scene self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.move_distance = 0.1 self.back_distance = 0.15
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('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.8) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(1) # 规划时间限制为2秒 arm.set_num_planning_attempts(2) # 规划两次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print arm.get_current_pose(eef_link).pose sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects', Float64MultiArray, self.callback) self.juggle_rects = Float64MultiArray() self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.move_distance = 0.1 self.back_distance = 0.15
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) arm.set_max_velocity_scaling_factor(0.4) arm.set_max_acceleration_scaling_factor(0.5) arm.set_planning_time(0.1) # 规划时间限制为2秒 # arm.set_num_planning_attempts(1) # 规划1次 # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print "[INFO] Current pose:\n", arm.get_current_pose().pose self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.broadcaster = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.gripper_len = 0.082 # 手爪实际长度0.165m, 虚拟夹爪深度0.075m 0.16-0.075=0.085m self.approach_distance = 0.05 self.back_distance = 0.05
def talker_by13(): #init moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_fk_demo') #cartesian = rospy.get_param('~cartesian', True) arm = MoveGroupCommander('manipulator') arm.set_pose_reference_frame('base_link') arm.allow_replanning(True) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # arm.set_max_acceleration_scaling_factor(0.5) #arm.set_max_velocity_scaling_factor(0.5) end_effector_link = arm.get_end_effector_link() #arm.set_named_target('home') arm.set_named_target('up') arm.go() rospy.sleep(2) target_pose = PoseStamped() target_pose.header.frame_id = 'base_link' target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.86 target_pose.pose.position.y = 0.25 target_pose.pose.position.z = 0.02832 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #next: find workspace arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) traj = arm.plan() arm.execute(traj) #arm.shift_pose_target(2,-0.05,end_effector_link) #arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class Tiago(): def __init__(self, group_name='arm_torso'): self.rate = rospy.Rate(10) self.robot = RobotCommander() rospy.sleep(1) self.move_group = MoveGroupCommander(group_name) self.velocity = Twist() self.velocity_publisher = rospy.Publisher( '/mobile_base_controller/cmd_vel', Twist, queue_size=10) def move(self, linear=(0, 0, 0), angular=(0, 0, 0)): self.velocity.linear.x = linear[ 0] # Forward or Backward with in m/sec. self.velocity.linear.y = linear[1] self.velocity.linear.z = linear[2] self.velocity.angular.x = angular[0] self.velocity.angular.y = angular[1] self.velocity.angular.z = angular[ 2] # Anti-clockwise/clockwise in radians per sec self.velocity_publisher.publish(self.velocity) def send_arm_goal(self, frame_id='base_footprint'): # SET EEF GOAL goal_pose = PoseStamped() goal_pose.header.frame_id = 'base_footprint' goal_pose.pose.position.x = 0.55 goal_pose.pose.position.y = -0.30 goal_pose.pose.position.z = 0.76 goal_pose.pose.orientation = Quaternion( *quaternion_from_euler(PI / 2, 0.0, 0.0)) # SEND GOAL self.move_group.set_pose_reference_frame(frame_id) self.move_group.set_pose_target(goal_pose) # EXECUTE EEF GOAL # print(dir(self.move_group)) self.move_group.go()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) self.arm = arm # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.01) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_pick_and_place_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 arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) arm.set_max_velocity_scaling_factor(1.0) arm.set_planning_time(0.05) # 规划时间限制 # 获取终端link的名称 eef_link = arm.get_end_effector_link() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 arm.set_named_target('work') arm.go() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander
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): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('TCP_Move', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('xarm6') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('link_base') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) 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() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 角度弧度转换 j1 = 90.0 / 180 * math.pi j2 = -18.6 / 180 * math.pi j3 = -28.1 / 180 * math.pi j4 = 1.0 / 180 * math.pi j5 = 47.6 / 180 * math.pi j6 = -0.9 / 180 * math.pi # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [j1, j2, j3, j4, j5, j6] arm.set_joint_value_target(joint_positions) arm.go() rospy.sleep(1) # 向下按压门把手 current_pose = arm.get_current_joint_values() current_pose[4] += (20.0 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() rospy.sleep(1) #推开门 current_pose = arm.get_current_joint_values() current_pose[0] -= (42.0 / 180.0) * math.pi current_pose[1] += (2.0 / 180.0) * math.pi current_pose[2] -= (11.4 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() 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('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.2) arm.set_goal_orientation_tolerance(0.1) # 设置允许的最大速度和加速度 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) # 控制机械臂先回到初始化位置 # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print(start_pose) # 初始化路点列表 waypoints = [] os.system("rosrun pick_test send_gripper.py --value 0.0") # 将初始位姿加入路点列表 # arm.set_named_target('test5') # arm.go() # rospy.sleep(1) ###### marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot) listener = tf.TransformListener() r = rospy.Rate(10) 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 ###### # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) print carrot wpose.position.z -= carrot.z waypoints.append(deepcopy(wpose)) # wpose.position.z -= carrot.z+0.1 # wpose.position.y += carrot.y # 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.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) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 控制机械臂先回到初始化位置 waypoints = [] test_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(test_pose) wpose.position.x -= carrot.x + 0.03 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.02, # 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) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) ######## os.system("rosrun pick_test send_gripper.py --value 0.8") ######## waypoints = [] pick_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(pick_pose) wpose.position.z += 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.02, # 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) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # 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 "grasp" pose stored in the SRDF file arm.set_named_target('left_arm_up') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.04 # 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.24 target_pose.pose.position.y = 0.275 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 blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 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 = 0.18 place_pose.pose.position.y = 0 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 grasp_pose.pose.position.x = 0.12792118579 + .1 grasp_pose.pose.position.y = 0.285290879999 + 0.05 grasp_pose.pose.position.z = 0.120301181892 #grasp_pose.pose.orientation = # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id, table_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) break # 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 = 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 = 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: # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('left_arm_rest') arm.go() # Open the gripper to the open position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # 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('robot') gripper = MoveGroupCommander("gripper") # 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.1) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) #(0.523599) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() gripper.set_named_target("GripperOpen") gripper.go(wait=True) rospy.sleep(3) #5 gripper.set_named_target("GripperClose") gripper.go(wait=True) rospy.sleep(1) # # Start in the "cartesian_z" (demo) configuration stored in the SRDF file arm.set_named_target('cartesian_z') # # Plan and execute a trajectory to the goal configuration arm.go(wait=True) rospy.sleep(5) # Execute the position again in order to start from there - !!!!workaround # arm.set_named_target('cartesian_z') # arm.go(wait=True) # 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) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.0 #05#0.01 wpose.position.y -= 0.0 wpose.position.z -= 0.25 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.0 #0.05#0.01 wpose.position.y += 0.0 wpose.position.z += 0.38 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.0 #0.05#0.01 wpose.position.y += 0.0 wpose.position.z -= 0.38 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) wpose.position.x += 0.0 wpose.position.y += 0.0 wpose.position.z += 0.25 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 #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.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.") arm.execute(plan, wait=True) 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 'transport_position' position # arm.set_named_target('cartesian_z') # arm.go(wait=True) rospy.sleep(22) arm.set_named_target('transport_position') arm.go(wait=True) rospy.sleep(27) gripper.set_named_target("GripperOpen") gripper.go(wait=True) rospy.sleep(5) gripper.set_named_target("GripperClose") gripper.go(wait=True) rospy.sleep(5) arm.set_named_target('transport_position') arm.go(wait=True) rospy.sleep(27) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
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): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') #Initialize robot robot = moveit_commander.RobotCommander() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # Create a publisher for displaying object frames self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the MoveIt! commander for the arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link eef = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) # right_arm.set_goal_position_tolerance(0.05) # right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Prepare Gazebo Subscriber self.pwh = None self.pwh_copy = None self.idx_targ = None self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback) # Prepare Gripper and open it self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction) self.ac.wait_for_server() g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100)) g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1)) self.ac.send_goal(g_open) rospy.sleep(2) # PREPARE THE SCENE while self.pwh is None: rospy.sleep(0.05) target_id = 'target' self.taid = self.pwh.name.index('wood_cube_5cm') table_id = 'table' self.tid = self.pwh.name.index('table') #obstacle1_id = 'obstacle1' #self.o1id = self.pwh.name.index('wood_block_10_2_1cm') # Remove leftover objects from a previous run scene.remove_world_object(target_id) scene.remove_world_object(table_id) #scene.remove_world_object(obstacle1_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Set the target size [l, w, h] target_size = [0.05, 0.05, 0.05] table_size = [1.5, 0.8, 0.03] #obstacle1_size = [0.1, 0.025, 0.01] ## Set the target pose on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose = self.pwh.pose[self.taid] target_pose.pose.position.z += 0.025 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose = self.pwh.pose[self.tid] table_pose.pose.position.z += 1 scene.add_box(table_id, table_pose, table_size) #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = REFERENCE_FRAME #obstacle1_pose.pose = self.pwh.pose[self.o1id] ## Add the target object to the scene #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.30 place_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) ### Make the target purple ### self.setColor(target_id, 0.6, 0, 1, 1.0) # Send the colors to the planning scene self.sendColors() #print target_pose self.object_frames_pub.publish(target_pose) rospy.sleep(2) # Initialize the grasp pose to the target pose grasp_pose = target_pose #grasp_pose.header.frame_id = 'gazebo_wolrd' # Shift the grasp pose by half the width of the target to center it # grasp_pose.pose.position.y -= target_size[1] / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x / 2.0 # grasp_pose.pose.position.x = target_pose.pose.position.x -0.07 # grasp_pose.pose.position.z += 0.18 #Allowed touch object list # ato = [target_id, 'r_forearm_link'] # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) #### [target_id] # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: # print grasp.grasp_pose self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation success = False n_attempts = 0 #Allowed touch link list atl = ['r_forearm_link'] # Repeat until we succeed or run out of attempts while success == False and n_attempts < max_pick_attempts: success = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(0.2) if success: self.ac.send_goal(g_close) rospy.sleep(3) ## If the pick was successful, attempt the place operation #if success: #success = False #n_attempts = 0 ## Generate valid place poses #places = self.make_places(place_pose) ## Repeat until we succeed or run out of attempts #while success == False and n_attempts < max_place_attempts: #for place in places: #success = right_arm.place(target_id, place) #if success: #break #n_attempts += 1 #rospy.loginfo("Place attempt: " + str(n_attempts)) #rospy.sleep(0.2) #if not success: #rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") #else: #rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") ## Return the arm to the "resting" pose stored in the SRDF file ##right_arm.set_named_target('resting') ##right_arm.go() ## Open the gripper to the neutral position #right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) #right_gripper.go() #rospy.sleep(1) #rospy.spin() # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
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)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME box2_pose.pose.position.x = 0.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class RazerControl(): def __init__(self): self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True) self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True) self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True) self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True) self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback) self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist) self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates) self.current_joint_states = None rospy.loginfo("Getting first joint_states") while self.current_joint_states == None: rospy.sleep(0.1) rospy.loginfo("Gotten!") rospy.loginfo("Connecting with right hand AS") self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction) self.right_hand_as.wait_for_server() rospy.loginfo("Connecting with left hand AS") self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction) self.left_hand_as.wait_for_server() rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)") self.right_arm_mgc = MoveGroupCommander("right_arm") self.right_arm_mgc.set_pose_reference_frame('base_link') self.left_arm_mgc = MoveGroupCommander("left_arm") self.left_arm_mgc.set_pose_reference_frame('base_link') self.torso_mgc = MoveGroupCommander("right_arm_torso") self.torso_mgc.set_pose_reference_frame('base_link') self.head_mgc = MoveGroupCommander("head") self.head_mgc.set_pose_reference_frame('base_link') self.last_hydra_message = None self.tmp_pose_right = PoseStamped() self.tmp_pose_left = PoseStamped() self.read_message = False def getJointStates(self, data): self.current_joint_states = data def create_hand_goal(self, hand_side="right", hand_pose="closed", values=0.0): """Returns the hand goal to send possible poses: closed, open, intermediate""" hand_goal = FollowJointTrajectoryGoal() hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_thumb_joint') hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_middle_joint') hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_index_joint') jtp = JointTrajectoryPoint() joint_list = ['hand_'+ hand_side +'_thumb_joint', 'hand_'+ hand_side +'_middle_joint', 'hand_'+ hand_side +'_index_joint'] ids_list = [] values_list = [] rospy.loginfo("current_joint_state is:\n" + str(self.current_joint_states)) for joint in joint_list: idx_in_message = self.current_joint_states.name.index(joint) ids_list.append(idx_in_message) values_list.append(self.current_joint_states.position[idx_in_message]) if hand_pose == "closed": jtp.positions.append(2.0) jtp.positions.append(values_list[1]) # TODO: read values and keep them jtp.positions.append(values_list[2]) # TODO: read values and keep them elif hand_pose == "open": jtp.positions.append(0.0) jtp.positions.append(values_list[1]) # TODO: read values and keep them jtp.positions.append(values_list[2]) # TODO: read values and keep them elif hand_pose == "intermediate": jtp.positions.append(values_list[0]) # TODO: read values and keep them jtp.positions.append(values) jtp.positions.append(values) jtp.velocities.append(0.0) jtp.velocities.append(0.0) jtp.velocities.append(0.0) jtp.time_from_start.secs = 2 hand_goal.trajectory.points.append(jtp) return hand_goal def hydraDataCallback(self, data): #rospy.loginfo("Received data from " + HYDRA_DATA_TOPIC) self.last_hydra_message = data self.tmp_pose_right = PoseStamped() self.tmp_pose_right.header.frame_id = 'base_link' self.tmp_pose_right.header.stamp = rospy.Time.now() self.tmp_pose_right.pose.position.x = self.last_hydra_message.paddles[1].transform.translation.x self.tmp_pose_right.pose.position.y = self.last_hydra_message.paddles[1].transform.translation.y self.tmp_pose_right.pose.position.z = self.last_hydra_message.paddles[1].transform.translation.z self.tmp_pose_right.pose.position.x += RIGHT_HAND_INITIAL_POINT.x self.tmp_pose_right.pose.position.y += RIGHT_HAND_INITIAL_POINT.y self.tmp_pose_right.pose.position.z += RIGHT_HAND_INITIAL_POINT.z self.tmp_pose_right.pose.orientation = self.last_hydra_message.paddles[1].transform.rotation self.tmp_pose_left = PoseStamped() self.tmp_pose_left.header.frame_id = 'base_link' self.tmp_pose_left.header.stamp = rospy.Time.now() self.tmp_pose_left.pose.position.x = self.last_hydra_message.paddles[0].transform.translation.x self.tmp_pose_left.pose.position.y = self.last_hydra_message.paddles[0].transform.translation.y self.tmp_pose_left.pose.position.z = self.last_hydra_message.paddles[0].transform.translation.z self.tmp_pose_left.pose.position.x += LEFT_HAND_INITIAL_POINT.x self.tmp_pose_left.pose.position.y += LEFT_HAND_INITIAL_POINT.y self.tmp_pose_left.pose.position.z += LEFT_HAND_INITIAL_POINT.z self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation if self.last_hydra_message.paddles[1].buttons[0] == True: self.pub_right_hand_pose.publish(self.tmp_pose_right) if self.last_hydra_message.paddles[0].buttons[0] == True: self.pub_left_hand_pose.publish(self.tmp_pose_left) self.pub_right_hand_pose_reference.publish(self.tmp_pose_right) self.pub_left_hand_pose_reference.publish(self.tmp_pose_left) self.read_message = False def run(self): rospy.loginfo("Press LB / RB to send the current pose") while self.last_hydra_message == None: rospy.sleep(0.1) rospy.loginfo("Got the first data of the razer... Now we can do stuff") sleep_rate=0.05 # check at 20Hz counter = 0 while True: counter += 1 rospy.loginfo("Loop #" + str(counter)) if not self.read_message: self.read_message = True if self.last_hydra_message.paddles[1].buttons[0] == True: # send curr left paddle pos to move_group right rospy.loginfo("sending curr right hand") self.right_arm_mgc.set_pose_target(self.tmp_pose_right) self.right_arm_mgc.go(wait=False) if self.last_hydra_message.paddles[0].buttons[0] == True: # send curr right paddle pos to move_group left rospy.loginfo("sending curr left hand") self.left_arm_mgc.set_pose_target(self.tmp_pose_left) self.left_arm_mgc.go(wait=False) if self.last_hydra_message.paddles[1].trigger > 0.0: # send goal right hand close proportional to trigger value (2.0 max?) rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0)) right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0) self.right_hand_as.send_goal(right_hand_goal) if self.last_hydra_message.paddles[0].trigger > 0.0: # send goal left hand close proportional to trigger value (2.0 max?) rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0)) left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0) self.left_hand_as.send_goal(left_hand_goal) if self.last_hydra_message.paddles[1].joy[0] != 0.0: # send torso rotation left(neg)/right (pos) rospy.loginfo("Rotation torso") curr_joint_val = self.torso_mgc.get_current_joint_values() self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1)) self.torso_mgc.go(wait=True) rospy.loginfo("Rotation torso sent!") if self.last_hydra_message.paddles[1].joy[1] != 0.0: # send torso inclination front(pos)/back(neg) rospy.loginfo("Inclination torso") curr_joint_val = self.torso_mgc.get_current_joint_values() self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1)) self.torso_mgc.go(wait=True) rospy.loginfo("Inclination torso sent!") if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0: twist_goal = Twist() twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1] twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0 self.pub_move_base.publish(twist_goal) # move base rotate left (neg)/ right(pos) rospy.loginfo("Move base") if self.last_hydra_message.paddles[1].buttons[3] == True: # thumb up rospy.loginfo("Right thumb up") right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open") self.right_hand_as.send_goal(right_thumb_up) if self.last_hydra_message.paddles[0].buttons[3] == True: # thumb up rospy.loginfo("Left thumb up") left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open") self.left_hand_as.send_goal(left_thumb_up) if self.last_hydra_message.paddles[1].buttons[1] == True: # thumb down rospy.loginfo("Right thumb down") right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed") self.right_hand_as.send_goal(right_thumb_up) if self.last_hydra_message.paddles[0].buttons[1] == True: # thumb down rospy.loginfo("Left thumb down") left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed") self.left_hand_as.send_goal(left_thumb_up) rospy.sleep(sleep_rate)
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): # 初始化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) rospy.Subscriber("chatter", Float64MultiArray, callback) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) gripper.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.2) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # cy_id = 'cy' box1_id = 'box1' box2_id = 'box2' box3_id = 'box3' sphere_id = 'sphere' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(box3_id) scene.remove_world_object(sphere_id) rospy.sleep(1) #控制机械臂先回到初始化位置 arm.set_named_target('init') arm.go() rospy.sleep(1) # arm.set_named_target('start') # arm.go() # rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(0) # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] box2_size = [0.01, 0.01, 0.19] box3_size = [0.005, 0.01, 0.3] sphere_R = 0.01 error = 0.03 # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 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) #scene.add_cylinder box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + 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.09 box2_pose.pose.position.y = -table_size[0] / 2.0 box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # box3_pose = PoseStamped() # box3_pose.header.frame_id = reference_frame # box3_pose.pose.position.x = pos_aim[0] # box3_pose.pose.position.y = pos_aim[1] # box3_pose.pose.position.z = box3_size[2]/2.0+0.1 # box3_pose.pose.orientation.w = 1.0 # scene.add_box(box3_id, box3_pose, box3_size) sphere_pose = PoseStamped() sphere_pose.header.frame_id = reference_frame sphere_pose.pose.position.x = pos_aim[0] + 0 sphere_pose.pose.position.y = pos_aim[1] sphere_pose.pose.position.z = pos_aim[2] sphere_pose.pose.orientation.w = 1.0 scene.add_sphere(sphere_id, sphere_pose, sphere_R) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) # self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box1_id, 1, 1, 1, 1.0) self.setColor(box2_id, 1, 1, 1, 1.0) self.setColor(box3_id, 1, 1, 1, 1.0) self.setColor(sphere_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # rospy.INFO("waiting...") # if(Recive_FLAG==1): # rospy.INFO("OK!") # 设置机械臂的运动目标位置 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = pos_aim[0] - error target_pose.pose.position.y = pos_aim[1] target_pose.pose.position.z = pos_aim[2] # target_pose.pose.orientation.w = 1.0 #####0.3 # 0.2 0.2 0.2 0.15 0.15 # 0.12 0.11 0.12 0.15 0.15 # 0.30 0.245 0.35 0.35 0.25 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.03]) gripper.go() rospy.sleep(1) # # 控制机械臂终端向x移动5cm arm.shift_pose_target(0, 0.01, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(1) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.15 # target_pose2.pose.position.y = 0 #-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('init') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) rospy.spin()
#!/usr/bin/env python import rospy from moveit_commander import MoveGroupCommander from sensor_msgs.msg import JointState from geometry_msgs.msg import Pose group = MoveGroupCommander("right_arm") # arm for fetch group.set_end_effector_link('r_gripper_tool_frame') group.set_pose_reference_frame('/base_footprint') def pose_callback(pose): pos = pose.position qtr = pose.orientation if qtr.x == 0 and qtr.y == 0 and qtr.z == 0 and qtr.w == 0: group.set_position_target([pos.x, pos.y, pos.z]) else: group.set_pose_target(pose) group.go() def joint_callback(msg): goal = map(lambda joint_name: msg.position[msg.name.index(joint_name)], group.get_active_joints()) group.set_joint_value_target(goal) group.go() if __name__ == "__main__": rospy.init_node('movescratch') rospy.Subscriber("movescratch/pose_goal", Pose, pose_callback) rospy.Subscriber("movescratch/joint_goal", JointState, joint_callback) rospy.spin()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_drawing', anonymous=True) # 初始化需要使用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.40 star_pose.pose.position.y = 0.0 star_pose.pose.position.z = 0.12 star_pose.pose.orientation.w = 1.0 # 设置机械臂终端运动的目标位姿 arm.set_pose_target(star_pose, end_effector_link) arm.go() radius = 0.1 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.113 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): # 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): # 初始化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) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 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) # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print start_pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.position.z -= 0.2 waypoints.append(deepcopy(wpose)) wpose.position.x += 0.1 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.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): # 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)
rospy.loginfo("Target Pose:\n{}".format(target_posestamped)) group.set_pose_target(target_posestamped) group.go() rospy.loginfo("Current Reference Frame: {}".format( group.get_pose_reference_frame())) # Go Back to Initial Pose group.set_pose_target(initial_pose.pose) group.go() # Relative Target with set_pose_reference_frame rospy.loginfo("Using set_pose_refercence_frame() and Pose") group.set_pose_reference_frame('/LARM_JOINT5_Link') rospy.loginfo("Current Reference Frame: {}".format( group.get_pose_reference_frame())) rospy.loginfo("Target Pose:\n{}".format(target_pose)) group.set_pose_target(target_pose) group.go() # Reset Pose Reference Frame group.set_pose_reference_frame(initial_reference_frame) rospy.loginfo("Current Reference Frame: {}".format( group.get_pose_reference_frame())) # Go Back to Initial Pose rospy.loginfo("Go Back to Initial Pose...") group.set_pose_target(initial_pose)
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
def callback(pose): object_position_info = pose.position object_orientation_info = pose.orientation print object_position_info 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('manipulator') ur5_gripper = MoveGroupCommander('gripper') 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() ur5_manipulator.set_named_target('home_j') ur5_manipulator.go() ur5_gripper.set_named_target('open') ur5_gripper.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 = object_position_info.z+0.25 wpose.position.x = object_position_info.x wpose.position.y = object_position_info.y 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.") rospy.sleep(3) ur5_gripper.set_named_target("close") plan = ur5_gripper.go() rospy.sleep(2) ur5_manipulator.set_named_target('home_j') ur5_manipulator.go() rospy.sleep(3) moveit_commander.roscpp_shutdown()
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): # 初始化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.001) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置每次运动规划的时间限制:1s arm.set_planning_time(1) # 控制机械臂运动到之前设置的“home”姿态 arm.set_named_target('home') arm.go() # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0, -1, 1, 0, -1, 0] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(0.5) # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置第二个路点数据,并加入路点列表 # 第二个路点需要向后运动0.3米,向右运动0.3米 wpose = deepcopy(start_pose) wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 wpose.position.x -= 0.2 # wpose.position.y -= 0.4 wpose.position.z -= 1 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第三个路点数据,并加入路点列表 wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 # wpose.position.x -= 0.5 wpose.position.y -= 0.5 wpose.position.z += 0.2 # wpose.position.x += 0.15 # wpose.position.y += 0.1 # 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_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' box1_id = 'box1' box2_id = 'box2' target_id = 'target' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) # 移除场景中之前与机器臂绑定的物体 scene.remove_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 设置桌面的高度 table_ground = 0.2 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.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.31 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.29 box2_pose.pose.position.y = -0.4 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # 设置目标物体的尺寸 target_size = [0.04, 0.04, 0.05] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.32 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 = 0.32 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # 将目标位置设置为机器人的抓取目标位置 grasp_pose = target_pose # 生成抓取姿态 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): # 初始化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('ur_arm') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(2) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.26 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_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.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.2 target_pose2.pose.position.y = -0.25 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(2) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() 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.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 left arm self.left_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the left gripper left_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 left arm reference frame accordingly self.left_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.left_arm.allow_replanning(False) # Set a position tolerance in meters self.left_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.left_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.left_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 left_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion left_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 left_arm shoulder lift frame #target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('left_rotate', 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.left_arm.set_named_target('l_start') self.left_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.left_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.left_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.left_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.left_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.left_arm.stop() # Move to the r_start position self.left_arm.set_named_target('r_start') self.left_arm.go() # Relax the servos self.relax_all_servos() os._exit(0)
from std_msgs.msg import Header moveit_error_dict = {} for name in MoveItErrorCodes.__dict__.keys(): if not name[:1] == '_': code = MoveItErrorCodes.__dict__[name] moveit_error_dict[code] = name if __name__=='__main__': rospy.init_node("pose_goal_test11") rospy.loginfo("Starting up move group commander for right arm") right_arm_mgc = MoveGroupCommander("right_arm_torso") goal_point = Point(0.4, -0.2, 1.1) goal_ori = Quaternion(0.0,0.0,0.0,1.0) right_arm_mgc.set_pose_reference_frame('base_link') list_goals = [] for i in range(1): goal_point.z += 0.05 rospy.loginfo(str(i) + ": Setting new goal:\n " + str(goal_point)) list_goals.append(Pose(goal_point, goal_ori)) rospy.loginfo("list of goals:\n" + str(list_goals)) right_arm_mgc.set_pose_targets(list_goals) rospy.loginfo("go()") right_arm_mgc.go()