2024-9-25

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -4,6 +4,8 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
+        - /Grid1
         - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Planned Path1
@@ -41,7 +43,7 @@ Visualization Manager:
         Z: 0
       Plane: XY
       Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
+      Reference Frame: world
       Value: true
     - Acceleration_Scaling_Factor: 0.1
       Class: moveit_rviz_plugin/MotionPlanning
@@ -123,6 +125,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -217,6 +223,10 @@ Visualization Manager:
             Show Axes: false
             Show Trail: false
             Value: true
+          world:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: false
@@ -280,6 +290,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -375,7 +389,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: world
     Frame Rate: 30
   Name: root
   Tools:
@@ -387,7 +401,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.9001314043998718
+      Distance: 8.636017799377441
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -395,17 +409,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.877734899520874
-        Y: -0.03542045131325722
-        Z: 0.13408130407333374
+        X: 0.3255424499511719
+        Y: -0.4020789861679077
+        Z: 0.04474440962076187
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.42520225048065186
+      Pitch: 0.3152022063732147
       Target Frame: base_link
-      Yaw: 5.237476825714111
+      Yaw: 3.320636510848999
     Saved: ~
 Window Geometry:
   Displays:

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -5,95 +5,15 @@ from scipy.spatial import KDTree
 import tf.transformations as tft
 import os
 import rospy
-from math import acos,tan,atan,degrees,sqrt
+from math import acos,tan,atan,degrees,sqrt,sin,cos
 
-def corner_angle_discrimination(point,data,fangxiang):
-    # 构建KDTree索引
-    tree = KDTree(data)
-    indices = np.array(tree.query_ball_point(point, r=30, p=2)) # 返回这些点的索引数组
-    distances = np.array([np.linalg.norm(point - data[index]) for index in indices])
-    distance_filter = distances >= 10
-    filtered_indices = indices[distance_filter]
-    data_circular = data[filtered_indices]
+def rpy2quaternion(roll, pitch, yaw):#zyx
+    x=sin(roll/2)*cos(pitch/2)*cos(yaw/2)-cos(roll/2)*sin(pitch/2)*sin(yaw/2)
+    y=cos(roll/2)*sin(pitch/2)*cos(yaw/2)+sin(roll/2)*cos(pitch/2)*sin(yaw/2)
+    z=cos(roll/2)*cos(pitch/2)*sin(yaw/2)-sin(roll/2)*sin(pitch/2)*cos(yaw/2)
+    w=cos(roll/2)*cos(pitch/2)*cos(yaw/2)+sin(roll/2)*sin(pitch/2)*sin(yaw/2)
+    return [x, y, z, w]
 
-    # 可视化
-    # pt_cloud_circular = o3d.geometry.PointCloud()
-    # pt_cloud_circular.points = o3d.utility.Vector3dVector(data_circular)
-    # o3d.visualization.draw_geometries([pt_cloud_circular])
-
-    # 获取与方向向量相似的点云
-    data_similar = []
-    for i in range(len(data_circular)):
-        v = np.array(data_circular[i]) - point
-        angle = acos(np.dot(v, fangxiang) / (np.linalg.norm(v) * np.linalg.norm(fangxiang)))
-        angle_deg = degrees(angle)
-        if angle_deg <= 15:
-            data_similar.append(data_circular[i])
-    
-    if len(data_similar) >= 1:
-        corner = True
-    else:
-        corner = False
-    return corner
-def set_yaw_angle(R_mat,yaw):
-    pz1 = np.array([[1, 0, 0],
-                    [0, np.cos(yaw), -np.sin(yaw)],
-                    [0, np.sin(yaw),  np.cos(yaw)]])
-    Z_PI = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
-                    [np.sin(np.pi), np.cos(np.pi), 0],
-                    [0, 0, 1]])
-    R1_mat = np.matmul(R_mat,pz1)
-    
-    sign = check_x_axis_direction(R1_mat)
-    if sign == 1:
-        pass
-    elif sign == -1:
-        R1_mat = np.matmul(R1_mat, Z_PI)
-
-    q = R.from_matrix(R1_mat).as_quat()
-    q = q.tolist()  # 将NumPy数组转换为列表
-    return q
-def calculate_angle_with_xy_plane(point1,point2):
-    # 计算方向向量
-    dir_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]])
-    
-    # 计算方向向量在 xy 平面上的投影
-    proj_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], 0])
-    
-    # 计算夹角
-    angle = acos(np.dot(dir_vector, proj_vector) / (np.linalg.norm(dir_vector) * np.linalg.norm(proj_vector)))
-    angle_deg = degrees(angle)
-
-    return angle_deg
-def check_x_axis_direction(rotation_matrix):
-    # 提取旋转后的 x 轴方向向量
-    x_prime = rotation_matrix[:, 0]
-    # 检查 x 轴的 z 分量
-    if x_prime[2] > 0:
-        return 1
-    elif x_prime[2] < 0:
-        return -1
-    else:
-        return 0
-def load_point_cloud_from_txt(file_path):
-    """导入点云数据"""
-    points = []
-    with open(file_path, 'r') as file:
-        for line in file:
-            # 假设每一行是“x y z”格式，没有多余空格
-            coordinates = line.strip().split()
-            if len(coordinates) == 3:  # 检查是否为有效的三维点
-                point = [float(coord) for coord in coordinates]
-                points.append(point)
-
-    return np.array(points)
-def load_point_cloud_from_binary_txt(file_path):
-    with open(file_path, 'rb') as f:
-        binary_data = f.read()
-
-        # 将二进制数据转换为 NumPy 数组
-        point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
-    return point_cloud
 
 def get_hanfeng(file_path):
     """从文件中读取焊缝起点、终点和中间点，并计算两点构成的向量"""
