2024-9-26

catkin_ws/src/mr12urdf20240605/CMakeLists.txt → catkin_ws/src/a9/CMakeLists.txt

@@ -1,14 +1,14 @@
-cmake_minimum_required(VERSION 2.8.3)
-
-project(mr12urdf20240605)
-
-find_package(catkin REQUIRED)
-
-catkin_package()
-
-find_package(roslaunch)
-
-foreach(dir config launch meshes urdf)
-	install(DIRECTORY ${dir}/
-		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
-endforeach(dir)
+cmake_minimum_required(VERSION 2.8.3)
+
+project(a9)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

catkin_ws/src/a9/config/joint_names_a9.yaml

@@ -0,0 +1 @@
+controller_joint_names: ['', 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', ]

catkin_ws/src/mr12urdf20240605/launch/display.launch → catkin_ws/src/a9/launch/display.launch

@@ -1,20 +1,20 @@
-<launch>
-  <arg
-    name="model" />
-  <param
-    name="robot_description"
-    textfile="$(find mr12urdf20240605)/urdf/mr12urdf20240605.urdf" />
-  <node
-    name="joint_state_publisher_gui"
-    pkg="joint_state_publisher_gui"
-    type="joint_state_publisher_gui" />
-  <node
-    name="robot_state_publisher"
-    pkg="robot_state_publisher"
-    type="robot_state_publisher" />
-  <node
-    name="rviz"
-    pkg="rviz"
-    type="rviz"
-    args="-d $(find mr12urdf20240605)/urdf.rviz" />
+<launch>
+  <arg
+    name="model" />
+  <param
+    name="robot_description"
+    textfile="$(find a9)/urdf/a9.urdf" />
+  <node
+    name="joint_state_publisher_gui"
+    pkg="joint_state_publisher_gui"
+    type="joint_state_publisher_gui" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="robot_state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find a9)/urdf.rviz" />
 </launch>

catkin_ws/src/mr12urdf20240605/launch/gazebo.launch → catkin_ws/src/a9/launch/gazebo.launch

@@ -1,20 +1,20 @@
-<launch>
-  <include
-    file="$(find gazebo_ros)/launch/empty_world.launch" />
-  <node
-    name="tf_footprint_base"
-    pkg="tf"
-    type="static_transform_publisher"
-    args="0 0 0 0 0 0 base_link base_footprint 40" />
-  <node
-    name="spawn_model"
-    pkg="gazebo_ros"
-    type="spawn_model"
-    args="-file $(find mr12urdf20240605)/urdf/mr12urdf20240605.urdf -urdf -model mr12urdf20240605"
-    output="screen" />
-  <node
-    name="fake_joint_calibration"
-    pkg="rostopic"
-    type="rostopic"
-    args="pub /calibrated std_msgs/Bool true" />
+<launch>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find a9)/urdf/a9.urdf -urdf -model a9"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
 </launch>

catkin_ws/src/mr12urdf20240605/package.xml → catkin_ws/src/a9/package.xml

@@ -1,21 +1,21 @@
-<package format="2">
-  <name>mr12urdf20240605</name>
-  <version>1.0.0</version>
-  <description>
-    <p>URDF Description package for mr12urdf20240605</p>
-    <p>This package contains configuration data, 3D models and launch files
-for mr12urdf20240605 robot</p>
-  </description>
-  <author>TODO</author>
-  <maintainer email="TODO@email.com" />
-  <license>BSD</license>
-  <buildtool_depend>catkin</buildtool_depend>
-  <depend>roslaunch</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>rviz</depend>
-  <depend>joint_state_publisher_gui</depend>
-  <depend>gazebo</depend>
-  <export>
-    <architecture_independent />
-  </export>
+<package format="2">
+  <name>a9</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for a9</p>
+    <p>This package contains configuration data, 3D models and launch files
+for a9 robot</p>
+  </description>
+  <author>TODO</author>
+  <maintainer email="TODO@email.com" />
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>roslaunch</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>rviz</depend>
+  <depend>joint_state_publisher_gui</depend>
+  <depend>gazebo</depend>
+  <export>
+    <architecture_independent />
+  </export>
 </package>

catkin_ws/src/a9/urdf/a9.csv

@@ -0,0 +1,9 @@
+Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
+base_link,-0.110121070288428,-0.0129999995820303,0.159000000973282,0,0,0,0.0148298653353077,2.02744416702812E-06,-1.54215522606031E-15,-3.37592192750916E-14,1.40267326461625E-06,-2.76126618174793E-15,1.40267288470545E-06,0,0,0,0,0,0,package://a9/meshes/base_link.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/base_link.STL,,FANUC M-10iA8L - 底座-1,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
+Link_1,0.802922442229256,0.00984539192698494,1.19598646678643,0,0,0,8.19341168850207,0.19323050511757,0.0386299594946521,-0.388616965148905,1.42567974975729,0.0128991341474255,1.24513273855516,0,0,0,0,0,0,package://a9/meshes/Link_1.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_1.STL,,FANUC M-10iA8L - 1-1,Origin_joint_1,Axis_joint_1,joint_1,revolute,0,0,0.278,0,0,0,base_link,0,0,1,0,0,-3,3,,,,,,,,
+Link_2,0.117746157865896,0.0426453911640452,1.20871486266949,0,0,0,8.19341168850207,0.723493788556202,0.0285843029943905,-0.653933580723206,1.42567974975729,0.0290103266895879,0.714869455116528,0,0,0,0,0,0,package://a9/meshes/Link_2.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_2.STL,,FANUC M-10iA8L - 2-1,Origin_joint_2,Axis_joint_2,joint_2,revolute,0.15,-0.0328,0.172,0,0.47052,0,Link_1,0,1,0,0,0,-3,3,,,,,,,,
+Link_3,0.00164806841162357,0.0743961632896021,-0.0125870623969895,0,0,0,1.10722558532838,0.00281804497282514,1.0820613146504E-05,-2.30905562044302E-05,0.00127732014523064,5.62797076557556E-06,0.0028367630953507,0,0,0,0,0,0,package://a9/meshes/Link_3.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_3.STL,,FANUC M-10iA8L - 3-1,Origin_joint_3,Axis_joint_3,joint_3,revolute,-0.59925,0,0.50037,0,-0.28892,0,Link_2,0,1,0,0,0,-3,3,,,,,,,,
+Link_4,0.751501000756241,-0.00505225715113682,0.690060032582326,0,0,0,8.19341168850207,0.419099372956719,-0.0348631653478693,-0.581012917159635,1.42564974048808,-0.0218580250133829,1.01929387998522,0,0,0,0,0,0,package://a9/meshes/Link_4.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_4.STL,,FANUC M-10iA8L - 4-1,Origin_joint_4,Axis_joint_4,joint_4,revolute,-0.084223,0.0347,0.16884,-3.119,-1.0471,-0.013042,Link_3,0.86603,0,0.5,0,0,-3,3,,,,,,,,
+Link_5,0.206551018322306,-0.00505225715114058,-0.104834036546721,0,0,0,8.19341168850207,0.745717188604768,0.0281122476978264,-0.653399595391099,1.42564974048808,0.030048545471202,0.69267606433717,0,0,0,0,0,0,package://a9/meshes/Link_5.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_5.STL,,FANUC M-10iA8L - 5-1,Origin_joint_5,Axis_joint_5,joint_5,revolute,0.92436,0,0.53588,3.1416,-0.25867,3.1416,Link_4,0,1,0,0,0,-3,3,,,,,,,,
+Link_6,0.12280626304832,0.10284172733946,-0.0673247079199191,0,0,0,8.19341168850207,1.25065484857243,-0.370837141231599,-0.214061773475593,0.581725625286335,-0.544088624970368,1.03166251957125,0,0,0,0,0,0,package://a9/meshes/Link_6.STL,1,1,0,1,0,0,0,0,0,0,package://a9/meshes/Link_6.STL,,FANUC M-10iA8L - 6-1,Origin_joint_6,Axis_joint_6,joint_6,revolute,0.037697,-0.0006997,-0.064094,0.44032,0.19669,-0.83053,Link_5,0.49999,0,-0.86603,0,0,-3,3,,,,,,,,
+Link_7,0.0296425829147993,-6.47728578861279E-06,0.162281442392356,0,0,0,7.07143232149748,0.0103947066541271,8.00167786886164E-07,0.00131122028195651,0.0112643862205598,-2.38846633634112E-06,0.0065729384023248,0,0,0,0,0,0,package://a9/meshes/Link_7.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://a9/meshes/Link_7.STL,,META激光跟踪-2,Origin_joint_72,Axis_joint_7,joint_7,fixed,0,0,0,-2.6972,0.28661,-2.1066,Link_6,0,0,0,,,,,,,,,,,,

catkin_ws/src/a9/urdf/a9.urdf

@@ -0,0 +1,465 @@
+<?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>

catkin_ws/src/lstrobot_moveit_config_0605/.setup_assistant

@@ -1,11 +1,11 @@
 moveit_setup_assistant_config:
   URDF:
-    package: mr12urdf20240605
-    relative_path: urdf/mr12urdf20240605.urdf
+    package: a9
+    relative_path: urdf/a9.urdf
     xacro_args: ""
   SRDF:
-    relative_path: config/mr12urdf20240605.srdf
+    relative_path: config/a9.srdf
   CONFIG:
     author_name: 123
-    author_email: 123@qq.com
-    generated_timestamp: 1717572194
+    author_email: 123@163.com
+    generated_timestamp: 1727315613

catkin_ws/src/lstrobot_moveit_config_0605/config/a9.srdf

@@ -0,0 +1,48 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!--This does not replace URDF, and is not an extension of URDF.
+    This is a format for representing semantic information about the robot structure.
+    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
+-->
+<robot name="a9">
+    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
+    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
+    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
+    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
+    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
+    <group name="manipulator">
+        <chain base_link="base_link" tip_link="end_link_now"/>
+    </group>
+    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
+    <group_state name="zero" group="manipulator">
+        <joint name="joint_1" value="0"/>
+        <joint name="joint_2" value="0"/>
+        <joint name="joint_3" value="0"/>
+        <joint name="joint_4" value="0"/>
+        <joint name="joint_5" value="0"/>
+        <joint name="joint_6" value="0"/>
+    </group_state>
+    <group_state name="home" group="manipulator">
+        <joint name="joint_1" value="0"/>
+        <joint name="joint_2" value="-1.1311"/>
+        <joint name="joint_3" value="1.459"/>
+        <joint name="joint_4" value="0"/>
+        <joint name="joint_5" value="2.8033"/>
+        <joint name="joint_6" value="2.6393"/>
+    </group_state>
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+    <disable_collisions link1="Link_1" link2="Link_2" reason="Adjacent"/>
+    <disable_collisions link1="Link_1" link2="Link_3" reason="Never"/>
+    <disable_collisions link1="Link_1" link2="base_link" reason="Adjacent"/>
+    <disable_collisions link1="Link_2" link2="Link_3" reason="Adjacent"/>
+    <disable_collisions link1="Link_2" link2="Link_5" reason="Never"/>
+    <disable_collisions link1="Link_2" link2="Link_6" reason="Never"/>
+    <disable_collisions link1="Link_3" link2="Link_4" reason="Adjacent"/>
+    <disable_collisions link1="Link_3" link2="Link_5" reason="Never"/>
+    <disable_collisions link1="Link_3" link2="Link_6" reason="Never"/>
+    <disable_collisions link1="Link_3" link2="Link_7" reason="Never"/>
+    <disable_collisions link1="Link_4" link2="Link_5" reason="Adjacent"/>
+    <disable_collisions link1="Link_4" link2="Link_6" reason="Never"/>
+    <disable_collisions link1="Link_5" link2="Link_6" reason="Adjacent"/>
+    <disable_collisions link1="Link_5" link2="Link_7" reason="Never"/>
+    <disable_collisions link1="Link_6" link2="Link_7" reason="Adjacent"/>
+</robot>

catkin_ws/src/lstrobot_moveit_config_0605/config/fake_controllers.yaml

@@ -2,12 +2,12 @@ controller_list:
   - name: fake_manipulator_controller
     type: $(arg fake_execution_type)
     joints:
-      - joint1
-      - joint2
-      - joint3
-      - joint4
-      - joint5
-      - joint6
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
 initial:  # Define initial robot poses per group
   - group: manipulator
-    pose: home
+    pose: zero

catkin_ws/src/lstrobot_moveit_config_0605/config/gazebo_a9.urdf

@@ -0,0 +1,296 @@
+<?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="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" upper="3" 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" upper="3" 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" upper="3" 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" upper="3" 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" upper="3" 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" upper="3" 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>
+    <transmission name="trans_joint_1">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_1">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_1_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint_2">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_2">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_2_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint_3">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_3">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_3_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint_4">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_4">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_4_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint_5">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_5">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_5_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <transmission name="trans_joint_6">
+        <type>transmission_interface/SimpleTransmission</type>
+        <joint name="joint_6">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+        </joint>
+        <actuator name="joint_6_motor">
+            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+            <mechanicalReduction>1</mechanicalReduction>
+        </actuator>
+    </transmission>
+    <gazebo>
+        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+            <robotNamespace>/</robotNamespace>
+        </plugin>
+    </gazebo>
+</robot>
+

catkin_ws/src/lstrobot_moveit_config_0605/config/gazebo_mr12urdf20240605.urdf

@@ -1,307 +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="mr12urdf20240605">
-    <link name="base_link">
-        <!-- <inertial>
-      <origin
-        xyz="-0.0389794721428083 3.18209550907445E-05 0.114464775769165"
-        rpy="0 0 0" />
-      <mass
-        value="27.8470958329326" />
-      <inertia
-        ixx="0.333719655951858"
-        ixy="-0.000162752951771119"
-        ixz="-0.0150515703369821"
-        iyy="0.572256719027697"
-        iyz="6.64144679179418E-05"
-        izz="0.74716855955823" />
-    </inertial> -->
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/base_link.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="0 1 1 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/base_link.STL" />
-            </geometry>
-        </collision>
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
-        </inertial>
-    </link>
-    <link name="Link1">
-        <inertial>
-            <origin xyz="0.0015836 0.02481 -0.16429" rpy="0 0 0" />
-            <mass value="28.856" />
-            <inertia ixx="0.80293" ixy="0.2052" ixz="-0.085925" iyy="0.84748" iyz="-0.048338" izz="0.87668" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link1.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="1 1 1 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link1.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint1" type="revolute">
-        <origin xyz="0 0 0.55" rpy="0 0 0" />
-        <parent link="base_link" />
-        <child link="Link1" />
-        <axis xyz="0 0 1" />
-        <limit lower="-2.878" upper="2.878" effort="150" velocity="3.541" />
-    </joint>
-    <link name="Link2">
-        <inertial>
-            <origin xyz="0.318649688799688 -0.000235571433145232 0.186136694407231" rpy="0 0 0" />
-            <mass value="7.80684304255034" />
-            <inertia ixx="0.0281236828911867" ixy="0.000281518665603689" ixz="-0.00939923269411962" iyy="0.185870193003645" iyz="1.16807601383937E-05" izz="0.200135929390865" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link2.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="1 1 1 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link2.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint2" type="revolute">
-        <origin xyz="0.14974 0.035343 0" rpy="1.5708 -1.5708 0" />
-        <parent link="Link1" />
-        <child link="Link2" />
-        <axis xyz="0 0 1" />
-        <limit lower="-2.529" upper="1.396" effort="150" velocity="3.541" />
-    </joint>
-    <link name="Link3">
-        <inertial>
-            <origin xyz="0.108553296371736 -0.0970725190395638 0.00155584136936515" rpy="0 0 0" />
-            <mass value="13.1020677700115" />
-            <inertia ixx="0.0894771695987357" ixy="0.00373169975921231" ixz="0.00305911419576535" iyy="0.0755527499176048" iyz="0.00497831742143842" izz="0.0627128517779011" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link3.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="1 1 1 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link3.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint3" type="revolute">
-        <origin xyz="0.76075 0 0.033754" rpy="0 0 0" />
-        <parent link="Link2" />
-        <child link="Link3" />
-        <axis xyz="0 0 1" />
-        <limit lower="-1.308" upper="2.529" effort="150" velocity="3.733" />
-    </joint>
-    <link name="Link4">
-        <inertial>
-            <origin xyz="0.0144892098854688 -0.000636132360795127 -0.376459479520837" rpy="0 0 0" />
-            <mass value="8.67998704574409" />
-            <inertia ixx="0.466226625992446" ixy="0.000500906822306766" ixz="-0.015294045664646" iyy="0.451075373457536" iyz="0.000649456671583942" izz="0.036563182959402" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link4.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="0 1 1 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link4.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint4" type="revolute">
-        <origin xyz="0.19783 -1.0829 0" rpy="1.5708 0 0" />
-        <parent link="Link3" />
-        <child link="Link4" />
-        <axis xyz="0 0 1" />
-        <limit lower="-3.314" upper="3.314" effort="88.5" velocity="6.838" />
-    </joint>
-    <link name="Link5">
-        <inertial>
-            <origin xyz="-6.1697E-06 -0.048434 -0.0024999" rpy="0 0 0" />
-            <mass value="1.2351" />
-            <inertia ixx="0.0041541" ixy="-7.1529E-06" ixz="-1.1135E-05" iyy="0.0031617" iyz="0.00015352" izz="0.0035434" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link5.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="0.79216 0.81961 0.93333 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link5.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint5" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 1.5708 0" />
-        <parent link="Link4" />
-        <child link="Link5" />
-        <axis xyz="0 0 1" />
-        <limit lower="-3.663" upper="0.75" effort="45.52" velocity="4.815" />
-    </joint>
-    <link name="Link6">
-        <inertial>
-            <origin xyz="0.048649 -0.00090428 0.27553" rpy="0 0 0" />
-            <mass value="1.8877" />
-            <inertia ixx="0.0086566" ixy="0.00012203" ixz="-0.00084157" iyy="0.0053122" iyz="0.00012437" izz="0.0064842" />
-        </inertial>
-        <visual>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link6.STL" />
-            </geometry>
-            <material name="">
-                <color rgba="0.75294 0.75294 0.75294 1" />
-            </material>
-        </visual>
-        <collision>
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <geometry>
-                <mesh filename="package://mr12urdf20240605/meshes/Link6.STL" />
-            </geometry>
-        </collision>
-    </link>
-    <joint name="joint6" type="revolute">
-        <origin xyz="0 0 0" rpy="1.5708 0 0" />
-        <parent link="Link5" />
-        <child link="Link6" />
-        <axis xyz="0 0 1" />
-        <limit lower="-3.838" upper="3.838" effort="32.64" velocity="23.655" />
-    </joint>
-    <joint name="joint_flp" type="fixed">
-        <parent link="Link6" />
-        <child link="link_flp" />
-        <!-- 法兰盘 -->
-        <origin rpy=" 0 0 3.1415926" xyz="0  0  0.100" />
-    </joint>
-    <link name="link_flp">
-        <collision>
-            <geometry>
-                <box size="0.0001 0.0001 0.0001" />
-            </geometry>
-            <origin rpy="0 0 0" xyz="0 0 0" />
-        </collision>
-        <inertial>
-            <mass value="0.1" />
-            <origin xyz="0 0 0" rpy="0 0 0" />
-            <inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
-        </inertial>
-    </link>
-    <joint name="joint_end" type="fixed">
-        <parent link="link_flp" />
-        <child link="link_end" />
-        <origin rpy="0.0 0.325888 3.1415926" xyz="-0.06371  0.004214  0.7387658" />
-    </joint>
-    <link name="link_end" />
-    <transmission name="trans_joint1">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint1">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint1_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <transmission name="trans_joint2">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint2">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint2_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <transmission name="trans_joint3">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint3">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint3_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <transmission name="trans_joint4">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint4">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint4_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <transmission name="trans_joint5">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint5">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint5_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <transmission name="trans_joint6">
-        <type>transmission_interface/SimpleTransmission</type>
-        <joint name="joint6">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-        </joint>
-        <actuator name="joint6_motor">
-            <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-            <mechanicalReduction>1</mechanicalReduction>
-        </actuator>
-    </transmission>
-    <gazebo>
-        <plugin name="gazebo_ros_control">
-            <robotNamespace>/</robotNamespace>
-        </plugin>
-    </gazebo>
-</robot>
-

catkin_ws/src/lstrobot_moveit_config_0605/config/joint_limits.yaml

@@ -8,33 +8,33 @@ default_acceleration_scaling_factor: 0.1
 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
-  joint1:
-    has_velocity_limits: true
-    max_velocity: 3.541
+  joint_1:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint2:
-    has_velocity_limits: true
-    max_velocity: 3.541
+  joint_2:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint3:
-    has_velocity_limits: true
-    max_velocity: 3.733
+  joint_3:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint4:
-    has_velocity_limits: true
-    max_velocity: 6.838
+  joint_4:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint5:
-    has_velocity_limits: true
-    max_velocity: 4.815
+  joint_5:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0
-  joint6:
-    has_velocity_limits: true
-    max_velocity: 23.655
+  joint_6:
+    has_velocity_limits: false
+    max_velocity: 0
     has_acceleration_limits: false
     max_acceleration: 0

catkin_ws/src/lstrobot_moveit_config_0605/config/kinematics.yaml

@@ -4,4 +4,4 @@ manipulator:
   kinematics_solver_timeout: 0.005
   goal_joint_tolerance: 0.0001
   goal_position_tolerance: 0.0001
-  goal_orientation_tolerance: 0.001
+  goal_orientation_tolerance: 0.001

catkin_ws/src/lstrobot_moveit_config_0605/config/mr12urdf20240605.srdf

@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<!--This does not replace URDF, and is not an extension of URDF.
-    This is a format for representing semantic information about the robot structure.
-    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
--->
-<robot name="mr12urdf20240605">
-    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
-    <group name="manipulator">
-        <chain base_link="base_link" tip_link="link_end_now"/>
-    </group>
-    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="home" group="manipulator">
-        <joint name="joint1" value="0"/>
-        <joint name="joint2" value="0"/>
-        <joint name="joint3" value="0"/>
-        <joint name="joint4" value="0"/>
-        <joint name="joint5" value="0"/>
-        <joint name="joint6" value="0"/>
-    </group_state>
-    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <disable_collisions link1="Link1" link2="Link2" reason="Adjacent"/>
-    <disable_collisions link1="Link1" link2="base_link" reason="Adjacent"/>
-    <disable_collisions link1="Link1" link2="link_flp" reason="Never"/>
-    <disable_collisions link1="Link2" link2="Link3" reason="Adjacent"/>
-    <disable_collisions link1="Link2" link2="Link5" reason="Never"/>
-    <disable_collisions link1="Link2" link2="link_flp" reason="Never"/>
-    <disable_collisions link1="Link3" link2="Link4" reason="Adjacent"/>
-    <disable_collisions link1="Link3" link2="Link5" reason="Never"/>
-    <disable_collisions link1="Link3" link2="link_flp" reason="Never"/>
-    <disable_collisions link1="Link4" link2="Link5" reason="Adjacent"/>
-    <disable_collisions link1="Link4" link2="link_flp" reason="Never"/>
-    <disable_collisions link1="Link5" link2="Link6" reason="Adjacent"/>
-    <disable_collisions link1="Link5" link2="link_flp" reason="Never"/>
-    <disable_collisions link1="Link6" link2="link_flp" reason="Adjacent"/>
-</robot>

catkin_ws/src/lstrobot_moveit_config_0605/config/ompl_planning.yaml

@@ -165,6 +165,7 @@ planner_configs:
     use_strict_queue_ordering: 0  # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
     find_approximate_solutions: 0  # track approximate solutions (1) or not (0). Default: 0
 manipulator:
+  default_planner_config: RRTConnect
   planner_configs:
     - AnytimePathShortening
     - SBL
@@ -193,5 +194,5 @@ manipulator:
     - AITstar
     - ABITstar
     - BITstar
-  projection_evaluator: joints(joint1,joint2)
-  longest_valid_segment_fraction: 0.001
+  projection_evaluator: joints(joint_1,joint_2)
+  longest_valid_segment_fraction: 0.005

catkin_ws/src/lstrobot_moveit_config_0605/config/sensors_3d.yaml

@@ -1,10 +1,2 @@
 sensors:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /pointcloud/output
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-
+  []

catkin_ws/src/lstrobot_moveit_config_0605/config/sensors_3d2.yaml

@@ -1,10 +0,0 @@
-sensors2:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /pointcloud/output2
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-

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/gazebo.launch

@@ -5,7 +5,7 @@
   <arg name="paused" default="false" doc="Start Gazebo paused"/>
   <arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
   <arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
-  <arg name="initial_joint_positions" default=" -J joint1 0 -J joint2 0 -J joint3 0 -J joint4 0 -J joint5 0 -J joint6 0" doc="Initial joint configuration of the robot"/>
+  <arg name="initial_joint_positions" default=" -J joint_1 0 -J joint_2 0 -J joint_3 0 -J joint_4 0 -J joint_5 0 -J joint_6 0" doc="Initial joint configuration of the robot"/>
 
   <!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch" pass_all_args="true">
@@ -13,7 +13,7 @@
   </include>
 
   <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" textfile="$(find lstrobot_moveit_config_0605)/config/gazebo_mr12urdf20240605.urdf" />
+  <param name="robot_description" textfile="$(find lstrobot_moveit_config_0605)/config/gazebo_a9.urdf" />
 
   <!-- Unpause the simulation after loading the robot model -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />

catkin_ws/src/lstrobot_moveit_config_0605/launch/move_group.launch

@@ -77,7 +77,7 @@
 
   <!-- Sensors Functionality -->
   <include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
-    <arg name="moveit_sensor_manager" value="mr12urdf20240605" />
+    <arg name="moveit_sensor_manager" value="a9" />
   </include>
 
   <!-- Start the actual move_group node/action server -->

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz

@@ -4,15 +4,10 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Global Options1
-        - /Grid1
-        - /MotionPlanning1
-        - /MotionPlanning1/Scene Geometry1
-        - /MotionPlanning1/Planned Path1
-        - /RobotModel1/Links1
-        - /Trajectory1/Links1
-      Splitter Ratio: 0.6529411673545837
-    Tree Height: 737
+        - /Axes1
+        - /TF1
+      Splitter Ratio: 0.5
+    Tree Height: 808
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -55,7 +50,7 @@ Visualization Manager:
       MoveIt_Allow_Replanning: false
       MoveIt_Allow_Sensor_Positioning: false
       MoveIt_Planning_Attempts: 10
-      MoveIt_Planning_Time: 25
+      MoveIt_Planning_Time: 5
       MoveIt_Use_Cartesian_Path: false
       MoveIt_Use_Constraint_Aware_IK: false
       MoveIt_Workspace:
@@ -77,59 +72,55 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link1:
+          Link_1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link2:
+          Link_2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link3:
+          Link_3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link4:
+          Link_4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link5:
+          Link_5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link6:
+          Link_6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_link:
+          Link_7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link_end:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          link_end_now:
+          base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          link_flp:
+            Value: true
+          end_link_now:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           world:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-        Loop Animation: false
+        Loop Animation: true
         Robot Alpha: 0.5
         Robot Color: 150; 50; 150
         Show Robot Collision: false
@@ -164,7 +155,7 @@ Visualization Manager:
         Scene Alpha: 1
         Scene Color: 50; 230; 50
         Scene Display Time: 0.009999999776482582
-        Show Scene Geometry: false
+        Show Scene Geometry: true
         Voxel Coloring: Z-Axis
         Voxel Rendering: Occupied Voxels
       Scene Robot:
@@ -175,54 +166,50 @@ Visualization Manager:
           Expand Link Details: false
           Expand Tree: false
           Link Tree Style: Links in Alphabetic Order
-          Link1:
+          Link_1:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link2:
+          Link_2:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link3:
+          Link_3:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link4:
+          Link_4:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link5:
+          Link_5:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          Link6:
+          Link_6:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          base_link:
+          Link_7:
             Alpha: 1
             Show Axes: false
             Show Trail: false
             Value: true
-          link_end:
-            Alpha: 1
-            Show Axes: false
-            Show Trail: false
-          link_end_now:
+          base_link:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-          link_flp:
+            Value: true
+          end_link_now:
             Alpha: 1
             Show Axes: false
             Show Trail: false
-            Value: true
           world:
             Alpha: 1
             Show Axes: false
@@ -232,7 +219,15 @@ Visualization Manager:
         Show Robot Visual: false
       Value: true
       Velocity_Scaling_Factor: 0.1
-    - Alpha: 0.5
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
       Enabled: true
@@ -242,54 +237,50 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        Link1:
+        Link_1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link2:
+        Link_2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link3:
+        Link_3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link4:
+        Link_4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link5:
+        Link_5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        Link6:
+        Link_6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        base_link:
+        Link_7:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        link_end:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        link_end_now:
+        base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        link_flp:
+          Value: true
+        end_link_now:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-          Value: true
         world:
           Alpha: 1
           Show Axes: false
@@ -300,81 +291,42 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
-    - Alpha: 1
-      Autocompute Intensity Bounds: true
-      Autocompute Value Bounds:
-        Max Value: 10
-        Min Value: -10
-        Value: true
-      Axis: Z
-      Channel Name: intensity
-      Class: rviz/PointCloud2
-      Color: 255; 255; 255
-      Color Transformer: ""
-      Decay Time: 0
-      Enabled: true
-      Invert Rainbow: false
-      Max Color: 255; 255; 255
-      Min Color: 0; 0; 0
-      Name: PointCloud2
-      Position Transformer: ""
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.0020000000949949026
-      Style: Flat Squares
-      Topic: ""
-      Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: true
     - Alpha: 1
       Class: rviz/Axes
       Enabled: true
-      Length: 0.029999999329447746
+      Length: 0.05000000074505806
       Name: Axes
-      Radius: 0.004000000189989805
-      Reference Frame: link_end_now
+      Radius: 0.009999999776482582
+      Reference Frame: Link_7
       Show Trail: false
       Value: true
-    - Class: moveit_rviz_plugin/Trajectory
-      Color Enabled: true
-      Enabled: false
-      Interrupt Display: false
-      Links:
-        All Links Enabled: false
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-      Loop Animation: false
-      Name: Trajectory
-      Robot Alpha: 1
-      Robot Color: 115; 210; 22
-      Robot Description: robot_description
-      Show Robot Collision: false
-      Show Robot Visual: false
-      Show Trail: true
-      State Display Time: 3x
-      Trail Step Size: 1
-      Trajectory Topic: move_group/display_planned_path
-      Use Sim Time: false
-      Value: false
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: /rviz_visual_tools
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
     - Class: rviz/TF
-      Enabled: false
+      Enabled: true
       Filter (blacklist): ""
       Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
-        All Enabled: true
+        All Enabled: false
+        Link_1:
+          Value: true
+        Link_2:
+          Value: true
+        Link_3:
+          Value: true
+        Link_4:
+          Value: true
+        Link_5:
+          Value: false
+        Link_6:
+          Value: false
+        Link_7:
+          Value: true
+        base_link:
+          Value: true
+        end_link_now:
+          Value: true
+        world:
+          Value: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -382,9 +334,19 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        {}
+        world:
+          base_link:
+            Link_1:
+              Link_2:
+                Link_3:
+                  Link_4:
+                    Link_5:
+                      Link_6:
+                        Link_7:
+                          end_link_now:
+                            {}
       Update Interval: 0
-      Value: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -401,7 +363,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 8.636017799377441
+      Distance: 1.121934413909912
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -409,17 +371,17 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: 0.3255424499511719
-        Y: -0.4020789861679077
-        Z: 0.04474440962076187
+        X: -0.9589470028877258
+        Y: -0.3118433952331543
+        Z: -1.5461976528167725
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.3152022063732147
+      Pitch: 0.6150007843971252
       Target Frame: base_link
-      Yaw: 3.320636510848999
+      Yaw: 1.7799382209777832
     Saved: ~
 Window Geometry:
   Displays:
@@ -433,9 +395,7 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd000000010000000000000156000003b9fc0200000009fb000000100044006900730070006c006100790073010000003d00000372000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000003b5000000410000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000210000001e60000017d00fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007200000003b500000041000000160000001600000624000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Trajectory - Trajectory Slider:
-    collapsed: false
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1920

catkin_ws/src/lstrobot_moveit_config_0605/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml

@@ -7,7 +7,7 @@
   <param name="request_adapters" value="$(arg planning_adapters)" />
 
   <!-- Define default planner (for all groups) -->
-  <param name="default_planner_config" value="RRTConnect" />
+  <param name="default_planner_config" value="PTP" />
 
   <!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
   <param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction

catkin_ws/src/lstrobot_moveit_config_0605/launch/planning_context.launch

@@ -6,10 +6,10 @@
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find mr12urdf20240605)/urdf/mr12urdf20240605.urdf"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find a9)/urdf/a9.urdf"/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find lstrobot_moveit_config_0605)/config/mr12urdf20240605.srdf" />
+  <param name="$(arg robot_description)_semantic" textfile="$(find lstrobot_moveit_config_0605)/config/a9.srdf" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">

