From a083c3639944da6b040f051fd180bdc95eefd5ca Mon Sep 17 00:00:00 2001
From: Vinman <vinman.cub@gmail.com>
Date: Tue, 24 Oct 2023 10:09:01 +0800
Subject: [PATCH] [feat] add kinematics_suffix parameter

---
 .gitignore                                    |   3 +-
 ReadMe.md                                     |   1 +
 ReadMe_cn.md                                  |   1 +
 uf_robot_moveit_config/Readme.md              |  17 +++
 uf_robot_moveit_config/Readme_cn.md           |  17 +++
 .../launch/__moveit_rviz_common.launch        |   4 +
 .../launch/__planning_context.launch          |   3 +
 .../launch/__planning_context_dual.launch     |   4 +
 .../launch/_dual_robot_moveit_fake.launch     |   6 +
 .../launch/_dual_robot_moveit_gazebo.launch   |   9 ++
 .../launch/_dual_robot_moveit_realmove.launch |   6 +
 .../launch/_robot_moveit_fake.launch          |   2 +
 .../launch/_robot_moveit_gazebo.launch        |   3 +
 .../launch/_robot_moveit_realmove.launch      |   2 +
 .../launch/dual_lite6_moveit_fake.launch      |   6 +
 .../launch/dual_lite6_moveit_gazebo.launch    |   6 +
 .../launch/dual_lite6_moveit_realmove.launch  |   6 +
 .../launch/dual_uf850_moveit_fake.launch      |   6 +
 .../launch/dual_uf850_moveit_gazebo.launch    |   6 +
 .../launch/dual_uf850_moveit_realmove.launch  |   6 +
 .../launch/dual_xarm5_moveit_fake.launch      |   6 +
 .../launch/dual_xarm5_moveit_gazebo.launch    |   6 +
 .../launch/dual_xarm5_moveit_realmove.launch  |   6 +
 .../launch/dual_xarm6_moveit_fake.launch      |   6 +
 .../launch/dual_xarm6_moveit_gazebo.launch    |   6 +
 .../launch/dual_xarm6_moveit_realmove.launch  |   6 +
 .../launch/dual_xarm7_moveit_fake.launch      |   6 +
 .../launch/dual_xarm7_moveit_gazebo.launch    |   6 +
 .../launch/dual_xarm7_moveit_realmove.launch  |   6 +
 .../launch/lite6_moveit_fake.launch           |   2 +
 .../launch/lite6_moveit_gazebo.launch         |   2 +
 .../launch/lite6_moveit_realmove.launch       |   2 +
 .../launch/uf850_moveit_fake.launch           |   2 +
 .../launch/uf850_moveit_gazebo.launch         |   2 +
 .../launch/uf850_moveit_realmove.launch       |   2 +
 .../launch/xarm5_moveit_fake.launch           |   2 +
 .../launch/xarm5_moveit_gazebo.launch         |   2 +
 .../launch/xarm5_moveit_realmove.launch       |   2 +
 .../launch/xarm6_moveit_fake.launch           |   2 +
 .../launch/xarm6_moveit_gazebo.launch         |   2 +
 .../launch/xarm6_moveit_realmove.launch       |   2 +
 .../launch/xarm7_moveit_fake.launch           |   2 +
 .../launch/xarm7_moveit_gazebo.launch         |   2 +
 .../launch/xarm7_moveit_realmove.launch       |   2 +
 .../default/lite6_default_kinematics.yaml     |  43 +++++++
 .../default/uf850_default_kinematics.yaml     |  43 +++++++
 .../default/xarm5_default_kinematics.yaml     |  36 ++++++
 .../default/xarm6_default_kinematics.yaml     |  43 +++++++
 .../default/xarm7_default_kinematics.yaml     |  50 ++++++++
 .../kinematics/gen_kinematics_params.py       |  57 +++++++++
 .../link_inertial/gen_link_inertial_params.py |  43 +++++--
 .../link_inertial/xarm6_type12_HT_LDBR2.yaml  | 118 +++++++++---------
 .../urdf/common/common.link.xacro             |   4 +
 .../urdf/dual_xarm_device.urdf.xacro          |   7 +-
 xarm_description/urdf/lite6/lite6.urdf.xacro  |  27 ++--
 .../urdf/lite6/lite6_robot_macro.xacro        |  10 +-
 xarm_description/urdf/uf850/uf850.urdf.xacro  |  27 ++--
 .../urdf/uf850/uf850_robot_macro.xacro        |   7 +-
 xarm_description/urdf/xarm5/xarm5.urdf.xacro  |  20 ++-
 .../urdf/xarm5/xarm5_robot_macro.xacro        |   9 +-
 xarm_description/urdf/xarm6/xarm6.urdf.xacro  |  25 ++--
 .../urdf/xarm6/xarm6_robot_macro.xacro        |   9 +-
 xarm_description/urdf/xarm7/xarm7.urdf.xacro  |  26 ++--
 .../urdf/xarm7/xarm7_robot_macro.xacro        |   9 +-
 xarm_description/urdf/xarm_device.urdf.xacro  |   4 +-
 xarm_description/urdf/xarm_device_macro.xacro |  25 +++-
 .../launch/_dual_robot_beside_table.launch    |   5 +
 xarm_gazebo/launch/_robot_beside_table.launch |   2 +
 68 files changed, 719 insertions(+), 128 deletions(-)
 create mode 100644 xarm_description/config/kinematics/default/lite6_default_kinematics.yaml
 create mode 100644 xarm_description/config/kinematics/default/uf850_default_kinematics.yaml
 create mode 100644 xarm_description/config/kinematics/default/xarm5_default_kinematics.yaml
 create mode 100644 xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml
 create mode 100644 xarm_description/config/kinematics/default/xarm7_default_kinematics.yaml
 create mode 100644 xarm_description/config/kinematics/gen_kinematics_params.py

diff --git a/.gitignore b/.gitignore
index a2102b9b..cb16d7c4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
 .vscode/
 .idea/
 .git_template
-**/old
\ No newline at end of file
+**/old
+xarm_description/config/kinematics/user
\ No newline at end of file
diff --git a/ReadMe.md b/ReadMe.md
index 185deb12..7b9a66cd 100755
--- 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
index 8cca374e..901ca2c5 100755
--- 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
index 32609f86..0dced205 100644
--- 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
index 1d06a98a..e1872be3 100644
--- 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
index c4210fd4..ea87c864 100755
--- 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
index ce920d82..83d99c54 100755
--- 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
index 86fb6af7..1f10bdb7 100755
--- 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
index 847fdcac..2fc33993 100755
--- 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
index fcff8f8e..885211ef 100755
--- 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
index 3857a571..bb73f54d 100755
--- 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
index 4c68998a..a94de9d6 100755
--- 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
index d4dd7819..45dc4e9f 100755
--- 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
index d738f527..385cde2f 100755
--- 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
index 743b4a7a..1560423d 100755
--- 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
index 4c559726..cb471aff 100755
--- 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
index 4d39b10d..2beb4859 100755
--- 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
index 5209bd04..3b20e2fd 100755
--- 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
index 8e361cdc..69ddede9 100755
--- 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
index 5fe90fa5..5542cb23 100755
--- 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
index ca436ed5..2e4ee009 100755
--- 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>
 
diff --git a/uf_robot_moveit_config/launch/dual_xarm5_moveit_gazebo.launch b/uf_robot_moveit_config/launch/dual_xarm5_moveit_gazebo.launch
index 0221e683..37f46f51 100755
--- a/uf_robot_moveit_config/launch/dual_xarm5_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm5_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="5" />
     <arg name="robot_type" value="xarm" />
@@ -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_xarm5_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch
index a1ff4381..c496b867 100755
--- a/uf_robot_moveit_config/launch/dual_xarm5_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm5_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="5" />
     <arg name="robot_type" value="xarm" />
@@ -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_xarm6_moveit_fake.launch b/uf_robot_moveit_config/launch/dual_xarm6_moveit_fake.launch
index fac744b6..cf3bc822 100755
--- a/uf_robot_moveit_config/launch/dual_xarm6_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm6_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="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>
 