@@ -124,322 +44,37 @@ def get_hanfeng(file_path):
 
     return start_points, end_points, midpoints, hanfeng
 
-def compute_Rc2w_tc2w(file_path_Tw2c):
-    """相机和基底坐标系的转换关系"""
-    # 读取文件
-    with open(file_path_Tw2c, 'r') as file:
-        lines = file.readlines()
-
-    lines = [line.strip().replace('[', '').replace(']', '').split() for line in lines]
-    data = [[float(num) for num in line] for line in lines]
-    data = np.array(data)
-    # 提取Rw2c和tw2c
-    Rw2c = data[:3,:3]
-    tw2c = data[:3,-1]
-
-    Rc2w = Rw2c.T
-    tc2w = -np.matmul(Rc2w, tw2c)
-    return Rc2w,tc2w
-
-def compute_pose_R(fangxiang, hanfeng,start_point, end_point):
-    """根据计算出的焊接时z轴负方向的向量和由起(fangxiang)点指向终点的向量(hanfeng)计算旋转矩阵"""
-    x, y, z = fangxiang
-
-    # 计算旋转角度
-    rotx = -np.arctan2(y, z)
-    roty = np.arctan2(x, np.sqrt(y**2 + z**2))
-
-    # 构造旋转矩阵
-    A = np.array([[1, 0, 0],
-                [0, np.cos(rotx), -np.sin(rotx)],
-                [0, np.sin(rotx), np.cos(rotx)]])
-    B = np.array([[np.cos(roty), 0, np.sin(roty)],
-                [0, 1, 0],
-                [-np.sin(roty), 0, np.cos(roty)]])
-    bb = np.matmul(hanfeng, np.matmul(A, B))
-    # 区分平缝竖缝
-    angle = calculate_angle_with_xy_plane(start_point,end_point)
-    ispingfeng = abs(angle) < 30
-    if ispingfeng: 
-        rotz = np.arctan2(bb[0], -bb[1])
-    else:
-        rotz = np.arctan2(bb[1], bb[0])
-
-    C = np.array([[np.cos(rotz), -np.sin(rotz), 0],
-                [np.sin(rotz), np.cos(rotz), 0],
-                [0, 0, 1]])
-    D = np.array([[1, 0, 0],
-                    [0, np.cos(np.pi), -np.sin(np.pi)],
-                    [0, np.sin(np.pi), np.cos(np.pi)]])
-
-    R_mat = np.matmul(A, np.matmul(B, np.matmul(C, D)))
-
-    return R_mat, ispingfeng
-
-def get_quaternion(data, midpoint, hanfeng, start_point, end_point,result_ping,result_shu,result_wai,file_path_Tw2c): 
-
-    # 构建KDTree索引
-    tree = KDTree(data)
-    # 查询半径范围内的点,找出"data"中所有距离"midpoint"点在r以内的点，这里使用的是欧几里得距离（由p=2表示）
-    indices = np.array(tree.query_ball_point(midpoint, r=30, p=2)) # 返回这些点的索引数组
-    distances = np.array([np.linalg.norm(midpoint - data[index]) for index in indices])
-    # 对numpy数组进行布尔索引
-    # distance_filter = np.logical_and(distances >= 10, distances <= 50)
-    distance_filter = distances >= 10
-    # 使用布尔索引过滤出符合条件的索引
-    filtered_indices = indices[distance_filter]
-    # 提取符合条件的点
-    data_circular = data[filtered_indices]
-
-    # 可视化
-    # pt_cloud_circular = o3d.geometry.PointCloud()
-    # pt_cloud_circular.points = o3d.utility.Vector3dVector(data_circular)
-    # o3d.visualization.draw_geometries([pt_cloud_circular])
-
-    # 获取竖直方向上的点云
-    data_shu = []
-    for i in range(len(data_circular)):
-        P = np.array(data_circular[i])
-        v = P - midpoint
-        anglecos = np.dot(v, hanfeng) / (np.linalg.norm(v) * np.linalg.norm(hanfeng))
-        
-        if abs(anglecos) <= 0.015:
-            # print(anglecos)
-            data_shu.append(P)
-    data_shu = np.array(data_shu)
-    # # 可视化
-    # pt_cloud_shu = o3d.geometry.PointCloud()
-    # pt_cloud_shu.points = o3d.utility.Vector3dVector(np.array(data_shu))
-    # o3d.visualization.draw_geometries([pt_cloud_shu])
-
- ##################################################
- ##################################################
-    random_point = data_shu[np.random.randint(0, len(data_shu))]
-    # 与midpoint一起定义直线向量
-    nohanfeng = random_point - midpoint  #data_shu中任意一点midpoint构成的向量，计算所有点和该向量的距离判断是否是外缝(只拍到一面)
-
-    distances_to_line = []
-
-    for i in range(data_shu.shape[0]):  # 遍历每一个点 P
-        P = data_shu[i]
-        A = midpoint
-        u = nohanfeng
-        v = P - A
-        distance = np.linalg.norm(np.cross(u, v)) / np.linalg.norm(u)
-        distances_to_line.append(distance)
-
-    distances_to_line = np.array(distances_to_line)
-
-    all_distances_less_than_10 = np.all(distances_to_line < 5)#都小于10为true，也就是data_shu只有一列，侧着拍得外缝
-
-    if all_distances_less_than_10:
-
-        # 随机选取data_shu中的第一个点
-        random_index = np.random.randint(0, data_shu.shape[0])
-        vector1 = data_shu[random_index] - midpoint
-        vector1 /= np.linalg.norm(vector1)  # 单位化vector1
-        fa_xian = np.cross(vector1, hanfeng)
-        fa_xian /= np.linalg.norm(fa_xian) 
-
-        point = []
-        point1 = midpoint + 50 * fa_xian
-        point2 = midpoint - 50 * fa_xian
-
-        Rc2w,tc2w = compute_Rc2w_tc2w(file_path_Tw2c)
-
-        point1_c = np.matmul(Rc2w, point1) + tc2w
-        point2_c = np.matmul(Rc2w, point2) + tc2w
-        a = np.linalg.norm(point1_c)
-        b = np.linalg.norm(point2_c)      
-
-        if a < b:
-            point = point1
-        else:
-            point = point2
-
-        faxian = point - midpoint
-        vector2 =  faxian / np.linalg.norm(faxian)  # 单位化vector2
-
-        fangxiang = vector1  + vector2*1.2
-
-        R_mat, ispingfeng = compute_pose_R(fangxiang, hanfeng, start_point, end_point)
-        if ispingfeng:
-            ##偏转##
-            pz1 = np.array([[1, 0, 0],
-                        [0, np.cos(yaw), -np.sin(yaw)],
-                        [0, np.sin(yaw),  np.cos(yaw)]])
-            pz2 = np.array([[1, 0, 0],
-                        [0, np.cos(-yaw), -np.sin(-yaw)],
-                        [0, np.sin(-yaw),  np.cos(-yaw)]])
-            Z_PI = np.array([[np.cos(np.pi), -np.sin(np.pi), 0],
-                    [np.sin(np.pi), np.cos(np.pi), 0],
-                    [0, 0, 1]])
-            R1_mat = np.matmul(R_mat,pz1)
-            R2_mat = np.matmul(R_mat,pz2)
-
-            sign = check_x_axis_direction(R1_mat)
-            if sign == 1:
-                pass
-            elif sign == -1:
-                R1_mat = np.matmul(R1_mat, Z_PI)
-                R2_mat = np.matmul(R2_mat, Z_PI)
-
-            q1 = R.from_matrix(R1_mat).as_quat()
-            q2 = R.from_matrix(R2_mat).as_quat()
-            q1 = q1.tolist()  # 将NumPy数组转换为列表
-            q2 = q2.tolist()
-            # print("一个点云面的平外缝")
-            # print(q1,q2)
-            result_ping.append(("pw", start_point, end_point, q1, q2))
-            
-            return result_ping,result_shu,result_wai
-        else:
-            ##偏转##
-            E = np.array([[np.cos(-pitch_of_Verticalweld), 0, np.sin(-pitch_of_Verticalweld)],
-                        [0, 1, 0],
-                        [-np.sin(-pitch_of_Verticalweld), 0, np.cos(-pitch_of_Verticalweld)]])
-
-            R_mat = np.matmul(R_mat,E)
-            
-            q1 = R.from_matrix(R_mat).as_quat()
-            q2 = R.from_matrix(R_mat).as_quat()
-            q1 = q1.tolist()  # 将NumPy数组转换为列表
-            q2 = q2.tolist()
-            # print("即一个点云面的竖外缝")
-            # print(q1,q2)
-            result_wai.append(("sw", start_point, end_point, q1, q2))
-            return result_ping,result_shu,result_wai
-
-    else:
-        # 选择位于平面两侧的点
-        # 随机选取data_shu中的第一个点
-        random_index = np.random.randint(0, data_shu.shape[0])
-        vector1 = data_shu[random_index] - midpoint
-        vector1 /= np.linalg.norm(vector1)  # 单位化vector1
-
-        # 随机选取下一个点，直到找到与vector1夹角大于10度的向量
-        found = False
-        while not found:
-            random_index = np.random.randint(0, data_shu.shape[0])
-            random_point = data_shu[random_index] - midpoint
-            vector2 = random_point / np.linalg.norm(random_point)  # 单位化vector2
-            
-            # 计算两个向量之间的夹角（弧度）
-            dot_product = np.dot(vector1, vector2)
-            angle_rad = np.arccos(dot_product)
-            
-            # 判断角度是否大于10度（转换为弧度）
-            if angle_rad > np.deg2rad(20) and angle_rad < np.deg2rad(180):
-                found = True
-                # 如果找到了，则保存vector2
-            else:
-                continue
-
-        # 计算并保存最终的fangxiang向量, vector1与vector2均为单位向量
-        vector1_sim_to_z = np.dot(vector1, [0,0,1]) / (np.linalg.norm(vector1))
-        vector2_sim_to_z = np.dot(vector2, [0,0,1]) / (np.linalg.norm(vector2))
-        if vector1_sim_to_z > vector2_sim_to_z:
-            vector_shu = vector1
-            vector_ping = vector2
-        else:
-            vector_shu = vector2
-            vector_ping = vector1
-        fangxiang = vector_ping + vector_shu*tan(pitch_of_Horizontalweld)
-        # fangxiang = vector1 + vector2
-        fangxiang /= np.linalg.norm(fangxiang)  # 单位化fangxiang
-
-        #########################################根据vector1和vector2判断内缝外缝#########################################
-        point1 = midpoint + 50 * fangxiang 
-        point2 = midpoint - 50 * fangxiang 
-
-        Rc2w,tc2w = compute_Rc2w_tc2w(file_path_Tw2c)
-        
-        point1_c = np.matmul(Rc2w, point1) + tc2w
-        point2_c = np.matmul(Rc2w, point2) + tc2w
-        a = np.linalg.norm(point1_c)
-        b = np.linalg.norm(point2_c)    
-
-        if a < b: # 内缝
-            # print("两个点云面的内缝")
-            fangxiang = fangxiang
-        else: # 外缝
-            # print("两个点云面的外缝")
-            fangxiang = -fangxiang
-
-        R_mat, ispingfeng = compute_pose_R(fangxiang, hanfeng, start_point, end_point)
-        if ispingfeng:
-            ##偏转##
-            corner_of_start_point = corner_angle_discrimination(start_point,data,fangxiang)
-            corner_of_end_point = corner_angle_discrimination(end_point,data,fangxiang)
-            pi4_pz = atan(1 / (sqrt(2) * tan(np.pi/4)))#CL公式
-            yaw_pz =atan(1 / (sqrt(2) * tan(yaw)))
-            if corner_of_start_point:
-                q1 = set_yaw_angle(R_mat,pi4_pz)
-                if not corner_of_end_point:
-                    len_hanfeng = np.linalg.norm(hanfeng)
-                    angle = pi4_pz-(pi4_pz)*len_hanfeng/yaw_rate
-                    if angle < -yaw_pz:
-                        angle =-yaw_pz
-                    q2 = set_yaw_angle(R_mat,angle)
-            if corner_of_end_point:
-                q2 = set_yaw_angle(R_mat,-pi4_pz)
-                if not corner_of_start_point:
-                    len_hanfeng = np.linalg.norm(hanfeng)
-                    angle = -pi4_pz+(pi4_pz)*len_hanfeng/yaw_rate
-                    if angle > yaw_pz:
-                        angle = yaw_pz
-                    q1 = set_yaw_angle(R_mat,angle)
-            if not corner_of_start_point and not corner_of_end_point:     
-                q1 = set_yaw_angle(R_mat,yaw_pz)
-                len_hanfeng = np.linalg.norm(hanfeng)
-                angle = yaw_pz-(pi4_pz)*len_hanfeng/yaw_rate
-                if angle < -yaw_pz:
-                    angle =-yaw_pz
-                q2 = set_yaw_angle(R_mat,angle)
-            
-            result_ping.append(("p", start_point, end_point, q1, q2))
-            
-            return result_ping,result_shu,result_wai
-        else:
-            ##偏转##
-            E = np.array([[np.cos(-pitch_of_Verticalweld), 0, np.sin(-pitch_of_Verticalweld)],
-                        [0, 1, 0],
-                        [-np.sin(-pitch_of_Verticalweld), 0, np.cos(-pitch_of_Verticalweld)]])
+def calculate_angle_with_xy_plane(point1,point2):
+    # 计算方向向量
+    dir_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], point2[2] - point1[2]])
+    
+    # 计算方向向量在 xy 平面上的投影
+    proj_vector = np.array([point2[0] - point1[0], point2[1] - point1[1], 0])
+    
+    # 计算夹角
+    angle = acos(np.dot(dir_vector, proj_vector) / (np.linalg.norm(dir_vector) * np.linalg.norm(proj_vector)))
+    angle_deg = degrees(angle)
 