catkin_ws/src/lstrobot_moveit_config_0605/launch/sensor_manager.launch.xml

@@ -4,18 +4,14 @@
 
   <!-- Params for 3D sensors config -->
   <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/sensors_3d.yaml" />
-  <!-- <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/sensors_3d2.yaml" /> -->
+
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.004" />
-  <param name="max_range" type="double" value="5.0" /> 
+  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="max_range" type="double" value="5.0" />
 
-  
-  
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
-  <arg name="moveit_sensor_manager" default="mr12urdf20240605" />
+  <arg name="moveit_sensor_manager" default="a9" />
   <include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
 
-
-
 </launch>

catkin_ws/src/lstrobot_moveit_config_0605/package.xml

@@ -3,16 +3,16 @@
   <name>lstrobot_moveit_config_0605</name>
   <version>0.3.0</version>
   <description>
-     An automatically generated package with all the configuration and launch files for using the mr12urdf20240605 with the MoveIt Motion Planning Framework
+     An automatically generated package with all the configuration and launch files for using the a9 with the MoveIt Motion Planning Framework
   </description>
-  <author email="123@qq.com">123</author>
-  <maintainer email="123@qq.com">123</maintainer>
+  <author email="123@163.com">123</author>
+  <maintainer email="123@163.com">123</maintainer>
 
   <license>BSD</license>
 
   <url type="website">http://moveit.ros.org/</url>
