base link inertia +srdf robot name fix

kroshu · Aug 1, 2023 · 99fe58c · 99fe58c
1 parent ff604a0
commit 99fe58c
Show file tree

Hide file tree

Showing 4 changed files with 29 additions and 8 deletions.
diff --git a/kuka_agilus_support/urdf/kr6_r700_sixx_macro.xacro b/kuka_agilus_support/urdf/kr6_r700_sixx_macro.xacro
@@ -14,7 +14,8 @@
     client_port="${client_port}"
     use_fake_hardware="${use_fake_hardware}" />
 
-    <link name="${prefix}base_link">
+    <link name="${prefix}base_link"/>
+    <link name="${prefix}base_link_inertia">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0" />
         <geometry>
@@ -122,9 +123,14 @@
     <!-- Following REP199, this frame shall be use to attach EEF or other equipment -->
     <link name="${prefix}flange" />
 
+    <joint name="${prefix}base_link-base_link_inertia" type="fixed">
+      <parent link="${prefix}base_link" />
+      <child link="${prefix}base_link_inertia" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
     <joint name="${prefix}joint_a1" type="revolute">
       <origin xyz="0 0 0.4" rpy="0 0 0"/>
-      <parent link="${prefix}base_link"/>
+      <parent link="${prefix}base_link_inertia"/>
       <child link="${prefix}link_1"/>
       <axis xyz="0 0 -1"/>
       <limit lower="${radians(-170)}" upper="${radians(170)}" effort="0" velocity="${radians(360)}"/>

diff --git a/kuka_agilus_support/urdf/kr6_r900_sixx_macro.xacro b/kuka_agilus_support/urdf/kr6_r900_sixx_macro.xacro
@@ -14,7 +14,8 @@
     client_port="${client_port}"
     use_fake_hardware="${use_fake_hardware}" />
 
-    <link name="${prefix}base_link">
+    <link name="${prefix}base_link"/>
+    <link name="${prefix}base_link_inertia">
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
@@ -122,9 +123,14 @@
     <!-- Following REP199, this frame shall be use to attach EEF or other equipment -->
     <link name="${prefix}flange" />
 
+    <joint name="${prefix}base_link-base_link_inertia" type="fixed">
+      <parent link="${prefix}base_link" />
+      <child link="${prefix}base_link_inertia" />
+      <origin xyz="0 0 0" rpy="0 0 0" />
+    </joint>
     <joint name="${prefix}joint_a1" type="revolute">
       <origin xyz="0 0 0.400" rpy="0 0 0"/>
-      <parent link="${prefix}base_link"/>
+      <parent link="${prefix}base_link_inertia"/>
       <child link="${prefix}link_1"/>
       <axis xyz="0 0 -1"/>
       <limit effort="0" lower="${-radians(170)}" upper="${radians(170)}" velocity="${radians(360)}"/>

diff --git a/kuka_kr_moveit_config/CMakeLists.txt b/kuka_kr_moveit_config/CMakeLists.txt
@@ -6,7 +6,16 @@ find_package(ament_cmake REQUIRED)
 find_package(urdf REQUIRED)
 find_package(xacro REQUIRED)
 
-xacro_add_files(TARGET kr_arm srdf/kr_arm.srdf.xacro OUTPUT kr_arm.srdf INSTALL DESTINATION urdf)
+# TODO: add more robots to this list
+set(supported_robots
+  kr6_r700_sixx
+  kr6_r900_sixx
+  kr16_r2010-2
+  )
+
+foreach(model IN LISTS supported_robots)
+  xacro_add_files(TARGET ${model} srdf/kr_arm.srdf.xacro REMAP robot_name:=${model} OUTPUT ${model}_arm.srdf INSTALL DESTINATION urdf)
+endforeach()
 
 install(DIRECTORY config
   DESTINATION share/${PROJECT_NAME})

diff --git a/kuka_kr_moveit_config/srdf/kr_arm.srdf.xacro b/kuka_kr_moveit_config/srdf/kr_arm.srdf.xacro
@@ -4,8 +4,8 @@
     A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
 -->
 
-<!--TODO(Svastits): add an argument for the robot name instead of kr, as srdf model parsers logs an error now-->
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kr">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="$(arg robot_name)">
+  <xacro:arg name="prefix" default=""/>
   <xacro:include filename="kr_arm_macro.xacro" />
-  <xacro:kuka_kr_arm prefix=""/>
+  <xacro:kuka_kr_arm prefix="$(arg prefix)"/>
 </robot>