diff --git a/uf_robot_moveit_config/launch/dual_xarm6_moveit_gazebo.launch b/uf_robot_moveit_config/launch/dual_xarm6_moveit_gazebo.launch
index d4b7760d..a7aa0ade 100755
--- a/uf_robot_moveit_config/launch/dual_xarm6_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm6_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="xarm" />
@@ -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_xarm6_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch
index d03876f7..772447b6 100755
--- a/uf_robot_moveit_config/launch/dual_xarm6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm6_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="xarm" />
@@ -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_xarm7_moveit_fake.launch b/uf_robot_moveit_config/launch/dual_xarm7_moveit_fake.launch
index 2dbcae54..658e0a03 100755
--- a/uf_robot_moveit_config/launch/dual_xarm7_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm7_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="7"/>
     <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>
 
diff --git a/uf_robot_moveit_config/launch/dual_xarm7_moveit_gazebo.launch b/uf_robot_moveit_config/launch/dual_xarm7_moveit_gazebo.launch
index 8ab874d2..fcbc1ad1 100755
--- a/uf_robot_moveit_config/launch/dual_xarm7_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm7_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="7" />
     <arg name="robot_type" value="xarm" />
@@ -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_xarm7_moveit_realmove.launch b/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch
index e1bfb5e6..6fe9d75c 100755
--- a/uf_robot_moveit_config/launch/dual_xarm7_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/dual_xarm7_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="7" />
     <arg name="robot_type" value="xarm" />
@@ -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/lite6_moveit_fake.launch b/uf_robot_moveit_config/launch/lite6_moveit_fake.launch
index 8223d3dd..e074a42d 100755
--- a/uf_robot_moveit_config/launch/lite6_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/lite6_moveit_fake.launch
@@ -2,6 +2,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'" />
@@ -34,6 +35,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" 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/lite6_moveit_gazebo.launch b/uf_robot_moveit_config/launch/lite6_moveit_gazebo.launch
index db235ad9..383252b4 100755
--- a/uf_robot_moveit_config/launch/lite6_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/lite6_moveit_gazebo.launch
@@ -4,6 +4,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'" />
@@ -37,6 +38,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/lite6_moveit_realmove.launch b/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
index e7d762a5..38194328 100755
--- a/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/lite6_moveit_realmove.launch
@@ -8,6 +8,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'" />
@@ -51,6 +52,7 @@
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="baud_checkset" value="$(arg baud_checkset)" />
     <arg name="default_gripper_baud" value="$(arg default_gripper_baud)" />
+    <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/uf850_moveit_fake.launch b/uf_robot_moveit_config/launch/uf850_moveit_fake.launch
index e22ae6b1..36da0689 100755
--- a/uf_robot_moveit_config/launch/uf850_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/uf850_moveit_fake.launch
@@ -2,6 +2,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'" />
@@ -34,6 +35,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" 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/uf850_moveit_gazebo.launch b/uf_robot_moveit_config/launch/uf850_moveit_gazebo.launch
index 4ba38357..8ac8a602 100755
--- a/uf_robot_moveit_config/launch/uf850_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/uf850_moveit_gazebo.launch
@@ -4,6 +4,7 @@
   <arg name="limited" default="true" />
   <arg name="hw_ns" default="ufactory"/>
   <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'" />
@@ -37,6 +38,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/uf850_moveit_realmove.launch b/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
index fbaa5350..4c2b49ef 100755
--- a/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/uf850_moveit_realmove.launch
@@ -8,6 +8,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'" />
@@ -51,6 +52,7 @@
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="baud_checkset" value="$(arg baud_checkset)" />
     <arg name="default_gripper_baud" value="$(arg default_gripper_baud)" />
+    <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/xarm5_moveit_fake.launch b/uf_robot_moveit_config/launch/xarm5_moveit_fake.launch
index 2cead8e5..cedf65a3 100755
--- a/uf_robot_moveit_config/launch/xarm5_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/xarm5_moveit_fake.launch
@@ -2,6 +2,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'" />
@@ -34,6 +35,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" 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/xarm5_moveit_gazebo.launch b/uf_robot_moveit_config/launch/xarm5_moveit_gazebo.launch
index cee122d2..95604449 100755
--- a/uf_robot_moveit_config/launch/xarm5_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/xarm5_moveit_gazebo.launch
@@ -4,6 +4,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'" />
@@ -37,6 +38,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/xarm5_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
index a0954dbc..ca17ddde 100755
--- a/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm5_moveit_realmove.launch
@@ -8,6 +8,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'" />
@@ -51,6 +52,7 @@
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="baud_checkset" value="$(arg baud_checkset)" />
     <arg name="default_gripper_baud" value="$(arg default_gripper_baud)" />
+    <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/xarm6_moveit_fake.launch b/uf_robot_moveit_config/launch/xarm6_moveit_fake.launch
index fab54c8c..8c88ee76 100755
--- a/uf_robot_moveit_config/launch/xarm6_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/xarm6_moveit_fake.launch
@@ -2,6 +2,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'" />
@@ -34,6 +35,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" 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/xarm6_moveit_gazebo.launch b/uf_robot_moveit_config/launch/xarm6_moveit_gazebo.launch
index 4303c0b5..77fe3712 100755
--- a/uf_robot_moveit_config/launch/xarm6_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/xarm6_moveit_gazebo.launch
@@ -4,6 +4,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'" />
@@ -37,6 +38,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/xarm6_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
index add11160..9fa8e27d 100755
--- a/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm6_moveit_realmove.launch
@@ -8,6 +8,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'" />
@@ -51,6 +52,7 @@
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="baud_checkset" value="$(arg baud_checkset)" />
     <arg name="default_gripper_baud" value="$(arg default_gripper_baud)" />
+    <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/xarm7_moveit_fake.launch b/uf_robot_moveit_config/launch/xarm7_moveit_fake.launch
index 0875fd80..a8162058 100755
--- a/uf_robot_moveit_config/launch/xarm7_moveit_fake.launch
+++ b/uf_robot_moveit_config/launch/xarm7_moveit_fake.launch
@@ -2,6 +2,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'" />
@@ -34,6 +35,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" 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/xarm7_moveit_gazebo.launch b/uf_robot_moveit_config/launch/xarm7_moveit_gazebo.launch
index 187c9651..ed2f2e57 100755
--- a/uf_robot_moveit_config/launch/xarm7_moveit_gazebo.launch
+++ b/uf_robot_moveit_config/launch/xarm7_moveit_gazebo.launch
@@ -4,6 +4,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'" />
@@ -37,6 +38,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/xarm7_moveit_realmove.launch b/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
index d8642615..ab0613c7 100755
--- a/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
+++ b/uf_robot_moveit_config/launch/xarm7_moveit_realmove.launch
@@ -8,6 +8,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'" />
@@ -51,6 +52,7 @@
     <arg name="velocity_control" value="$(arg velocity_control)"/>
     <arg name="baud_checkset" value="$(arg baud_checkset)" />
     <arg name="default_gripper_baud" value="$(arg default_gripper_baud)" />
