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) # 初始化需要使用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) # 是否需要使用笛卡尔空间的运动规划,获取参数,如果没有设置,则默认为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)
#!/usr/bin/env python import rospy from moveit_commander import MoveGroupCommander from actionlib_msgs.msg import GoalStatusArray if __name__ == '__main__': rospy.init_node('move_to_start') rospy.wait_for_message('move_group/status', GoalStatusArray) commander = MoveGroupCommander('panda_arm') commander.set_max_velocity_scaling_factor(0.2) commander.set_max_acceleration_scaling_factor(0.1) commander.set_named_target('ready') commander.go()
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
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('move_continue_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 # arm.set_goal_position_tolerance(0.01) # arm.set_goal_orientation_tolerance(0.1) arm.set_max_velocity_scaling_factor(0.5) arm.set_max_acceleration_scaling_factor(0.5) arm.set_start_state_to_current_state() arm.set_named_target("fusion_left") arm.go() # 轨迹速度加速度缩放 def scale_trajectory_vel_acc(traj, vel_scale, acc_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 / vel_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] * vel_scale point.accelerations[ j] = point.accelerations[j] * acc_scale * acc_scale points[i] = point new_traj.joint_trajectory.points = points return new_traj # 重新计算轨迹时间 def retime_trajectory(plan, scale): ref_state = robot.get_current_state() retimed_plan = arm.retime_trajectory(ref_state, plan, velocity_scaling_factor=scale) return retimed_plan # 拼接轨迹点 def stitch_positions(positions): plan_list = [] state = robot.get_current_state() # 路径规划, 连接各路径点 for i in range(len(positions)): # 设置前一路径点为待规划的路径起点, 起始点除外 if i > 0: state.joint_state.position = positions[i - 1] arm.set_start_state(state) # 设置目标状态 arm.set_joint_value_target(positions[i]) plan = arm.plan() plan_list.append(plan) # 创建新轨迹, 重新计算时间 new_traj = RobotTrajectory() new_traj.joint_trajectory.joint_names = plan_list[ 0].joint_trajectory.joint_names # 轨迹点拼接 new_points = [] for plan in plan_list: new_points += list(plan.joint_trajectory.points) new_traj.joint_trajectory.points = new_points # 重新计算轨迹时间 new_traj = retime_trajectory(new_traj, scale=0.5) return new_traj # positions = [[0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575], # [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575], # [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575]] # # plan = stitch_positions(positions) # arm.execute(plan) positions_name = [ 'cali_3', 'fusion_left2', 'fusion_left1', 'fusion_1', 'fusion_right1', 'fusion_right2' ] positions = [] for name in positions_name: position_dict = arm.get_named_target_values(name) joint_names = sorted(position_dict.keys()) position = [] for joint_name in joint_names: position.append(position_dict[joint_name]) positions.append(position) plan = stitch_positions(positions) # 速度加速度缩放 # plan = scale_trajectory_vel_acc(plan, 0.25, 0.25) # new_traj = retime_trajectory(plan, scale=0.25) arm.execute(plan) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) exit() # arm.get_named_target_values() # ########################################### test ############################################# # 轨迹列表 plan_list = [] # 设置初始状态 state = robot.get_current_state() arm.set_start_state(state) # 设置目标状态 aim_position1 = [0.055, 0.317, -0.033, -1.292, -0.099, -0.087, -1.575] arm.set_joint_value_target(aim_position1) plan1 = arm.plan() plan_list.append(plan1) # 设置初始状态 state.joint_state.position = aim_position1 arm.set_start_state(state) # 设置目标状态 aim_position2 = [1.055, 1.317, -1.033, -1.292, -0.099, -0.087, -1.575] arm.set_joint_value_target(aim_position2) plan2 = arm.plan() plan_list.append(plan2) # 设置初始状态 state.joint_state.position = aim_position2 arm.set_start_state(state) # 设置目标状态 aim_position3 = [1.055, -1.317, 1.033, 1.292, -0.099, 1.087, -1.575] arm.set_joint_value_target(aim_position3) plan3 = arm.plan() plan_list.append(plan3) new_traj = RobotTrajectory() new_traj.joint_trajectory.joint_names = plan1.joint_trajectory.joint_names # 轨迹点拼接 new_points = [] for plan in plan_list: new_points += list(plan.joint_trajectory.points) new_traj.joint_trajectory.points = new_points # 重新计算轨迹时间 new_traj = retime_trajectory(new_traj, scale=0.5) # 执行轨迹 arm.execute(new_traj) # 关闭并退出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('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)
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) # 是否需要使用笛卡尔空间的运动规划 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.1) arm.set_goal_orientation_tolerance(0.01) # 设置允许的最大速度和加速度 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) # 将初始位姿加入路点列表 rospy.sleep(1) test_pose = arm.get_current_pose(end_effector_link).pose print(test_pose) ###### 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 x = euler_from_quaternion(rot) quaternion = quaternion_from_euler(x[0] + 1.5707, x[1], x[2] + 3.1415926) print x arm.set_named_target('test5') # arm.go() rospy.sleep(1) ###### pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = quaternion[0] pose_target.orientation.y = quaternion[1] pose_target.orientation.z = quaternion[2] pose_target.orientation.w = quaternion[3] pose_target.position.x = 0.33 pose_target.position.y = -0.134 pose_target.position.z = 1.0484 arm.set_start_state_to_current_state() arm.set_pose_target(pose_target, end_effector_link) #_, traj, _, _ = arm_group.plan() traj = arm.plan() plan1 = arm.execute(traj) # 设置路点数据,并加入路点列表 # 控制机械臂先回到初始化位置 # arm.set_named_target('test1') # 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 # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度)u 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) # 获取当前位姿数据最为机械臂点对点运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # 初始化路点列表 waypoints = [] #放入点的位置 # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.position.z += 0.053 waypoints.append(deepcopy(wpose)) # z方向上的位移 wpose.position.x += 0.2018 waypoints.append(deepcopy(wpose)) # x方向上的位移 #wpose.position.y -= 0.0 #waypoints.append(deepcopy(wpose)) # y方向上的位移 if cartesian: fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") #rospy.loginfo(str(waypoints) ) # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.2) arm.set_goal_orientation_tolerance(0.1) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.1) arm.set_max_velocity_scaling_factor(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() print(end_effector_link) # 控制机械臂先回到初始化位置 # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print(start_pose) # 初始化路点列表 waypoints = [] os.system("rosrun pick_test send_gripper.py --value 0.0") # 将初始位姿加入路点列表 # arm.set_named_target('test5') # arm.go() # rospy.sleep(1) ###### marker = rospy.Subscriber("/ar_pose_marker", AlvarMarkers, getCarrot) listener = tf.TransformListener() r = rospy.Rate(10) while not rospy.is_shutdown(): try: (trans, rot) = listener.lookupTransform('/base_link', '/ar_marker_4', rospy.Time(0)) break except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue ###### # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) print carrot wpose.position.z -= carrot.z waypoints.append(deepcopy(wpose)) # wpose.position.z -= carrot.z+0.1 # wpose.position.y += carrot.y # waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 控制机械臂先回到初始化位置 waypoints = [] test_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(test_pose) wpose.position.x -= carrot.x + 0.03 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.02, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) ######## os.system("rosrun pick_test send_gripper.py --value 0.8") ######## waypoints = [] pick_pose = arm.get_current_pose(end_effector_link).pose wpose = deepcopy(pick_pose) wpose.position.z += 0.1 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.02, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) if fraction < 1.0: moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.5) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print start_pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.position.z -= 0.2 waypoints.append(deepcopy(wpose)) wpose.position.x += 0.1 waypoints.append(deepcopy(wpose)) wpose.position.y += 0.1 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化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
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 __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)
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)
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): # 初始化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)
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