[feat] Unify the URDF file and adapt the inertia parameters of the link

1. ROS1/ROS2 use the same set of URDF files 2. Added robot_sn parameter to adapt loading link inertial parameter 3. Added parameters such as attach_to to allow the robotic arm model to attach to other models 4. Fix .setup_assistant configuration needed for moveit_setup_assistant 5. Fix calling the boxPoints method compatible with minor cv2 version
xArm-Developer · Apr 20, 2023 · 91d6608 · 91d6608
1 parent 2983907
commit 91d6608
Show file tree

Hide file tree

Showing 197 changed files with 6,192 additions and 3,352 deletions.
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,4 @@
 .vscode/
 .idea/
-.git_template
+.git_template
+**/old
diff --git a/ReadMe.md b/ReadMe.md
@@ -86,9 +86,13 @@ For **kinetic** users, please use the [kinetic branch](https://github.com/xArm-D
    * (2022-09-07) Update submodule xarm-sdk to version 1.11.0
    * (2022-11-16) Add torque related services: /xarm/ft_sensor_enable, /xarm/ft_sensor_app_set, /xarm/ft_sensor_set_zero, /xarm/ft_sensor_cali_load, /xarm/get_ft_sensor_error
    * (2023-02-10) Added xarm_moveit_servo to support xbox controller/SpaceMouse/keyboard control
-   * (2022-02-18) Automatically saving in servie(/xarm/ft_sensor_cali_load) and add torque related service(/xarm/ft_sensor_iden_load)
+   * (2022-02-18) Automatically saving in service(/xarm/ft_sensor_cali_load) and add torque related service(/xarm/ft_sensor_iden_load)
    * (2023-02-27) Added service to control Lite6 Gripper(/ufactory/open_lite6_gripper, /ufactory/close_lite6_gripper, /ufactory/stop_lite6_gripper)(Note: Once stop, close will be invalid, you must open first to enable control)
    * (2023-03-29) Added the launch parameter model1300 (default is false), and replaced the model of the end of the xarm robot arm with the 1300 series
+   * (2023-04-20) Update the URDF file, adapt to ROS1 and ROS2, and load the inertia parameters of the link from the configuration file according to the SN
+   * (2023-04-20) Added the launch parameter `add_d435i_camera_link` (default is false), decide whether to increase the link relationship of D435i, it is only useful when add_realsense_d435i is true
+   * (2023-04-20) Added the launch parameter `robot_sn` to adapt the inertial parameters of the link
+   * (2023-04-20) Add launch parameters `attach_to`/`attach_xyz`/`attach_rpy` to support attaching robot models to other models
 
 # 3. Preparations before using this package
 
@@ -749,9 +753,9 @@ Please note it will use previously mentioned sample handeye calibration result,
 
 ## 7.4 Adding RealSense D435i model to simulated xArm：
 For installation with camera stand provided by UFACTORY, the cam model can be attached by following modifications (use xarm7 as example):    
-1.Together with xArm Gripper model: Set `add_realsense_d435i` default value to be `true` in [xarm7_with_gripper.xacro](./xarm_description/urdf/xarm7_with_gripper.xacro).  
-2.Together with xArm Vacuum Gripper model: Set `add_realsense_d435i` default value to be `true` in [xarm7_with_vacuum_gripper.xacro](./xarm_description/urdf/xarm7_with_vacuum_gripper.xacro).  
-3.Purely the d435i: Set `add_realsense_d435i` default value to be `true` in [xarm7_robot.urdf.xacro](./xarm_description/urdf/xarm7_robot.urdf.xacro).  
+```bash
+ $ roslaunch xarm7_moveit_config demo.launch add_realsense_d435i:=true
+```
 
 ## 7.5 Color Cube Grasping Demo
 

diff --git a/ReadMe_cn.md b/ReadMe_cn.md
@@ -82,6 +82,10 @@
    * (2023-02-18) 给service(/xarm/ft_sensor_cali_load)增加保存操作, 增加力矩相关service(/xarm/ft_sensor_iden_load)
    * (2023-02-27) 增加控制Lite6 Gripper的service(/ufactory/open_lite6_gripper, /ufactory/close_lite6_gripper, /ufactory/stop_lite6_gripper)(注： 一旦stop之后，close将无效，必须先open才能启用控制)
    * (2023-03-29) 新增launch启动参数model1300(默认为false), 更换xarm机械臂末端模型为1300系列的
+   * (2023-04-20) 更新URDF文件，适配ROS1和ROS2，并根据SN从配置文件加载连杆的惯性参数
+   * (2023-04-20) 新增launch启动参数`add_d435i_camera_link`(默认为false), 决定是否增加D435i的连杆关系，在add_realsense_d435i为true时才有用
+   * (2023-04-20) 新增launch启动参数`robot_sn`来适配连杆的惯性参数
+   * (2023-04-20) 新增launch启动参数 `attach_to`/`attach_xyz`/`attach_rpy`支持把机械臂模型附在其它模型之上
 
 # 3. 准备工作
 
@@ -729,9 +733,9 @@ $ roslaunch d435i_xarm_setup grasp_node_xarm_api.launch
 
 ## 7.4 在仿真的xArm模型末端添加RealSense D435i模型：
 如果使用UFACTORY提供的camera stand固定，可以通过以下设置添加到虚拟模型（以xarm7为例）：  
-1.同时带机械爪的模型： 设置[xarm7_with_gripper.xacro](./xarm_description/urdf/xarm7_with_gripper.xacro)的`add_realsense_d435i`参数为`true`。  
-2.同时带真空吸头的模型： 设置[xarm7_with_vacuum_gripper.xacro](./xarm_description/urdf/xarm7_with_vacuum_gripper.xacro)的`add_realsense_d435i`参数为`true`。  
-3.单纯附加相机在末端： 设置[xarm7_robot.urdf.xacro](./xarm_description/urdf/xarm7_robot.urdf.xacro)中`add_realsense_d435i`的默认值为`true`。  
+```bash
+ $ roslaunch xarm7_moveit_config demo.launch add_realsense_d435i:=true
+```  
 
 ## 7.5 颜色块抓取例子
 

diff --git a/dual_xarm6_moveit_config/.setup_assistant b/dual_xarm6_moveit_config/.setup_assistant
@@ -1,11 +1,11 @@
 moveit_setup_assistant_config:
   URDF:
     package: xarm_description
-    relative_path: urdf/dual_xarm6_robot.urdf.xacro
-    xacro_args: "--inorder "
+    relative_path: urdf/dual_xarm_device.urdf.xacro
+    xacro_args: "--inorder dof_1:=6 dof_2:=6"
   SRDF:
-    relative_path: config/xarm6.srdf
+    relative_path: config/dual_xarm6.srdf
   CONFIG:
-    author_name: jason_peng
+    author_name: Jason Peng
     author_email: [email protected]
     generated_timestamp: 1587982483
diff --git a/dual_xarm6_moveit_config/config/dual_xarm6.srdf b/dual_xarm6_moveit_config/config/dual_xarm6.srdf
@@ -0,0 +1,98 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from src/xarm_ros/xarm_description/srdf/dual_xarm.srdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="dual_xarm">
+  <group name="L_xarm6">
+    <joint name="L_world_joint"/>
+    <joint name="L_joint1"/>
+    <joint name="L_joint2"/>
+    <joint name="L_joint3"/>
+    <joint name="L_joint4"/>
+    <joint name="L_joint5"/>
+    <joint name="L_joint6"/>
+  </group>
+  <group name="L_tool_end">
+    <joint name="L_joint_eef"/>
+  </group>
+  <end_effector group="L_tool_end" name="L_end_effector" parent_link="L_link6"/>
+  <!-- GROUP STATES, Purpose, Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms' -->
+  <group_state group="L_xarm6" name="home">
+    <joint name="L_joint1" value="0"/>
+    <joint name="L_joint2" value="0"/>
+    <joint name="L_joint3" value="0"/>
+    <joint name="L_joint4" value="0"/>
+    <joint name="L_joint5" value="0"/>
+    <joint name="L_joint6" value="0"/>
+  </group_state>
+  <group_state group="L_xarm6" name="hold-up">
+    <joint name="L_joint1" value="0"/>
+    <joint name="L_joint2" value="0"/>
+    <joint name="L_joint3" value="0"/>
+    <joint name="L_joint4" value="0"/>
+    <joint name="L_joint5" value="-1.5708"/>
+    <joint name="L_joint6" value="0"/>
+  </group_state>
+  <disable_collisions link1="L_link1" link2="L_link2" reason="Adjacent"/>
+  <disable_collisions link1="L_link1" link2="L_link3" reason="Never"/>
+  <disable_collisions link1="L_link1" link2="L_link_base" reason="Adjacent"/>
+  <disable_collisions link1="L_link2" link2="L_link3" reason="Adjacent"/>
+  <disable_collisions link1="L_link2" link2="L_link4" reason="Never"/>
+  <disable_collisions link1="L_link2" link2="L_link_base" reason="Never"/>
+  <disable_collisions link1="L_link3" link2="L_link4" reason="Adjacent"/>
+  <disable_collisions link1="L_link3" link2="L_link5" reason="Never"/>
+  <disable_collisions link1="L_link3" link2="L_link6" reason="Never"/>
+  <disable_collisions link1="L_link4" link2="L_link5" reason="Adjacent"/>
+  <disable_collisions link1="L_link4" link2="L_link6" reason="Never"/>
+  <disable_collisions link1="L_link5" link2="L_link6" reason="Adjacent"/>
+  <disable_collisions link1="L_link3" link2="L_link_eef" reason="Never"/>
+  <disable_collisions link1="L_link5" link2="L_link_eef" reason="Never"/>
+  <disable_collisions link1="L_link6" link2="L_link_eef" reason="Adjacent"/>
+  <group name="R_xarm6">
+    <joint name="R_world_joint"/>
+    <joint name="R_joint1"/>
+    <joint name="R_joint2"/>
+    <joint name="R_joint3"/>
+    <joint name="R_joint4"/>
+    <joint name="R_joint5"/>
+    <joint name="R_joint6"/>
+  </group>
+  <group name="R_tool_end">
+    <joint name="R_joint_eef"/>
+  </group>
+  <end_effector group="R_tool_end" name="R_end_effector" parent_link="R_link6"/>
+  <!-- GROUP STATES, Purpose, Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms' -->
+  <group_state group="R_xarm6" name="home">
+    <joint name="R_joint1" value="0"/>
+    <joint name="R_joint2" value="0"/>
+    <joint name="R_joint3" value="0"/>
+    <joint name="R_joint4" value="0"/>
+    <joint name="R_joint5" value="0"/>
+    <joint name="R_joint6" value="0"/>
+  </group_state>
+  <group_state group="R_xarm6" name="hold-up">
+    <joint name="R_joint1" value="0"/>
+    <joint name="R_joint2" value="0"/>
+    <joint name="R_joint3" value="0"/>
+    <joint name="R_joint4" value="0"/>
+    <joint name="R_joint5" value="-1.5708"/>
+    <joint name="R_joint6" value="0"/>
+  </group_state>
+  <disable_collisions link1="R_link1" link2="R_link2" reason="Adjacent"/>
+  <disable_collisions link1="R_link1" link2="R_link3" reason="Never"/>
+  <disable_collisions link1="R_link1" link2="R_link_base" reason="Adjacent"/>
+  <disable_collisions link1="R_link2" link2="R_link3" reason="Adjacent"/>
+  <disable_collisions link1="R_link2" link2="R_link4" reason="Never"/>
+  <disable_collisions link1="R_link2" link2="R_link_base" reason="Never"/>
+  <disable_collisions link1="R_link3" link2="R_link4" reason="Adjacent"/>
+  <disable_collisions link1="R_link3" link2="R_link5" reason="Never"/>
+  <disable_collisions link1="R_link3" link2="R_link6" reason="Never"/>
+  <disable_collisions link1="R_link4" link2="R_link5" reason="Adjacent"/>
+  <disable_collisions link1="R_link4" link2="R_link6" reason="Never"/>
+  <disable_collisions link1="R_link5" link2="R_link6" reason="Adjacent"/>
+  <disable_collisions link1="R_link3" link2="R_link_eef" reason="Never"/>
+  <disable_collisions link1="R_link5" link2="R_link_eef" reason="Never"/>
+  <disable_collisions link1="R_link6" link2="R_link_eef" reason="Adjacent"/>
+</robot>
+
diff --git a/dual_xarm6_moveit_config/launch/demo.launch b/dual_xarm6_moveit_config/launch/demo.launch
@@ -1,7 +1,14 @@
 <launch>
   <arg name="ns" default="" />
   <arg name="jnt_stat_pub_rate" default="10" />
-  <arg name="model1300" default="false" />
+  <arg name="model1300_1" default="false" />
+  <arg name="model1300_2" default="false" />
+  <arg name="add_realsense_d435i_1" default="false" />
+  <arg name="add_realsense_d435i_2" default="false" />
+  <arg name="add_d435i_camera_link_1" default="false" />
+  <arg name="add_d435i_camera_link_2" default="false" />
+  <arg name="robot_sn_1" default="" />
+  <arg name="robot_sn_2" default="" />
 
   <remap from="$(arg ns)/source_list" to="/source_list" />
   <include file="$(find dual_xarm6_moveit_config)/launch/moveit_rviz_common.launch">
@@ -11,7 +18,14 @@
     <arg name="load_move_group" value="true" />
     <!-- whether to use fake_execution controller to drive the motion -->
     <arg name="fake_execution" value="true" />
-    <arg name="model1300" value="$(arg model1300)" />
+    <arg name="model1300_1" value="$(arg model1300_1)" />
+    <arg name="model1300_2" value="$(arg model1300_2)" />
+    <arg name="add_realsense_d435i_1" value="$(arg add_realsense_d435i_1)"/>
+    <arg name="add_realsense_d435i_2" value="$(arg add_realsense_d435i_2)"/>
+    <arg name="add_d435i_camera_link_1" value="$(arg add_d435i_camera_link_1)"/>
+    <arg name="add_d435i_camera_link_2" value="$(arg add_d435i_camera_link_2)"/>
+    <arg name="robot_sn_1" value="$(arg robot_sn_1)"/>
+    <arg name="robot_sn_2" value="$(arg robot_sn_2)"/>
   </include>
 
 </launch>
diff --git a/dual_xarm6_moveit_config/launch/moveit_rviz_common.launch b/dual_xarm6_moveit_config/launch/moveit_rviz_common.launch
@@ -24,12 +24,26 @@
   <arg name="NO_GUI_CTRL" default="false"/>
   <!-- option to show rviz or not -->
   <arg name="show_rviz" default="true" />
-  <arg name="model1300" default="false" />
+  <arg name="model1300_1" default="false" />
+  <arg name="model1300_2" default="false" />
+  <arg name="add_realsense_d435i_1" default="false" />
+  <arg name="add_realsense_d435i_2" default="false" />
+  <arg name="add_d435i_camera_link_1" default="false" />
+  <arg name="add_d435i_camera_link_2" default="false" />
+  <arg name="robot_sn_1" default="" />
+  <arg name="robot_sn_2" default="" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server, limit joint range within [-pi, pi] -->
   <include file="$(find dual_xarm6_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
-    <arg name="model1300" value="$(arg model1300)"/>
+    <arg name="model1300_1" value="$(arg model1300_1)" />
+    <arg name="model1300_2" value="$(arg model1300_2)" />
+    <arg name="add_realsense_d435i_1" value="$(arg add_realsense_d435i_1)"/>
+    <arg name="add_realsense_d435i_2" value="$(arg add_realsense_d435i_2)"/>
+    <arg name="add_d435i_camera_link_1" value="$(arg add_d435i_camera_link_1)"/>
+    <arg name="add_d435i_camera_link_2" value="$(arg add_d435i_camera_link_2)"/>
+    <arg name="robot_sn_1" value="$(arg robot_sn_1)"/>
+    <arg name="robot_sn_2" value="$(arg robot_sn_2)"/>
   </include>
 
   <!-- If needed, broadcast static tf for robot root -->

diff --git a/dual_xarm6_moveit_config/launch/planning_context.launch b/dual_xarm6_moveit_config/launch/planning_context.launch
@@ -1,16 +1,33 @@
 <launch>
   <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
   <arg name="load_robot_description" default="false"/>
-  <arg name="model1300" default="false"/>
+  <arg name="model1300_1" default="false" />
+  <arg name="model1300_2" default="false" />
+  <arg name="add_realsense_d435i_1" default="false" />
+  <arg name="add_realsense_d435i_2" default="false" />
+  <arg name="add_d435i_camera_link_1" default="false" />
+  <arg name="add_d435i_camera_link_2" default="false" />
+  <arg name="robot_sn_1" default="" />
+  <arg name="robot_sn_2" default="" />
 
   <!-- The name of the parameter under which the URDF is loaded -->
   <arg name="robot_description" default="robot_description"/>
 
   <!-- Load universal robot description format (URDF) -->
-  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro --inorder  '$(find xarm_description)/urdf/dual_xarm6_robot.urdf.xacro' limited:=true model1300:=$(arg model1300)"/>
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)" 
+    command="xacro --inorder  '$(find xarm_description)/urdf/dual_xarm_device.urdf.xacro' dof_1:=6 dof_2:=6 limited:=true 
+      model1300_1:=$(arg model1300_1)
+      model1300_2:=$(arg model1300_2)
+      add_realsense_d435i_1:=$(arg add_realsense_d435i_1)
+      add_realsense_d435i_2:=$(arg add_realsense_d435i_2)
+      add_d435i_camera_link_1:=$(arg add_d435i_camera_link_1)
+      add_d435i_camera_link_2:=$(arg add_d435i_camera_link_2)
+      robot_sn_1:=$(arg robot_sn_1)
+      robot_sn_2:=$(arg robot_sn_2)
+    "/>
 
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find dual_xarm6_moveit_config)/config/xarm6.srdf" />
+  <param if="$(arg load_robot_description)" name="$(arg robot_description)_semantic" command="xacro --inorder  '$(find xarm_description)/srdf/dual_xarm.srdf.xacro' dof_1:=6 dof_2:=6" />
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">