+    <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/xarm_description/config/kinematics/default/lite6_default_kinematics.yaml b/xarm_description/config/kinematics/default/lite6_default_kinematics.yaml
new file mode 100644
index 00000000..3e1d0e3b
--- /dev/null
+++ b/xarm_description/config/kinematics/default/lite6_default_kinematics.yaml
@@ -0,0 +1,43 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.2435
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: -1.5708
+    yaw: 3.1416
+  joint3:
+    x: 0.2002
+    y: 0
+    z: 0
+    roll: -3.1416
+    pitch: 0
+    yaw: 1.5708
+  joint4:
+    x: 0.087
+    y: -0.22761
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint6:
+    x: 0
+    y: 0.0625
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
\ No newline at end of file
diff --git a/xarm_description/config/kinematics/default/uf850_default_kinematics.yaml b/xarm_description/config/kinematics/default/uf850_default_kinematics.yaml
new file mode 100644
index 00000000..65f62a37
--- /dev/null
+++ b/xarm_description/config/kinematics/default/uf850_default_kinematics.yaml
@@ -0,0 +1,43 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.364
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: -1.5708
+    yaw: 0
+  joint3:
+    x: 0.39
+    y: 0
+    z: 0
+    roll: -3.1416
+    pitch: 0
+    yaw: -1.5708
+  joint4:
+    x: 0.15
+    y: 0.426
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint6:
+    x: 0
+    y: -0.09
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
\ No newline at end of file
diff --git a/xarm_description/config/kinematics/default/xarm5_default_kinematics.yaml b/xarm_description/config/kinematics/default/xarm5_default_kinematics.yaml
new file mode 100644
index 00000000..547e89f4
--- /dev/null
+++ b/xarm_description/config/kinematics/default/xarm5_default_kinematics.yaml
@@ -0,0 +1,36 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.267
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint3:
+    x: 0.0535
+    y: -0.2845
+    z: 0
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint4:
+    x: 0.0775
+    y: 0.3425
+    z: 0
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0.076
+    y: 0.097
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
\ No newline at end of file
diff --git a/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml b/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml
new file mode 100644
index 00000000..4047e93b
--- /dev/null
+++ b/xarm_description/config/kinematics/default/xarm6_default_kinematics.yaml
@@ -0,0 +1,43 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.267
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint3:
+    x: 0.0535
+    y: -0.2845
+    z: 0
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint4:
+    x: 0.0775
+    y: 0.3425
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint6:
+    x: 0.076
+    y: 0.097
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
\ No newline at end of file
diff --git a/xarm_description/config/kinematics/default/xarm7_default_kinematics.yaml b/xarm_description/config/kinematics/default/xarm7_default_kinematics.yaml
new file mode 100644
index 00000000..06e08d78
--- /dev/null
+++ b/xarm_description/config/kinematics/default/xarm7_default_kinematics.yaml
@@ -0,0 +1,50 @@
+kinematics:
+  joint1:
+    x: 0
+    y: 0
+    z: 0.267
+    roll: 0
+    pitch: 0
+    yaw: 0
+  joint2:
+    x: 0
+    y: 0
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
+  joint3:
+    x: 0
+    y: -0.293
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint4:
+    x: 0.0525
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint5:
+    x: 0.0775
+    y: -0.3425
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint6:
+    x: 0
+    y: 0
+    z: 0
+    roll: 1.5708
+    pitch: 0
+    yaw: 0
+  joint7:
+    x: 0.076
+    y: 0.097
+    z: 0
+    roll: -1.5708
+    pitch: 0
+    yaw: 0
\ No newline at end of file
diff --git a/xarm_description/config/kinematics/gen_kinematics_params.py b/xarm_description/config/kinematics/gen_kinematics_params.py
new file mode 100644
index 00000000..2fe844f1
--- /dev/null
+++ b/xarm_description/config/kinematics/gen_kinematics_params.py
@@ -0,0 +1,57 @@
+import os
+import sys
+import socket
+import struct
+
+try:
+    from yaml import dump
+except:
+    def dump(data, f, indent=0, **kwargs):
+        buf = []
+        for key, val in data.items():
+            if isinstance(val, dict):
+                buf.append('{}{}:'.format(' ' * indent, key))
+                buf += dump(val, None, indent=indent+2, **kwargs)
+            else:
+                buf.append('{}{}: {}'.format(' ' * indent, key, val))
+        if f is not None:
+            f.write('\n'.join(buf))
+        return buf
+
+if __name__ == '__main__':
+    if len(sys.argv) < 3:
+        print('Usage: {} {{robot_ip}} {{kinematics_suffix}}'.format(sys.argv[0]))
+        exit(1)
+    robot_ip = sys.argv[1]
+    kinematics_suffix = sys.argv[2]
+    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+    sock.connect((robot_ip, 502))
+
+    send_data = [0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x08]
+    sock.send(bytes(send_data))
+    recv_data = sock.recv(179)
+    if len(recv_data) == 179 and recv_data[8]:
+        robot_dof = recv_data[9]
+        robot_type = recv_data[10]
+        robot_name = 'uf850' if robot_dof == 6 and robot_type == 12 else 'lite6' if robot_dof == 6 and robot_type == 9 else 'xarm{}'.format(robot_dof)
+        ouput_dir = os.path.join(os.path.dirname(__file__), 'user')
+        if not os.path.exists(ouput_dir):
+            os.makedirs(ouput_dir)
+        output_file = os.path.join(ouput_dir, '{}_kinematics_{}.yaml'.format(robot_name, kinematics_suffix))
+        params = struct.unpack('<42f', recv_data[11:])
+        kinematics = {}
+        data = {'kinematics': kinematics}
+        for i in range(robot_dof):
+            joint_param = {}
+            kinematics['joint{}'.format(i + 1)] = joint_param
+            joint_param['x'] = params[i * 6]
+            joint_param['y'] = params[i * 6 + 1]
+            joint_param['z'] = params[i * 6 + 2]
+            joint_param['roll'] = params[i * 6 + 3]
+            joint_param['pitch'] = params[i * 6 + 4]
+            joint_param['yaw'] = params[i * 6 + 5]
+        with open(output_file, 'w', encoding='utf-8') as f:
+            dump(data, f, default_flow_style=False, allow_unicode=True, sort_keys=False)
+        print('[Success] save to {}'.format(output_file))
+    else:
+        print('[Failed] recv_len={}, valid={}'.format(len(recv_data), 0 if len(recv_data) < 9 else recv_data[8]))
\ No newline at end of file
diff --git a/xarm_description/config/link_inertial/gen_link_inertial_params.py b/xarm_description/config/link_inertial/gen_link_inertial_params.py
index 2ce1c55d..2248b55c 100644
--- a/xarm_description/config/link_inertial/gen_link_inertial_params.py
+++ b/xarm_description/config/link_inertial/gen_link_inertial_params.py
@@ -333,23 +333,40 @@ class XArm6x12Dynamics_HT_LDBR2(__XArmDynamics):
     ufacoty 850
     """
     CONFIG_NAME = 'xarm6_type12_HT_LDBR2.yaml'
-    MASS = [4.881, 3.166, 2.247, 2.12, 1.346, 0.225]
+    MASS = [4.807, 3.28, 2.237, 2.041, 1.385, 0.225]
     ORIGIN = [
-        [-4.9E-5, 0.013916, -0.020311],
-        [0.327984, -7.4E-5, 0.006482],
-        [0.130225, 0.105183, 0.00897],
-        [1.6E-5, -0.038469, -0.098295],
-        [-4.17E-4, 0.0176, -0.001416],
-        [1.46E-4, 0.001375, -0.013972]
+        [5.7E-5, 0.013842, -0.02055],
+        [0.323425, 9.5E-5, 0.007586],
+        [0.128834, 0.11341, 0.00978],
+        [2.66E-4, -0.03833, -0.101915],
+        [5.7E-5, -0.015267, -0.001015],
+        [4.84E-4, -3.34E-4, -0.014217]
     ]
     INERTIA = [
-        [21983.729*1E-06, -10.987*1E-06, -27.03*1E-06, 18990.524*1E-06, 446.74*1E-06, 12105.911*1E-06],
-        [6829.422*1E-06, 20.62*1E-06, -7814.433*1E-06, 59551.2*1E-06, 12.173*1E-06, 56839.03*1E-06],
-        [8058.663*1E-06, 4438.763*1E-06, -2484.54*1E-06, 8295.211*1E-06, -2066.521*1E-06, 12253.1*1E-06],
-        [19533.998*1E-06, 1.324*1E-06, -5.081*1E-06, 17008.797*1E-06, -5905.503*1E-06, 4277.698*1E-06],
-        [1839.637*1E-06, -23.466*1E-06, -1.032*1E-06, 1082.135*1E-06, -30.806*1E-06, 1829.654*1E-06],
-        [101.547*1E-06, -0.599*1E-06, -0.101*1E-06, 120.327*1E-06, -1.924*1E-06, 197.854*1E-06]
+        [22001.729*1E-06, 11.152*1E-06, -6.738*1E-06, 18900.685*1E-06, 364.084*1E-06, 12270.188*1E-06],
+        [7611.984*1E-06, 33.561*1E-06, -8240.021*1E-06, 64208.54*1E-06, 16.145*1E-06, 61175.7*1E-06],
+        [9920.314*1E-06, 5137.841*1E-06, -2628.426*1E-06, 8923.236*1E-06, -2412.869*1E-06, 14284.81*1E-06],
+        [19918.597*1E-06, 23.185*1E-06, -58.169*1E-06, 17515.194*1E-06, -5878.608*1E-06, 4283.807*1E-06],
+        [2077.454*1E-06, 5.026*1E-06, 0.699*1E-06, 1109.183*1E-06, -6.747*1E-06, 2035.7*1E-06],
+        [130.252*1E-06, 4.285*1E-06, -0.319*1E-06, 128.308*1E-06, 0.386*1E-06, 232.5*1E-06]
     ]
+    # MASS = [4.881, 3.166, 2.247, 2.12, 1.346, 0.225]
+    # ORIGIN = [
+    #     [-4.9E-5, 0.013916, -0.020311],
+    #     [0.327984, -7.4E-5, 0.006482],
+    #     [0.130225, 0.105183, 0.00897],
+    #     [1.6E-5, -0.038469, -0.098295],
+    #     [-4.17E-4, 0.0176, -0.001416],
+    #     [1.46E-4, 0.001375, -0.013972]
+    # ]
+    # INERTIA = [
+    #     [21983.729*1E-06, -10.987*1E-06, -27.03*1E-06, 18990.524*1E-06, 446.74*1E-06, 12105.911*1E-06],
+    #     [6829.422*1E-06, 20.62*1E-06, -7814.433*1E-06, 59551.2*1E-06, 12.173*1E-06, 56839.03*1E-06],
+    #     [8058.663*1E-06, 4438.763*1E-06, -2484.54*1E-06, 8295.211*1E-06, -2066.521*1E-06, 12253.1*1E-06],
+    #     [19533.998*1E-06, 1.324*1E-06, -5.081*1E-06, 17008.797*1E-06, -5905.503*1E-06, 4277.698*1E-06],
+    #     [1839.637*1E-06, -23.466*1E-06, -1.032*1E-06, 1082.135*1E-06, -30.806*1E-06, 1829.654*1E-06],
+    #     [101.547*1E-06, -0.599*1E-06, -0.101*1E-06, 120.327*1E-06, -1.924*1E-06, 197.854*1E-06]
+    # ]
     # ORIGIN = [
     #     [-4.9E-5, 0.013916, -0.020311],
     #     [0.327984, -7.4E-5, -0.006482],
diff --git a/xarm_description/config/link_inertial/xarm6_type12_HT_LDBR2.yaml b/xarm_description/config/link_inertial/xarm6_type12_HT_LDBR2.yaml
index 72c73dd7..35daee53 100644
--- a/xarm_description/config/link_inertial/xarm6_type12_HT_LDBR2.yaml
+++ b/xarm_description/config/link_inertial/xarm6_type12_HT_LDBR2.yaml
@@ -1,78 +1,78 @@
 link1:
   inertia:
-    ixx: 0.021983728999999997
-    ixy: 1.0987e-05
-    ixz: 2.703e-05
-    iyy: 0.018990524
-    iyz: -0.00044674
-    izz: 0.012105910999999999
-  mass: 4.881
+    ixx: 0.022001728999999998
+    ixy: -1.1152e-05
+    ixz: 6.7380000000000005e-06
+    iyy: 0.018900685
+    iyz: -0.000364084
+    izz: 0.012270188
+  mass: 4.807
   origin:
-    x: -4.9e-05
-    y: 0.013916
-    z: -0.020311
+    x: 5.7e-05
+    y: 0.013842
+    z: -0.02055
 link2:
   inertia:
-    ixx: 0.006829421999999999
-    ixy: -2.062e-05
-    ixz: 0.007814432999999999
-    iyy: 0.05955119999999999
-    iyz: -1.2173e-05
-    izz: 0.05683903
-  mass: 3.166
+    ixx: 0.0076119840000000005
+    ixy: -3.3561e-05
+    ixz: 0.008240021
+    iyy: 0.06420854
+    iyz: -1.6145e-05
+    izz: 0.06117569999999999
+  mass: 3.28
   origin:
-    x: 0.327984
-    y: -7.4e-05
-    z: 0.006482
+    x: 0.323425
+    y: 9.5e-05
+    z: 0.007586
 link3:
   inertia:
-    ixx: 0.008058662999999999
-    ixy: -0.004438763
-    ixz: 0.00248454
-    iyy: 0.008295210999999999
-    iyz: 0.002066521
-    izz: 0.0122531
-  mass: 2.247
+    ixx: 0.009920314
+    ixy: -0.005137841
+    ixz: 0.0026284259999999997
+    iyy: 0.008923236000000001
+    iyz: 0.002412869
+    izz: 0.014284809999999998
+  mass: 2.237
   origin:
-    x: 0.130225
-    y: 0.105183
-    z: 0.00897
+    x: 0.128834
+    y: 0.11341
+    z: 0.00978
 link4:
   inertia:
-    ixx: 0.019533998
-    ixy: -1.324e-06
-    ixz: 5.081e-06
-    iyy: 0.017008797
-    iyz: 0.0059055029999999994
-    izz: 0.004277698
-  mass: 2.12
+    ixx: 0.019918597
+    ixy: -2.3185e-05
+    ixz: 5.816899999999999e-05
+    iyy: 0.017515193999999998
+    iyz: 0.005878608
+    izz: 0.004283807
+  mass: 2.041
   origin:
-    x: 1.6e-05
-    y: -0.038469
-    z: -0.098295
+    x: 0.000266
+    y: -0.03833
+    z: -0.101915
 link5:
   inertia:
-    ixx: 0.0018396369999999999
-    ixy: 2.3466e-05
-    ixz: 1.032e-06
-    iyy: 0.001082135
-    iyz: 3.0806e-05
-    izz: 0.001829654
-  mass: 1.346
+    ixx: 0.0020774540000000003
+    ixy: -5.026e-06
+    ixz: -6.989999999999999e-07
+    iyy: 0.001109183
+    iyz: 6.747e-06
+    izz: 0.0020357
+  mass: 1.385
   origin:
-    x: -0.000417
-    y: 0.0176
-    z: -0.001416
+    x: 5.7e-05
+    y: -0.015267
+    z: -0.001015
 link6:
   inertia:
-    ixx: 0.000101547
-    ixy: 5.989999999999999e-07
-    ixz: 1.01e-07
-    iyy: 0.00012032699999999999
-    iyz: 1.9239999999999997e-06
-    izz: 0.000197854
+    ixx: 0.000130252
+    ixy: -4.285e-06
+    ixz: 3.19e-07
+    iyy: 0.000128308
+    iyz: -3.86e-07
+    izz: 0.00023249999999999999
   mass: 0.225
   origin:
-    x: 0.000146
-    y: 0.001375
-    z: -0.013972
+    x: 0.000484
+    y: -0.000334
+    z: -0.014217
diff --git a/xarm_description/urdf/common/common.link.xacro b/xarm_description/urdf/common/common.link.xacro
index 9c89c35d..8e85864e 100644
--- a/xarm_description/urdf/common/common.link.xacro
+++ b/xarm_description/urdf/common/common.link.xacro
@@ -55,6 +55,10 @@
     </collision>
   </xacro:macro>
 
+  <xacro:macro name="common_joint_origin" params="joint_kinematic_params">
+    <origin xyz="${joint_kinematic_params['x']} ${joint_kinematic_params['y']} ${joint_kinematic_params['z']}" rpy="${joint_kinematic_params['roll']} ${joint_kinematic_params['pitch']} ${joint_kinematic_params['yaw']}" />
+  </xacro:macro>
+
   <xacro:macro name="parse_link_inertial_filename" params="dof robot_type robot_sn model1300">
     <!-- default inertial params filename -->
     <xacro:property name="inertial_params_filename" value="xarm${dof}_type${dof}_HT_BR2"/>
diff --git a/xarm_description/urdf/dual_xarm_device.urdf.xacro b/xarm_description/urdf/dual_xarm_device.urdf.xacro
index a7863142..b3e6d63f 100755
--- a/xarm_description/urdf/dual_xarm_device.urdf.xacro
+++ b/xarm_description/urdf/dual_xarm_device.urdf.xacro
@@ -65,6 +65,9 @@
 
   <xacro:arg name="mesh_suffix" default="stl"/>
 
+  <xacro:arg name="kinematics_suffix_1" default=""/>
+  <xacro:arg name="kinematics_suffix_2" default=""/>
+
   <!-- load xarm device -->
   <xacro:include filename="$(find xarm_description)/urdf/xarm_device_macro.xacro" />
     
@@ -85,7 +88,7 @@
     geometry_mesh_tcp_xyz="$(arg geometry_mesh_tcp_xyz_1)" geometry_mesh_tcp_rpy="$(arg geometry_mesh_tcp_rpy_1)"
     robot_ip="$(arg robot_ip_1)" report_type="$(arg report_type_1)"
     baud_checkset="$(arg baud_checkset_1)" default_gripper_baud="$(arg default_gripper_baud_1)"
-    model1300="$(arg model1300_1)" robot_sn="$(arg robot_sn_1)" mesh_suffix="$(arg mesh_suffix)" />
+    model1300="$(arg model1300_1)" robot_sn="$(arg robot_sn_1)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix_1)" />
   <xacro:xarm_device prefix="$(arg prefix_2)" hw_ns="$(arg hw_ns)" limited="$(arg limited)" 
     effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
     add_gripper="$(arg add_gripper_2)" add_vacuum_gripper="$(arg add_vacuum_gripper_2)" dof="$(arg dof_2)"
@@ -103,6 +106,6 @@
     geometry_mesh_tcp_xyz="$(arg geometry_mesh_tcp_xyz_2)" geometry_mesh_tcp_rpy="$(arg geometry_mesh_tcp_rpy_2)"
     robot_ip="$(arg robot_ip_2)" report_type="$(arg report_type_2)"
     baud_checkset="$(arg baud_checkset_2)" default_gripper_baud="$(arg default_gripper_baud_2)"
-    model1300="$(arg model1300_2)" robot_sn="$(arg robot_sn_2)" mesh_suffix="$(arg mesh_suffix)" />
+    model1300="$(arg model1300_2)" robot_sn="$(arg robot_sn_2)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix_2)" />
 
 </robot>
diff --git a/xarm_description/urdf/lite6/lite6.urdf.xacro b/xarm_description/urdf/lite6/lite6.urdf.xacro
index 5daf3af1..30ea3d49 100755
--- a/xarm_description/urdf/lite6/lite6.urdf.xacro
+++ b/xarm_description/urdf/lite6/lite6.urdf.xacro
@@ -7,7 +7,7 @@
     joint4_lower_limit:=${-2.0*pi}  joint4_upper_limit:=${2.0*pi}
     joint5_lower_limit:=${-2.1642}  joint5_upper_limit:=${2.1642}
     joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}
-    inertial_params_filename:='' "
+    inertial_params_filename:='' kinematics_params_filename:='' "
     >
 
     <!-- load params and define property -->
@@ -20,10 +20,13 @@
     </xacro:unless>
     <xacro:if value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${xacro.load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${xacro.load_yaml(kinematics_params_filename)}"/>
     </xacro:if>
     <xacro:unless value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${load_yaml(kinematics_params_filename)}"/>
     </xacro:unless>
+    <xacro:property name="kinematics_params" value="${kinematics_config['kinematics']}"/>
     
     <link name="${prefix}link_base">
       <inertial>
@@ -58,7 +61,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint1" type="revolute">
-      <origin xyz="0 0 0.2435" rpy="0 0 0" />
+      <!-- <origin xyz="0 0 0.2435" rpy="0 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint1']}" />
       <parent link="${prefix}link_base" />
       <child link="${prefix}link1" />
       <axis xyz="0 0 1" />
@@ -75,16 +79,17 @@
         origin_rpy="0 0 0"/>
       <xacro:common_link_visual 
         mesh_filename="lite6/visual/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${-kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0"
         material_name="${prefix}White" />
       <xacro:common_link_collision 
         mesh_filename="lite6/collision/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${-kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint2" type="revolute">
-      <origin xyz="0 0 0" rpy="1.5708 -1.5708 3.1416" />
+      <!-- <origin xyz="0 0 0" rpy="1.5708 -1.5708 3.1416" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint2']}" />
       <parent link="${prefix}link1" />
       <child link="${prefix}link2" />
       <axis xyz="0 0 1" />
@@ -110,7 +115,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint3" type="revolute">
-      <origin xyz="0.2002 0 0" rpy="-3.1416 0 1.5708" />
+      <!-- <origin xyz="0.2002 0 0" rpy="-3.1416 0 1.5708" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint3']}" />
       <parent link="${prefix}link2" />
       <child link="${prefix}link3" />
       <axis xyz="0 0 1" />
@@ -136,7 +142,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint4" type="revolute">
-      <origin xyz="0.087 -0.22761 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0.087 -0.22761 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint4']}" />
       <parent link="${prefix}link3" />
       <child link="${prefix}link4" />
       <axis xyz="0 0 1" />
@@ -162,7 +169,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint5" type="revolute">
-      <origin xyz="0 0 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0 0 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint5']}" />
       <parent link="${prefix}link4" />
       <child link="${prefix}link5" />
       <axis xyz="0 0 1" />
@@ -188,7 +196,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint6" type="revolute">
-      <origin xyz="0 0.0625 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0 0.0625 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint6']}" />
       <parent link="${prefix}link5" />
       <child link="${prefix}link6" />
       <axis xyz="0 0 1" />
diff --git a/xarm_description/urdf/lite6/lite6_robot_macro.xacro b/xarm_description/urdf/lite6/lite6_robot_macro.xacro
index 4f623253..907313e8 100755
--- a/xarm_description/urdf/lite6/lite6_robot_macro.xacro
+++ b/xarm_description/urdf/lite6/lite6_robot_macro.xacro
@@ -12,6 +12,7 @@
     baud_checkset:=true
     default_gripper_baud:=2000000
     inertial_params_filename:=''
+    kinematics_params_filename:=''
     hard_interface:='PositionJointInterface' "
     >
 
@@ -45,7 +46,9 @@
         joint4_lower_limit="${-pi*0.99}" joint4_upper_limit="${pi*0.99}"
         joint5_lower_limit="${-2.1642}" joint5_upper_limit="${2.1642}"
         joint6_lower_limit="${-pi*0.99}" joint6_upper_limit="${pi*0.99}"
-        inertial_params_filename="${inertial_params_filename}" />
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:if>
     <xacro:unless value="${limited}">
       <xacro:if value="${is_ros2}">
@@ -55,7 +58,10 @@
           robot_ip="${robot_ip}" report_type="${report_type}"
           baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
       </xacro:if>
-      <xacro:lite6_urdf prefix="${prefix}" inertial_params_filename="${inertial_params_filename}"/>
+      <xacro:lite6_urdf prefix="${prefix}" 
+        inertial_params_filename="${inertial_params_filename}" 
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:unless>
 
     <xacro:lite6_transmission prefix="${prefix}" hard_interface="${hard_interface}" />
diff --git a/xarm_description/urdf/uf850/uf850.urdf.xacro b/xarm_description/urdf/uf850/uf850.urdf.xacro
index 653c6e91..ee420b31 100644
--- a/xarm_description/urdf/uf850/uf850.urdf.xacro
+++ b/xarm_description/urdf/uf850/uf850.urdf.xacro
@@ -8,7 +8,7 @@
     joint5_lower_limit:=${-2.1642}  joint5_upper_limit:=${2.1642}
     joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}
     model1300:=false
-    inertial_params_filename:='' "
+    inertial_params_filename:='' kinematics_params_filename:='' "
     >
     
     <!-- load params and define property -->
@@ -21,10 +21,13 @@
     </xacro:unless>
     <xacro:if value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${xacro.load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${xacro.load_yaml(kinematics_params_filename)}"/>
     </xacro:if>
     <xacro:unless value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${load_yaml(kinematics_params_filename)}"/>
     </xacro:unless>
+    <xacro:property name="kinematics_params" value="${kinematics_config['kinematics']}"/>
 
     <link name="${prefix}link_base">
       <inertial>
@@ -63,7 +66,8 @@
     <joint name="${prefix}joint1" type="revolute">
       <parent link="${prefix}link_base"/>
       <child link="${prefix}link1"/>
-      <origin xyz="0 0 0.364" rpy="0 0 0"/>
+      <!-- <origin xyz="0 0 0.364" rpy="0 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint1']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint1_lower_limit}"
@@ -79,19 +83,20 @@
         origin_rpy="0 0 0"/>
       <xacro:common_link_visual 
         mesh_filename="uf850/visual/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0"
         material_name="${prefix}White" />
       <xacro:common_link_collision 
         mesh_filename="uf850/collision/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0" />
     </link>
 
     <joint name="${prefix}joint2" type="revolute">
       <parent link="${prefix}link1"/>
       <child link="${prefix}link2"/>
-      <origin xyz="0 0 0" rpy="1.5708 -1.5708 0" />
+      <!-- <origin xyz="0 0 0" rpy="1.5708 -1.5708 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint2']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint2_lower_limit}"
@@ -119,7 +124,8 @@
     <joint name="${prefix}joint3" type="revolute">
       <parent link="${prefix}link2"/>
       <child link="${prefix}link3"/>
-      <origin xyz="0.39 0 0" rpy="-3.1416 0 -1.5708"/>
+      <!-- <origin xyz="0.39 0 0" rpy="-3.1416 0 -1.5708"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint3']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint3_lower_limit}"
@@ -147,7 +153,8 @@
     <joint name="${prefix}joint4" type="revolute">
       <parent link="${prefix}link3"/>
       <child link="${prefix}link4"/>
-      <origin xyz="0.15 0.426 0" rpy="-1.5708 0 0"/>
+      <!-- <origin xyz="0.15 0.426 0" rpy="-1.5708 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint4']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint4_lower_limit}"
@@ -175,7 +182,8 @@
     <joint name="${prefix}joint5" type="revolute">
       <parent link="${prefix}link4"/>
       <child link="${prefix}link5"/>
-      <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+      <!-- <origin xyz="0 0 0" rpy="-1.5708 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint5']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint5_lower_limit}"
@@ -203,7 +211,8 @@
     <joint name="${prefix}joint6" type="revolute">
       <parent link="${prefix}link5"/>
       <child link="${prefix}link6"/>
-      <origin xyz="0 -0.09 0" rpy="1.5708 0 0"/>
+      <!-- <origin xyz="0 -0.09 0" rpy="1.5708 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint6']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint6_lower_limit}"
diff --git a/xarm_description/urdf/uf850/uf850_robot_macro.xacro b/xarm_description/urdf/uf850/uf850_robot_macro.xacro
index 66d2c920..5eaaf4f4 100644
--- a/xarm_description/urdf/uf850/uf850_robot_macro.xacro
+++ b/xarm_description/urdf/uf850/uf850_robot_macro.xacro
@@ -13,6 +13,7 @@
     default_gripper_baud:=2000000 
     model1300:=false
     inertial_params_filename:=''
+    kinematics_params_filename:=''
     hard_interface:='PositionJointInterface' "
     >
 
@@ -47,7 +48,8 @@
         joint5_lower_limit="${-2.1642}" joint5_upper_limit="${2.1642}"
         joint6_lower_limit="${-pi*0.99}" joint6_upper_limit="${pi*0.99}"
         model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"/>
     </xacro:if>
     <xacro:unless value="${limited}">
       <xacro:if value="${is_ros2}">
@@ -58,7 +60,8 @@
           baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
       </xacro:if>
       <xacro:uf850_urdf prefix="${prefix}" model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"/>
     </xacro:unless>
 
     <xacro:uf850_transmission prefix="${prefix}" hard_interface="${hard_interface}" />
diff --git a/xarm_description/urdf/xarm5/xarm5.urdf.xacro b/xarm_description/urdf/xarm5/xarm5.urdf.xacro
index bfbccc05..8fc8fa9e 100755
--- a/xarm_description/urdf/xarm5/xarm5.urdf.xacro
+++ b/xarm_description/urdf/xarm5/xarm5.urdf.xacro
@@ -8,7 +8,7 @@
     joint4_lower_limit:=${-1.69297}  joint4_upper_limit:=${pi}
     joint5_lower_limit:=${-2.0*pi}  joint5_upper_limit:=${2.0*pi}
     model1300:=false
-    inertial_params_filename:='' "
+    inertial_params_filename:='' kinematics_params_filename:='' "
     >
     
     <!-- define property -->
@@ -31,10 +31,13 @@
     </xacro:unless>
     <xacro:if value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${xacro.load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${xacro.load_yaml(kinematics_params_filename)}"/>
     </xacro:if>
     <xacro:unless value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${load_yaml(kinematics_params_filename)}"/>
     </xacro:unless>
+    <xacro:property name="kinematics_params" value="${kinematics_config['kinematics']}"/>
 
     <link name="${prefix}link_base">
       <inertial>
@@ -69,7 +72,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint1" type="revolute">
-      <origin xyz="0 0 0.267" rpy="0 0 0" />
+      <!-- <origin xyz="0 0 0.267" rpy="0 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint1']}" />
       <parent link="${prefix}link_base" />
       <child link="${prefix}link1" />
       <axis xyz="0 0 1" />
@@ -95,7 +99,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint2" type="revolute">
-      <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0 0 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint2']}" />
       <parent link="${prefix}link1" />
       <child link="${prefix}link2" />
       <axis xyz="0 0 1" />
@@ -121,7 +126,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint3" type="revolute">
-      <origin xyz="0.0535 -0.2845 0" rpy="0 0 0" />
+      <!-- <origin xyz="0.0535 -0.2845 0" rpy="0 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint3']}" />
       <parent link="${prefix}link2" />
       <child link="${prefix}link3" />
       <axis xyz="0 0 1" />
@@ -147,7 +153,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint4" type="revolute">
-      <origin xyz="0.0775 0.3425 0" rpy="0 0 0" />
+      <!-- <origin xyz="0.0775 0.3425 0" rpy="0 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint4']}" />
       <parent link="${prefix}link3" />
       <child link="${prefix}link4" />
       <axis xyz="0 0 1" />
@@ -173,7 +180,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint5" type="revolute">
-      <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint5']}" />
       <parent link="${prefix}link4" />
       <child link="${prefix}link5" />
       <axis xyz="0 0 1" />
diff --git a/xarm_description/urdf/xarm5/xarm5_robot_macro.xacro b/xarm_description/urdf/xarm5/xarm5_robot_macro.xacro
index 5c9ca10f..ac167c9d 100755
--- a/xarm_description/urdf/xarm5/xarm5_robot_macro.xacro
+++ b/xarm_description/urdf/xarm5/xarm5_robot_macro.xacro
@@ -13,6 +13,7 @@
     default_gripper_baud:=2000000 
     model1300:=false
     inertial_params_filename:='' 
+    kinematics_params_filename:='' 
     hard_interface:='PositionJointInterface' "
     >
 
@@ -45,7 +46,9 @@
         joint4_lower_limit="${-1.69297}" joint4_upper_limit="${pi*0.99}"
         joint5_lower_limit="${-pi*0.99}" joint5_upper_limit="${pi*0.99}"
         model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:if>
     <xacro:unless value="${limited}">
       <xacro:if value="${is_ros2}">
@@ -56,7 +59,9 @@
           baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
       </xacro:if>
       <xacro:xarm5_urdf prefix="${prefix}" model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:unless>
 
     <xacro:xarm5_transmission prefix="${prefix}" hard_interface="${hard_interface}" />
diff --git a/xarm_description/urdf/xarm6/xarm6.urdf.xacro b/xarm_description/urdf/xarm6/xarm6.urdf.xacro
index 7b88edcb..26764ab5 100755
--- a/xarm_description/urdf/xarm6/xarm6.urdf.xacro
+++ b/xarm_description/urdf/xarm6/xarm6.urdf.xacro
@@ -8,7 +8,7 @@
     joint5_lower_limit:=${-1.69297}  joint5_upper_limit:=${pi}
     joint6_lower_limit:=${-2.0*pi}  joint6_upper_limit:=${2.0*pi}
     model1300:=false
-    inertial_params_filename:='' "
+    inertial_params_filename:='' kinematics_params_filename:='' "
     >
 
     <!-- define property -->
@@ -31,10 +31,13 @@
     </xacro:unless>
     <xacro:if value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${xacro.load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${xacro.load_yaml(kinematics_params_filename)}"/>
     </xacro:if>
     <xacro:unless value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${load_yaml(kinematics_params_filename)}"/>
     </xacro:unless>
+    <xacro:property name="kinematics_params" value="${kinematics_config['kinematics']}"/>
 
     <link name="${prefix}link_base">
       <inertial>
@@ -73,7 +76,8 @@
     <joint name="${prefix}joint1" type="revolute">
       <parent link="${prefix}link_base"/>
       <child link="${prefix}link1"/>
-      <origin xyz="0 0 0.267" rpy="0 0 0"/>
+      <!-- <origin xyz="0 0 0.267" rpy="0 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint1']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint1_lower_limit}"
@@ -89,19 +93,20 @@
         origin_rpy="0 0 0"/>
       <xacro:common_link_visual 
         mesh_filename="xarm6/visual/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${-kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0"
         material_name="${prefix}White" />
       <xacro:common_link_collision 
         mesh_filename="xarm6/visual/link2"  
-        origin_xyz="0 0 0" 
+        origin_xyz="0 0 ${-kinematics_params['joint2']['y']}" 
         origin_rpy="0 0 0" />
     </link>
 
     <joint name="${prefix}joint2" type="revolute">
       <parent link="${prefix}link1"/>
       <child link="${prefix}link2"/>
-      <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0 0 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint2']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint2_lower_limit}"
@@ -129,7 +134,8 @@
     <joint name="${prefix}joint3" type="revolute">
       <parent link="${prefix}link2"/>
       <child link="${prefix}link3"/>
-      <origin xyz="0.0535 -0.2845 0" rpy="0 0 0"/>
+      <!-- <origin xyz="0.0535 -0.2845 0" rpy="0 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint3']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint3_lower_limit}"
@@ -157,7 +163,8 @@
     <joint name="${prefix}joint4" type="revolute">
       <parent link="${prefix}link3"/>
       <child link="${prefix}link4"/>
-      <origin xyz="0.0775 0.3425 0" rpy="-1.5708 0 0"/>
+      <!-- <origin xyz="0.0775 0.3425 0" rpy="-1.5708 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint4']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint4_lower_limit}"
@@ -186,6 +193,7 @@
       <parent link="${prefix}link4"/>
       <child link="${prefix}link5"/>
       <origin xyz="0 0 0" rpy="1.5708 0 0"/>
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint5']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint5_lower_limit}"
@@ -213,7 +221,8 @@
     <joint name="${prefix}joint6" type="revolute">
       <parent link="${prefix}link5"/>
       <child link="${prefix}link6"/>
-      <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0"/>
+      <!-- <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0"/> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint6']}" />
       <axis xyz="0 0 1"/>
       <limit
         lower="${joint6_lower_limit}"
diff --git a/xarm_description/urdf/xarm6/xarm6_robot_macro.xacro b/xarm_description/urdf/xarm6/xarm6_robot_macro.xacro
index 8f293a09..01d04504 100755
--- a/xarm_description/urdf/xarm6/xarm6_robot_macro.xacro
+++ b/xarm_description/urdf/xarm6/xarm6_robot_macro.xacro
@@ -13,6 +13,7 @@
     default_gripper_baud:=2000000 
     model1300:=false
     inertial_params_filename:=''
+    kinematics_params_filename:=''
     hard_interface:='PositionJointInterface' "
     >
 
@@ -47,7 +48,9 @@
         joint5_lower_limit="${-1.69297}" joint5_upper_limit="${pi*0.99}"
         joint6_lower_limit="${-pi*0.99}" joint6_upper_limit="${pi*0.99}"
         model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:if>
     <xacro:unless value="${limited}">
       <xacro:if value="${is_ros2}">
@@ -58,7 +61,9 @@
           baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
       </xacro:if>
       <xacro:xarm6_urdf prefix="${prefix}" model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:unless>
 
     <xacro:xarm6_transmission prefix="${prefix}" hard_interface="${hard_interface}" />
diff --git a/xarm_description/urdf/xarm7/xarm7.urdf.xacro b/xarm_description/urdf/xarm7/xarm7.urdf.xacro
index 5175a7f8..7cb941c3 100755
--- a/xarm_description/urdf/xarm7/xarm7.urdf.xacro
+++ b/xarm_description/urdf/xarm7/xarm7.urdf.xacro
@@ -10,7 +10,7 @@
     joint6_lower_limit:=${-1.69297}  joint6_upper_limit:=${pi}
     joint7_lower_limit:=${-2.0*pi}  joint7_upper_limit:=${2.0*pi}
     model1300:=false
-    inertial_params_filename:='' "
+    inertial_params_filename:='' kinematics_params_filename:='' "
     >
 
     <!-- define property -->
@@ -33,10 +33,13 @@
     </xacro:unless>
     <xacro:if value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${xacro.load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${xacro.load_yaml(kinematics_params_filename)}"/>
     </xacro:if>
     <xacro:unless value="${use_xacro_load_yaml}">
       <xacro:property name="inertial_params" value="${load_yaml(link_inertial_params_file)}"/>
+      <xacro:property name="kinematics_config" value="${load_yaml(kinematics_params_filename)}"/>
     </xacro:unless>
+    <xacro:property name="kinematics_params" value="${kinematics_config['kinematics']}"/>
 
     <link name="${prefix}link_base">
       <inertial>
@@ -71,7 +74,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint1" type="revolute">
-      <origin xyz="0 0 0.267" rpy="0 0 0" />
+      <!-- <origin xyz="0 0 0.267" rpy="0 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint1']}" />
       <parent link="${prefix}link_base" />
       <child link="${prefix}link1" />
       <axis xyz="0 0 1" />
@@ -97,7 +101,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint2" type="revolute">
-      <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0 0 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint2']}" />
       <parent link="${prefix}link1" />
       <child link="${prefix}link2" />
       <axis xyz="0 0 1" />
@@ -123,7 +128,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint3" type="revolute">
-      <origin xyz="0 -0.293 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0 -0.293 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint3']}" />
       <parent link="${prefix}link2" />
       <child link="${prefix}link3" />
       <axis xyz="0 0 1" />
@@ -149,7 +155,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint4" type="revolute">
-      <origin xyz="0.0525 0 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0.0525 0 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint4']}" />
       <parent link="${prefix}link3" />
       <child link="${prefix}link4" />
       <axis xyz="0 0 1" />
@@ -175,7 +182,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint5" type="revolute">
-      <origin xyz="0.0775 -0.3425 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0.0775 -0.3425 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint5']}" />
       <parent link="${prefix}link4" />
       <child link="${prefix}link5" />
       <axis xyz="0 0 1" />
@@ -201,7 +209,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint6" type="revolute">
-      <origin xyz="0 0 0" rpy="1.5708 0 0" />
+      <!-- <origin xyz="0 0 0" rpy="1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint6']}" />
       <parent link="${prefix}link5" />
       <child link="${prefix}link6" />
       <axis xyz="0 0 1" />
@@ -227,7 +236,8 @@
         origin_rpy="0 0 0" />
     </link>
     <joint name="${prefix}joint7" type="revolute">
-      <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0" />
+      <!-- <origin xyz="0.076 0.097 0" rpy="-1.5708 0 0" /> -->
+      <xacro:common_joint_origin joint_kinematic_params="${kinematics_params['joint7']}" />
       <parent link="${prefix}link6" />
       <child link="${prefix}link7" />
       <axis xyz="0 0 1" />
diff --git a/xarm_description/urdf/xarm7/xarm7_robot_macro.xacro b/xarm_description/urdf/xarm7/xarm7_robot_macro.xacro
index d391be02..e8a35ee6 100644
--- a/xarm_description/urdf/xarm7/xarm7_robot_macro.xacro
+++ b/xarm_description/urdf/xarm7/xarm7_robot_macro.xacro
@@ -13,6 +13,7 @@
     default_gripper_baud:=2000000 
     model1300:=false
     inertial_params_filename:=''
+    kinematics_params_filename:=''
     hard_interface:='PositionJointInterface' "
     >  
 
@@ -49,7 +50,9 @@
         joint6_lower_limit="${-1.75}" joint6_upper_limit="${pi*0.99}"
         joint7_lower_limit="${-pi*0.99}" joint7_upper_limit="${pi*0.99}"
         model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:if>
     <xacro:unless value="${limited}">
       <xacro:if value="${is_ros2}">
@@ -60,7 +63,9 @@
           baud_checkset="${baud_checkset}" default_gripper_baud="${default_gripper_baud}" />
       </xacro:if>
       <xacro:xarm7_urdf prefix="${prefix}" model1300="${model1300}"
-        inertial_params_filename="${inertial_params_filename}"/>
+        inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
+        />
     </xacro:unless>
 
     <xacro:xarm7_transmission prefix="${prefix}" hard_interface="${hard_interface}" />
diff --git a/xarm_description/urdf/xarm_device.urdf.xacro b/xarm_description/urdf/xarm_device.urdf.xacro
index 350696d0..0901e72f 100755
--- a/xarm_description/urdf/xarm_device.urdf.xacro
+++ b/xarm_description/urdf/xarm_device.urdf.xacro
@@ -43,6 +43,8 @@
   <xacro:arg name="attach_rpy" default="0 0 0"/>
   <xacro:arg name="mesh_suffix" default="stl"/>
 
+  <xacro:arg name="kinematics_suffix" default=""/>
+
   <!-- load xarm device -->
   <xacro:include filename="$(find xarm_description)/urdf/xarm_device_macro.xacro" />
   <xacro:xarm_device prefix="$(arg prefix)" hw_ns="$(arg hw_ns)" limited="$(arg limited)" 
@@ -62,6 +64,6 @@
     robot_ip="$(arg robot_ip)" report_type="$(arg report_type)"
     baud_checkset="$(arg baud_checkset)" default_gripper_baud="$(arg default_gripper_baud)"
     model1300="$(arg model1300)" attach_to="$(arg attach_to)" attach_xyz="$(arg attach_xyz)" attach_rpy="$(arg attach_rpy)"
-    robot_sn="$(arg robot_sn)" use_gazebo_camera="$(arg use_gazebo_camera)" mesh_suffix="$(arg mesh_suffix)" />
+    robot_sn="$(arg robot_sn)" use_gazebo_camera="$(arg use_gazebo_camera)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix)" />
 
 </robot>
diff --git a/xarm_description/urdf/xarm_device_macro.xacro b/xarm_description/urdf/xarm_device_macro.xacro
index 266f80b8..cdcf4560 100644
--- a/xarm_description/urdf/xarm_device_macro.xacro
+++ b/xarm_description/urdf/xarm_device_macro.xacro
@@ -38,7 +38,9 @@
     default_gripper_baud:=2000000 
     model1300:=false
     robot_sn:='' 
-    mesh_suffix:='stl' "
+    mesh_suffix:='stl' 
+    kinematics_suffix:=''
+    "
     > 
 
     <!-- Load Common Material -->
@@ -72,7 +74,7 @@
 
     <joint name="${prefix}world_joint" type="fixed">
       <parent link="${attach_to}" />
-      <child link = "${prefix}link_base" />
+      <child link="${prefix}link_base" />
       <origin xyz="${attach_xyz}" rpy="${attach_rpy}" />
     </joint>
 
@@ -87,6 +89,20 @@
         <xacro:property name="hard_interface" value="PositionJointInterface" />
       </xacro:unless>
     </xacro:unless>
+
+    <xacro:if value="${robot_type == 'uf850'}"> 
+      <xacro:property name="robot_name" value="${robot_type}" />
+    </xacro:if>
+    <xacro:unless value="${robot_type == 'uf850'}"> 
+      <xacro:property name="robot_name" value="${robot_type}${dof}" />
+    </xacro:unless>
+
+    <xacro:if value="${kinematics_suffix != ''}">
+      <xacro:property name="kinematics_params_filename" value="$(find xarm_description)/config/kinematics/user/${robot_name}_kinematics_${kinematics_suffix}.yaml"/>
+    </xacro:if>
+    <xacro:unless value="${kinematics_suffix != ''}">
+      <xacro:property name="kinematics_params_filename" value="$(find xarm_description)/config/kinematics/default/${robot_name}_default_kinematics.yaml"/>
+    </xacro:unless>
     
     <xacro:if value="${dof == 5}">
       <!-- Load XArm5 Robot Model URDF -->
@@ -101,6 +117,7 @@
         default_gripper_baud="${default_gripper_baud}" 
         model1300="${new_model1300}"
         inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
         hard_interface="${hard_interface}" />
     </xacro:if>
     <xacro:if value="${dof == 6}">
@@ -117,6 +134,7 @@
           default_gripper_baud="${default_gripper_baud}"
           model1300="${new_model1300}"
           inertial_params_filename="${inertial_params_filename}"
+          kinematics_params_filename="${kinematics_params_filename}"
           hard_interface="${hard_interface}" />
       </xacro:if>
       <xacro:if value="${robot_type == 'lite'}">
@@ -131,6 +149,7 @@
           baud_checkset="${baud_checkset}"
           default_gripper_baud="${default_gripper_baud}"
           inertial_params_filename="${inertial_params_filename}"
+          kinematics_params_filename="${kinematics_params_filename}"
           hard_interface="${hard_interface}" />
       </xacro:if>
       <xacro:unless value="${robot_type == 'lite' or robot_type == 'uf850'}">
@@ -146,6 +165,7 @@
           default_gripper_baud="${default_gripper_baud}"
           model1300="${new_model1300}"
           inertial_params_filename="${inertial_params_filename}"
+          kinematics_params_filename="${kinematics_params_filename}"
           hard_interface="${hard_interface}" />
       </xacro:unless>
     </xacro:if>
@@ -162,6 +182,7 @@
         default_gripper_baud="${default_gripper_baud}" 
         model1300="${new_model1300}"
         inertial_params_filename="${inertial_params_filename}"
+        kinematics_params_filename="${kinematics_params_filename}"
         hard_interface="${hard_interface}" />
     </xacro:unless>
 
diff --git a/xarm_gazebo/launch/_dual_robot_beside_table.launch b/xarm_gazebo/launch/_dual_robot_beside_table.launch
index e949aa88..65492efd 100755
--- a/xarm_gazebo/launch/_dual_robot_beside_table.launch
+++ b/xarm_gazebo/launch/_dual_robot_beside_table.launch
@@ -32,6 +32,9 @@
   <arg name="geometry_mesh_origin_rpy" default="'0 0 0'"/>
   <arg name="geometry_mesh_tcp_xyz" default="'0 0 0'"/>
   <arg name="geometry_mesh_tcp_rpy" default="'0 0 0'"/>
+
+  <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 'dual_uf850' if arg('robot_type') == 'uf850' else 'dual_' + arg('robot_type') + str(arg('robot_dof')))" /> -->
@@ -57,6 +60,8 @@
     robot_sn_2:=$(arg robot_sn)
     hw_ns:=$(arg hw_ns)
     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)
     effort_control:=$(arg effort_control) 
diff --git a/xarm_gazebo/launch/_robot_beside_table.launch b/xarm_gazebo/launch/_robot_beside_table.launch
index 47351121..eb245825 100755
--- a/xarm_gazebo/launch/_robot_beside_table.launch
+++ b/xarm_gazebo/launch/_robot_beside_table.launch
@@ -11,6 +11,7 @@
   <arg name="hw_ns" default="xarm"/>
   <arg name="velocity_control" default="false"/>
   <arg name="effort_control" default="false"/>
+  <arg name="kinematics_suffix" default=""/>
 
   <arg name="attach_to" default="world" />
   <arg name="attach_xyz" default="'0 0 0'" />
@@ -76,6 +77,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)
     "/>
 
   <!-- spawn robot model in gazebo, located on the table -->