-            R_mat = np.matmul(R_mat,E)
-            
-            q1 = R.from_matrix(R_mat).as_quat()
-            q2 = R.from_matrix(R_mat).as_quat()
-            q1 = q1.tolist()  # 将NumPy数组转换为列表
-            q2 = q2.tolist()
-            # print(q1,q2)
-            result_wai.append(("s", start_point, end_point, q1, q2))
-            return result_ping,result_shu,result_wai        
+    return angle_deg  
 
 def run():
-    global yaw,yaw_rate,pitch_of_Horizontalweld,pitch_of_Verticalweld
-    # yaw = np.pi/8
-    yaw = np.pi/2-rospy.get_param("yaw")
-    pitch_of_Verticalweld = rospy.get_param("pitch_of_Verticalweld")
-    pitch_of_Horizontalweld = rospy.get_param("pitch_of_Horizontalweld")
-    yaw_rate = rospy.get_param("yaw_rate")
     file_path = rospy.get_param("folder_path")
-    # file_path = ('/home/chen/catkin_ws/data/0603')
-    file_path_pointcloud = os.path.join(file_path, 'pointcloud.txt')
     file_path_points = os.path.join(file_path, 'points_guihua.txt')
     file_path_result = os.path.join(file_path, 'result.txt')
-    file_path_Tw2c = os.path.join(file_path, 'Tw2c.txt')
 