-  <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
-  <url type="repository">https://github.com/ros-planning/moveit</url>
+  <url type="bugtracker">https://github.com/moveit/moveit/issues</url>
+  <url type="repository">https://github.com/moveit/moveit</url>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -35,7 +35,7 @@
   <!-- <run_depend>gazebo_ros_control</run_depend> -->
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
-  <run_depend>mr12urdf20240605</run_depend>
+  <run_depend>a9</run_depend>
 
 
 </package>

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py

@@ -67,12 +67,12 @@ def run():
     # 计算位姿
     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)
+            q1 = rpy2quaternion(0,0,90)
+            q2 = rpy2quaternion(0,0,90)
             result.append(("s", start_points[i], end_points[i], q1, q2))
         else:
-            q1 = rpy2quaternion(0,np.pi,0)
-            q2 = rpy2quaternion(0,np.pi,0)
+            q1 = rpy2quaternion(0,0,90)
+            q2 = rpy2quaternion(0,0,90)
             result.append(("s", start_points[i], end_points[i], q1, q2))
 
 

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py

@@ -64,56 +64,62 @@ class MoveIt_Control:
         self.go_home()
         self.home_pose=self.get_now_pose()
 
-        # 关节规划，输入6个关节角度（单位：弧度）
-    def move_j(self, joint_configuration=None,a=1,v=1):
-        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
-        if joint_configuration==None:
-            joint_configuration = [0, 0, 0, 0, 0, 0]
-        self.arm.set_max_acceleration_scaling_factor(a)
-        self.arm.set_max_velocity_scaling_factor(v)
-        self.arm.set_joint_value_target(joint_configuration)
-        rospy.loginfo("move_j:"+str(joint_configuration))
-        self.arm.go()
-        #rospy.sleep(1)
-
     
