renyuan

a9/urdf/a9（复件）.urdf

@@ -1,465 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="a9">
-  <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>
-      <origin
-        xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282"
-        rpy="0 0 0" />
-      <mass
-        value="0.0148298653353077" />
-      <inertia
-        ixx="2.02744416702812E-06"
-        ixy="-1.54215522606031E-15"
-        ixz="-3.37592192750916E-14"
-        iyy="1.40267326461625E-06"
-        iyz="-2.76126618174793E-15"
-        izz="1.40267288470545E-06" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="Link_1">
-    <inertial>
-      <origin
-        xyz="0.802922442229256 0.00984539192698494 1.19598646678643"
-        rpy="0 0 0" />
-      <mass
-        value="8.19341168850207" />
-      <inertia
-        ixx="0.19323050511757"
-        ixy="0.0386299594946521"
-        ixz="-0.388616965148905"
-        iyy="1.42567974975729"
-        iyz="0.0128991341474255"
-        izz="1.24513273855516" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_1.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.278"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="Link_1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_2">
-    <inertial>
-      <origin
-        xyz="0.117746157865896 0.0426453911640452 1.20871486266949"
-        rpy="0 0 0" />
-      <mass
-        value="8.19341168850207" />
-      <inertia
-        ixx="0.723493788556202"
-        ixy="0.0285843029943905"
-        ixz="-0.653933580723206"
-        iyy="1.42567974975729"
-        iyz="0.0290103266895879"
-        izz="0.714869455116528" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_2"
-    type="revolute">
-    <origin
-      xyz="0.15 -0.0328 0.172"
-      rpy="0 0.47052 0" />
-    <parent
-      link="Link_1" />
-    <child
-      link="Link_2" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_3">
-    <inertial>
-      <origin
-        xyz="0.00164806841162357 0.0743961632896021 -0.0125870623969895"
-        rpy="0 0 0" />
-      <mass
-        value="1.10722558532838" />
-      <inertia
-        ixx="0.00281804497282514"
-        ixy="1.0820613146504E-05"
-        ixz="-2.30905562044302E-05"
-        iyy="0.00127732014523064"
-        iyz="5.62797076557556E-06"
-        izz="0.0028367630953507" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_3.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_3"
-    type="revolute">
-    <origin
-      xyz="-0.59925 0 0.50037"
-      rpy="0 -0.28892 0" />
-    <parent
-      link="Link_2" />
-    <child
-      link="Link_3" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_4">
-    <inertial>
-      <origin
-        xyz="0.751501000756241 -0.00505225715113682 0.690060032582326"
-        rpy="0 0 0" />
-      <mass
-        value="8.19341168850207" />
-      <inertia
-        ixx="0.419099372956719"
-        ixy="-0.0348631653478693"
-        ixz="-0.581012917159635"
-        iyy="1.42564974048808"
-        iyz="-0.0218580250133829"
-        izz="1.01929387998522" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_4"
-    type="revolute">
-    <origin
-      xyz="-0.084223 0.0347 0.16884"
-      rpy="-3.119 -1.0471 -0.013042" />
-    <parent
-      link="Link_3" />
-    <child
-      link="Link_4" />
-    <axis
-      xyz="0.86603 0 0.5" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_5">
-    <inertial>
-      <origin
-        xyz="0.206551018322306 -0.00505225715114058 -0.104834036546721"
-        rpy="0 0 0" />
-      <mass
-        value="8.19341168850207" />
-      <inertia
-        ixx="0.745717188604768"
-        ixy="0.0281122476978264"
-        ixz="-0.653399595391099"
-        iyy="1.42564974048808"
-        iyz="0.030048545471202"
-        izz="0.69267606433717" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_5"
-    type="revolute">
-    <origin
-      xyz="0.92436 0 0.53588"
-      rpy="3.1416 -0.25867 3.1416" />
-    <parent
-      link="Link_4" />
-    <child
-      link="Link_5" />
-    <axis
-      xyz="0 1 0" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_6">
-    <inertial>
-      <origin
-        xyz="0.12280626304832 0.10284172733946 -0.0673247079199191"
-        rpy="0 0 0" />
-      <mass
-        value="8.19341168850207" />
-      <inertia
-        ixx="1.25065484857243"
-        ixy="-0.370837141231599"
-        ixz="-0.214061773475593"
-        iyy="0.581725625286335"
-        iyz="-0.544088624970368"
-        izz="1.03166251957125" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 0 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_6"
-    type="revolute">
-    <origin
-      xyz="0.037697 -0.0006997 -0.064094"
-      rpy="0.44032 0.19669 -0.83053" />
-    <parent
-      link="Link_5" />
-    <child
-      link="Link_6" />
-    <axis
-      xyz="0.49999 0 -0.86603" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="0"
-      velocity="0" />
-  </joint>
-  <link
-    name="Link_7">
-    <inertial>
-      <origin
-        xyz="0.0296425829147993 -6.47728578861279E-06 0.162281442392356"
-        rpy="0 0 0" />
-      <mass
-        value="7.07143232149748" />
-      <inertia
-        ixx="0.0103947066541271"
-        ixy="8.00167786886164E-07"
-        ixz="0.00131122028195651"
-        iyy="0.0112643862205598"
-        iyz="-2.38846633634112E-06"
-        izz="0.0065729384023248" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_7.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://a9/meshes/Link_7.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint_7"
-    type="fixed">
-    <origin
-      xyz="0 0 0"
-      rpy="-2.6972 0.28661 -2.1066" />
-    <parent
-      link="Link_6" />
-    <child
-      link="Link_7" />
-    <axis
-      xyz="0 0 0" />
-  </joint>
-  
-  <link name="end_link_now" />
-  
-  <joint name="end_joint" type="fixed">
-    <parent link="Link_7" />
-    <child link = "end_link_now" />
-    <origin xyz="-0.045 0.0 0.4" rpy="0.0 0.4 3.14" />
-  </joint>
-  
-</robot>

