[feat] add kinematics_suffix parameter

utiasSTARS · Oct 24, 2023 · a083c36 · a083c36
1 parent 03110c7
commit a083c36
Show file tree

Hide file tree

Showing 68 changed files with 719 additions and 128 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,4 +1,5 @@
 .vscode/
 .idea/
 .git_template
-**/old
+**/old
+xarm_description/config/kinematics/user
diff --git a/ReadMe.md b/ReadMe.md
@@ -107,6 +107,7 @@ For **kinetic** users, please use the [kinetic branch](https://github.com/xArm-D
       - xarm7_gripper_moveit_config
       - xarm7_vacuum_moveit_config
       - lite6_moveit_config
+   * (2023-10-12) Added the generation and use of joint kinematics parameter files (only supports __uf_robot_moveit_config__, see the description of the general parameter `kinematics_suffix` in [uf_robot_moveit_config](uf_robot_moveit_config/Readme.md))
 
 # 3. Preparations before using this package
 

diff --git a/ReadMe_cn.md b/ReadMe_cn.md
@@ -100,6 +100,7 @@
       - xarm7_gripper_moveit_config
       - xarm7_vacuum_moveit_config
       - lite6_moveit_config
+   * (2023-10-12) 新增关节kinematics参数文件的生成与使用 (仅支持 __uf_robot_moveit_config__，参见[uf_robot_moveit_config](uf_robot_moveit_config/Readme_cn.md)里的通用参数`kinematics_suffix`的说明)
 
 # 3. 准备工作
 

diff --git a/uf_robot_moveit_config/Readme.md b/uf_robot_moveit_config/Readme.md
@@ -297,6 +297,23 @@
     roslaunch uf_robot_moveit_config xarm7_moveit_fake.launch add_other_geometry:=true geometry_type:=mesh geometry_mesh_filename:=package://xarm_description/meshes/vacuum_gripper/xarm/visual/vacuum_gripper.stl geometry_mesh_tcp_xyz:='"0 0 0.126"'
     ```
   - __jnt_stat_pub_rate__: Publish frequency of joint_state_publisher, default is 10
+  - __kinematics_suffix__: Specify joint Kinematics parameter file suffix
+    - Generation of Kinematics parameter file: 
+      ```bash
+      cd src/xarm_ros/xarm_description/config/kinematics
+      python gen_kinematics_params.py {robot_ip} {kinematics_suffix}
+
+      # Note
+      # 1. robot_ip represents the IP of the robot arm. You need to connect to the robot arm to obtain the actual parameters.
+      # 2. kinematics_suffix represents the suffix of the generated parameter file. If successful, the configuration file will be generated in the xarm_description/config/kinematics/user directory. If kinematics_suffix is AAA, then the corresponding file name is as follows
+      #   xarm5: xarm_description/config/kinematics/user/xarm5_kinematics_AAA.yaml
+      #   xarm6: xarm_description/config/kinematics/user/xarm6_kinematics_AAA.yaml
+      #   xarm7: xarm_description/config/kinematics/user/xarm7_kinematics_AAA.yaml
+      #   lite6: xarm_description/config/kinematics/user/lite6_kinematics_AAA.yaml
+      #   uf850: xarm_description/config/kinematics/user/uf850_kinematics_AAA.yaml
+      ```
+    - Use of Kinematics parameter file: Specify this parameter when starting the launch file
+      - Note that before specifying this parameter, make sure that the corresponding configuration file exists. If it does not exist, you need to connect the robot arm through a script to generate it.
 
 - ### Special parameters
   - __hw_ns__: Namespace, xarm series defaults to __xarm__, others default to __ufactory__, only valid in gazebo/realmove startup scripts, the corresponding service name is <`hw_ns`>/<service_name>

diff --git a/uf_robot_moveit_config/Readme_cn.md b/uf_robot_moveit_config/Readme_cn.md
@@ -294,6 +294,23 @@
     roslaunch uf_robot_moveit_config xarm7_moveit_fake.launch add_other_geometry:=true geometry_type:=mesh geometry_mesh_filename:=package://xarm_description/meshes/vacuum_gripper/xarm/visual/vacuum_gripper.stl geometry_mesh_tcp_xyz:='"0 0 0.126"'
     ```
   - __jnt_stat_pub_rate__: joint_state_publisher的发布频率，默认为10
+  - __kinematics_suffix__: 指定关节Kinematics参数文件后缀
+    - 参数文件的生成: 
+      ```bash
+      cd src/xarm_ros/xarm_description/config/kinematics
+      python gen_kinematics_params.py {robot_ip} {kinematics_suffix}
+
+      # 注意
+      # 1. robot_ip表示机械臂IP，需要连接机械臂获取实际的参数
+      # 2. kinematics_suffix表示生成的参数文件的后缀，如果成功，会在xarm_description/config/kinematics/user目录下生成配置文件, 假如 kinematics_suffix 为 AAA, 那么对应的文件名如下
+      #   xarm5: xarm_description/config/kinematics/user/xarm5_kinematics_AAA.yaml
+      #   xarm6: xarm_description/config/kinematics/user/xarm6_kinematics_AAA.yaml
+      #   xarm7: xarm_description/config/kinematics/user/xarm7_kinematics_AAA.yaml
+      #   lite6: xarm_description/config/kinematics/user/lite6_kinematics_AAA.yaml
+      #   uf850: xarm_description/config/kinematics/user/uf850_kinematics_AAA.yaml
+      ```
+    - 参数文件的使用: 在启动launch文件时指定该参数
+      - 注意指定该参数之前要保证对应的配置文件存在，如果不存在，需要先通过脚本连接机械臂生成
 
 - ### 专用参数
   - __hw_ns__: 命名空间，xarm系列默认为 __xarm__, 其它默认为 __ufactory__, 仅在gazebo/realmove启动脚本有效, 对应的服务名就是 <`hw_ns`>/<service_name>

diff --git a/uf_robot_moveit_config/launch/__moveit_rviz_common.launch b/uf_robot_moveit_config/launch/__moveit_rviz_common.launch
@@ -38,6 +38,8 @@
   <arg name="attach_xyz" default="'0 0 0'" />
   <arg name="attach_rpy" default="'0 0 0'" />
   <arg name="robot_sn" default="" />
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
 
   <arg name="sim_gazebo" default="false"/>
   <arg name="add_gripper" default="false"/>
@@ -78,6 +80,8 @@
     <arg name="attach_xyz" value="$(arg attach_xyz)"/>
     <arg name="attach_rpy" value="$(arg attach_rpy)"/>
     <arg name="robot_sn" value="$(arg robot_sn)"/>
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
     <arg name="add_gripper" value="$(arg add_gripper)"/>
     <arg name="add_vacuum_gripper" value="$(arg add_vacuum_gripper)"/>

diff --git a/uf_robot_moveit_config/launch/__planning_context.launch b/uf_robot_moveit_config/launch/__planning_context.launch
@@ -13,6 +13,8 @@
   <arg name="robot_type" default="xarm" />
   <arg name="robot_name" default="xarm7" />
   <arg name="velocity_control" default="false"/>
+  <arg name="kinematics_suffix_1" default=""/>
+  <arg name="kinematics_suffix_2" default=""/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
@@ -60,6 +62,7 @@
       geometry_mesh_origin_rpy:=$(arg geometry_mesh_origin_rpy)
       geometry_mesh_tcp_xyz:=$(arg geometry_mesh_tcp_xyz)
       geometry_mesh_tcp_rpy:=$(arg geometry_mesh_tcp_rpy)
+      kinematics_suffix:=$(arg kinematics_suffix_1)
     "/>
 
   <param if="$(arg load_robot_description)" name="$(arg robot_description)_semantic" 

diff --git a/uf_robot_moveit_config/launch/__planning_context_dual.launch b/uf_robot_moveit_config/launch/__planning_context_dual.launch
@@ -13,6 +13,8 @@
   <arg name="robot_type" default="xarm" />
   <arg name="robot_name" default="xarm7" />
   <arg name="velocity_control" default="false"/>
+  <arg name="kinematics_suffix_1" default=""/>
+  <arg name="kinematics_suffix_2" default=""/>
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
@@ -42,6 +44,8 @@
       robot_sn_1:=$(arg robot_sn)
       robot_sn_2:=$(arg robot_sn)
       limited:=$(arg limited)      
+      kinematics_suffix_1:=$(arg kinematics_suffix_1)      
+      kinematics_suffix_2:=$(arg kinematics_suffix_2)      
       model1300_1:=$(arg model1300)
       model1300_2:=$(arg model1300)
       velocity_control:=$(arg velocity_control)

diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_fake.launch
@@ -32,6 +32,9 @@
   <!-- only use in xarm_planner -->
   <arg name="no_gui_plan" default="false" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <remap from="$(arg ext_ns)/source_list" to="/source_list" />
   <include file="$(find uf_robot_moveit_config)/launch/__moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[move_group/fake_controller_joint_states]" />
@@ -71,6 +74,9 @@
 
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
+
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
   </include>
 

diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_gazebo.launch
@@ -31,6 +31,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
   <arg name="robot_name" value="$(eval 'uf850' if arg('robot_type') == 'uf850' else arg('robot_type') + str(arg('robot_dof')))" />
 
@@ -82,6 +85,9 @@
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)" />
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
   <include file="$(find xarm_gazebo)/launch/_dual_robot_beside_table.launch">