-    def plan_cartesian_path_LLL(self,point,points,trajectory,trajectory_with_type,a=1,v=1):
+    def plan_cartesian_path_LLL(self,point_s,point_e,waypoints,trajectory,trajectory_with_type,a=1,v=1):
         fraction = 0.0  # 路径规划覆盖率
-        maxtries = 100  # 最大尝试规划次数
+        maxtries = 50  # 最大尝试规划次数
         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 
+        pose = self.arm.get_current_pose(self.end_effector_link).pose
+        wpose = deepcopy(pose)
+        waypoint=[]
+        print(waypoint)
+        if waypoints:
+            wpose.position.x=waypoints[-1].position.x
+            wpose.position.y=waypoints[-1].position.y
+            wpose.position.z=waypoints[-1].position.z
+            wpose.orientation.x=waypoints[-1].orientation.x
+            wpose.orientation.y=waypoints[-1].orientation.y
+            wpose.orientation.z=waypoints[-1].orientation.z
+            wpose.orientation.w=waypoints[-1].orientation.w
+            waypoint.append(deepcopy(wpose))
+        # else:
+        #     wpose.position.x=self.home_pose[0]
+        #     wpose.position.y=self.home_pose[1]
+        #     wpose.position.z=self.home_pose[2]
+        #     wpose.orientation.x=self.home_pose[3]
+        #     wpose.orientation.y=self.home_pose[4]
+        #     wpose.orientation.z=self.home_pose[5]
+        #     wpose.orientation.w=self.home_pose[6]
+        #     waypoint.append(deepcopy(wpose))
+            #waypoints.append(deepcopy(wpose))
+
+        #wpose = deepcopy(pose)
+        wpose.position.x=point_s[0]
+        wpose.position.y=point_s[1]
+        wpose.position.z=point_s[2]
+        wpose.orientation.x=point_s[3]
+        wpose.orientation.y=point_s[4]
+        wpose.orientation.z=point_s[5]
+        wpose.orientation.w=point_s[6]
+        waypoint.append(deepcopy(wpose))
+        waypoints.append(deepcopy(wpose))
+
+        wpose.position.x=point_e[0]
+        wpose.position.y=point_e[1]
+        wpose.position.z=point_e[2]
+        wpose.orientation.x=point_e[3]
+        wpose.orientation.y=point_e[4]
+        wpose.orientation.z=point_e[5]
+        wpose.orientation.w=point_e[6]
+        waypoint.append(deepcopy(wpose))
+        waypoints.append(deepcopy(wpose))
+
         # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
         while fraction < 1.0 and attempts < maxtries:
             (plan, fraction) = self.arm.compute_cartesian_path(
-                waypoints,  # waypoint poses，路点列表
+                waypoint,  # waypoint poses，路点列表
                 0.001,  # eef_step，终端步进值
                 0.0,  # jump_threshold，跳跃阈值
                 True)  # avoid_collisions，避障规划