-    # 获取数据
-    data = load_point_cloud_from_binary_txt(file_path_pointcloud)
+    result=[]
     start_points, end_points, midpoints, hanfeng = get_hanfeng(file_path_points)
-
     # 计算位姿
-    result_ping,result_shu,result_wai = [],[],[]
-    for i in range(len(midpoints)):
-        result_ping,result_shu,result_wai = get_quaternion(data, midpoints[i], hanfeng[i], start_points[i], end_points[i],result_ping,result_shu,result_wai, file_path_Tw2c)
-        # print(q1," ",q2)
-    result = result_ping + result_shu + result_wai
+    for i in range(len(hanfeng)): 
+        if(calculate_angle_with_xy_plane(start_points[i], end_points[i])<45):#平缝
+            q1 = rpy2quaternion(0,np.pi,0)
+            q2 = rpy2quaternion(0,np.pi,0)
+            result.append(("s", start_points[i], end_points[i], q1, q2))
+        else:
+            q1 = rpy2quaternion(0,np.pi,0)
+            q2 = rpy2quaternion(0,np.pi,0)
+            result.append(("s", start_points[i], end_points[i], q1, q2))
+
 
     # 打开文件准备写入
     with open(file_path_result, "w") as file:

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -76,6 +76,73 @@ class MoveIt_Control:
         self.arm.go()
         #rospy.sleep(1)
 