a9/urdf/a9.urdf → reyuan/a9/urdf/a9.urdf

@@ -105,10 +105,10 @@
     <axis
       xyz="0 0 1" />
     <limit
-      lower="-2.965"
-      upper="2.965"
+      lower="-2.967060"
+      upper="2.967060"
       effort="0"
-      velocity="0" />
+      velocity="3.403392" />
   </joint>
   <link
     name="Link_2">
@@ -163,10 +163,10 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-2.27"
-      upper="2.27"
+      lower="-1.745329"
+      upper="2.792527"
       effort="0"
-      velocity="0" />
+      velocity="3.054326" />
   </joint>
   <link
     name="Link_3">
@@ -221,10 +221,10 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-4.0"
-      upper="4.0"
+      lower="-3.228859"
+      upper="4.766843"
       effort="0"
-      velocity="0" />
+      velocity="3.141593" />
   </joint>
   <link
     name="Link_4">
@@ -279,10 +279,10 @@
     <axis
       xyz="0.86603 0 0.5" />
     <limit
-      lower="-3.49"
-      upper="3.49"
+      lower="-3.490658"
+      upper="3.490658"
       effort="0"
-      velocity="0" />
+      velocity="6.283185" />
   </joint>
   <link
     name="Link_5">
@@ -337,10 +337,10 @@
     <axis
       xyz="0 1 0" />
     <limit
-      lower="-2.49"
-      upper="2.49"
+      lower="-2.443461"
+      upper="2.443461"
       effort="0"
-      velocity="0" />
+      velocity="6.283185" />
   </joint>
   <link
     name="Link_6">
@@ -395,10 +395,10 @@
     <axis
       xyz="0.49999 0 -0.86603" />
     <limit
-      lower="-7.855"
-      upper="7.855"
+      lower="-4.712389"
+      upper="4.712389"
       effort="0"
-      velocity="0" />
+      velocity="9.599311" />
   </joint>
   <link
     name="Link_7">

reyuan/lst_robot_r/.idea/.gitignore

@@ -0,0 +1,3 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml

reyuan/lst_robot_r/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

reyuan/lst_robot_r/.idea/lst_robot_r.iml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="inheritedJdk" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+  <component name="PyDocumentationSettings">
+    <option name="format" value="PLAIN" />
+    <option name="myDocStringFormat" value="Plain" />
+  </component>
+</module>

reyuan/lst_robot_r/.idea/misc.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="Black">
+    <option name="sdkName" value="Python 3.8" />
+  </component>
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.8" project-jdk-type="Python SDK" />
+</project>

reyuan/lst_robot_r/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/lst_robot_r.iml" filepath="$PROJECT_DIR$/.idea/lst_robot_r.iml" />
+    </modules>
+  </component>
+</project>

reyuan/lst_robot_r/README.md