@@ -126,11 +132,10 @@ class MoveIt_Control:
             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)
+            moveit_server.arm.execute(trajj)
             trajectory_with_type.append(trajj_with_type)
         else:
-            rospy.loginfo(plan[3].val)
             rospy.loginfo(
                 "fraction=" + str(fraction) + " success after " + str(maxtries))   
             
@@ -140,7 +145,7 @@ class MoveIt_Control:
             # 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
+        return waypoints,trajectory,trajectory_with_type
     
 
     def get_now_pose(self):
@@ -266,154 +271,9 @@ class MoveIt_Control:
         
         return points,trajectory,trajectory_with_type,er
     
-    def create_path_constraints3(self,start_point,end_point):#创建路径约束
-        ocm=moveit_msgs.msg.OrientationConstraint()  
-        ocm_constraint=moveit_msgs.msg.Constraints()
-
-
-
-        
-                #计算起点指向终点的向量
-        vector = np.array([end_point[0]- start_point[0], end_point[1]- start_point[1], end_point[2]- start_point[2]])
-        height = np.linalg.norm(vector)+0.002
-        radius = 0.01
-
-
-        
-        # 位置约束
-        position_constraint = PositionConstraint()
-        position_constraint.header.frame_id = "base_link"
-        position_constraint.link_name = self.end_effector_link
-        position_constraint.target_point_offset = Vector3(0, 0, 0)
-        #构建 shape_msgs/SolidPrimitive 消息
-        bounding_volume = SolidPrimitive()
-        bounding_volume.type = SolidPrimitive.CYLINDER
-        # bounding_volume.dimensions = [0.003,1]
-        bounding_volume.dimensions = [height,radius]
-        #构建 geometry_msgs/Pose 消息,用于指定圆柱体在空间中的位置和姿态
-        pose = Pose()
-        pose.position.x = start_point[0] + vector[0] / 2
-        pose.position.y = start_point[1] + vector[1] / 2 
-        pose.position.z = start_point[2] + vector[2] / 2
-        
-        # 计算圆柱体的旋转矩阵
-        z_axis = np.array([0, 0, 1])
-        axis = np.cross(z_axis, vector)
-        angle = np.arccos(np.dot(z_axis, vector) / np.linalg.norm(vector))
-        q = tf.transformations.quaternion_about_axis(angle, axis)
-        pose.orientation.x = q[0]
-        pose.orientation.y = q[1]
-        pose.orientation.z = q[2]
-        pose.orientation.w = q[3]
-
-        position_constraint.constraint_region.primitives.append(bounding_volume)
-        position_constraint.constraint_region.primitive_poses.append(pose)
-        position_constraint.weight = 1.0
-
-        ocm_constraint.position_constraints.append(position_constraint)
-
-        ocm.link_name = "link_end_now"  #or whatever your end effector link is
-        ocm.header.frame_id = "link_end_now"  # or whatever your planning frame is
-        ocm.orientation.x=end_point[3]
-        ocm.orientation.y=end_point[4]
-        ocm.orientation.z=end_point[5]
-        ocm.orientation.w=end_point[6]
-#        ocm.orientation.w = 1.0
-
-        ocm.absolute_x_axis_tolerance = 0.001
-        ocm.absolute_y_axis_tolerance = 0.001
-        ocm.absolute_z_axis_tolerance = 0.001
-        ocm.weight = 1.0
-
-        
-        ocm_constraint.orientation_constraints.append(ocm)
-        return ocm_constraint
-
-    
-    def move_pl(self,point,points,trajectory,trajectory_with_type,a=1,v=1):#焊缝的规划  只规划当前的点到上一个点
-        
-        self.arm.set_max_acceleration_scaling_factor(a)
-        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"  
-            trajj_with_type.points[-1].type = "end"
-            points.append(point)
-            trajectory.append(trajj)
-            trajectory_with_type.append(trajj_with_type)
-        else:
-            rospy.loginfo("本次焊缝规划失败")
-            points.pop()#将本条焊缝的起点从规划中删除
-            trajectory.pop()
-            trajectory_with_type.pop()
-            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_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.01
 