+    
+    def plan_cartesian_path_LLL(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
+        fraction = 0.0  # 路径规划覆盖率
+        maxtries = 100  # 最大尝试规划次数
+        attempts = 0  # 已经尝试规划次数
+
+        self.arm.set_max_acceleration_scaling_factor(a)
+        self.arm.set_max_velocity_scaling_factor(v)
+
+        waypoints=[]
+        if points:
+        #起点位置设置为规划组最后一个点
+            pose = self.arm.get_current_pose(self.end_effector_link).pose
+            wpose = deepcopy(pose)
+            wpose.position.x=points[-1][0]
+            wpose.position.y=points[-1][1]
+            wpose.position.z=points[-1][2]
+            wpose.orientation.x=points[-1][3]
+            wpose.orientation.y=points[-1][4]
+            wpose.orientation.z=points[-1][5]
+            wpose.orientation.w=points[-1][6]
+            waypoints.append(deepcopy(wpose))
+
+            wpose.position.x=point[0]
+            wpose.position.y=point[1]
+            wpose.position.z=point[2]
+            wpose.orientation.x=point[3]
+            wpose.orientation.y=point[4]
+            wpose.orientation.z=point[5]
+            wpose.orientation.w=point[6]
+            waypoints.append(deepcopy(wpose))
+        else:
+            rospy.loginfo("error empty trajectory")
+            return 
+        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
+        while fraction < 1.0 and attempts < maxtries:
+            (plan, fraction) = self.arm.compute_cartesian_path(
+                waypoints,  # waypoint poses，路点列表
+                0.001,  # eef_step，终端步进值
+                0.0,  # jump_threshold，跳跃阈值
+                True)  # avoid_collisions，避障规划
+            attempts += 1
+
+        if fraction == 1.0 :
+            rospy.loginfo("Path computed successfully.")
+            #traj = plan
+            trajj = plan #取出规划的信息
+            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(plan[3].val)
+            rospy.loginfo(
+                "fraction=" + str(fraction) + " success after " + str(maxtries))   
+            
+            # rospy.loginfo("本次焊缝规划失败")
+            # points.pop()#将本条焊缝的起点从规划中删除
+            # trajectory.pop()
+            # trajectory_with_type.pop()
+            # self.hf_fail.append(welding_sequence[self.hf_num])#将焊缝添加到失败列表
+        #self.hf_num=self.hf_num+1#焊缝计数
+        return points,trajectory,trajectory_with_type
+    
+
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -134,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)
@@ -157,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]
@@ -174,31 +242,94 @@ class MoveIt_Control:
                     trajectory_with_type.append(trajj_with_type)
                     points.append(point)
                     trajectory.append(trajj)  