@@ -0,0 +1,43 @@
+## README
+### 1.运行代码
+1. source工作空间
+2. sudo chmod +x *.py文件
+3. roslaunch lst_robot_r start.launch
+
+### 2.package(SRC)
+1. **lst_robot_r**
+1.1 **launch**-launch启动文件
+2.2 **msg**-自定义消息
+3.3 **scripts**-Python代码
+
+2. **a9**
+2.1 **config**-机械臂先关配置文件
+2.2 **launch**-launch启动文件
+2.3 **meshes**-机械臂STL文件
+2.4 **urdf**-机械臂urdf模型
+
+3. **robot_config**
+3.1 **config**-moveit配置文件，常用srdf(修改机器人配置)
+3.2 **launch**-launch启动文件，包括demo.launch文件
+
+4. **Data**
+4.1 点云数据，起始点终点数据，导出的其他的数据(轨迹关节值、末端执行器笛卡尔坐标)
+
+5. **visual**
+5.1 规划结果可视化
+
+## 2024.9.30
+- 添加修改笛卡尔路径参数的函数(速度，加速度)
+- 修正机械臂的初始姿态(home位姿)
+- 修改焊枪位姿，防止规划失败
+
+## 2024.10.12
+- 增加点云数据，起始点终点数据，测试碰撞检测
+
+## 2024.10.17
+- 修改规划逻辑，规划完一条焊缝后，末端执行器向上移动0.1m，然后继续规划下一条焊缝
+- 参照：https://github.com/ros-industrial/fanuc/blob/kinetic/fanuc_m20ia_support/urdf/m20ia_macro.xacro
+修改机器人关节限位参数
+
+
+

reyuan/lst_robot_r/scripts/joint2ee.py

@@ -0,0 +1,76 @@
+#!/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/moveir_ws/data/123/outtt.txt'
+    joint_value_list = read_joint_value(input_filename)
+    
+    output_filename = '/home/tong/moveir_ws/data/123/ee_pose.txt'
+    write_ee_pose(joint_value_list, planner, output_filename)
+    
+if __name__ == '__main__':
+    main()

lst_robot_r/scripts/moveitServer2.py → reyuan/lst_robot_r/scripts/moveitServer2.py

@@ -22,7 +22,7 @@ import tf
 from tf.transformations import euler_from_quaternion,quaternion_from_euler,quaternion_from_euler, quaternion_multiply
 import math
 import traceback
-from lstrobot_planning.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
+from lst_robot_r.msg import JointTrajectoryPoint_ex,JointTrajectory_ex
 import json
 import termios
 from decorator_time import decorator_time
@@ -112,9 +112,8 @@ class MoveIt_Control:
             (plan, fraction) = self.arm.compute_cartesian_path(
                 waypoint,  # waypoint poses，路点列表
                 0.001,  # eef_step，终端步进值
-                #0.0,  # jump_threshold，跳跃阈值
-                True,
-                None)  # avoid_collisions，避障规划
+                0.0,    # jump_threshold，跳跃阈值
+                True)   # avoid_collisions，避障规划
             attempts += 1
 
         if fraction == 1.0 :
@@ -335,9 +334,18 @@ class MoveIt_Control:
             point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
             point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
             way_points,trajectory,trajectory_with_type = self.plan_cartesian_path_LLL(point11,point22,way_points,trajectory,trajectory_with_type,v=speed_v)