-        constraints = Constraints()
-        
-        # 位置约束
-        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
-
-        constraints.position_constraints.append(position_constraint)
-
-        return constraints
-     
     #TODO go_home
     @decorator_time  
     def go_home(self,a=1,v=1):
@@ -446,8 +306,8 @@ class MoveIt_Control:
     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 = [],[],[]
+
+        way_points,trajectory,trajectory_with_type = [],[],[]
         for i in range(len(all_data)):
             rospy.loginfo("共读取到%d条焊缝，开始规划第%d条",len(all_data),i+1)
             start_point = all_data[i][0]
@@ -456,20 +316,15 @@ 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,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=speed_v)
+            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 gohome:
+        #if gohome:
             #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)
+            #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)
         rospy.loginfo("All of The trajectory Plan successfully")
@@ -568,24 +423,6 @@ def merge_trajectories_with_type(trajectory_with_type):
     
     return merged_trajectory_with_type
 
-def pub_trajectories(trajectory_with_type_merge):
-   
-    pub = rospy.Publisher("/joint_path", JointTrajectory_ex, queue_size=5)
-    count = 1
-    rate = rospy.Rate(1)
-    rospy.loginfo("正在发布轨迹信息..")
-    while not rospy.is_shutdown():
-        #轨迹接受完毕，关闭当前发布
-        sign_traj_accepted = str(rospy.get_param("sign_traj_accepted"))
-        if sign_traj_accepted == "1":
-            rospy.set_param("sign_traj_accepted",0)
-            break
-
-        pub.publish(trajectory_with_type_merge)
-        # rospy.loginfo("发布计数,count = %d",count)
-        # count += 1
-        rate.sleep()
-
 
 class py_msgs():
     fial=[]