@@ -114,6 +120,9 @@
     <arg name="geometry_mesh_tcp_xyz" value="$(arg geometry_mesh_tcp_xyz)"/>
     <arg name="geometry_mesh_tcp_rpy" value="$(arg geometry_mesh_tcp_rpy)"/>
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch b/uf_robot_moveit_config/launch/_dual_robot_moveit_realmove.launch
@@ -42,6 +42,9 @@
   <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <arg name="robot_dof" value="$(eval arg('dof') if arg('robot_type') == 'xarm' else 6)" />
   <arg name="robot_name" value="$(eval 'uf850' if arg('robot_type') == 'uf850' else arg('robot_type') + str(arg('robot_dof')))" />
 
@@ -148,6 +151,9 @@
     <arg name="geometry_mesh_origin_rpy" value="$(arg geometry_mesh_origin_rpy)"/>
     <arg name="geometry_mesh_tcp_xyz" value="$(arg geometry_mesh_tcp_xyz)"/>
     <arg name="geometry_mesh_tcp_rpy" value="$(arg geometry_mesh_tcp_rpy)"/>
+
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
   </include>
 

diff --git a/uf_robot_moveit_config/launch/_robot_moveit_fake.launch b/uf_robot_moveit_config/launch/_robot_moveit_fake.launch
@@ -4,6 +4,7 @@
   <arg name="robot_sn" default="" />
   <arg name="model1300" default="false" />
   <arg name="limited" default="true" />
