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) # 是否需要使用笛卡尔空间的运动规划 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): # 初始化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 __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) # 是否需要使用笛卡尔空间的运动规划 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) # 初始化需要使用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 __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('gripper_test') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') arm.set_max_velocity_scaling_factor(0.3) arm.set_max_acceleration_scaling_factor(0.5) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander('gripper') self.gripper = gripper # 控制机械臂先回到初始位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(0.5) # 控制机械臂到目标位置 arm.set_named_target('test_2') arm.go() # 控制夹爪闭合 gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() # 控制机械臂先回到初始位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def init_mp(cls): movegroup_ns = ManipulatorActions.get_ns() robot_description = ManipulatorActions.get_robdesc_ns() sc = PlanningSceneInterface(ns=movegroup_ns) mg = MoveGroupCommander('arm', ns=movegroup_ns, robot_description=robot_description) mg.set_max_acceleration_scaling_factor(0.5) mg.set_max_velocity_scaling_factor(0.6) mg.set_end_effector_link('tool_frame') rospy.sleep(1.0) sc.remove_world_object() ManipulatorActions.rc = RobotCommander(ns=movegroup_ns) ManipulatorActions.mg = mg ManipulatorActions.sc = sc ManipulatorActions.add_table_plane() ManipulatorActions.measure_weight(calibrate=True) ManipulatorActions.move_up(home=True) ManipulatorActions.measure_weight(calibrate=True) return mg, sc
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(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)
def inverse_kinematics(): # Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 50 request.ik_request.pose_stamped.header.frame_id = "base" # Create joint constraints #This is joint constraint will need to be set at the group constraints = Constraints() joint_constr = JointConstraint() joint_constr.joint_name = "right_j0" joint_constr.position = TORSO_POSITION joint_constr.tolerance_above = TOLERANCE joint_constr.tolerance_below = TOLERANCE joint_constr.weight = 0.5 constraints.joint_constraints.append(joint_constr) # Get the transformed AR Tag (x,y,z) coordinates # Only care about the x coordinate of AR tag; tells use # how far away wall is # x,y, z tell us the origin of the AR Tag x_coord = board_x # DONT CHANGE y_coord = bounding_points[0] z_coord = bounding_points[1] y_width = bounding_points[2] z_height = bounding_points[3] #Creating Path Planning waypoints = [] z_bais = 0 print( "OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!" ) print(bounding_points) for i in range(int(float(z_height) / .03)): target_pose = Pose() target_pose.position.x = float(x_coord - PLANNING_BIAS) if i % 2 == 0: #Starting positions target_pose.position.y = float(y_coord) else: target_pose.position.y = y_coord + float(y_width) #Starting positions target_pose.position.z = float(z_coord + z_bais) target_pose.orientation.y = 1.0 / 2**(1 / 2.0) target_pose.orientation.w = 1.0 / 2**(1 / 2.0) waypoints.append(target_pose) z_bais += .03 #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose try: #Send the request to the service response = compute_ik(request) group = MoveGroupCommander("right_arm") group.set_max_velocity_scaling_factor(0.75) #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose #Creating a Robot Trajectory for the Path Planning jump_thres = 0.0 eef_step = 0.1 path, fraction = group.compute_cartesian_path([target_pose], eef_step, jump_thres) print("Path fraction: {}".format(fraction)) #Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) #Setting the Joint constraint group.set_path_constraints(constraints) if fraction < 0.5: group.go() else: group.execute(path) if i < int(float(z_height) / 0.03) and i > 0: target2 = target_pose target2.position.z += 0.03 path, fraction = group.compute_cartesian_path([target2], eef_step, jump_thres) group.set_path_constraints(constraints) group.execute(path) except rospy.ServiceException, e: print "Service call failed: %s" % e
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling, max_acceleration_scaling, angle_delta, translation_delta, move_group_namespace='/'): # self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic if not move_group_namespace.endswith('/'): move_group_namespace = move_group_namespace + '/' if move_group_namespace != '/': self.mgc = MoveGroupCommander( move_group_name, robot_description=move_group_namespace + 'robot_description', ns=move_group_namespace) else: self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault" ) # TODO: this is only needed for the UR5 self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [ math.radians(350) ] # TODO: make param self.target_poses = [] self.current_pose_index = None self.current_plan = None self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] self.angle_delta = angle_delta self.translation_delta = translation_delta def set_and_check_starting_position(self): # sets the starting position to the current robot cartesian EE position and checks movement in all directions # TODO: make repeatable # - save the home position and each target position as joint position # - plan to each target position and back, save plan if not crazy # - return true if can plan to each target position without going crazy self.start_pose = self.mgc.get_current_pose() self.target_poses = self._compute_poses_around_state( self.start_pose, self.angle_delta, self.translation_delta) self.current_pose_index = -1 ret = self._check_target_poses(self.joint_limits) if ret: rospy.loginfo("Set current pose as home") return True else: rospy.logerr("Can't calibrate from this position!") self.start_pose = None self.target_poses = None return False def select_target_pose(self, i): number_of_target_poses = len(self.target_poses) if 0 <= i < number_of_target_poses: rospy.loginfo("Selected pose {} for next movement".format(i)) self.current_pose_index = i return True else: rospy.logerr( "Index {} is out of bounds: there are {} target poses".format( i, number_of_target_poses)) return False def plan_to_start_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal rospy.loginfo("Planning to home pose") return self._plan_to_pose(self.start_pose) def plan_to_current_target_pose(self): # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal i = self.current_pose_index rospy.loginfo("Planning to target pose {}".format(i)) return self._plan_to_pose(self.target_poses[i]) def execute_plan(self): if self.plan is None: rospy.logerr("No plan found!") return False if CalibrationMovements._is_crazy_plan(self.plan, self.fallback_joint_limits): rospy.logerr("Crazy plan found, not executing!") return False self.mgc.execute(self.plan) return True def _plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if CalibrationMovements._is_crazy_plan(plan, self.fallback_joint_limits): rospy.logwarn("Planning failed") self.plan = None return False else: rospy.loginfo("Planning successful") self.plan = plan return True def _check_target_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.target_poses: self.mgc.set_pose_target(fp) ret = self.mgc.plan() if type(ret) is tuple: # noetic success, plan, planning_time, error_code = ret else: # melodic plan = ret if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements._is_crazy_plan( plan, joint_limits): return False return True @staticmethod def _compute_poses_around_state(start_pose, angle_delta, translation_delta): basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: accept a list of delta values pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(zip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) return final_poses @staticmethod def _rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def _is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
class MoveItObstaclesDemo: def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group self.arm = MoveGroupCommander('manipulator') # 设置机械臂工作空间 self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100]) # 设置机械臂最大速度 self.arm.set_max_velocity_scaling_factor(value=0.1) # 获取终端link的名称 self.end_effector_link = self.arm.get_end_effector_link() rospy.loginfo('end effector link: {}'.format(self.end_effector_link)) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 # self.arm.allow_replanning(True) self.arm.set_num_planning_attempts(10) # self.arm.allow_looking(True) # 设置目标位置所使用的参考坐标系 self.reference_frame = 'base_link' self.arm.set_pose_reference_frame(self.reference_frame) # 设置每次运动规划的时间限制:5s self.arm.set_planning_time(3) # 控制机械臂先回到初始化位置 self.arm.set_named_target('home') self.arm.go() rospy.sleep(2) def planning(self, start_point, end_point): """ 功能:动态避障 :param start_point: 起始点, type: dict :param end_point: 终点, type: dict :return: None """ # 移动机械臂到指定位置,并获取当前位姿数据为机械臂运动的起始位姿 if start_point: self.move_arm(p=start_point) self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose # 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中 # way_points = self.get_way_points(start_point, end_point) while True: print('set planner id: RRT') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('up') self.arm.go() rospy.sleep(5) print('set planner id: PRM') self.arm.set_planner_id('TRRTkConfigDefault') self.arm.set_named_target('home') self.arm.go() rospy.sleep(5) # ------------------------------------------------------------------- def get_way_points(self, a, b): way_points = [] # plan 1 self.arm.set_named_target('up') traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") else: rospy.logerr("Trajectory is empty. Planning false!") self.arm.clear_pose_targets() # plan 2 # traj = self.moveit_planning(p=b) for i, p in enumerate(traj.joint_trajectory.points): # rospy.loginfo('waypoint: {}'.format(i)) if i%2 == 0: point = { 'x': p.positions[0], 'y': p.positions[1], 'z': p.positions[2], 'ox': p.positions[3], 'oy': p.positions[4], 'oz': p.positions[5] } way_points.append(point) rospy.loginfo('waypoint: \n {}'.format(way_points)) rospy.loginfo(len(way_points)) return way_points def moveit_planning(self, p): """ 使用moveit求解器规划路径 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start moveit planning...') target_pose = PoseStamped() target_pose.header.frame_id = self.reference_frame target_pose.pose.position.x = p['x'] target_pose.pose.position.y = p['y'] target_pose.pose.position.z = p['z'] if 'ox' in p.keys() and p['ox']: target_pose.pose.orientation.x = p['ox'] if 'oy' in p.keys() and p['oy']: target_pose.pose.orientation.y = p['oy'] if 'oz' in p.keys() and p['oz']: target_pose.pose.orientation.z = p['oz'] if 'ow' in p.keys() and p['ow']: target_pose.pose.orientation.w = p['ow'] # 传入一个PoseStamped # self.arm.set_pose_target(target_pose, self.end_effector_link) # 尝试直接传入一个列表 self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link) traj = self.arm.plan() if traj.joint_trajectory.points: # True if trajectory contains points rospy.loginfo("get trajectory success") return traj else: rospy.logerr("Trajectory is empty. Planning false!") def move_arm(self, p): """ 移动机械臂到p点 :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0} :return: """ rospy.loginfo('start arm moving...') traj = self.moveit_planning(p) self.arm.execute(traj) # 设置当前位置为起始位置 self.arm.set_start_state_to_current_state() rospy.sleep(1) def stop_arm(self): """ 急停 :return: """ pass def exist_danger_obstacle(self): """ 环境中是否存在危险的障碍物 :return: True or False """ return False def get_obstacle_octomap(self): """ 获取环境的octomap信息 :return: """ pass
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划,获取参数,如果没有设置,则默认为True,即走直线 cartesian = True #rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('armc_arm') # 当运动规划失败后,允许重新规划 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('init_pose') arm.go() rospy.sleep(1) # 设置机械臂运动的起始位姿 joint_goal = arm.get_current_joint_values() joint_goal[0] = 0 joint_goal[1] = -pi / 6 joint_goal[2] = 0 joint_goal[3] = pi / 3 joint_goal[4] = 0 joint_goal[5] = pi / 3 joint_goal[6] = 0 arm.go(joint_goal) rospy.sleep(1) #获取当前位置为规划初始位置 start_pose = arm.get_current_pose(end_effector_link).pose # 初始化路点列表 waypoints = [] # 如果为True,将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置路点数据,并加入路点列表,所有的点都加入 wpose = deepcopy(start_pose) #拷贝对象 wpose.position.x += 0.1 waypoints.append(deepcopy(wpose)) if cartesian: #如果设置为True,那么走直线 #wpose.position.x += 0.020 waypoints.append(deepcopy(wpose)) else: #否则就走曲线 arm.set_pose_target(wpose) #自由曲线 arm.go() rospy.sleep(1) wpose.position.x += 0.01 if cartesian: #wpose.position.x += 0.030 waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) wpose.position.y += 0.1 if cartesian: #wpose.position.x += 0.040 waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) wpose.position.x -= 0.15 wpose.position.y -= 0.1 if cartesian: #wpose.position.x += 0.050 waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) 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: #规划路径 ,fraction返回1代表规划成功 (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表,这里是5个点 0.01, # eef_step,终端步进值,每隔0.01m计算一次逆解判断能否可达 0.0, # jump_threshold,跳跃阈值,设置为0代表不允许跳跃 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(5) # 控制机械臂先回到初始化位置 #arm.set_named_target('init_pose') #arm.go() #rospy.sleep(5) # 关闭并退出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_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, pickPos, placePos): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm pick_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the momitive group for the right gripper pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = pick_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) pick_arm.set_goal_position_tolerance(0.05) pick_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution pick_arm.allow_replanning(True) # Set the right arm reference frame pick_arm.set_pose_reference_frame(REFERENCE_FRAME) #pick_arm.set_planner_id("RRTConnectkConfigDefault") #pick_arm.set_planner_id("RRTstarkConfigDefault") pick_arm.set_planner_id("RRTstarkConfigDefault") # Allow 5 seconds per planning attempt pick_arm.set_planning_time(3) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name base_table_id = 'base_table' target_id = 'target' limit_table_id = 'limit_table' #tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(base_table_id) #scene.remove_world_object(table_id) #scene.remove_world_object(box1_id) #scene.remove_world_object(box2_id) scene.remove_world_object(target_id) #scene.remove_world_object(tool_id) scene.remove_world_object(limit_table_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) # Set the height of the table off the ground # table_ground = 0.6 table_ground = 0.0 # Set the dimensions of the scene objects [l, w, h] base_table_size = [2, 2, 0.04] #table_size = [0.2, 0.7, 0.01] #box1_size = [0.1, 0.05, 0.05] #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.055, 0.055, 0.12] target_radius = 0.03305 target_height = 0.12310 limit_table_size = [0.6, 1.8, 0.04] # Add a base table to the scene base_table_pose = PoseStamped() base_table_pose.header.frame_id = REFERENCE_FRAME base_table_pose.pose.position.x = 0.0 base_table_pose.pose.position.y = 0.0 base_table_pose.pose.position.z = -0.3 base_table_pose.pose.orientation.w = 1.0 scene.add_box(base_table_id, base_table_pose, base_table_size) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() rospy.sleep(1) # Add a table top and two boxes to the scene #table_pose = PoseStamped() #table_pose.header.frame_id = REFERENCE_FRAME #table_pose.pose.position.x = 0.5 #table_pose.pose.position.y = -0.4 #table_pose.pose.position.z = table_ground + table_size[2] / 2.0 #table_pose.pose.orientation.w = 1.0 #scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = pickPos.pose.position.x target_pose.pose.position.y = pickPos.pose.position.y target_pose.pose.position.z = pickPos.pose.position.z target_pose.pose.orientation.w = pickPos.pose.orientation.w # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) #scene.add_cylinder(target_id,target_pose,target_height,target_radius) #Add the limit_table object to the scene limit_table_pose = PoseStamped() limit_table_pose.header.frame_id = REFERENCE_FRAME limit_table_pose.pose.position.x = 0.58 limit_table_pose.pose.position.y = -0.4 limit_table_pose.pose.position.z = 0.18 limit_table_pose.pose.orientation.w = 1.0 #scene.add_box(limit_table_id, limit_table_pose, limit_table_size) self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0) # Make the table red and the boxes orange #self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) # self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object #pick_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up #指定拾取后的放置目标姿态 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = placePos.pose.position.x place_pose.pose.position.y = placePos.pose.position.y place_pose.pose.position.z = placePos.pose.position.z place_pose.pose.orientation.w = placePos.pose.orientation.w place_pose.pose.orientation.x = placePos.pose.orientation.x place_pose.pose.orientation.y = placePos.pose.orientation.y place_pose.pose.orientation.z = placePos.pose.orientation.z #设置机器人运行最大速度位百分之~ pick_arm.set_max_velocity_scaling_factor(0.5) # Initialize the grasp pose to the target pose #初始化抓取目标点位 grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it # this way is just used by PR2 robot #grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RV for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts #重复直到成功或者超出尝试的次数 #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: while result != 1 and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) #moveit中的pick接口,target_id为moveit添加场景的id,此处为目标物体,grasps为可尝试抓取的点位序列 result = pick_arm.pick(target_id, grasps) #打印信息 rospy.logerr("pick_arm.pick: " + str(result)) rospy.sleep(0.2) # If the pick was successful, attempt the place operation #如果抓取成功,尝试放置操作 result_g = None n_attempts = 2 #if result == MoveItErrorCodes.SUCCESS: while result_g != True and n_attempts < max_pick_attempts: #and result == 1: # Generate valid place poses #places = self.make_places(place_pose) n_attempts += 1 print("-------------------") print(place_pose) #更新当前的机械臂状态 pick_arm.set_start_state_to_current_state() #设置moveit运动的目标点位 pick_arm.set_pose_target(place_pose) # Repeat until we succeed or run out of attempts #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: # n_attempts += 1 # rospy.loginfo("Place attempt: " + str(n_attempts)) # for place in places: # result = pick_arm.place(target_id, place) # if result == MoveItErrorCodes.SUCCESS: # break # rospy.sleep(0.2) #moveit的运动接口,wait=True表示等到执行完成才返回 result_g = pick_arm.go(wait=True) rospy.logerr("pick_arm.go: " + str(result_g)) #打开夹爪 open_client(500) rospy.sleep(0.2) #if result == "False" # rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") #else: # rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "home" pose stored in the SRDF file #将机械臂返回到SRDF中的“home”姿态 #scene.remove_world_object(target_id) #scene.remove_attached_object(GRIPPER_FRAME, target_id) #scene.remove_world_object(target_id) rospy.sleep(1) open_client(500) pick_arm.set_named_target(ARM_HOME_POSE) pick_arm.go() # Open the gripper to the neutral position # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL) # pick_gripper.go() rospy.sleep(1) rospy.logerr("pick_server over ")
class CalibrationMovements: def __init__(self, move_group_name, max_velocity_scaling=0.5, max_acceleration_scaling=0.5): #self.client = HandeyeClient() # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic self.mgc = MoveGroupCommander(move_group_name) self.mgc.set_planner_id("RRTConnectkConfigDefault") self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling) self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling) self.start_pose = self.mgc.get_current_pose() self.poses = [] self.current_pose_index = -1 self.fallback_joint_limits = [math.radians(90)] * 4 + [ math.radians(90) ] + [math.radians(180)] + [math.radians(350)] if len(self.mgc.get_active_joints()) == 6: self.fallback_joint_limits = self.fallback_joint_limits[1:] def compute_poses_around_current_state(self, angle_delta, translation_delta): self.start_pose = self.mgc.get_current_pose() basis = np.eye(3) pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave final_rots = [] for qd in quaternion_deltas: final_rots.append(list(qd)) # TODO: clean up pos_deltas = [ quaternion_from_euler(*rot_axis * angle_delta / 2) for rot_axis in basis ] neg_deltas = [ quaternion_from_euler(*rot_axis * (-angle_delta / 2)) for rot_axis in basis ] quaternion_deltas = list( chain.from_iterable(izip(pos_deltas, neg_deltas))) # interleave for qd in quaternion_deltas: final_rots.append(list(qd)) final_poses = [] for rot in final_rots: fp = deepcopy(self.start_pose) ori = fp.pose.orientation combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w], rot) fp.pose.orientation = Quaternion(*combined_rot) final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x += translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.x -= translation_delta / 2 final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y += translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.y -= translation_delta final_poses.append(fp) fp = deepcopy(self.start_pose) fp.pose.position.z += translation_delta / 3 final_poses.append(fp) self.poses = final_poses self.current_pose_index = -1 def check_poses(self, joint_limits): if len(self.fallback_joint_limits) == 6: joint_limits = joint_limits[1:] for fp in self.poses: self.mgc.set_pose_target(fp) plan = self.mgc.plan() if len(plan.joint_trajectory.points ) == 0 or CalibrationMovements.is_crazy_plan( plan, joint_limits): return False return True def plan_to_start_pose(self): return self.plan_to_pose(self.start_pose) def plan_to_pose(self, pose): self.mgc.set_start_state_to_current_state() self.mgc.set_pose_target(pose) plan = self.mgc.plan() return plan def execute_plan(self, plan): if CalibrationMovements.is_crazy_plan(plan, self.fallback_joint_limits): raise RuntimeError("got crazy plan!") self.mgc.execute(plan) @staticmethod def rot_per_joint(plan, degrees=False): np_traj = np.array([p.positions for p in plan.joint_trajectory.points]) if len(np_traj) == 0: raise ValueError np_traj_max_per_joint = np_traj.max(axis=0) np_traj_min_per_joint = np_traj.min(axis=0) ret = abs(np_traj_max_per_joint - np_traj_min_per_joint) if degrees: ret = [math.degrees(j) for j in ret] return ret @staticmethod def is_crazy_plan(plan, max_rotation_per_joint): abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan) if (abs_rot_per_joint > max_rotation_per_joint).any(): return True else: return False
class pick_place(): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.ur5 = MoveGroupCommander("manipulator") self.gripper = MoveGroupCommander("end_effector") print("======================================================") print(self.robot.get_group_names()) print(self.robot.get_link_names()) print(self.robot.get_joint_names()) print(self.robot.get_planning_frame()) print(self.ur5.get_end_effector_link()) print("======================================================") self.ur5.set_max_velocity_scaling_factor(0.2) self.ur5.set_max_acceleration_scaling_factor(0.2) self.ur5.set_end_effector_link("fts_toolside") self.ur5.set_planning_time(10.0) #self.ur5.set_planner_id("RRTkConfigDefault") self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller') self.gripper_ac.wait_for_server() self.gripper_ac.initiate() self.gripper_ac.send_goal(0.08) self.gripper_ac.wait_for_result() self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) def single_exuete(self, position, mode): offset = 0.01 rospy.loginfo("let do a single exuete") rospy.sleep(1) position_copy = deepcopy(position) position_copy += [0.14] position_copy[1] = position_copy[1] + offset pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2]) post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2]) grasp_position = define_grasp(position_copy) self.ur5.set_pose_target(pre_position) rospy.loginfo("let's go to pre position") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) self.ur5.set_pose_target(grasp_position) rospy.loginfo("let's do this") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() if mode == "pick": self.gripper_ac.send_goal(0) if mode == "place": self.gripper_ac.send_goal(0.08) self.gripper_ac.wait_for_result() rospy.loginfo("I got this") rospy.sleep(1) self.ur5.set_pose_target(post_position) rospy.loginfo("move out") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) def pair_exuete(self, pick_position, place_position): rospy.loginfo("here we go pair") if pick_position and place_position: self.single_exuete(pick_position, "pick") self.single_exuete(place_position, "place") #rospy.sleep(1) rospy.loginfo("let's go and get some rest") rest_position = define_grasp([0.486, -0.152, 0.342]) self.ur5.set_pose_target(rest_position) self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) def pickplace_cb(self, msg): #print(msg) print(msg.data) a = list(msg.data) mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)]) mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)]) num_goals = (len(msg.data) -2)/2 rospy.loginfo("there is {} goals".format(num_goals)) for i in range(0, len(a)-2, 2): pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1]) leeway_x = int(msg.data[i] - mean_x) leeway_y = int(msg.data[i+1] - mean_y) place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y) print(pick_x, pick_y) print(place_x, place_y) self.pair_exuete([pick_x, pick_y], [place_x, place_y])
def __init__(self, handeye_client, mode): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) if mode == "sim": arm.set_max_velocity_scaling_factor(1.0) elif mode == "real": arm.set_max_velocity_scaling_factor(0.5) arm.set_planning_time(0.05) # 规划时间限制 # 获取终端link的名称 eef_link = arm.get_end_effector_link() # print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('calib1') # arm.go() # self.go_to_joint_state([2.5228, 0.8964, 0.9746, 0.8614, 2.7515, -1.8821, 0.1712]) self.mode = mode self.listener = tf.TransformListener() self.handeye_client = handeye_client self.im_converter = image_converter() self.aruco_pix_listener = aruco_pix_listener() self.data_num = 0 if mode == "sim": self.camera_link = "camera_link_optical" self.states = [ [2.5230, 0.8961, 0.9750, 0.8609, 2.7510, -1.8819, 0.1710], [2.4861, 0.9171, 0.9988, 0.8576, 2.8072, -1.8743, 0.1717], [2.5231, 0.8961, 0.9738, 0.8604, 2.7517, -1.8818, 0.1700], [2.4003, 0.7911, 0.9820, 1.0503, 2.8352, -1.6524, 0.3760], [2.5049, 0.9006, 0.9405, 0.8542, 2.8436, -1.8724, 0.4127], [2.6273, 0.8107, 0.9139, 0.9618, 2.7511, -1.7930, 0.5062], [2.7162, 0.8069, 0.8263, 0.9200, 2.7147, -1.8470, 0.7433], [2.7065, 0.8050, 0.8584, 0.9477, 2.4444, -1.8806, 0.1071], [2.2901, 0.7806, 1.0289, 1.2211, 2.7996, -1.7092, 0.1444], [2.1136, 0.7398, 1.0706, 1.2892, 2.7842, -1.5650, -0.0094], [2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462], [2.7180, 0.7731, 1.0472, 1.2663, 2.4937, -1.7672, 0.5462], [2.6515, 0.6861, 1.0594, 1.4848, 2.5607, -1.5177, 0.8066], [2.7958, 0.8070, 0.9759, 1.2940, 2.4786, -1.6845, 0.7778], [2.8772, 0.7243, 0.9153, 1.4943, 2.3478, -1.4724, 1.0759], [2.3152, 0.8412, 1.1005, 1.1974, 2.5480, -1.6883, 0.2808], [2.0790, 0.8522, 1.0670, 1.6003, 2.6245, -1.2628, 0.5669], [2.1475, 1.0192, 1.0940, 1.3640, 2.6444, -1.3627, 0.3612], [0.2200, -0.5008, -2.8969, 1.7277, 2.6600, -1.1654, 1.5059], [0.4877, -0.7181, -0.2344, -1.3239, -0.1559, -1.4678, 0.5835], [0.3773, -0.6314, -0.2302, -1.4444, -0.1600, -1.3351, 0.4067], [-2.2371, 1.3616, 1.5669, -1.2129, 0.9923, -1.8344, -0.1351], [-2.3273, 1.3885, 1.5713, -0.9358, 1.0748, -1.9300, 0.5635], [-2.2465, 1.2267, 1.8483, -1.0450, 0.7416, -1.6672, 0.3275], [-1.7879, 1.2150, 1.6576, -1.5896, 0.7221, -1.4276, 0.6940], [-2.2530, 1.0939, 1.8129, -1.3149, 0.7196, -1.6466, 0.4342], [-2.1126, 1.1457, 1.7960, -1.2610, 0.6560, -1.6326, 0.4547], [-2.1270, 1.2311, 1.8561, -1.0124, 0.5739, -1.8181, 0.4290], ] elif mode == "real": self.camera_link = "camera_color_optical_frame" self.states = [ [-0.1629, -0.1209, -0.0899, -0.6650, 0.1250, -1.9180, -1.7391], [0.1768, -0.2480, -0.0637, -0.6867, -0.2229, -1.8912, -1.2967], [0.1064, -0.0347, -0.0530, -1.1423, -0.0809, -1.5052, -1.6230], [0.4235, -0.0861, -0.1177, -1.1818, -0.2675, -1.4303, -1.5148], [0.4016, -0.0026, -0.0992, -1.2851, -0.3538, -1.4122, -1.5428], [0.2777, -0.0294, -0.1911, -1.2395, -0.0141, -1.3563, -2.0564], [2.9142, 0.1833, -2.7987, -1.0661, -0.0965, -1.4191, -1.8634], [2.8310, -0.1007, -2.8406, -1.3918, 0.0263, -1.2798, -1.8718], [2.6899, -0.0448, -2.9686, -1.3379, 0.2609, -1.3600, -2.1981], [2.7554, -0.2997, -2.5511, -1.5645, -0.2672, -1.2493, -1.5705], [2.7252, 0.2366, -2.6045, -0.9946, -0.2552, -1.5880, -1.2153], [2.3309, 0.2733, -2.6384, -1.1497, 0.2391, -1.3979, -1.6642], [2.1696, 0.5283, -2.6540, -0.8638, 0.3913, -1.5963, -1.8724], [2.2318, 0.5134, -2.7120, -0.8413, 0.3381, -1.6116, -1.6351], [2.3469, 0.5802, -2.7714, -0.6768, 0.2250, -1.7614, -1.4197], [2.3452, 0.4840, -2.7232, -0.8959, 0.2437, -1.5798, -1.5464], [2.3888, 0.3196, -2.7301, -1.1076, 0.1213, -1.4436, -1.5656], [2.5365, 0.7079, -2.6854, -0.5944, 0.1202, -1.7089, -1.5256], [2.3243, 0.5048, -2.5923, -1.0267, 0.1985, -1.3861, -1.6988], [2.6772, 0.3100, -2.6433, -1.0222, -0.1752, -1.4295, -1.1358], [2.5951, 0.3157, -2.5408, -1.0630, -0.2109, -1.3997, -1.1196], [2.5800, 0.3364, -2.4705, -1.0478, -0.2944, -1.4289, -1.6568], [2.7948, 0.3616, -2.5705, -1.0294, -0.4474, -1.5064, -0.9428], [2.7595, 0.2951, -2.5461, -1.1454, -0.4809, -1.3790, -1.1017], ]
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.01) # arm.set_goal_orientation_tolerance(0.1) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('cali_2') # arm.set_named_target('test_1') arm.set_joint_value_target( [0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575]) arm.go() # exit() pose = cal_end_pose_by_quat(arm.get_current_pose().pose, -0.2, 2) arm.set_pose_target(pose) arm.go() # raw_input() # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose().pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) # 按末端坐标系方向向量平移, 计算终点位姿 wpose = cal_end_pose_by_quat(start_pose, 0.35, 2) waypoints.append(deepcopy(wpose)) def scale_trajectory_speed(traj, scale): new_traj = RobotTrajectory() new_traj.joint_trajectory = traj.joint_trajectory n_joints = len(traj.joint_trajectory.joint_names) n_points = len(traj.joint_trajectory.points) points = list(traj.joint_trajectory.points) for i in range(n_points): point = JointTrajectoryPoint() point.positions = traj.joint_trajectory.points[i].positions point.time_from_start = traj.joint_trajectory.points[ i].time_from_start / scale point.velocities = list( traj.joint_trajectory.points[i].velocities) point.accelerations = list( traj.joint_trajectory.points[i].accelerations) for j in range(n_joints): point.velocities[j] = point.velocities[j] * scale point.accelerations[ j] = point.accelerations[j] * scale * scale points[i] = point new_traj.joint_trajectory.points = points return new_traj def move_cartesian(waypoints, scale): group = arm # 开始笛卡尔空间轨迹规划 fraction = 0.0 # 路径规划覆盖率 maxtries = 10 # 最大尝试规划次数 attempts = 0 # 已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 group.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") plan = scale_trajectory_speed(plan, scale) group.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") move_cartesian(waypoints, 0.5) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
joint_target_4, joint_target_5, joint_target_init ] joint_targets = [] for i in range(0, 361, 45): for j in range(-20, 21, 20): i_rads = i * math.pi / 180 j_rads = j * math.pi / 180 joint_target = rotate_joint(joint_target_init, i_rads, JOINT_INDEX_BASE) joint_target = rotate_joint(joint_target, j_rads, JOINT_INDEX_UPPER_ARM) joint_targets.append(joint_target) for joint_target in joint_targets: group.set_joint_value_target(joint_target) group.set_max_velocity_scaling_factor(0.1) plan = group.plan() group.go(wait=True) group.stop() if rospy.is_shutdown(): break rospy.sleep(1) roscpp_shutdown()
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)
import rospy from moveit_commander import MoveGroupCommander if __name__ == '__main__': #init_node() rospy.init_node('message', anonymous=True) group = MoveGroupCommander("manipulator") exec_vel = 0.5 while True: rospy.loginfo("joint1 start") group.set_max_velocity_scaling_factor(exec_vel) group.set_joint_value_target([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) group.go() rospy.loginfo("joint1 end") rospy.loginfo("pose1 start") group.set_max_velocity_scaling_factor(exec_vel) group.set_pose_target([0.5, -0.2, 0.2, 0.0, 1.0, 0.0]) group.go() rospy.loginfo("pose1 end") rospy.loginfo("pose2 start") group.set_max_velocity_scaling_factor(exec_vel) group.set_pose_target([0.5, -0.2, 0.7, 0.0, 1.0, 0.0]) group.go() rospy.loginfo("pose2 end")
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)
class MoveCup(): def __init__(self): #basic initiatioon moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_tutorial') self.robot = moveit_commander.RobotCommander() ################ Collision Object self.scene = moveit_commander.PlanningSceneInterface() table = CollisionObject() primitive = SolidPrimitive() primitive.type = primitive.BOX box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = 'tablelol' box_pose.pose.position.x = 1.25 box_pose.pose.position.y = 0.0 box_pose.pose.position.z = -0.6 table.primitives.append(primitive) table.primitive_poses.append(box_pose) table.operation = table.ADD self.scene.add_box('table', box_pose, size=(.077, .073, .122)) #use joint_group parameter to change which arm it uses? self.joint_group = rospy.get_param('~arm', default="left_arm") self.group = MoveGroupCommander(self.joint_group) #self.group.set_planner_id("BKPIECEkConfigDefault") #this node will scale any tf pose requests to be at most max_reach from the base frame self.max_reach = rospy.get_param('~max_reach', default=1.1) #define a start pose that we can move to before stuff runs self.start_pose = PoseStamped() self.start_pose = self.get_start_pose() #remove this when working for realz self.display_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) self.rate = rospy.Rate(1) self.ik_srv = rospy.ServiceProxy( 'ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) self.limb = baxter_interface.Limb('left') self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point, self.rangecb) self.stop = False self.planning = False def rangecb(self, point): if self.planning and point.z == 0: rospy.loginfo('stop') self.stop = True self.move_start() self.rangesub.unregister() def callback(self, targetarray): #callback that moves in a constrained path to anything published to /target_poses ##First, scale the position to be withing self.max_reach #new_target = self.project_point(targetarray.data) # rospy.loginfo(self.stop) if not self.stop: self.planning = True new_target = self.project_point(targetarray) target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = 'base' target.pose.position = new_target #change orientation to be upright target.pose.orientation = self.start_pose.pose.orientation #clear group info and set it again self.group.clear_pose_targets() # self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(10) # self.group.set_pose_target(target) ################### Try joint space planning jt_state = JointState() jt_state.header.stamp = rospy.Time.now() angles = self.limb.joint_angles() jt_state.name = list(angles.keys()) jt_state.position = list(angles.values()) jt_state.header.frame_id = 'base' result = self.ik_srv([target], [jt_state], 0) angles = {} i = 0 for name in result.joints[0].name: angles[name] = result.joints[0].position[i] i = i + 1 self.group.set_joint_value_target(angles) #plan and execute plan. If I find a way, I should add error checking her #currently, if the plan fails, it just doesn't move and waits for another pose to be published plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def scale_movegroup(self, vel=.9, acc=.9): #slows down baxters arm so we stop getting all those velocity limit errors self.group.set_max_velocity_scaling_factor(vel) self.group.set_max_acceleration_scaling_factor(acc) def unscale_movegroup(self): self.group.set_max_velocity_scaling_factor(1) self.group.set_max_acceleration_scaling_factor(1) def start_baxter_interface(self): #I copied this from an example but have no idea what it does self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16, queue_size=10) self._left_arm = baxter_interface.limb.Limb("left") self._left_joint_names = self._left_arm.joint_names() print(self._left_arm.endpoint_pose()) self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # set joint state publishing to 100Hz self._pub_rate.publish(100) return def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]): #define a starting position for the move_start method new_p = PoseStamped() new_p.header.frame_id = self.robot.get_planning_frame() new_p.pose.position.x = point[0] new_p.pose.position.y = point[1] new_p.pose.position.z = point[2] p_orient = tf.transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) p_orient = Quaternion(*p_orient) new_p.pose.orientation = p_orient return (new_p) def project_point(self, multiarray): #scales an array and returns a point (see: Pose.position) to be within self.max_reach #convert points from sonar ring frame to shoulder frame x = multiarray.data[2] + sr[0] - lls[0] y = multiarray.data[0] + sr[1] - lls[1] z = (-1 * multiarray.data[1]) + sr[2] - lls[2] #scale point to a finite reach distance from the shoulder obj_dist = math.sqrt(x**2 + y**2 + z**2) scale_val = min(self.max_reach / obj_dist, .99) point_scaled = Point() #scale point and bring into the base frames point_scaled.x = scale_val * x + lls[0] point_scaled.y = scale_val * y + lls[1] point_scaled.z = scale_val * z + lls[2] return (point_scaled) def move_random(self): #moves baxter to a random position. used for testing randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_planning_time(8) self.scale_movegroup() plan = self.group.plan() while len( plan.joint_trajectory.points) == 1 and not rospy.is_shutdown(): print('plan no work') plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def move_random_constrained(self): #move baxter to a random position with constrained path planning. also for testing self.scale_movegroup() randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(100) self.scale_movegroup() constrained_plan = self.group.plan() self.group.execute(constrained_plan) self.unscale_movegroup() rospy.sleep(3) return def move_start(self): #move baxter to the self.start_pose pose self.group.clear_pose_targets() self.group.set_pose_target(self.start_pose) self.group.set_planning_time(10) print('moving to start') plan = self.group.plan() self.group.execute(plan) print('at start') self.rate.sleep() return def get_constraint(self, euler_orientation=[0, math.pi / 2, 0], tol=[3, 3, .5]): #method takes euler-angle inputs, this converts it to a quaternion q_orientation = tf.transformations.quaternion_from_euler( euler_orientation[0], euler_orientation[1], euler_orientation[2]) orientation_msg = Quaternion(q_orientation[0], q_orientation[1], q_orientation[2], q_orientation[3]) #defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down constraint = Constraints() constraint.name = 'upright_wrist' upright_orientation = OrientationConstraint() upright_orientation.link_name = self.group.get_end_effector_link() upright_orientation.orientation = orientation_msg upright_orientation.absolute_x_axis_tolerance = tol[0] upright_orientation.absolute_y_axis_tolerance = tol[1] upright_orientation.absolute_z_axis_tolerance = tol[2] upright_orientation.weight = 1.0 upright_orientation.header = self.start_pose.header constraint.orientation_constraints.append(upright_orientation) return (constraint)
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 group_name = "arm" arm = MoveGroupCommander(group_name) robot = moveit_commander.RobotCommander() # Get joint bounds joint_names = robot.get_joint_names(group=group_name) joint_bounds = [] for joint_name in joint_names: joint = robot.get_joint(joint_name) joint_bounds.append(joint.bounds()) print("[INFO] " + joint_name + "_bounds:", joint.bounds()) # 当运动规划失败后,允许重新规划 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.joint_bounds = joint_bounds 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
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): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('oval_trajectory_planning', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' 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.3) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('start') arm.go() rospy.sleep(1) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.38665 target_pose.pose.position.y = 0 target_pose.pose.position.z = 0.37226 target_pose.pose.orientation.x = 1 target_pose.pose.orientation.y = 0.00007 target_pose.pose.orientation.z = -0.0024684 target_pose.pose.orientation.w = 0.00003 # 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) arm.go() # 初始化路点列表 waypoints = [] # 将圆弧上的路径点加入列表 waypoints.append(target_pose.pose) centerA = target_pose.pose.position.y centerB = target_pose.pose.position.z long_axis = 0.03 short_axis=0.06 for th in numpy.arange(0, 6.28, 0.005): target_pose.pose.position.y = centerA + long_axis * math.cos(th)-0.03 target_pose.pose.position.z = centerB + short_axis * math.sin(th) wpose = deepcopy(target_pose.pose) waypoints.append(deepcopy(wpose)) #print('%f, %f' % (Y, Z)) 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) rospy.loginfo("位姿控制---->椭圆轨迹规划 成功!") # 控制机械臂先回到初始化位置 arm.set_named_target('start') 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('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) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(1) arm.set_max_velocity_scaling_factor(1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() listener = tf.TransformListener() while not rospy.is_shutdown(): try: # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换 (trans, rot) = listener.lookupTransform('/base_link', '/iron_block', rospy.Time(0)) object_detect_x = trans[0] object_detect_y = trans[1] # object_detect_z = trans[2] # if(object_detect_x>0.033 or object_detect_x<0.027): # # print("detect x error!") # continue if (object_detect_y > 0.25 or object_detect_y < 0.2): # print("detect y error!") continue # if(trans[2]>-0.04 or trans[2]<-0.08): # print("detect z error!") # continue print(trans) # print(rot) # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # 姿态使用四元数描述,基于base_link坐标系 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = object_detect_x target_pose.pose.position.y = object_detect_y - 0.05 target_pose.pose.position.z = 0.15 target_pose.pose.orientation.x = -rot[0] target_pose.pose.orientation.y = -rot[1] target_pose.pose.orientation.z = -rot[2] target_pose.pose.orientation.w = -rot[3] # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 设置机械臂终端运动的目标位姿 arm.set_pose_target(target_pose, end_effector_link) # 规划运动路径 traj = arm.plan() num_of_points = len(traj.joint_trajectory.points) d = rospy.Duration.from_sec(1) print traj.joint_trajectory.points[num_of_points - 1].time_from_start for index in range(num_of_points): traj.joint_trajectory.points[index].time_from_start *= \ d/traj.joint_trajectory.points[num_of_points-1].time_from_start # 按照规划的运动路径控制机械臂运动 arm.execute(traj) # # 设置机械臂工作空间中的目标位姿,位置使用x、y、z坐标描述, # # 姿态使用四元数描述,基于base_link坐标系 # target_pose = PoseStamped() # target_pose.header.frame_id = reference_frame # target_pose.header.stamp = rospy.Time.now() # target_pose.pose.position.x = object_detect_x # target_pose.pose.position.y = object_detect_y-0.1 # target_pose.pose.position.z = 0.10 # target_pose.pose.orientation.x = -rot[0] # target_pose.pose.orientation.y = -rot[1] # target_pose.pose.orientation.z = -rot[2] # target_pose.pose.orientation.w = -rot[3] # # 设置机器臂当前的状态作为运动初始状态 # arm.set_start_state_to_current_state() # # 设置机械臂终端运动的目标位姿 # arm.set_pose_target(target_pose, end_effector_link) # # 规划运动路径 # traj = arm.plan() # num_of_points = len(traj.joint_trajectory.points) # d = rospy.Duration.from_sec(0.2) # print traj.joint_trajectory.points[num_of_points-1].time_from_start # for index in range(num_of_points): # traj.joint_trajectory.points[index].time_from_start *= \ # d/traj.joint_trajectory.points[num_of_points-1].time_from_start # # 按照规划的运动路径控制机械臂运动 # arm.execute(traj) # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) wpose = deepcopy(start_pose) wpose.position.y -= 0.3 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 10 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.1, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # print plan # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo( "Path computed successfully. Moving the arm.") # num_of_points = len(plan.joint_trajectory.points) # d = rospy.Duration.from_sec(6) # scale_factor = d/plan.joint_trajectory.points[num_of_points-1].time_from_start # print scale_factor # for index in range(num_of_points): # plan.joint_trajectory.points[index].time_from_start *= scale_factor # print plan # Scale the trajectory speed by a factor of 0.25 new_traj = self.scale_trajectory_speed(plan, 0.25) arm.execute(new_traj) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # #rospy.sleep(1) # # os.system("python ~/MQTT/ros_gripper_control.py 1100") # # rospy.sleep(1) # 回起始点 # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # rospy.loginfo("go origin position") # joint_positions = [-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05[-1.1825355285207154e-05, -0.5670001723145425, 0.9280267992893177, 5.3613237943855825e-06, -1.9227286805556647, -6.314999961870072e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # os.system("python ~/MQTT/ros_gripper_control.py 0") # rospy.sleep(1) # #回起始点 # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() rospy.sleep(2) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue # listener = tf.TransformListener() # while not rospy.is_shutdown(): # try: # # 获取源坐标系base_link与目标坐标系iron_block之间最新的一次坐标转换 # (trans,rot) = listener.lookupTransform('/camera_color_optical_frame', '/iron_block', rospy.Time(0)) # object_detect_x = trans[0] # object_detect_y = trans[1] # if(object_detect_x>0.033 or object_detect_x<0.027): # # print("detect x error!") # continue # if(object_detect_y>0.040 or object_detect_y<0.020): # # print("detect y error!") # continue # # if(trans[2]>-0.04 or trans[2]<-0.08): # # print("detect z error!") # # continue # print(trans) # # print(rot) # # 获取当前位姿数据最为机械臂运动的起始位姿 # start_pose = arm.get_current_pose(end_effector_link).pose # # 初始化路点列表 # waypoints = [] # # 将初始位姿加入路点列表 # waypoints.append(start_pose) # wpose = deepcopy(start_pose) # wpose.position.y -= 0.2 # wpose.position.z -= 0.155 # waypoints.append(deepcopy(wpose)) # wpose.position.y -= 0.1 # waypoints.append(deepcopy(wpose)) # fraction = 0.0 #路径规划覆盖率 # maxtries = 100 #最大尝试规划次数 # attempts = 0 #已经尝试规划次数 # # 设置机器臂当前的状态作为运动初始状态 # arm.set_start_state_to_current_state() # # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 # while fraction < 1.0 and attempts < maxtries: # (plan, fraction) = arm.compute_cartesian_path ( # waypoints, # waypoint poses,路点列表 # 0.001, # eef_step,终端步进值 # 0.0, # jump_threshold,跳跃阈值 # True) # avoid_collisions,避障规划 # # 尝试次数累加 # attempts += 1 # # 打印运动规划进程 # if attempts % 10 == 0: # rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # #print plan # # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 # if fraction == 1.0: # rospy.loginfo("Path computed successfully. Moving the arm.") # num_of_points = len(plan.joint_trajectory.points) # d = rospy.Duration.from_sec(3) # print plan.joint_trajectory.points[num_of_points-1].time_from_start # for index in range(num_of_points): # plan.joint_trajectory.points[index].time_from_start *= \ # d/plan.joint_trajectory.points[num_of_points-1].time_from_start # #print plan # arm.execute(plan) # rospy.loginfo("Path execution complete.") # # 如果路径规划失败,则打印失败信息 # else: # rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # #rospy.sleep(1) # # os.system("python ~/MQTT/ros_gripper_control.py 1100") # # rospy.sleep(1) # # 回起始点 # rospy.loginfo("go origin position") # joint_positions = [-0.007717277486910995, -0.9330994764397906, 0.46196335078534034, 7.155322862129146e-05, -1.08595462478185, -0.007682373472949389] # arm.set_joint_value_target(joint_positions) # # 控制机械臂完成运动 # arm.go() # # # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) # # joint_positions = [-0.8188115183246074, -0.43414310645724263, -0.2707591623036649, -0.008221640488656196, -0.8719249563699826, -0.8136649214659686] # # arm.set_joint_value_target(joint_positions) # # # 控制机械臂完成运动 # # arm.go() # # os.system("python ~/MQTT/ros_gripper_control.py 0") # # rospy.sleep(1) # # #回起始点 # # joint_positions = [-3.490401396160559e-05, -0.6098499127399651, 1.0162722513089006, 8.37696335078534e-05, -1.959607329842932, 2.268760907504363e-05] # # arm.set_joint_value_target(joint_positions) # # # 控制机械臂完成运动 # # arm.go() # rospy.sleep(2) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)