123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651 |
- from logging.handlers import RotatingFileHandler
- import os
- import moveit_msgs.msg
- import rospy, sys
- import moveit_commander
- import moveit_msgs
- from visualization_msgs.msg import Marker, MarkerArray
- from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
- from moveit_msgs.msg import PlanningScene, ObjectColor,CollisionObject, AttachedCollisionObject,RobotTrajectory
- from moveit_msgs.msg import RobotState
- from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
- from shape_msgs.msg import SolidPrimitive
- from sensor_msgs.msg import JointState
- import tf.transformations
- from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
- from geometry_msgs.msg import PoseStamped, Pose,Point,Vector3
- from copy import deepcopy
- import numpy as np
- import tf
- from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
- import math
- import traceback
- from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
- import json
- import termios
- from decorator_time import decorator_time
- import check
- from redis_publisher import Publisher
- pi = np.pi
- class MoveIt_Control:
-
- def __init__(self, is_use_gripper=False):
- self.home_pose = []
- self.hf_num = 0
- self.hf_fail = []
-
-
- moveit_commander.roscpp_initialize(sys.argv)
- self.robot = moveit_commander.RobotCommander()
-
- self.arm = moveit_commander.MoveGroupCommander('manipulator')
- self.arm.set_goal_joint_tolerance(0.0001)
- self.arm.set_goal_position_tolerance(0.0005)
- self.arm.set_goal_orientation_tolerance(0.1)
- self.end_effector_link = self.arm.get_end_effector_link()
-
- self.reference_frame = 'base_link'
- self.arm.set_pose_reference_frame(self.reference_frame)
-
- self.arm.set_planning_time(10.0)
- self.arm.allow_replanning(True)
- self.arm.set_max_acceleration_scaling_factor(1)
- self.arm.set_max_velocity_scaling_factor(1)
- self.arm.set_num_planning_attempts(10)
-
-
- self.go_home()
- self.home_pose = self.get_now_pose()
-
- def move_j(self, joint_configuration=None, a=1, v=1):
-
- if joint_configuration == None:
- joint_configuration = [0, 0, 0, 0, 0, 0]
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
- self.arm.set_joint_value_target(joint_configuration)
- rospy.loginfo("move_j:" + str(joint_configuration))
- self.arm.go()
-
-
-
- def get_now_pose(self):
- current_pose = self.arm.get_current_pose(self.end_effector_link).pose
- pose = []
- pose.append(current_pose.position.x)
- pose.append(current_pose.position.y)
- pose.append(current_pose.position.z)
- pose.append(current_pose.orientation.x)
- pose.append(current_pose.orientation.y)
- pose.append(current_pose.orientation.z)
- pose.append(current_pose.orientation.w)
-
-
- return pose
-
-
- def create_path_constraints2(self, start_point, end_point, r):
-
- vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
- height = np.linalg.norm(vector) + 0.05
- radius = r
-
-
- position_constraint = PositionConstraint()
- position_constraint.header.frame_id = "base_link"
- position_constraint.link_name = self.end_effector_link
- position_constraint.target_point_offset = Vector3(0, 0, 0)
- position_constraint.weight = 1.0
-
- bounding_volume = SolidPrimitive()
- bounding_volume.type = SolidPrimitive.CYLINDER
- bounding_volume.dimensions = [height, radius]
- position_constraint.constraint_region.primitives.append(bounding_volume)
-
- pose = Pose()
- pose.position.x = start_point[0] + vector[0] / 2
- pose.position.y = start_point[1] + vector[1] / 2
- pose.position.z = start_point[2] + vector[2] / 2
-
-
- z_axis = np.array([0, 0, 1])
- axis = np.cross(z_axis, vector)
- angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))
- q = tf.transformations.quaternion_about_axis(angle, axis)
- pose.orientation.x = q[0]
- pose.orientation.y = q[1]
- pose.orientation.z = q[2]
- pose.orientation.w = q[3]
- position_constraint.constraint_region.primitive_poses.append(pose)
- constraints = Constraints()
- constraints.position_constraints.append(position_constraint)
-
- return constraints
-
-
- def move_p_flexible(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
- rrr = 0.2
- er = 0
-
- if trajectory:
-
- state = self.arm.get_current_state()
- state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
- path_constraints = self.create_path_constraints2(points[-1], point, rrr)
- else:
-
- self.go_home()
- self.home_pose = self.get_now_pose()
- state = self.arm.get_current_state()
- path_constraints = self.create_path_constraints2(self.home_pose, point, rrr)
- self.arm.set_path_constraints(path_constraints)
- self.arm.set_pose_target(point, self.end_effector_link)
- self.arm.set_start_state(state)
-
-
- b = 10
- for i in range(b):
- traj = self.arm.plan()
- error = traj[3]
- if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
- trajj = traj[1]
- rospy.loginfo("规划完成! 正在检查移动轨迹有效性...")
- error_c, limit_margin = check.check_joint_positions(trajj)
- if not error_c:
- rospy.loginfo("本次移动OK")
- rospy.loginfo("*******************")
- trajj_with_type = mark_the_traj(trajj,"during-p", welding_sequence)
- trajectory_with_type.append(trajj_with_type)
- points.append(point)
- trajectory.append(trajj)
- break
- else:
- rospy.loginfo("check failed! 移动轨迹无效")
- rospy.loginfo("移动轨迹检查失败-开始第{}次重新规划".format(i+1))
- self.arm.clear_path_constraints()
- rrr += 0.2
- rospy.loginfo("R值: {}".format(rrr))
- if trajectory:
- path_constraints = self.create_path_constraints2(points[-1], point, rrr)
- else:
- path_constraints = self.create_path_constraints2(self.home_pose, point, rrr)
- self.arm.set_path_constraints(path_constraints)
-
- if(i >= (b-5)):
- rospy.loginfo("约束移动规划尝试失败,取消约束重新规划")
- self.arm.clear_path_constraints()
-
- if(i == (b-1)):
- rospy.loginfo("所有移动规划尝试失败,焊缝起点不可达!")
- er = 1
- break
- else:
- er = 1
- rospy.loginfo("移动规划失败,焊缝起点不可达!")
- break
-
-
- self.arm.clear_path_constraints()
-
- return points, trajectory, trajectory_with_type, er
-
-
- def create_path_constraints(self, start_point, end_point):
-
- vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
- height = np.linalg.norm(vector) + 0.002
- radius = 0.01
- constraints = Constraints()
-
-
- position_constraint = PositionConstraint()
- position_constraint.header.frame_id = "base_link"
- position_constraint.link_name = self.end_effector_link
- position_constraint.target_point_offset = Vector3(0, 0, 0)
-
- bounding_volume = SolidPrimitive()
- bounding_volume.type = SolidPrimitive.CYLINDER
-
- bounding_volume.dimensions = [height, radius]
-
- pose = Pose()
- pose.position.x = start_point[0] + vector[0] / 2
- pose.position.y = start_point[1] + vector[1] / 2
- pose.position.z = start_point[2] + vector[2] / 2
-
-
- z_axis = np.array([0, 0, 1])
- axis = np.cross(z_axis, vector)
- angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))
- q = tf.transformations.quaternion_about_axis(angle, axis)
- pose.orientation.x = q[0]
- pose.orientation.y = q[1]
- pose.orientation.z = q[2]
- pose.orientation.w = q[3]
- position_constraint.constraint_region.primitives.append(bounding_volume)
- position_constraint.constraint_region.primitive_poses.append(pose)
- position_constraint.weight = 1.0
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- constraints.position_constraints.append(position_constraint)
-
- return constraints
-
- def move_pl(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
-
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
-
- if trajectory:
- state = self.arm.get_current_state()
-
- state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
- self.arm.set_start_state(state)
-
- else:
- """
- 第一次规划超时(Time_out),但并未返回Error_code;此时move_p并不会返回任何路径点信息
- 到points列表中,此时机器人应还处在home点位置,在此将home点直接添加到points列表中,与
- 焊缝终点做路径约束,并规划路径 (虽然不再报错继续执行,但该条焊缝仍规划失败)
- """
- points.append(self.home_pose)
-
- self.arm.set_pose_target(point, self.end_effector_link)
-
-
- path_constraints = self.create_path_constraints(points[-1], point)
- self.arm.set_path_constraints(path_constraints)
- traj = self.arm.plan()
- error = traj[3]
- if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
- rospy.loginfo("本次焊缝规划 OK")
- rospy.loginfo("*******************")
- trajj = traj[1]
- trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
- trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"
- trajj_with_type.points[-1].type = "end"
- points.append(point)
- trajectory.append(trajj)
- trajectory_with_type.append(trajj_with_type)
- else:
- rospy.loginfo("本次焊缝规划失败")
- points.pop()
-
- if trajectory:
- trajectory.pop()
- trajectory_with_type.pop()
-
- self.hf_fail.append(welding_sequence[self.hf_num])
- self.arm.clear_path_constraints()
- self.hf_num = self.hf_num + 1
- return points, trajectory, trajectory_with_type
-
- def move_pl_check(self, point, points, trajectory, trajectory_with_type, a=1, v=1):
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
-
-
- a=10
- for j in range(a):
- if trajectory:
- state = self.arm.get_current_state()
- state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
- self.arm.set_start_state(state)
-
- self.arm.set_pose_target(point, self.end_effector_link)
-
- path_constraints = self.create_path_constraints(points[-1], point)
- self.arm.set_path_constraints(path_constraints)
-
-
- b=10
- for i in range(b):
- traj = self.arm.plan()
- error = traj[3]
- if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
- trajj = traj[1]
- rospy.loginfo("正在检查焊缝轨迹有效性...")
- error_c,limit_margin = check.check_joint_positions(trajj)
- if not error_c:
- rospy.loginfo("本次焊缝规划OK")
- rospy.loginfo("*******************")
- break
- if not limit_margin or i == b-1:
- prepoint = points.pop()
- trajectory.pop()
- trajectory_with_type.pop()
-
- self.arm.clear_path_constraints()
- points, trajectory, trajectory_with_type, er1 = self.move_p_flexible(prepoint, points, trajectory, trajectory_with_type)
- break
- rospy.loginfo("The {}-{}th replanning".format(j,i+1))
- if not error_c:
- break
- if error_c:
- rospy.loginfo("本次焊缝规划失败")
- rospy.loginfo("*******************")
- self.hf_fail.append(welding_sequence[self.hf_num])
-
- self.arm.clear_path_constraints()
- self.hf_num = self.hf_num + 1
-
- trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
- trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"
- trajj_with_type.points[-1].type = "end"
- points.append(point)
- trajectory.append(trajj)
- trajectory_with_type.append(trajj_with_type)
-
- return points, trajectory, trajectory_with_type
-
-
- @decorator_time
- def go_home(self, a=1, v=1):
- rospy.loginfo("go_home start")
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
- self.arm.set_named_target('home')
- rospy.loginfo("get_home end")
- self.arm.go()
- rospy.loginfo("go_home end")
-
-
-
- def go_home_justplan(self, trajectory, trajectory_with_type, a=1, v=1):
- self.arm.set_max_acceleration_scaling_factor(a)
- self.arm.set_max_velocity_scaling_factor(v)
- state = self.arm.get_current_state()
- state.joint_state.position = trajectory[-1].joint_trajectory.points[-1].positions
- self.arm.set_start_state(state)
- self.arm.set_named_target('home')
- traj = self.arm.plan()
- trajj = traj[1]
- traj_with_type = mark_the_traj(trajj, "go-home", welding_sequence)
- trajectory.append(trajj)
- trajectory_with_type.append(traj_with_type)
- return trajectory, trajectory_with_type
-
- @decorator_time
- def path_planning(self, folder_path, gohome=True):
- file_path_result = os.path.join(folder_path, 'result.txt')
- all_data = process_welding_data(file_path_result)
- err = 0
- points, trajectory, trajectory_with_type = [],[],[]
- for i in range(len(all_data)):
- rospy.loginfo("共读取到%d条焊缝,开始规划第%d条", len(all_data), i+1)
- start_point = all_data[i][0]
- end_point = all_data[i][1]
- q1 = all_data[i][2]
- q2 = all_data[i][3]
- point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0], q1[1], q1[2], q1[3]]
- points, trajectory, trajectory_with_type, err = self.move_p_flexible(point11, points, trajectory, trajectory_with_type)
-
- if err == 1:
- self.hf_fail.append(welding_sequence[self.hf_num])
- self.hf_num = self.hf_num+1
- continue
- point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0], q2[1], q2[2], q2[3]]
- points, trajectory, trajectory_with_type = self.move_pl(point22, points, trajectory, trajectory_with_type, v=speed_v)
- rospy.loginfo("第%d条焊缝规划完毕", i+1)
- rospy.loginfo("*******************")
- if gohome:
- points, trajectory, trajectory_with_type, err = self.move_p_flexible(self.home_pose, points, trajectory, trajectory_with_type, v=speed_v)
- trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
- traj_merge = merge_trajectories(trajectory)
- trajectory_with_type_merge = merge_trajectories_with_type(trajectory_with_type)
- rospy.loginfo("All of The trajectory Plan successfully")
- return trajectory, traj_merge, trajectory_with_type_merge
- def process_welding_data(filename):
- all_data = []
- with open(filename, 'r') as file:
- for line in file:
- parts = line.strip().split('/')
- coordinates_and_poses = [part.split(',') for part in parts[1:]]
-
- start_point = tuple(map(float, coordinates_and_poses[0][:3]))
- end_point = tuple(map(float, coordinates_and_poses[1][:3]))
- q1 = tuple(map(float, coordinates_and_poses[2][:4]))
- q2 = tuple(map(float, coordinates_and_poses[3][:4]))
-
-
- all_data.append((start_point, end_point, q1, q2))
- return all_data
- def mark_the_traj(traj, TYPE, SEQUENCE):
- traj_with_type = JointTrajectory_ex()
- traj_with_type.header = traj.joint_trajectory.header
- traj_with_type.joint_names = traj.joint_trajectory.joint_names
- traj_with_type.points = [
- JointTrajectoryPoint_ex(
- positions = point.positions,
- velocities = point.velocities,
- accelerations = point.accelerations,
- effort = point.effort,
- type = TYPE,
- sequence = SEQUENCE
- ) for point in traj.joint_trajectory.points
- ]
- return traj_with_type
- def merge_trajectories(trajectories):
- if not trajectories:
- return None
-
-
- merged_trajectory = JointTrajectory()
- merged_trajectory.header = trajectories[0].joint_trajectory.header
- merged_trajectory.joint_names = trajectories[0].joint_trajectory.joint_names
-
-
- last_time_from_start = rospy.Duration(0)
-
-
- for traj in trajectories:
- for point in traj.joint_trajectory.points:
-
- new_point = deepcopy(point)
-
- new_point.time_from_start += last_time_from_start
- merged_trajectory.points.append(new_point)
-
-
- if traj.joint_trajectory.points:
- last_time_from_start = traj.joint_trajectory.points[-1].time_from_start + last_time_from_start
-
- return merged_trajectory
- def merge_trajectories_with_type(trajectory_with_type):
- if not trajectory_with_type:
- return None
-
-
- merged_trajectory_with_type = JointTrajectory_ex()
- merged_trajectory_with_type.header = trajectory_with_type[0].header
- merged_trajectory_with_type.joint_names = trajectory_with_type[0].joint_names
-
-
- last_time_from_start = rospy.Duration(0)
-
-
- for traj in trajectory_with_type:
- for point in traj.points:
-
- new_point = deepcopy(point)
-
- new_point.time_from_start += last_time_from_start
- merged_trajectory_with_type.points.append(new_point)
-
-
- if traj.points:
- last_time_from_start = traj.points[-1].time_from_start + last_time_from_start
-
- return merged_trajectory_with_type
- def pub_trajectories(trajectory_with_type_merge):
-
- pub = rospy.Publisher("/joint_path", JointTrajectory_ex, queue_size=5)
- count = 1
- rate = rospy.Rate(1)
- rospy.loginfo("正在发布轨迹信息..")
- while not rospy.is_shutdown():
-
- sign_traj_accepted = str(rospy.get_param("sign_traj_accepted"))
- if sign_traj_accepted == "1":
- rospy.set_param("sign_traj_accepted",0)
- break
- pub.publish(trajectory_with_type_merge)
-
-
- rate.sleep()
- class py_msgs():
- fial = []
- shun_xv = []
- class point():
- xyz_list = []
- type = []
- def ROS2PY_msgs(msgs, m_sr):
- for i in range(len(msgs.points)):
- py_msgs.point.xyz_list.append(msgs.points[i].positions)
- py_msgs.point.type.append(msgs.points[i].type)
- py_msgs.fail = m_sr.hf_fail.copy()
- py_msgs.shun_xv = msgs.points[0].sequence.copy()
-
-
-
- message = {
- 'positions': py_msgs.point.xyz_list,
- 'flags': py_msgs.point.type,
- 'weld_order': py_msgs.shun_xv,
- 'failed': py_msgs.fail,
- }
- return json.dumps(message)
-
-
-
- def get_valid_input(factor, default):
- while True:
- user_input = input(factor)
- if user_input == "":
- return default
- try:
- value = float(user_input)
- if 0 < value <= 1:
- return value
- else:
- rospy.logerr("缩放因子必须在(0, 1]范围内,请重新输入!")
- except ValueError:
- rospy.logerr("输入值无效,请根据提示重新输入!")
- def get_user_input():
- """获取用户输入的速度和加速度缩放因子,并在控制台中打印信息和错误提示"""
- DEFUALT_V = 1.0
- DEFUALT_A = 1.0
-
- try:
- termios.tcflush(sys.stdin, termios.TCIFLUSH)
- rospy.loginfo("输入缓存区已清空!")
-
- vv = get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
-
-
-
- return vv
- except Exception as e:
- rospy.logerr(f"发生错误:{e}")
-
- def ROS2_msgs(msgs, m_sr):
- for i in range(len(msgs.points)):
- py_msgs.point.xyz_list.append(msgs.points[i].positions)
-
- f_p = os.path.join(folder_path, 'outtt.txt')
- with open(f_p, 'w') as file:
- for point in py_msgs.point.xyz_list:
- file.write(' '.join(str(value) for value in point) + "\n")
- if __name__ =="__main__":
- folder_path = rospy.get_param("folder_path")
- rospy.init_node('moveit_control_server', anonymous=False)
- moveit_server = MoveIt_Control(is_use_gripper=False)
- welding_sequence = rospy.get_param('welding_sequence')
- speed_v = 1.0
-
-
- trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
-
-
-
- moveit_server.arm.execute(trajectory_merge)
- print(f"本次规划失败焊缝序号:{moveit_server.hf_fail}")
-
-
-
-
-
-
- rospy.set_param("sign_control",0)
|