-                    break
+                    break  
+            else:
+                rospy.loginfo("check failed! 移动轨迹无效")
+                rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
+                self.arm.clear_path_constraints()
+                rrr=rrr+0.2#每次增加20厘米半径
+                rospy.loginfo("R值：{}".format(rrr))
+                if trajectory:
+                    path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
                 else:
-                    rospy.loginfo("check failed! 移动轨迹无效")
-                    rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
+                    path_constraints = self.create_path_constraints2(self.home_pose,point,rrr)#将最后一个点和起点（焊缝的起始点）做圆柱轨迹约束
+                self.arm.set_path_constraints(path_constraints)#设定约束
+                if(i>=(b-2)):
+                    rospy.loginfo("所有移动规划尝试失败 取消路径约束")
                     self.arm.clear_path_constraints()
-                    rrr=rrr+0.2#每次增加20厘米半径
-                    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  
+                if(i==(b-1)):
+                    rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
+                    er=1
+                    break
         #清除约束
         self.arm.clear_path_constraints()
         
         return points,trajectory,trajectory_with_type,er
     
-    #TODO move_pl
+    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)
@@ -216,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:
@@ -240,7 +370,7 @@ class MoveIt_Control:
         self.hf_num=self.hf_num+1#焊缝计数
         return points,trajectory,trajectory_with_type
 