+  <arg name="kinematics_suffix" default="" />
 
   <arg name="attach_to" default="world" />
   <arg name="attach_xyz" default="'0 0 0'" />
@@ -49,6 +50,7 @@
     <arg name="robot_sn" value="$(arg robot_sn)"/>
     <arg name="model1300" value="$(arg model1300)"/>
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix)"/>
 
     <arg name="attach_to" value="$(arg attach_to)"/>
     <arg name="attach_xyz" value="$(arg attach_xyz)"/>

diff --git a/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch b/uf_robot_moveit_config/launch/_robot_moveit_gazebo.launch
@@ -6,6 +6,7 @@
   <arg name="limited" default="true" />
   <arg name="hw_ns" default="xarm"/>
   <arg name="velocity_control" default="false"/>
+  <arg name="kinematics_suffix" default=""/>
 
   <arg name="attach_to" default="world" />
   <arg name="attach_xyz" default="'0 0 0'" />
@@ -53,6 +54,7 @@
     <arg name="model1300" value="$(arg model1300)"/>
     <arg name="limited" value="$(arg limited)"/>
     <arg name="velocity_control" value="$(arg velocity_control)" />
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix)" />
 
     <arg name="attach_to" value="$(arg attach_to)"/>
     <arg name="attach_xyz" value="$(arg attach_xyz)"/>
@@ -89,6 +91,7 @@
     <arg name="limited" value="$(arg limited)"/>
     <arg name="hw_ns" value="$(arg hw_ns)"/>
     <arg name="velocity_control" value="$(arg velocity_control)" />
+    <arg name="kinematics_suffix" value="$(arg kinematics_suffix)" />
 
     <arg name="attach_to" value="$(arg attach_to)"/>
     <arg name="attach_xyz" value="$(arg attach_xyz)"/>