-
+            
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             rospy.loginfo("*******************")
+            
+            #向上移动末端执行器
+            if i<len(all_data)-1:
+                up_value=0.1
+                point_up=[point22[0], point22[1], point22[2]-up_value, point22[3], point22[4],point22[5],point22[6]]
+                way_points, trajectory, trajectory_with_type = self.plan_cartesian_path_LLL(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
+                rospy.loginfo("末端执行器上移完毕")
+                rospy.loginfo("*******************")
+            
         rospy.loginfo("All of The trajectory Plan successfully")
         rospy.loginfo("*******************")
         if gohome:
@@ -455,6 +463,7 @@ def ROS2PY_msgs(msgs, m_sr):
     py_msgs.fail = m_sr.hf_fail.copy()
     py_msgs.shun_xv = msgs.points[0].sequence.copy()
 
+    # 打印规划信息
     # for i in range(len(py_msgs.point.type)):
     #     print(py_msgs.point.xyz_list[i])
     #     print(py_msgs.point.type[i])
@@ -499,7 +508,17 @@ def get_user_input():
         #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
         return vv
     except Exception as e:
-        rospy.logerr(f"发生错误：{e}") 
+        rospy.logerr(f"发生错误：{e}")
+
+#TODO 获取规划路径点结果，写入outtt.txt     
+def ROS2_msgs_write(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") 
         
 
 if __name__ =="__main__":
@@ -517,9 +536,10 @@ if __name__ =="__main__":
 
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
 
-    #moveit_server.go_ready()
+    #moveit_server.go_ready()       #合并后的轨迹也需要从ready点开始执行
     rospy.loginfo("开始执行合并后轨迹")
     moveit_server.arm.execute(trajectory_merge)
+    #ROS2_msgs_write(trajectory_with_type_merge,moveit_server)
     rospy.loginfo("合并后轨迹执行完毕")
     
     rospy.set_param("sign_control",0)

lst_robot_r/scripts/start.py → reyuan/lst_robot_r/scripts/start.py

@@ -47,7 +47,7 @@ if __name__ == "__main__":
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
 
-    rospy.set_param("folder_path","/home/tong/moveit_ws/data/3")
+    rospy.set_param("folder_path","/home/tong/moveir_ws/data/123")
     #rospy.set_param("folder_path","/home/tong/moveit_ws/data/renyuan_2")
 
    
@@ -79,9 +79,9 @@ if __name__ == "__main__":
             # 清除场景
             clear_octomap()
             #点云计算并发布
-            #process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            #process.start()
-            #input("请在rviz中查看点云，然后按回车键继续...")
+            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            process.start()
+            input("请在rviz中查看点云，然后按回车键继续...")
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()

robot_config/config/joint_limits.yaml → reyuan/robot_config/config/joint_limits.yaml

@@ -9,32 +9,32 @@ default_acceleration_scaling_factor: 0.1
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
   joint_1:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 3.403392
     has_acceleration_limits: false
     max_acceleration: 0
   joint_2:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 3.054326
     has_acceleration_limits: false
     max_acceleration: 0
   joint_3:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 3.141593
     has_acceleration_limits: false
     max_acceleration: 0
   joint_4:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 6.283185
     has_acceleration_limits: false
     max_acceleration: 0
   joint_5:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 6.283185
     has_acceleration_limits: false
     max_acceleration: 0
   joint_6:
-    has_velocity_limits: false
-    max_velocity: 0
+    has_velocity_limits: true
+    max_velocity: 9.599311
     has_acceleration_limits: false
-    max_acceleration: 0
+    max_acceleration: 0

robot_config/launch/moveit.rviz → reyuan/robot_config/launch/moveit.rviz

@@ -12,7 +12,6 @@ Panels:
         - /RobotModel1
         - /RobotModel1/Links1
         - /MarkerArray1
-        - /MarkerArray1/Status1
       Splitter Ratio: 0.5
     Tree Height: 356
   - Class: rviz/Help
@@ -297,9 +296,10 @@ Visualization Manager:
       Namespaces:
         Path: true
         Text: true
-        point0  0.072094  -0.030473  0.455566: true
-        point0  0.073125  0.041527  0.449824: true
-        point1  0.109523  0.042320  0.457250: true
+        point0  1.260560  -0.018740  1.496050: true
+        point0  1.334610  -0.019910  1.496050: true
+        point1  1.260620  0.020340  1.496050: true
+        point1  1.334480  0.021110  1.496050: true
       Queue Size: 100
       Value: true
     - Alpha: 1
@@ -344,7 +344,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 2.6588308811187744
+      Distance: 1.1933258771896362
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -352,17 +352,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.715438961982727
-        Y: -0.06119576096534729
-        Z: 1.1657675504684448
+        X: -1.124035358428955
+        Y: -0.2431718409061432
+        Z: 1.108873724937439
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4800015091896057
+      Pitch: 0.9297969341278076
       Target Frame: world
-      Yaw: 3.261983871459961
+      Yaw: 4.546980857849121
     Saved: ~
 Window Geometry:
   Displays:

reyuan/visual/.vscode/c_cpp_properties.json

@@ -0,0 +1,38 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/tong/moveir_ws/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/chomp/chomp_motion_planner/include/**",
+        "/home/tong/moveir_ws/src/geometric_shapes/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/chomp/chomp_interface/include/**",
+        "/home/tong/moveir_ws/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/benchmarks/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_plugins/moveit_ros_control_interface/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/move_group/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/occupancy_map_monitor/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/robot_interaction/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_ros/moveit_servo/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_setup_assistant/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_plugins/moveit_simple_controller_manager/include/**",
+        "/home/tong/moveir_ws/src/moveit_visual_tools/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner/include/**",
+        "/home/tong/moveir_ws/src/moveit/moveit_planners/pilz_industrial_motion_planner_testutils/include/**",
+        "/home/tong/moveir_ws/src/rviz_visual_tools/include/**",
+        "/home/tong/moveir_ws/src/srdfdom/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

reyuan/visual/.vscode/settings.json

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