@@ -647,39 +484,9 @@ 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
@@ -697,7 +504,7 @@ if __name__ =="__main__":
     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)
+    # moveit_server.go_home()
+    # moveit_server.arm.execute(trajectory_merge)
     
     rospy.set_param("sign_control",0)

catkin_ws/src/mr12urdf20240605/config/joint_names_mr12urdf20240605.yaml

@@ -1 +0,0 @@
-controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', ]

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.csv

@@ -1,8 +0,0 @@
-Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
-base_link,-0.0389794721428083,3.18209550907445E-05,0.114464775769165,0,0,0,27.8470958329326,0.333719655951858,-0.000162752951771119,-0.0150515703369821,0.572256719027697,6.64144679179418E-05,0.74716855955823,0,0,0,0,0,0,package://mr12urdf20240605/meshes/base_link.STL,0,1,1,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/base_link.STL,,base_link-1,坐标系word,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-Link1,0.0015836,0.02481,-0.16429,0,0,0,28.856,0.80293,0.2052,-0.085925,0.84748,-0.048338,0.87668,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link1.STL,1,1,1,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link1.STL,,link1rebuild-1,坐标系1,基准轴1,joint1,revolute,0,0,0.55,0,0,0,base_link,0,0,1,150,3.541,-2.878,2.878,,,,,,,,
-Link2,0.318649688799688,-0.000235571433145232,0.186136694407231,0,0,0,7.80684304255034,0.0281236828911867,0.000281518665603689,-0.00939923269411962,0.185870193003645,1.16807601383937E-05,0.200135929390865,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link2.STL,1,1,1,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link2.STL,,link2-1,坐标系2,基准轴2,joint2,revolute,0.14974,0.035343,0,1.5708,-1.5708,0,Link1,0,0,-1,150,3.541,-2.529,1.396,,,,,,,,
-Link3,0.108553296371736,-0.0970725190395638,0.00155584136936515,0,0,0,13.1020677700115,0.0894771695987357,0.00373169975921231,0.00305911419576535,0.0755527499176048,0.00497831742143842,0.0627128517779011,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link3.STL,1,1,1,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link3.STL,,link3rebuild2-1,坐标系3,基准轴3,joint3,revolute,0.76075,0,0.033754,0,0,0,Link2,0,0,-1,150,3.733,-1.308,2.529,,,,,,,,
-Link4,0.0144892098854688,-0.000636132360795127,-0.376459479520837,0,0,0,8.67998704574409,0.466226625992446,0.000500906822306766,-0.015294045664646,0.451075373457536,0.000649456671583942,0.036563182959402,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link4.STL,0,1,1,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link4.STL,,link4-1,坐标系4,基准轴4,joint4,revolute,0.19783,-1.0829,0,1.5708,0,0,Link3,0,0,1,88.5,6.838,-3.314,3.314,,,,,,,,
-Link5,-6.1697E-06,-0.048434,-0.0024999,0,0,0,1.2351,0.0041541,-7.1529E-06,-1.1135E-05,0.0031617,0.00015352,0.0035434,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link5.STL,0.79216,0.81961,0.93333,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link5.STL,,link5-1,坐标系5,基准轴5,joint5,revolute,0,0,0,1.5708,1.5708,0,Link4,0,0,-1,45.52,4.815,-3.663,0.75,,,,,,,,
-Link6,0.048649,-0.00090428,0.27553,0,0,0,1.8877,0.0086566,0.00012203,-0.00084157,0.0053122,0.00012437,0.0064842,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link6.STL,0.75294,0.75294,0.75294,1,0,0,0,0,0,0,package://mr12urdf20240605/meshes/Link6.STL,,hanqiangrebuild-2;0517A-1;0516h-1,坐标系6,基准轴6,joint6,revolute,0,0,0,1.5708,0,0,Link5,0,0,-1,32.64,23.655,-3.838,3.838,,,,,,,,