diff --git a/dual_xarm6_moveit_config/launch/realMove_exec.launch b/dual_xarm6_moveit_config/launch/realMove_exec.launch
@@ -17,7 +17,14 @@
   <arg name="no_gui_plan" default="false" />
   <arg name="ext_ns" default="" />
   <arg name="jnt_stat_pub_rate" default="10" />
-  <arg name="model1300" default="false" />
+  <arg name="model1300_1" default="false" />
+  <arg name="model1300_2" default="false" />
+  <arg name="add_realsense_d435i_1" default="false" />
+  <arg name="add_realsense_d435i_2" default="false" />
+  <arg name="add_d435i_camera_link_1" default="false" />
+  <arg name="add_d435i_camera_link_2" default="false" />
+  <arg name="robot_sn_1" default="" />
+  <arg name="robot_sn_2" default="" />
 
   <!-- common: bring up node to enable real xArm control -->
   <include file="$(find xarm_bringup)/launch/dual_xarm6_server.launch">
@@ -62,7 +69,14 @@
     <arg name="show_rviz" value="$(arg show_rviz)" />
     <arg name="NO_GUI_CTRL" value="$(arg no_gui_plan)"/>
     <arg name="jnt_stat_pub_rate" value="$(arg jnt_stat_pub_rate)"/>