diff --git a/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch b/uf_robot_moveit_config/launch/_robot_moveit_realmove.launch
@@ -10,6 +10,7 @@
   <arg name="velocity_control" default="false"/>
   <arg name="baud_checkset" default="true" />
 	<arg name="default_gripper_baud" default="2000000" />
+	<arg name="kinematics_suffix" default="" />
 
   <arg name="attach_to" default="world" />
   <arg name="attach_xyz" default="'0 0 0'" />
@@ -95,6 +96,7 @@
     <arg name="robot_sn" value="$(arg robot_sn)"/>
     <arg name="model1300" value="$(arg model1300)"/>
     <arg name="limited" value="$(arg limited)"/>
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix)"/>
 
     <arg name="attach_to" value="$(arg attach_to)"/>
     <arg name="attach_xyz" value="$(arg attach_xyz)"/>

diff --git a/uf_robot_moveit_config/launch/dual_lite6_moveit_fake.launch b/uf_robot_moveit_config/launch/dual_lite6_moveit_fake.launch
@@ -28,6 +28,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_fake.launch">
     <arg name="dof" value="6"/>
     <arg name="robot_type" value="lite"/>
@@ -59,6 +62,9 @@
     <arg name="ext_ns" value="$(arg ext_ns)" />
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
+
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
   </include>
 

diff --git a/uf_robot_moveit_config/launch/dual_lite6_moveit_gazebo.launch b/uf_robot_moveit_config/launch/dual_lite6_moveit_gazebo.launch
@@ -29,6 +29,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_gazebo.launch">
     <arg name="dof" value="6" />
     <arg name="robot_type" value="lite" />
@@ -62,6 +65,9 @@
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)" />
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_lite6_moveit_realmove.launch
@@ -40,6 +40,9 @@
   <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
     <arg name="dof" value="6" />
     <arg name="robot_type" value="lite" />
@@ -82,6 +85,9 @@
     <arg name="show_rviz" value="$(arg show_rviz)" />
     <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/dual_uf850_moveit_fake.launch b/uf_robot_moveit_config/launch/dual_uf850_moveit_fake.launch
@@ -28,6 +28,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_fake.launch">
     <arg name="dof" value="6"/>
     <arg name="robot_type" value="uf850"/>
@@ -59,6 +62,9 @@
     <arg name="ext_ns" value="$(arg ext_ns)" />
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
+
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
   </include>
 

diff --git a/uf_robot_moveit_config/launch/dual_uf850_moveit_gazebo.launch b/uf_robot_moveit_config/launch/dual_uf850_moveit_gazebo.launch
@@ -29,6 +29,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_gazebo.launch">
     <arg name="dof" value="6" />
     <arg name="robot_type" value="uf850" />
@@ -62,6 +65,9 @@
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)" />
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_uf850_moveit_realmove.launch
@@ -40,6 +40,9 @@
   <!-- no_gui_plan means that Rviz configuration is just for display purpose, no (Moveit) control panel loaded -->
   <arg name="no_gui_plan" default="false" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_realmove.launch">
     <arg name="dof" value="6" />
     <arg name="robot_type" value="uf850" />
@@ -82,6 +85,9 @@
     <arg name="show_rviz" value="$(arg show_rviz)" />
     <arg name="no_gui_plan" value="$(arg no_gui_plan)"/>
 
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
+
   </include>
 
 </launch>
diff --git a/uf_robot_moveit_config/launch/dual_xarm5_moveit_fake.launch b/uf_robot_moveit_config/launch/dual_xarm5_moveit_fake.launch
@@ -28,6 +28,9 @@
   <arg name="sensors_3d" default="false" />
   <arg name="jnt_stat_pub_rate" default="10" />
 
+  <arg name="kinematics_suffix_1" default="" />
+  <arg name="kinematics_suffix_2" default="" />
+
   <include file="$(find uf_robot_moveit_config)/launch/_dual_robot_moveit_fake.launch">
     <arg name="dof" value="5"/>
     <arg name="robot_type" value="xarm"/>
@@ -59,6 +62,9 @@
     <arg name="ext_ns" value="$(arg ext_ns)" />
     <arg name="sensors_3d" value="$(arg sensors_3d)" />
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
+
+    <arg name="kinematics_suffix_1" value="$(arg kinematics_suffix_1)"/>
+    <arg name="kinematics_suffix_2" value="$(arg kinematics_suffix_2)"/>
 
   </include>