catkin_ws/src/mr12urdf20240605/urdf/mr12urdf20240605.urdf

@@ -1,449 +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="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>
-      <origin
-        xyz="-0.0389794721428083 3.18209550907445E-05 0.114464775769165"
-        rpy="0 0 0" />
-      <mass
-        value="27.8470958329326" />
-      <inertia
-        ixx="0.333719655951858"
-        ixy="-0.000162752951771119"
-        ixz="-0.0150515703369821"
-        iyy="0.572256719027697"
-        iyz="6.64144679179418E-05"
-        izz="0.74716855955823" />
-    </inertial> -->
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link
-    name="Link1">
-    <inertial>
-      <origin
-        xyz="0.0015836 0.02481 -0.16429"
-        rpy="0 0 0" />
-      <mass
-        value="28.856" />
-      <inertia
-        ixx="0.80293"
-        ixy="0.2052"
-        ixz="-0.085925"
-        iyy="0.84748"
-        iyz="-0.048338"
-        izz="0.87668" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link1.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link1.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint1"
-    type="revolute">
-    <origin
-      xyz="0 0 0.55"
-      rpy="0 0 0" />
-    <parent
-      link="base_link" />
-    <child
-      link="Link1" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.878"
-      upper="2.878"
-      effort="150"
-      velocity="3.541" />
-  </joint>
-  <link
-    name="Link2">
-    <inertial>
-      <origin
-        xyz="0.318649688799688 -0.000235571433145232 0.186136694407231"
-        rpy="0 0 0" />
-      <mass
-        value="7.80684304255034" />
-      <inertia
-        ixx="0.0281236828911867"
-        ixy="0.000281518665603689"
-        ixz="-0.00939923269411962"
-        iyy="0.185870193003645"
-        iyz="1.16807601383937E-05"
-        izz="0.200135929390865" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link2.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link2.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint2"
-    type="revolute">
-    <origin
-      xyz="0.14974 0.035343 0"
-      rpy="1.5708 -1.5708 0" />
-    <parent
-      link="Link1" />
-    <child
-      link="Link2" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-2.529"
-      upper="1.396"
-      effort="150"
-      velocity="3.541" />
-  </joint>
-  <link
-    name="Link3">
-    <inertial>
-      <origin
-        xyz="0.108553296371736 -0.0970725190395638 0.00155584136936515"
-        rpy="0 0 0" />
-      <mass
-        value="13.1020677700115" />
-      <inertia
-        ixx="0.0894771695987357"
-        ixy="0.00373169975921231"
-        ixz="0.00305911419576535"
-        iyy="0.0755527499176048"
-        iyz="0.00497831742143842"
-        izz="0.0627128517779011" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link3.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="1 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link3.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint3"
-    type="revolute">
-    <origin
-      xyz="0.76075 0 0.033754"
-      rpy="0 0 0" />
-    <parent
-      link="Link2" />
-    <child
-      link="Link3" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-1.308"
-      upper="2.529"
-      effort="150"
-      velocity="3.733" />
-  </joint>
-  <link
-    name="Link4">
-    <inertial>
-      <origin
-        xyz="0.0144892098854688 -0.000636132360795127 -0.376459479520837"
-        rpy="0 0 0" />
-      <mass
-        value="8.67998704574409" />
-      <inertia
-        ixx="0.466226625992446"
-        ixy="0.000500906822306766"
-        ixz="-0.015294045664646"
-        iyy="0.451075373457536"
-        iyz="0.000649456671583942"
-        izz="0.036563182959402" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link4.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0 1 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link4.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint4"
-    type="revolute">
-    <origin
-      xyz="0.19783 -1.0829 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="Link3" />
-    <child
-      link="Link4" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.3"
-      upper="3.3"
-      effort="88.5"
-      velocity="6.838" />
-  </joint>
-  <link
-    name="Link5">
-    <inertial>
-      <origin
-        xyz="-6.1697E-06 -0.048434 -0.0024999"
-        rpy="0 0 0" />
-      <mass
-        value="1.2351" />
-      <inertia
-        ixx="0.0041541"
-        ixy="-7.1529E-06"
-        ixz="-1.1135E-05"
-        iyy="0.0031617"
-        iyz="0.00015352"
-        izz="0.0035434" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link5.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.79216 0.81961 0.93333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link5.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint5"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="1.5708 1.5708 0" />
-    <parent
-      link="Link4" />
-    <child
-      link="Link5" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.66"
-      upper="0.75"
-      effort="45.52"
-      velocity="4.815" />
-  </joint>
-  <link
-    name="Link6">
-    <inertial>
-      <origin
-        xyz="0.048649 -0.00090428 0.27553"
-        rpy="0 0 0" />
-      <mass
-        value="1.8877" />
-      <inertia
-        ixx="0.0086566"
-        ixy="0.00012203"
-        ixz="-0.00084157"
-        iyy="0.0053122"
-        iyz="0.00012437"
-        izz="0.0064842" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link6.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://mr12urdf20240605/meshes/Link6.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint
-    name="joint6"
-    type="revolute">
-    <origin
-      xyz="0 0 0"
-      rpy="1.5708 0 0" />
-    <parent
-      link="Link5" />
-    <child
-      link="Link6" />
-    <axis
-      xyz="0 0 1" />
-    <limit
-      lower="-3.8"
-      upper="3.8"
-      effort="32.64"
-      velocity="23.655" />
-  </joint>
-
-    <joint
-  name="joint_flp"
-  type="fixed">
-    <parent
-	link="Link6"/>
-    <child
-	link="link_flp"/>
-    <!-- 法兰盘 -->
-    <origin rpy=" 0 0 3.1415926" xyz="0  0  0.100"/>
-  </joint>
-
-  <link name="link_flp">
-    <collision>
-      <geometry>
-        <box size="0.0001 0.0001 0.0001"/>
-      </geometry>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-    </collision>
-  </link>
-
-  <joint
-  name="joint_end"
-  type="fixed">
-    <parent
-	link="link_flp"/>
-    <child
-	link="link_end"/>
-    <origin rpy="0.0 0.325888 3.1415926" xyz="-0.06371  0.004214  0.7387658"/>
-  </joint>
-
-  <link name="link_end">
-  </link>
-
-  <joint
-  name="joint_end2"
-  type="fixed">
-    <parent
-	link="link_end"/>
-    <child
-	link="link_end_now"/>
-    <origin rpy="0.0 0.0 0.0" xyz="0.0  0.0  0.01"/>
-  </joint>
-
-  <link name="link_end_now">
-  </link>
-
-</robot>

catkin_ws/需求.txt

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