机械臂代码优化

catkin_ws/src/.vscode/c_cpp_properties.json

@@ -0,0 +1,26 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/tong/catkin_ws/devel/include/**",
+        "/home/tong/moveit_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/tong/moveit_ws/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/tong/moveit_ws/src/moveit_visual_tools/include/**",
+        "/home/tong/moveit_ws/src/rviz_visual_tools/include/**",
+        "/home/tong/moveit_ws/src/srdfdom/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

catkin_ws/src/.vscode/settings.json

@@ -0,0 +1,12 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
+        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
+        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -5,12 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /MotionPlanning1
-        - /MotionPlanning1/Scene Geometry1
-        - /MotionPlanning1/Planning Request1
+        - /MotionPlanning1/Planned Path1
         - /RobotModel1/Links1
         - /Trajectory1/Links1
-      Splitter Ratio: 0.4285714328289032
-    Tree Height: 371
+      Splitter Ratio: 0.6529411673545837
+    Tree Height: 492
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -387,7 +386,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.0678346157073975
+      Distance: 1.3760178089141846
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -395,22 +394,22 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.0571471452713013
-        Y: -0.049734726548194885
-        Z: 0.29948994517326355
+        X: 1.022902250289917
+        Y: 0.31514620780944824
+        Z: 1.2667551040649414
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6502028107643127
+      Pitch: 0.8252028226852417
       Target Frame: base_link
-      Yaw: 5.132473468780518
+      Yaw: 1.48431396484375
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1043
+  Height: 802
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -419,11 +418,11 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000009fb000000100044006900730070006c006100790073010000003d00000204000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000247000001af0000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff000000160000001600000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000010000000000000156000002c8fc0200000009fb000000100044006900730070006c006100790073010000003d0000027d000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000002c0000000450000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000210000001e60000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000003b5000000410000001600000016000003a9000002c800000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Trajectory - Trajectory Slider:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1920
-  X: 0
-  Y: 0
+  Width: 1285
+  X: 485
+  Y: 90

catkin_ws/src/lstrobot_planning/launch/publish_pointcloud.launch

@@ -1,7 +1,6 @@
 <launch>
   <!-- 启动publish_pointcloud节点 -->
-  <node pkg="lstrobot_planning" name="publish_pointcloud" type="dycl.py" required="true" output="screen">
+  <node pkg="lstrobot_planning" name="publish_pointcloud" type="dycl_0506.py" required="true" output="screen">
   </node>
 
 </launch>
-

catkin_ws/src/lstrobot_planning/scripts/check.py

@@ -0,0 +1,145 @@
+import rospy, sys
+import numpy as np
+from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
+
+#####检查关节限位#####
+def check_joint_positions(joint_trajectory):
+    is_limited = False      #初始化为满足限制条件
+    Limit_margin = True     #限制边界，初始化为True，用来检查是否超出限制条件
+    
+    for i in range(len(joint_trajectory.joint_trajectory.points) - 1):  #遍历除了最后一个关节轨迹点其他所有轨迹点
+        if  not is_limited:
+            joint_positions1 = joint_trajectory.joint_trajectory.points[i].positions    #选择两个相邻的关节轨迹点
+            joint_positions2 = joint_trajectory.joint_trajectory.points[i + 1].positions 
+
+            for j in range(len(joint_positions1)):    #遍历但前轨迹点的每个关节点，这里是joint_positions1
+                position_diff = abs(joint_positions1[j] - joint_positions2[j])  #计算相邻关节轨迹点的关节点绝对差值，归一化到[-pi,pi]
+                
+                if position_diff > 6:   #限位阈值，6rad=343.8度
+                    rospy.loginfo("发生大角度翻转: point{}-joint{}:{}".format(i,j+1,joint_positions1))
+                    
+                    #关节限位值
+                    joint_limt =[[],[],[],[-1.6,1.6],[],[-3.838,3.838]] 
+                    
+                    #设定限位点为当前关节点
+                    Limit_point = joint_positions1[j]  
+                     
+                    #计算限位点(当前点)与起点位置的偏移量的绝对值
+                    distance_to_Limit_point = abs(joint_trajectory.joint_trajectory.points[0].positions[j]- Limit_point)
+                    Joint_range = abs(joint_limt[j][0] - joint_limt[j][1])  #计算关节限位范围
+                    margin = distance_to_Limit_point / Joint_range          #计算限位余量：偏移量的绝对值/关节限位范围
+                    if margin > 0.3:        
+                        Limit_margin = True  
+                    else:
+                        Limit_margin = False
+                    rospy.loginfo("margin = {}".format(margin))
+                    is_limited = True           
+                    break
+                
+                if position_diff > 3:    #限位阈值，3rad=171.9度
+                    rospy.loginfo("point{}-joint{}:{}".format(i,j+1,joint_positions1))
+      
+                    Limit_margin = True
+                    is_limited = True
+                    break
+        if  is_limited:
+            break
+    if not is_limited:
+        rospy.loginfo("Check OK! 轨迹有效")         
+    return is_limited,Limit_margin
+
+#姿态调整
+def angle_adjustment(pose,axis,angle):      
+    if len(pose) == 6:
+        q_initial = quaternion_from_euler(pose[3], pose[4], pose[5])    #初始姿态四元数，将欧拉角转换为四元数
+    else:
+        q_initial = (pose[3], pose[4], pose[5],pose[6])     #len>6，默认为四元数
+    if axis == 'z':
+        q_rotation = quaternion_from_euler(0, 0, angle)     #绕z轴旋转，转换得到关于z轴的旋转四元数
+    if axis == 'y':
+        q_rotation = quaternion_from_euler(0, angle, 0)
+    if axis == 'x':
+        q_rotation = quaternion_from_euler(angle, 0, 0)    
+    q_new = quaternion_multiply(q_initial, q_rotation)      #q_new为调整后的姿态四元数，值初始姿态四元数乘以旋转四元数
+    pose[3:] = q_new    #将q_new放入pose列表中的3号索引以后的位置
+    return pose
+
+
+
+# def traj_validity_check(trajectory):
+#     if type (trajectory)is moveit_msgs.msg._RobotTrajectory.RobotTrajectory:
+#         point_num=len(trajectory.joint_trajectory.point)
+#         trajectory.joint_trajectory.point.positions
+        
+#         # for i
+#         trajectory.joint_trajectory.point.velocities
+
+#         trajectory.joint_trajectory.point.accelerations
+        
+#         return 0
+#     # else:
+#     #         raise MoveItCommanderException(
+#     #             "Expected value in the range from 0 to 1 for scaling factor"
+#     #         )
+#     def scale_trajectory_speed(traj, scale):
+#        # Create a new trajectory object
+#        new_traj = RobotTrajectory()
+       
+#        # Initialize the new trajectory to be the same as the input trajectory
+#        new_traj.joint_trajectory = traj.joint_trajectory
+       
+#        # Get the number of joints involved
+#        n_joints = len(traj.joint_trajectory.joint_names)
+       
+#        # Get the number of points on the trajectory
+#        n_points = len(traj.joint_trajectory.points)
+        
+#        # Store the trajectory points
+#        points = list(traj.joint_trajectory.points)
+       
+#        # Cycle through all points and joints and scale the time from start,
+#        # as well as joint speed and acceleration
+#        for i in range(n_points):
+#            point = JointTrajectoryPoint()
+           
+#            # The joint positions are not scaled so pull them out first
+#            point.positions = list(traj.joint_trajectory.points[i].positions)
+
+#            # Next, scale the time_from_start for this point
+#             # point.time_from_start = traj.joint_trajectory.points[i].time_from_start / scale
+           
+#            # Get the joint velocities for this point
+#            point.velocities = list(traj.joint_trajectory.points[i].velocities)
+           
+#            # Get the joint accelerations for this point
+#            point.accelerations = list(traj.joint_trajectory.points[i].accelerations)
+           
+#            # Scale the velocity and acceleration for each joint at this point
+#            for j in range(n_joints):
+#             #    if point.positions[j]
+#                if has_velocity_limits:
+#                 if point.velocities[j] > joint_max_velocity[j]:
+#                     vel_exception_point
+#                     rospy.loginfo("velocities Test OK") 
+#                 else:
+#                     raise MoveItCommanderException("Expected value in the range from 0 to 1 for scaling factor")     
+#                if has_acceleration_limits:
+#                 point.accelerations[j] = point.accelerations[j] * scale * scale
+        
+#            # Store the scaled trajectory point
+#            points[i] = point
+
+#        rospy.loginfo("velocities Check OK") 
+
+#        # Assign the modified points to the new trajectory
+#        new_traj.joint_trajectory.points = points
+
+#        # Return the new trajecotry
+#        return new_traj
+
+#     else:
+#         print("traj type is not std")
+    
+
+
+

catkin_ws/src/lstrobot_planning/scripts/command.py

@@ -20,7 +20,7 @@ def close_rviz(terminal_name):
             os.kill(int(pid), signal.SIGTERM)
         except ProcessLookupError:
             pass  # 进程可能已经不存在
-        
+
 def load_visual():
     command = "rosrun visual visual_node"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])

catkin_ws/src/lstrobot_planning/scripts/decorator_time.py

@@ -0,0 +1,16 @@
+#!/usr/bin/env python3
+import rospy
+import time
+
+#定义一个装饰器，用来计算函数运行时间
+#使用方法：在需要计算运行时间的函数前面加上@decorator_time
+
+def decorator_time(func):
+    def wrapper(*args, **kwargs):
+        start_time = time.time()
+        result = func(*args, **kwargs)
+        end_time = time.time()
+        elapsed_time = end_time - start_time
+        rospy.loginfo(f"Time taken by {func.__name__}: {elapsed_time:.4f} seconds")
+        return result
+    return wrapper

catkin_ws/src/lstrobot_planning/scripts/joint2ee.py

@@ -0,0 +1,75 @@
+#!/usr/bin/env python3
+import sys
+import moveit_commander
+import rospy
+
+"""1.roslaunch lstrobot_moveit_config demo.launch"""
+"""2.rosrun lstrobot_planning joint2ee.py"""
+
+class MoveitPlanner:
+    def __init__(self, group_name):
+        # 初始化 moveit_commander
+        moveit_commander.roscpp_initialize(sys.argv)
+        rospy.init_node('1111_node', anonymous=True)
+        self.robot = moveit_commander.RobotCommander()
+        self.group = moveit_commander.MoveGroupCommander(group_name)
+        self.planning_scene_interface = moveit_commander.PlanningSceneInterface()
+        self.end_effector_link = self.group.get_end_effector_link()
+
+    def get_robot_state(self):
+        # 获取当前机械臂状态
+        return self.robot.get_current_state()
+
+    def get_end_effector_pose(self):
+        # 获取末端执行器的位姿
+        return self.group.get_current_pose(self.end_effector_link)
+
+        
+#从文件中读取关节角度值
+def read_joint_value(input_filename):
+    joint_value_list = []
+    with open(input_filename, 'r') as file:
+        for line in file:
+            #将每行的关节值转换为浮点数并去掉换行符
+            joint_value=[float(val) for val in line.strip().split()]
+            joint_value_list.append(joint_value)
+            
+    return joint_value_list
+
+#写入末端执行器位姿到目标文件
+def write_ee_pose(joint_value_list, planner, output_filename):
+    with open(output_filename, 'w') as file:
+       for idx,joint_value in enumerate(joint_value_list):
+           #设置关节值为当前轨迹点的关节位置
+           planner.group.set_joint_value_target(joint_value)
+           
+           plan=planner.group.plan()
+           
+           if plan:
+                planner.group.execute(plan[1], wait=True)
+                
+                fk_result = planner.get_end_effector_pose()
+                position=fk_result.pose.position
+                orientation=fk_result.pose.orientation
+           
+                #格式化位姿数据
+                pose_data = f"Point {idx + 1}:\n"
+                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)
+                
+    print(f"End effector pose saved to {output_filename}.")
+    
+def main():
+    planner = MoveitPlanner("manipulator")
+    
+    input_filename = '/home/tong/catkin_ws/data/test/outtt.txt'
+    joint_value_list = read_joint_value(input_filename)
+    
+    output_filename = '/home/tong/catkin_ws/data/test/ee_pose.txt'
+    write_ee_pose(joint_value_list, planner, output_filename)
+    
+if __name__ == '__main__':
+    main()

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -25,6 +25,8 @@ import traceback
 from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
+from decorator_time import decorator_time
+import check
 
 pi = np.pi
 
@@ -36,6 +38,7 @@ class MoveIt_Control:
         self.hf_fail=[]
         # Init ros config
         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)
@@ -88,7 +91,8 @@ 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):#创建路径约束
         #计算起点指向终点的向量
         vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
@@ -130,11 +134,12 @@ class MoveIt_Control:
         # constraints.orientation_constraints.append(orientation_constraint)
         return constraints
     
-    def move_p_flexible(self,point,points,trajectory,trajectory_with_type,v=1,a=1):
-        self.arm.set_max_velocity_scaling_factor(v)
+    #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)
-        rrr=0.05#初始允许半径
- 
+        self.arm.set_max_velocity_scaling_factor(v)
+        rrr=0.09#初始允许半径 9cm
+        er=0
         if trajectory:
             #起点位置设置为规划组最后一个点
             state = self.arm.get_current_state()
@@ -157,20 +162,24 @@ class MoveIt_Control:
             traj = self.arm.plan()#调用plan进行规划
             error=traj[3]
             if  error.val == moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes.SUCCESS:
-                rospy.loginfo("本次移动OK")
                 trajj = traj[1] #取出规划的信息
-                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"      
-                trajectory_with_type.append(trajj_with_type)
-                points.append(point)
-                trajectory.append(trajj)  
-                break
+                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"      
+                    trajectory_with_type.append(trajj_with_type)
+                    points.append(point)
+                    trajectory.append(trajj)  
+                    break  
             else:
-                rospy.loginfo(error)
+                rospy.loginfo("check failed! 移动轨迹无效")
                 rospy.loginfo("移动规划失败-开始第{}次重新规划".format(i+1))
                 self.arm.clear_path_constraints()
-                rrr=rrr+0.2#每次增加10厘米
+                rrr=rrr+0.2#每次增加20厘米半径
                 rospy.loginfo("R值：{}".format(rrr))
                 if trajectory:
                     path_constraints = self.create_path_constraints2(points[-1],point,rrr)#将最后一个点和输入的点（焊缝的起始点）做圆柱轨迹约束
@@ -181,37 +190,38 @@ class MoveIt_Control:
                     rospy.loginfo("所有移动规划尝试失败 取消路径约束")
                     self.arm.clear_path_constraints()
                 if(i==(b-1)):
-                    rospy.loginfo("所有移动规划尝试失败  程序退出")
-                    rospy.set_param("sign_control",0)
-                    sys.exit(0)
+                    rospy.loginfo("所有移动规划尝试失败  焊缝起点不可达")
+                    er=1
+                    break
         #清除约束
         self.arm.clear_path_constraints()
         
-        return points,trajectory,trajectory_with_type
+        return points,trajectory,trajectory_with_type,er
     
-    
-    def move_pl(self,point,points,trajectory,trajectory_with_type,v=1,a=1):#焊缝的规划  只规划当前的点到上一个点
+    #TODO move_pl
+    def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划  只规划当前的点到上一个点
         
-        self.arm.set_max_velocity_scaling_factor(v)
         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)
-        
+                
         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()#调用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"  
@@ -224,17 +234,78 @@ class MoveIt_Control:
             points.pop()#将本条焊缝的起点从规划中删除
             trajectory.pop()
             trajectory_with_type.pop()
-            self.hf_fail.append(welding_sequence[self.hf_num])
+            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
 
+    #TODO move_pl_check
+    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):
         #计算起点指向终点的向量
         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.001
+        radius = 0.01
 
         constraints = Constraints()
         
@@ -271,7 +342,9 @@ class MoveIt_Control:
         constraints.position_constraints.append(position_constraint)
 
         return constraints
-       
+     
+    #TODO go_home
+    @decorator_time  
     def go_home(self,a=1,v=1):
         rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
@@ -282,6 +355,7 @@ class MoveIt_Control:
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
     
+    #TODO go_home_justplan
     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)
@@ -296,10 +370,12 @@ class MoveIt_Control:
         trajectory_with_type.append(traj_with_type)
         return trajectory,trajectory_with_type
 
+    #TODO path_planning
+    @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)
@@ -309,12 +385,19 @@ class MoveIt_Control:
             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 = self.move_p_flexible(point11,points,trajectory,trajectory_with_type)
+            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=v, a=a)
+            points,trajectory,trajectory_with_type = self.move_pl_check(point22,points,trajectory,trajectory_with_type,v=speed_v)
+
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
+            rospy.loginfo("*******************")
         if gohome:
-            points,trajectory,trajectory_with_type = self.move_p_flexible(self.home_pose,points,trajectory,trajectory_with_type)
+            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)
@@ -339,6 +422,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
@@ -355,6 +439,7 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
+#TODO merge_trajectories
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -383,6 +468,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
@@ -458,37 +544,71 @@ def ROS2PY_msgs(msgs, m_sr):
     # for i in range(len(py_msgs.point.type)):
     #     moveit_server.move_j(py_msgs.point.xyz_list[i])
     #     #input("任意键继续")
-    
+
+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
-    
-    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("输入值无效，请根据提示重新输入！")
-    
+     
     try:
         termios.tcflush(sys.stdin, termios.TCIFLUSH)   #清空输入缓存
         rospy.loginfo("输入缓存区已清空！")
         
-        v=get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
-        a=get_valid_input("请输入加速度缩放因子(0-1, 默认为1): ", DEFUALT_A)
+        vv=get_valid_input("请输入速度缩放因子(0-1, 默认为1): ", DEFUALT_V)
+        #a=get_valid_input("请输入加速度缩放因子(0-1, 默认为1): ", DEFUALT_A)
           
-        rospy.loginfo(f"用户输入的速度缩放因子为{v}, 加速度缩放因子为{a}")
-        return v, a
+        #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
+        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")
+            
+# 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
@@ -496,15 +616,17 @@ 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)
-    
+    moveit_server = MoveIt_Control(is_use_gripper=False)  
     welding_sequence = rospy.get_param('welding_sequence')
-    v,a=get_user_input()
+
+
+    speed_v=get_user_input()
+    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)
     
-    # plan_result = ROS2PY_msgs(trajectory_with_type_merge, moveit_server)
-    # publisher.pub_plan_result(plan_result)
-
     moveit_server.arm.execute(trajectory_merge)
     
     rospy.set_param("sign_control",0)

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -17,6 +17,7 @@ from rospy.service import ServiceException
 import actionlib_msgs.msg._GoalStatusArray
 import sys
 import time
+import termios
 def wait_for_topic(topic_name, message_type):
     try:
         message = rospy.wait_for_message(topic_name, message_type, timeout=None)
@@ -35,6 +36,7 @@ def clear_octomap():
 
 waited_once = False
 tim_list=[]
+
 if __name__ == "__main__":
     command.load_rviz()
     rospy.init_node("set_update_paramter_p")
@@ -53,7 +55,8 @@ if __name__ == "__main__":
     rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
     rospy.set_param("pitch_of_Verticalweld",np.pi/5)
     rospy.set_param("culling_radius",0) #焊缝剔除半径
-    rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
+    #rospy.set_param("folder_path","/home/tong/catkin_ws/data/20240913-1")     #4条焊缝
+    rospy.set_param("folder_path","/home/tong/catkin_ws/data/6_Welds")      #6条焊缝
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
     rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
@@ -68,13 +71,15 @@ if __name__ == "__main__":
         if sign_control == "0":
             if not waited_once:
                 print("等待点云数据准备完成...")
-                sys.stdin.flush()#清空键盘缓冲区
-                aaa=input("请选择 1-2: ")
+                termios.tcflush(sys.stdin, termios.TCIFLUSH) #清空键盘缓冲区
+                print("---1.重新加载点云数据并规划---\n---2.仅重新规划---\n---按任意键退出程序---")
+                
+                user_input=input("请选择 1-2: ")
                 #aaa='2'
 
                 rospy.set_param("cishu",rospy.get_param("cishu")+1)
-                if(aaa =='1' or aaa=='2'):
-                    rospy.set_param("sign_control",aaa)
+                if(user_input =='1' or user_input=='2'):
+                    rospy.set_param("sign_control", user_input)
                 else:
                     command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
                     exit(0)
@@ -95,7 +100,7 @@ if __name__ == "__main__":
             # 等待 /move_group/monitored_planning_scene 话题发布消息
             rospy.loginfo("等待场景地图加载完毕...")
             wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            rospy.loginfo(f"场景地图已加载完毕")
+            rospy.loginfo("场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")
             

catkin_ws/src/lstrobot_planning/scripts/visual.py

@@ -0,0 +1,94 @@
+import rospy
+import moveit_commander
+import moveit_visual_tools
+import numpy as np
+import os, sys
+
+from geometry_msgs.msg import Pose
+from math import pi, cos, sin
+
+def euler2quat(phi, theta, psi):
+    c1 = cos(phi / 2)
+    c2 = cos(theta / 2)
+    c3 = cos(psi / 2)
+    s1 = sin(phi / 2)
+    s2 = sin(theta / 2)
+    s3 = sin(psi / 2)
+    quat = [
+        c1 * c2 * c3 + s1 * s2 * s3,
+        s1 * c2 * c3 - c1 * s2 * s3,
+        c1 * s2 * c3 + s1 * c2 * s3,
+        c1 * c2 * s3 - s1 * s2 * c3
+    ]
+    return quat
+
+def read_points(file_path):
+    with open(file_path, 'r') as file:
+        lines = file.readlines()
+    return lines
+
+def split_str(src, delimiter):
+    return [elem for elem in src.split(delimiter) if elem]
+
+def main():
+    rospy.init_node("visual_node", anonymous=True)
+
+    moveit_commander.roscpp_initialize(sys.argv)
+    visual_tools = moveit_visual_tools.MoveItVisualTools('base_link')
+
+    visual_tools.deleteAllMarkers()
+    visual_tools.loadRemoteControl()
+    
+    text_pose = visual_tools.get_identity_pose()
+    text_pose.position.z = 1.75
+
+    waypoints = [[] for _ in range(20)]
+
+    # 从参数服务器获取文件路径
+    folder_path = rospy.get_param("folder_path", "")
+    file_path = os.path.join(folder_path, "result.txt")
+
+    num = 0
+    lines = read_points(file_path)
+    for line in lines:
+        l = split_str(line, "sp,/")
+
+        target_pose1 = Pose()
+        target_pose2 = Pose()
+
+        target_pose1.position.x = float(l[0]) / 1000
+        target_pose1.position.y = float(l[1]) / 1000
+        target_pose1.position.z = float(l[2]) / 1000
+
+        target_pose2.position.x = float(l[3]) / 1000
+        target_pose2.position.y = float(l[4]) / 1000
+        target_pose2.position.z = float(l[5]) / 1000
+
+        target_pose1.orientation.x = float(l[6])
+        target_pose1.orientation.y = float(l[7])
+        target_pose1.orientation.z = float(l[8])
+        target_pose1.orientation.w = float(l[9])
+
+        target_pose2.orientation.x = float(l[10])
+        target_pose2.orientation.y = float(l[11])
+        target_pose2.orientation.z = float(l[12])
+        target_pose2.orientation.w = float(l[13])
+
+        waypoints[num].append(target_pose1)
+        waypoints[num].append(target_pose2)
+        num += 1
+
+    visual_tools.deleteAllMarkers()
+    visual_tools.publishText(text_pose, "robot", visual_tools.WHITE, visual_tools.XLARGE)
+
+    for k in range(num):
+        visual_tools.publishPath(waypoints[k], visual_tools.PINK, visual_tools.XXXSMALL)
+        for i, wp in enumerate(waypoints[k]):
+            label = f"point{i}  {wp.position.x:.2f} {wp.position.y:.2f} {wp.position.z:.2f}"
+            visual_tools.publishAxisLabeled(wp, label, visual_tools.XXXSMALL)
+        visual_tools.trigger()
+
+    moveit_commander.roscpp_shutdown()
+
+if __name__ == '__main__':
+    main()

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -387,8 +387,8 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-2.1"
-      upper="2.1"
+      lower="-3.838"
+      upper="3.838"
       effort="32.64"
       velocity="23.655" />
   </joint>

catkin_ws/更新记录.txt

@@ -1,3 +1,6 @@
 2024-9-3
 
 增加 自定义设置速度因子v和加速度因子a接口
+
+2024-9-18
+修改 使用check来检测第六轴翻转，不再限制第六轴关节角度