Enable initial position param in kinova xacros (Kinovarobotics#190)

Author: dyackzan

kortex_description/CMakeLists.txt

@@ -9,7 +9,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 
 install(
-  DIRECTORY arms grippers launch multiple_robots robots rviz
+  DIRECTORY arms grippers launch multiple_robots robots rviz config
   DESTINATION share/${PROJECT_NAME}
 )
 

kortex_description/config/initial_positions.yaml

@@ -0,0 +1,8 @@
+# NOTE: Used for both 6 and 7 dof bots. The joint_7 value will be unused for 6 dof bots
+joint_1: 0.0
+joint_2: 0.0
+joint_3: 0.0
+joint_4: 0.0
+joint_5: 0.0
+joint_6: 0.0
+joint_7: 0.0

kortex_description/robots/gen3.xacro

@@ -27,6 +27,10 @@
     <xacro:arg name="use_external_cable" default="false" />
 
     <xacro:include filename="$(find kortex_description)/robots/kortex_robot.xacro" />
+    <!-- initial position for simulations (Mock Hardware, Gazebo, Ignition) -->
+    <xacro:arg name="initial_positions_file" default="$(find kortex_description)/config/initial_positions.yaml"/>
+    <!-- convert to property to use substitution in function -->
+    <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
     <link name="world" />
     <xacro:load_robot
         parent="world"
@@ -51,7 +55,8 @@
         sim_gazebo="$(arg sim_gazebo)"
         sim_ignition="$(arg sim_ignition)"
         sim_isaac="$(arg sim_isaac)"
-        use_external_cable="$(arg use_external_cable)" >
+        use_external_cable="$(arg use_external_cable)"
+        initial_positions="${xacro.load_yaml(initial_positions_file)}" >
         <origin xyz="0 0 0" rpy="0 0 0" />  <!-- position robot in the world -->
     </xacro:load_robot>
 

kortex_description/robots/kinova.urdf.xacro

@@ -36,6 +36,12 @@
   <!-- import main macro -->
   <xacro:include filename="$(find kortex_description)/robots/kortex_robot.xacro" />
 
+  <!-- initial position for simulations (Mock Hardware, Gazebo, Ignition) -->
+  <xacro:arg name="initial_positions_file" default="$(find kortex_description)/config/initial_positions.yaml"/>
+
+  <!-- convert to property to use substitution in function -->
+  <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
+
   <!-- create link fixed to the "world" -->
   <link name="world" />
 
@@ -61,7 +67,8 @@
     fake_sensor_commands="$(arg fake_sensor_commands)"
     sim_gazebo="$(arg sim_gazebo)"
     sim_ignition="$(arg sim_ignition)"
-    sim_isaac="$(arg sim_isaac)" >
+    sim_isaac="$(arg sim_isaac)"
+    initial_positions="${xacro.load_yaml(initial_positions_file)}" >
     <origin xyz="0 0 0" rpy="0 0 0" />  <!-- position robot in the world -->
   </xacro:load_robot>
 

kortex_description/robots/kortex_robot.xacro

@@ -27,6 +27,7 @@
     isaac_joint_commands:=/isaac_joint_commands
     isaac_joint_states:=/isaac_joint_states
     use_external_cable:=false
+    initial_positions:=${dict(joint_1=0.0,joint_2=0.0,joint_3=0.0,joint_4=0.0,joint_5=0.0,joint_6=0.0,joint_7=0.0)}
     gripper_max_velocity:=100.0
     gripper_max_force:=100.0">
 
@@ -57,7 +58,8 @@
       gripper_joint_name="${gripper_joint_name}"
       gripper_max_velocity="${gripper_max_velocity}"
       gripper_max_force="${gripper_max_force}"
-      use_external_cable="${use_external_cable}">
+      use_external_cable="${use_external_cable}"
+      initial_positions="${initial_positions}">
       <xacro:insert_block name="origin" />
     </xacro:load_arm>