-    
+
     #TODO move_pl_path_constraints
     def create_path_constraints(self,start_point,end_point):
         #计算起点指向终点的向量
@@ -338,7 +468,7 @@ class MoveIt_Control:
             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)
@@ -363,7 +493,7 @@ def process_welding_data(filename):
 
     return all_data
 
-#TODO mark_the_traj
+
 def mark_the_traj(traj,TYPE,SEQUENCE):
     traj_with_type = JointTrajectory_ex()
     traj_with_type.header = traj.joint_trajectory.header
@@ -380,7 +510,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
-#TODO merge_trajectories
+
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -409,7 +539,7 @@ def merge_trajectories(trajectories):
     
     return merged_trajectory
 
-#TODO merge_trajectories_with_type
+
 def merge_trajectories_with_type(trajectory_with_type):
     if not trajectory_with_type:
         return None
@@ -517,8 +647,39 @@ def get_user_input():
     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")
+            
+# def write_ee(msgs, m_sr):
+#     for i in range(len(msgs.points)):
+#         py_msgs.point.xyz_list.append(msgs.points[i].positions)
+        
+#     f_p1 = os.path.join(folder_path, 'ee_pos.txt')
+#     with open(f_p1,'w') as file:
+#         for point in py_msgs.point.xyz_list:
+#             #更新机械臂状态
+#             state=m_sr.arm.get_current_state()
+#             #state.joint_state.position=point
+            
+#             #获取末端执行器的位姿
+#             fk_result=m_sr.arm.get_current_pose(moveit_server.end_effector_link)
+            
+#             #获取位置信息
+#             position=fk_result.pose.position
+#             orientation=fk_result.pose.orientation
             
+#             #格式化数据
+#             pose_data = f"Position: x={position.x}, y={position.y}, z={position.z}\n"
+#             pose_data += f"Orientation (Quaternion): x={orientation.x}, y={orientation.y}, z={orientation.z}, w={orientation.w}\n"
+#             pose_data += "-" * 50 + "\n"
+            
+#             file.write(pose_data)
 
 if __name__ =="__main__":
     # from redis_publisher import Publisher
@@ -534,7 +695,8 @@ if __name__ =="__main__":
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
     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)
     

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -94,15 +94,15 @@ if __name__ == "__main__":
             # 清除场景
             clear_octomap()
             #点云计算并发布
-            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            process.start()
+            # process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            # process.start()
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()
             command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
             #rospy.loginfo("等待场景地图加载完毕...")
-            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            #wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
             #rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -4,6 +4,13 @@
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot
   name="mr12urdf20240605">
+  <link name="world" />
+  
+  <joint name="world_joint" type="fixed">
+    <parent link="world" />
+    <child link = "base_link" />
+    <origin xyz="0.0 0.0 2.17" rpy="0.0 3.14 0.0" />
+  </joint>
   <link
     name="base_link">
     <!-- <inertial>
@@ -271,8 +278,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-1.6"
-      upper="1.6"
+      lower="-3.3"
+      upper="3.3"
       effort="88.5"
       velocity="6.838" />
   </joint>
@@ -329,7 +336,7 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-1.6"
+      lower="-3.66"
       upper="0.75"
       effort="45.52"
       velocity="4.815" />
@@ -387,8 +394,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-2.8"
-      upper="2.8"
+      lower="-3.8"
+      upper="3.8"
       effort="32.64"
       velocity="23.655" />
   </joint>

catkin_ws/需求.txt

@@ -0,0 +1 @@
+记得修改焊接顺序规划