2024-9-1

catkin_ws/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

catkin_ws/src/lstrobot_moveit_config_0605/config/stomp_planning.yaml

@@ -36,4 +36,4 @@ stomp/manipulator:
         error_rgb: [255, 0, 0]
         publish_intermediate: True
         marker_topic: stomp_trajectory
-        marker_namespace: optimized
+        marker_namespace: optimized

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -4,12 +4,13 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Planning Request1
         - /RobotModel1/Links1
         - /Trajectory1/Links1
       Splitter Ratio: 0.4285714328289032
-    Tree Height: 386
+    Tree Height: 371
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -74,6 +75,54 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          Link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_end:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          link_end_now:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          link_flp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
         Loop Animation: false
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
@@ -120,6 +169,54 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
+          Link1:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link2:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link3:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link4:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link5:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          Link6:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          base_link:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
+          link_end:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          link_end_now:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+          link_flp:
+            Alpha: 1
+            Show Axes: false
+            Show Trail: false
+            Value: true
         Robot Alpha: 0.5
         Show Robot Collision: false
         Show Robot Visual: false
@@ -130,7 +227,7 @@ Visualization Manager:
       Collision Enabled: false
       Enabled: true
       Links:
-        All Links Enabled: false
+        All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
@@ -139,27 +236,27 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         Link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         Link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         Link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         Link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         Link6:
           Alpha: 1
           Show Axes: false
@@ -169,7 +266,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
         link_end:
           Alpha: 1
           Show Axes: false
@@ -182,7 +279,7 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -290,7 +387,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 0.48905709385871887
+      Distance: 1.0678346157073975
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -298,17 +395,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 1.0607073307037354
-        Y: -0.08724305778741837
-        Z: 0.4003370702266693
+        X: 1.0571471452713013
+        Y: -0.049734726548194885
+        Z: 0.29948994517326355
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.9597961902618408
+      Pitch: 0.6502028107643127
       Target Frame: base_link
-      Yaw: 6.070621013641357
+      Yaw: 5.132473468780518
     Saved: ~
 Window Geometry:
   Displays:
@@ -322,7 +419,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000009fb000000100044006900730070006c006100790073010000003d00000213000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000256000001a00000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff000000160000001600000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000009fb000000100044006900730070006c006100790073010000003d00000204000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000247000001af0000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff000000160000001600000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Trajectory - Trajectory Slider:
     collapsed: false
   Views:

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -139,7 +139,6 @@ def compute_Rc2w_tc2w(file_path_Tw2c):
 
     Rc2w = Rw2c.T
     tc2w = -np.matmul(Rc2w, tw2c)
-    
     return Rc2w,tc2w
 
 def compute_pose_R(fangxiang, hanfeng,start_point, end_point):

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -171,7 +171,10 @@ class MoveIt_Control:
                 self.arm.clear_path_constraints()
                 rrr=rrr+0.2#每次增加10厘米
                 rospy.loginfo("R值：{}".format(rrr))
-                path_constraints = self.create_path_constraints2(points[-1],point,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("所有移动规划尝试失败 取消路径约束")
@@ -461,11 +464,10 @@ if __name__ =="__main__":
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
     # trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning_test(folder_path,1,True)
     #执行动作
-
+    
     #ROS2PY_msgs(trajectory_with_type_merge,moveit_server)
 
-
-    moveit_server.arm.execute(trajectory_merge)
+    #moveit_server.arm.execute(trajectory_merge)
     
 
     # for i in range(1 * 2 - 0): #执行到第i条焊缝

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py

@@ -16,6 +16,7 @@ from rospy.service import ServiceException
 
 import actionlib_msgs.msg._GoalStatusArray
 import sys
+import time
 def wait_for_topic(topic_name, message_type):
     try:
         message = rospy.wait_for_message(topic_name, message_type, timeout=None)
@@ -33,12 +34,17 @@ def clear_octomap():
         rospy.loginfo("Failed to call /clear_octomap service or it is not available. Skipping operation.")
 
 waited_once = False
+tim_list=[]
 if __name__ == "__main__":
     command.load_rviz()
     rospy.init_node("set_update_paramter_p")
 
     # 初始化各种类型参数
-    rospy.set_param("sign_control",0)
+
+    rospy.set_param("cishu",0)
+    rospy.set_param("time",0)
+
+    rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
     rospy.set_param("sign_traj_accepted",0)
 
@@ -47,7 +53,7 @@ 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/robot/ROS/catkin_ws/data/22")
 
     rospy.loginfo("当前参数值:")# 显示参数当前值
     rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
@@ -62,8 +68,11 @@ if __name__ == "__main__":
         if sign_control == "0":
             if not waited_once:
                 print("等待点云数据准备完成...")
-                sys.stdin.flush()#清空键盘缓冲区
-                aaa=input("请选择 1-2: ")
+                # sys.stdin.flush()#清空键盘缓冲区
+                # aaa=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)
                 else:
@@ -100,15 +109,25 @@ if __name__ == "__main__":
             waited_once = False
  
         elif sign_control == "2":
+            tim=time.time()
+            rospy.set_param("time",0)
             # 重置参数
             #rospy.set_param("sign_control",0)
             rospy.set_param("sign_traj_accepted",0)
-
+            hjsx.run()
+            hanqiangpose.run()
             process = multiprocessing.Process(target=command.launch_moveit_control_server_background)
             process.start()
 
             while rospy.get_param("sign_control"):
                 pass
+
+            tim=time.time()-tim
+            tim_list.append(tim)
+            print("-------------------------------------")
+            print(f'第{rospy.get_param("cishu")}次规划：')
+            print(tim_list)
+            print("")
             #运行结束，重置参数
             waited_once = False
 

catkin_ws/更新记录.txt

@@ -38,5 +38,8 @@
 	在焊接姿态处理中应该可以让焊缝焊接过程不进行翻转从而保证焊接的连续性（可能已经预留了末端执行器关节角度代码？  也大概率没有）  
 	那就得移动过程中来解决旋转的问题（有的焊接顺序就是避免不了翻转  焊缝姿态定了 那移动过程就会不可避免的要翻转 ） 而移动过程的翻转面临的最大问题就是 翻转过程不检测碰撞
 	
-	
+2024-9-1
+	修复从起点规划到第一个失败后的重新设定约束逻辑问题
+	六轴翻转问题依旧没有解决  其问题会带来运动碰撞风险 不止是极限角度  也是之前轴翻转的根源
+	新隐患：