-    <arg name="model1300" value="$(arg model1300)"/>
+    <arg name="model1300_1" value="$(arg model1300_1)" />
+    <arg name="model1300_2" value="$(arg model1300_2)" />
+    <arg name="add_realsense_d435i_1" value="$(arg add_realsense_d435i_1)"/>
+    <arg name="add_realsense_d435i_2" value="$(arg add_realsense_d435i_2)"/>
+    <arg name="add_d435i_camera_link_1" value="$(arg add_d435i_camera_link_1)"/>
+    <arg name="add_d435i_camera_link_2" value="$(arg add_d435i_camera_link_2)"/>
+    <arg name="robot_sn_1" value="$(arg robot_sn_1)"/>
+    <arg name="robot_sn_2" value="$(arg robot_sn_2)"/>
   </include>
 
 </launch>
diff --git a/dual_xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch b/dual_xarm6_moveit_config/launch/xarm6_moveit_gazebo.launch
@@ -1,14 +1,28 @@
 <launch>
-  <arg name="model1300" default="false" />
+  <arg name="model1300_1" default="false" />
+  <arg name="model1300_2" default="false" />
+  <arg name="add_realsense_d435i_1" default="false" />
+  <arg name="add_realsense_d435i_2" default="false" />
+  <arg name="add_d435i_camera_link_1" default="false" />
+  <arg name="add_d435i_camera_link_2" default="false" />
+  <arg name="robot_sn_1" default="" />
+  <arg name="robot_sn_2" default="" />
   <!-- Remap the name space so it can send command to gazebo loaded controller -->
   <!-- this remap must appear BEFORE move_group launch -->
   <remap from="/follow_joint_trajectory" to="/xarm/xarm6_traj_controller/follow_joint_trajectory"/>
 
