|
|
@@ -86,7 +86,7 @@ class MoveIt_Control:
|
|
|
self.arm.set_max_velocity_scaling_factor(v)
|
|
|
|
|
|
waypoints=[]
|
|
|
- if trajectory:
|
|
|
+ if points:
|
|
|
#起点位置设置为规划组最后一个点
|
|
|
pose = self.arm.get_current_pose(self.end_effector_link).pose
|
|
|
wpose = deepcopy(pose)
|
|
|
@@ -201,6 +201,7 @@ class MoveIt_Control:
|
|
|
# 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):
|
|
|
self.arm.set_max_acceleration_scaling_factor(a)
|
|
|
@@ -224,7 +225,7 @@ class MoveIt_Control:
|
|
|
self.arm.set_start_state(state)#起点位置设置为规划组最后一个点 或者当前状态（第一个点时）
|
|
|
|
|
|
#尝试规划次数
|
|
|
- b = 10 #尝试规划5次 无约束5次
|
|
|
+ b = 4 #尝试规划5次 无约束5次
|
|
|
for i in range(b):
|
|
|
traj = self.arm.plan()#调用plan进行规划
|
|
|
error=traj[3]
|
|
|
@@ -253,7 +254,7 @@ class MoveIt_Control:
|
|
|
else:
|
|
|
path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
|
|
|
self.arm.set_path_constraints(path_constraints)#设定约束
|
|
|
- if(i>=(b-5)):
|
|
|
+ if(i>=(b-2)):
|
|
|
rospy.loginfo("所有移动规划尝试失败 取消路径约束")
|
|
|
self.arm.clear_path_constraints()
|
|
|
if(i==(b-1)):
|
|
|
@@ -265,7 +266,70 @@ class MoveIt_Control:
|
|
|
|
|
|
return points,trajectory,trajectory_with_type,er
|
|
|
|
|
|
-
|
|
|
+ def create_path_constraints3(self,start_point,end_point):#创建路径约束
|
|
|
+ ocm=moveit_msgs.msg.OrientationConstraint()
|
|
|
+ ocm_constraint=moveit_msgs.msg.Constraints()
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ #计算起点指向终点的向量
|
|
|
+ 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
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ # 位置约束
|
|
|
+ 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
|
|
|
+
|
|
|
+ ocm_constraint.position_constraints.append(position_constraint)
|
|
|
+
|
|
|
+ ocm.link_name = "link_end_now" #or whatever your end effector link is
|
|
|
+ ocm.header.frame_id = "link_end_now" # or whatever your planning frame is
|
|
|
+ ocm.orientation.x=end_point[3]
|
|
|
+ ocm.orientation.y=end_point[4]
|
|
|
+ ocm.orientation.z=end_point[5]
|
|
|
+ ocm.orientation.w=end_point[6]
|
|
|
+# ocm.orientation.w = 1.0
|
|
|
+
|
|
|
+ ocm.absolute_x_axis_tolerance = 0.001
|
|
|
+ ocm.absolute_y_axis_tolerance = 0.001
|
|
|
+ ocm.absolute_z_axis_tolerance = 0.001
|
|
|
+ ocm.weight = 1.0
|
|
|
+
|
|
|
+
|
|
|
+ ocm_constraint.orientation_constraints.append(ocm)
|
|
|
+ return ocm_constraint
|
|
|
+
|
|
|
+
|
|
|
def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划 只规划当前的点到上一个点
|
|
|
|
|
|
self.arm.set_max_acceleration_scaling_factor(a)
|
|
|
@@ -283,7 +347,6 @@ class MoveIt_Control:
|
|
|
# 创建路径约束
|
|
|
path_constraints = self.create_path_constraints(points[-1],point)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
|
|
|
self.arm.set_path_constraints(path_constraints)#设定约束
|
|
|
-
|
|
|
traj = self.arm.plan()#调用plan进行规划
|
|
|
error=traj[3]
|
|
|
if error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
|
|
|
@@ -307,64 +370,6 @@ class MoveIt_Control:
|
|
|
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
|
|
|
|
|
|
#TODO move_pl_path_constraints
|
|
|
def create_path_constraints(self,start_point,end_point):
|
|
|
@@ -458,12 +463,12 @@ class MoveIt_Control:
|
|
|
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.plan_cartesian_path_LLL(point22,points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
+ 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)
|
|
|
+ #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)
|
bzx