|
|
@@ -5,6 +5,7 @@ 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
|
|
|
@@ -27,24 +28,25 @@ import json
|
|
|
import termios
|
|
|
from decorator_time import decorator_time
|
|
|
import check
|
|
|
+from redis_publisher import Publisher
|
|
|
|
|
|
pi = np.pi
|
|
|
|
|
|
class MoveIt_Control:
|
|
|
- # 初始化程序
|
|
|
+ # TODO初始化程序
|
|
|
def __init__(self, is_use_gripper=False):
|
|
|
- self.home_pose=[]
|
|
|
- self.hf_num=0#焊缝计数 从0开始
|
|
|
- self.hf_fail=[]
|
|
|
+ self.home_pose = []
|
|
|
+ self.hf_num = 0
|
|
|
+ self.hf_fail = []
|
|
|
+
|
|
|
# Init ros config
|
|
|
moveit_commander.roscpp_initialize(sys.argv)
|
|
|
- self.robot = moveit_commander.RobotCommander()
|
|
|
+ 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.arm.set_planner_id("RRTConnet")#不知道能不能用
|
|
|
|
|
|
self.end_effector_link = self.arm.get_end_effector_link()
|
|
|
# 设置机械臂基座的参考系
|
|
|
@@ -62,24 +64,24 @@ class MoveIt_Control:
|
|
|
# 机械臂初始姿态
|
|
|
#self.move_j() #备注，是否可以用move_j代替go_home来节省时间(待测试)
|
|
|
self.go_home()
|
|
|
- self.home_pose=self.get_now_pose()
|
|
|
+ self.home_pose = self.get_now_pose()
|
|
|
|
|
|
- # 关节规划，输入6个关节角度（单位：弧度）
|
|
|
- def move_j(self, joint_configuration=None,a=1,v=1):
|
|
|
+ # TODO关节规划，输入6个关节角度（单位：弧度）
|
|
|
+ def move_j(self, joint_configuration=None, a=1, v=1):
|
|
|
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
|
|
|
- if joint_configuration==None:
|
|
|
+ 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))
|
|
|
+ rospy.loginfo("move_j:" + str(joint_configuration))
|
|
|
self.arm.go()
|
|
|
#rospy.sleep(1)
|
|
|
-
|
|
|
+
|
|
|
+ # TODO获取机械臂当前位姿
|
|
|
def get_now_pose(self):
|
|
|
- # 获取机械臂当前位姿
|
|
|
current_pose = self.arm.get_current_pose(self.end_effector_link).pose
|
|
|
- pose=[]
|
|
|
+ pose = []
|
|
|
pose.append(current_pose.position.x)
|
|
|
pose.append(current_pose.position.y)
|
|
|
pose.append(current_pose.position.z)
|
|
|
@@ -92,138 +94,212 @@ class MoveIt_Control:
|
|
|
#rospy.lohome_poseinfo("Current Pose: {}".format(pose))
|
|
|
return pose
|
|
|
|
|
|
- #TODO move_p_path_constraints
|
|
|
- def create_path_constraints2(self,start_point,end_point,r):#创建路径约束
|
|
|
+ #TODO move_p 路径约束
|
|
|
+ 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.16 #高度延长16cm
|
|
|
+ height = np.linalg.norm(vector) + 0.05 #高度延长16cm
|
|
|
radius = r
|
|
|
|
|
|
# 位置约束
|
|
|
- position_constraint = PositionConstraint()#创建点位约束
|
|
|
- position_constraint.header.frame_id = "base_link"#此约束所指的机器人链接
|
|
|
+ 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#质量 OR 加权因子？
|
|
|
+ position_constraint.target_point_offset = Vector3(0, 0, 0) #目标点的偏移量
|
|
|
+ position_constraint.weight = 1.0 #权重，'1'为硬约束
|
|
|
|
|
|
#构建 shape_msgs/SolidPrimitive 消息
|
|
|
bounding_volume = SolidPrimitive()
|
|
|
- bounding_volume.type = SolidPrimitive.CYLINDER#圆柱
|
|
|
- # bounding_volume.dimensions = [0.003,1]
|
|
|
- bounding_volume.dimensions = [height,radius]#尺寸 高度 半径
|
|
|
+ bounding_volume.type = SolidPrimitive.CYLINDER #圆柱约束
|
|
|
+ bounding_volume.dimensions = [height, radius] #圆柱的尺寸，[高度，半径]
|
|
|
position_constraint.constraint_region.primitives.append(bounding_volume)
|
|
|
|
|
|
#构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
|
|
|
pose = Pose()
|
|
|
- pose.position.x = start_point[0] + vector[0] / 2#定义位置（找了个几何中心）
|
|
|
+ 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) #计算两个向量（向量数组）的叉乘 叉乘返回的数组既垂直于a，又垂直于b
|
|
|
- angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))#反余弦求角度
|
|
|
- q = tf.transformations.quaternion_about_axis(angle, axis)#通过旋转轴和旋转角返回四元数
|
|
|
+ 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 = Constraints() #创建一个新的约束信息
|
|
|
constraints.position_constraints.append(position_constraint)
|
|
|
- # constraints.orientation_constraints.append(orientation_constraint)
|
|
|
+
|
|
|
return constraints
|
|
|
|
|
|
- #TODO move_p_flexible
|
|
|
- def move_p_flexible(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
|
|
|
+ #TODO move_p_flexible 转点规划
|
|
|
+ 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.09#初始允许半径 9cm
|
|
|
- er=0
|
|
|
+ rrr = 0.2 #初始允许半径 20cm, 点到点转焊缝，可适当放宽初始半径
|
|
|
+ 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)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
+ path_constraints = self.create_path_constraints2(points[-1], point, rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
else:
|
|
|
#刚开始规划 起点位置设定为当前状态 按理来说是home点
|
|
|
self.go_home()
|
|
|
- self.home_pose=self.get_now_pose()
|
|
|
+ self.home_pose = self.get_now_pose()
|
|
|
state = self.arm.get_current_state()
|
|
|
- path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将当前的点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
+ 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)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
|
|
|
+ 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 #尝试规划5次 无约束5次
|
|
|
+ #尝试规划次数共10次，带约束规划5次仍失败取消约束重新规划5次
|
|
|
+ b = 10
|
|
|
for i in range(b):
|
|
|
- traj = self.arm.plan()#调用plan进行规划
|
|
|
- error=traj[3]
|
|
|
- if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
|
|
|
+ traj = self.arm.plan() #调用plan进行规划(OMPL)
|
|
|
+ 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)
|
|
|
+ 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)
|
|
|
- trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"
|
|
|
- trajj_with_type.points[-1].type = "end"
|
|
|
+ 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))
|
|
|
+ rospy.loginfo("移动轨迹检查失败-开始第{}次重新规划".format(i+1))
|
|
|
self.arm.clear_path_constraints()
|
|
|
- rrr=rrr+0.2#每次增加20厘米半径
|
|
|
- rospy.loginfo("R值：{}".format(rrr))
|
|
|
+ rrr += 0.2 #每次增加20厘米半径
|
|
|
+ rospy.loginfo("R值: {}".format(rrr))
|
|
|
if trajectory:
|
|
|
- path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
+ 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("所有移动规划尝试失败 取消路径约束")
|
|
|
+ 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
|
|
|
+
|
|
|
+ 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
|
|
|
+ return points, trajectory, trajectory_with_type, er
|
|
|
+
|
|
|
+ #TODO move_pl 路径约束
|
|
|
+ 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)
|
|
|
+ #构建 shape_msgs/SolidPrimitive 消息
|
|
|
+ bounding_volume = SolidPrimitive()
|
|
|
+ bounding_volume.type = SolidPrimitive.CYLINDER
|
|
|
+ # bounding_volume.dimensions = [0.003,1]
|
|
|
+ bounding_volume.dimensions = [height, radius]
|
|
|
+ #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
|
|
|
+ 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
|
|
|
+
|
|
|
+ # 角度约束
|
|
|
+ # target_pose = Pose()
|
|
|
+ # target_pose.orientation.x = start_point[3]
|
|
|
+ # target_pose.orientation.y = start_point[4]
|
|
|
+ # target_pose.orientation.z = start_point[5]
|
|
|
+ # target_pose.orientation.w = start_point[6]
|
|
|
+
|
|
|
+ # Orientation_Constraint = OrientationConstraint()
|
|
|
+ # Orientation_Constraint.link_name = self.end_effector_link
|
|
|
+ # Orientation_Constraint.header.frame_id = "base_link"
|
|
|
+ # Orientation_Constraint.orientation = target_pose.orientation
|
|
|
+ # Orientation_Constraint.absolute_x_axis_tolerance = 0.01
|
|
|
+ # Orientation_Constraint.absolute_y_axis_tolerance = 0.01
|
|
|
+ # Orientation_Constraint.absolute_z_axis_tolerance = 0.01
|
|
|
+ # Orientation_Constraint.weight = 1.0 # 权重，硬约束
|
|
|
+
|
|
|
+ constraints.position_constraints.append(position_constraint)
|
|
|
+ # constraints.orientation_constraints.append(Orientation_Constraint)
|
|
|
|
|
|
- #TODO move_pl
|
|
|
- def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划 只规划当前的点到上一个点
|
|
|
+ return constraints
|
|
|
+
|
|
|
+ #TODO move_pl 焊缝规划，规划当前点(焊缝起点)和焊缝终点之间的路径
|
|
|
+ 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)
|
|
|
|
|
|
- #设定约束
|
|
|
+ #move_p规划成功，则move_p规划路径中的最后一个关节轨迹点为move_pl的起点
|
|
|
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)#设定目标点为传入的点
|
|
|
+ self.arm.set_pose_target(point, self.end_effector_link) #设定目标点为焊缝终点
|
|
|
|
|
|
#创建路径约束
|
|
|
- path_constraints = self.create_path_constraints(points[-1],point)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
+ path_constraints = self.create_path_constraints(points[-1], point) #将最后一个点(应为起始点)和输入的点(焊缝的起始点)做圆柱轨迹约束
|
|
|
self.arm.set_path_constraints(path_constraints)#设定约束
|
|
|
|
|
|
- traj = self.arm.plan()#调用plan进行规划
|
|
|
- error=traj[3]
|
|
|
+ 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 = 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)
|
|
|
@@ -231,17 +307,20 @@ class MoveIt_Control:
|
|
|
trajectory_with_type.append(trajj_with_type)
|
|
|
else:
|
|
|
rospy.loginfo("本次焊缝规划失败")
|
|
|
- points.pop()#将本条焊缝的起点从规划中删除
|
|
|
- trajectory.pop()
|
|
|
- trajectory_with_type.pop()
|
|
|
- self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
|
|
|
- #清除约束
|
|
|
+ points.pop() #将本条焊缝的起点从规划中删除
|
|
|
+ # 同样第一次move_p提示timeout, trajectory中同样不会有路径信息
|
|
|
+ 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
|
|
|
+ self.hf_num = self.hf_num + 1 #焊缝计数
|
|
|
+ return points, trajectory, trajectory_with_type
|
|
|
|
|
|
- #TODO move_pl_check暂未使用
|
|
|
- def move_pl_check(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
|
|
|
+ # 暂未使用
|
|
|
+ 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)
|
|
|
|
|
|
@@ -249,35 +328,35 @@ class MoveIt_Control:
|
|
|
a=10
|
|
|
for j in range(a):
|
|
|
if trajectory:
|
|
|
- state=self.arm.get_current_state()
|
|
|
+ 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)
|
|
|
+ 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]
|
|
|
+ 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)
|
|
|
+ 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()
|
|
|
+ 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)
|
|
|
+ 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:
|
|
|
@@ -288,9 +367,9 @@ class MoveIt_Control:
|
|
|
self.hf_fail.append(welding_sequence[self.hf_num])
|
|
|
|
|
|
self.arm.clear_path_constraints()
|
|
|
- self.hf_num=self.hf_num+1#焊缝计数
|
|
|
+ self.hf_num = self.hf_num + 1#焊缝计数
|
|
|
|
|
|
- trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
|
|
|
+ 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"
|
|
|
|
|
|
@@ -298,54 +377,11 @@ class MoveIt_Control:
|
|
|
trajectory.append(trajj)
|
|
|
trajectory_with_type.append(trajj_with_type)
|
|
|
|
|
|
- return points,trajectory,trajectory_with_type
|
|
|
-
|
|
|
- #TODO move_pl_path_constraints
|
|
|
- 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)
|
|
|
- #构建 shape_msgs/SolidPrimitive 消息
|
|
|
- bounding_volume = SolidPrimitive()
|
|
|
- bounding_volume.type = SolidPrimitive.CYLINDER
|
|
|
- # bounding_volume.dimensions = [0.003,1]
|
|
|
- bounding_volume.dimensions = [height,radius]
|
|
|
- #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
|
|
|
- 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
|
|
|
+ return points, trajectory, trajectory_with_type
|
|
|
|
|
|
#TODO go_home
|
|
|
@decorator_time
|
|
|
- def go_home(self,a=1,v=1):
|
|
|
+ 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)
|
|
|
@@ -356,7 +392,7 @@ class MoveIt_Control:
|
|
|
# rospy.sleep(1)
|
|
|
|
|
|
#TODO go_home_justplan
|
|
|
- def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
|
|
|
+ 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()
|
|
|
@@ -365,44 +401,45 @@ class MoveIt_Control:
|
|
|
self.arm.set_named_target('home')
|
|
|
traj = self.arm.plan()
|
|
|
trajj = traj[1]
|
|
|
- traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
|
|
|
+ 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
|
|
|
+ return trajectory, trajectory_with_type
|
|
|
|
|
|
#TODO path_planning
|
|
|
@decorator_time
|
|
|
- def path_planning(self,folder_path,gohome=True):
|
|
|
+ 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 = [],[],[]
|
|
|
+ err = 0
|
|
|
+ points, trajectory, trajectory_with_type = [],[],[]
|
|
|
for i in range(len(all_data)):
|
|
|
- rospy.loginfo("共读取到%d条焊缝，开始规划第%d条",len(all_data),i+1)
|
|
|
+ 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:#焊缝起点不可达 则跳过此次焊缝和移动的规划 并将焊缝添加到失败列表
|
|
|
+ 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#焊缝计数
|
|
|
+ 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)
|
|
|
+ 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("第%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)
|
|
|
+ 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)
|
|
|
+ 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
|
|
|
+ return trajectory, traj_merge, trajectory_with_type_merge
|
|
|
|
|
|
###########################################################class end#############################################################
|
|
|
def process_welding_data(filename):
|
|
|
@@ -422,24 +459,24 @@ def process_welding_data(filename):
|
|
|
|
|
|
return all_data
|
|
|
|
|
|
-#TODO mark_the_traj
|
|
|
-def mark_the_traj(traj,TYPE,SEQUENCE):
|
|
|
+#TODO mark_the_traj 标记轨迹类型(during-p, during-l, go-home)
|
|
|
+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
|
|
|
+ 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
|
|
|
|
|
|
-#TODO merge_trajectories
|
|
|
+#TODO merge_trajectories 合并所有关节轨迹点
|
|
|
def merge_trajectories(trajectories):
|
|
|
if not trajectories:
|
|
|
return None
|
|
|
@@ -468,7 +505,7 @@ def merge_trajectories(trajectories):
|
|
|
|
|
|
return merged_trajectory
|
|
|
|
|
|
-#TODO merge_trajectories_with_type
|
|
|
+#TODO merge_trajectories_with_type 合并所有带焊缝起始点标记的关节轨迹点
|
|
|
def merge_trajectories_with_type(trajectory_with_type):
|
|
|
if not trajectory_with_type:
|
|
|
return None
|
|
|
@@ -517,12 +554,13 @@ def pub_trajectories(trajectory_with_type_merge):
|
|
|
|
|
|
|
|
|
class py_msgs():
|
|
|
- fial=[]
|
|
|
- shun_xv=[]
|
|
|
+ fial = []
|
|
|
+ shun_xv = []
|
|
|
class point():
|
|
|
- xyz_list=[]
|
|
|
- type=[]
|
|
|
+ xyz_list = []
|
|
|
+ type = []
|
|
|
|
|
|
+# ROS2PY_msgs 将规划后的轨迹信息打包成json格式发送到redis
|
|
|
def ROS2PY_msgs(msgs, m_sr):
|
|
|
for i in range(len(msgs.points)):
|
|
|
py_msgs.point.xyz_list.append(msgs.points[i].positions)
|
|
|
@@ -536,30 +574,31 @@ def ROS2PY_msgs(msgs, m_sr):
|
|
|
|
|
|
message = {
|
|
|
'positions': py_msgs.point.xyz_list, # 规划路径点结果
|
|
|
- 'flags': py_msgs.point.type, # 规划路径点的类型，焊接点移动点
|
|
|
- 'weld_order': py_msgs.shun_xv, # 规划路径顺序
|
|
|
- 'failed': py_msgs.fail, # 规划路径失败的焊缝
|
|
|
+ 'flags': py_msgs.point.type, # 规划路径点的类型，焊接点移动点
|
|
|
+ 'weld_order': py_msgs.shun_xv, # 规划路径顺序
|
|
|
+ 'failed': py_msgs.fail, # 规划路径失败的焊缝
|
|
|
}
|
|
|
return json.dumps(message)
|
|
|
# for i in range(len(py_msgs.point.type)):
|
|
|
# moveit_server.move_j(py_msgs.point.xyz_list[i])
|
|
|
# #input("任意键继续")
|
|
|
|
|
|
-#TODO get_valid_input
|
|
|
+#TODO get_valid_input 监听用户输入
|
|
|
def get_valid_input(factor, default):
|
|
|
while True:
|
|
|
- user_input=input(factor)
|
|
|
- if user_input=="":
|
|
|
+ user_input = input(factor)
|
|
|
+ if user_input == "":
|
|
|
return default
|
|
|
try:
|
|
|
- value=float(user_input)
|
|
|
- if 0<value<=1:
|
|
|
+ value = float(user_input)
|
|
|
+ if 0 < value <= 1:
|
|
|
return value
|
|
|
else:
|
|
|
rospy.logerr("缩放因子必须在(0, 1]范围内，请重新输入!")
|
|
|
except ValueError:
|
|
|
rospy.logerr("输入值无效，请根据提示重新输入！")
|
|
|
|
|
|
+# TODO get_uesr_input 获取用户输入
|
|
|
def get_user_input():
|
|
|
"""获取用户输入的速度和加速度缩放因子，并在控制台中打印信息和错误提示"""
|
|
|
DEFUALT_V = 1.0
|
|
|
@@ -569,7 +608,7 @@ def get_user_input():
|
|
|
termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空输入缓存
|
|
|
rospy.loginfo("输入缓存区已清空！")
|
|
|
|
|
|
- vv=get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
|
|
|
+ vv = get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
|
|
|
#a=get_valid_input("请输入加速度缩放因子(0-1, 默认为1): ", DEFUALT_A)
|
|
|
|
|
|
#rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
|
|
|
@@ -582,28 +621,31 @@ def ROS2_msgs(msgs, m_sr):
|
|
|
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:
|
|
|
+ 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")
|
|
|
-
|
|
|
+ file.write(' '.join(str(value) for value in point) + "\n")
|
|
|
|
|
|
if __name__ =="__main__":
|
|
|
- # from redis_publisher import Publisher
|
|
|
- # publisher = Publisher()
|
|
|
|
|
|
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
|
|
|
+ speed_v = 1.0
|
|
|
# speed_v=get_user_input()
|
|
|
# rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
|
|
|
|
|
|
- trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
|
|
|
+ trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
|
|
|
#ROS2_msgs(trajectory_with_type_merge,moveit_server)
|
|
|
#write_ee(trajectory_with_type_merge, moveit_server)
|
|
|
|
|
|
moveit_server.arm.execute(trajectory_merge)
|
|
|
+ print(f"本次规划失败焊缝序号:{moveit_server.hf_fail}")
|
|
|
+
|
|
|
+ #使用redis发布规划后的轨迹信息
|
|
|
+ #message = ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
|
|
|
+ #publisher = Publisher()
|
|
|
+ #publisher.pub_plan_result(message)
|
|
|
|
|
|
rospy.set_param("sign_control",0)
|
lsttry