-  <include file="$(find xarm6_moveit_config)/launch/moveit_rviz_common.launch">
+  <include file="$(find dual_xarm6_moveit_config)/launch/moveit_rviz_common.launch">
     <arg name="jnt_stat_source" value="[/xarm/joint_states]" />
     <arg name="load_move_group" value="true" />
     <arg name="pub_tf" value="false" />
-    <arg name="model1300" value="$(arg model1300)"/>
+    <arg name="model1300_1" value="$(arg model1300_1)" />
+    <arg name="model1300_2" value="$(arg model1300_2)" />
+    <arg name="add_realsense_d435i_1" value="$(arg add_realsense_d435i_1)"/>
+    <arg name="add_realsense_d435i_2" value="$(arg add_realsense_d435i_2)"/>
+    <arg name="add_d435i_camera_link_1" value="$(arg add_d435i_camera_link_1)"/>
+    <arg name="add_d435i_camera_link_2" value="$(arg add_d435i_camera_link_2)"/>
+    <arg name="robot_sn_1" value="$(arg robot_sn_1)"/>
+    <arg name="robot_sn_2" value="$(arg robot_sn_2)"/>
   </include>
 
 

diff --git a/examples/xarm5_vacuum_gripper_moveit_config/.setup_assistant b/examples/xarm5_vacuum_gripper_moveit_config/.setup_assistant
@@ -1,11 +1,11 @@
 moveit_setup_assistant_config:
   URDF:
     package: xarm_description
-    relative_path: urdf/xarm5_with_vacuum_gripper.xacro
-    xacro_args: "--inorder "
+    relative_path: urdf/xarm_device.urdf.xacro
+    xacro_args: "--inorder robot_type:=xarm dof:=5 add_vacuum_gripper:=true"
   SRDF:
     relative_path: config/xarm5_with_vacuum_gripper.srdf
   CONFIG:
-    author_name: jason peng
+    author_name: Jason Peng
     author_email: [email protected]
     generated_timestamp: 1607073727