From 4c35d2c2c0b7d0905755f44eeebee2bfe2b3a570 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Mon, 27 Nov 2023 16:10:57 -0500
Subject: [PATCH] Enable initial position param in kinova xacros (#190)

---
 kortex_description/CMakeLists.txt                | 2 +-
 kortex_description/config/initial_positions.yaml | 8 ++++++++
 kortex_description/robots/gen3.xacro             | 7 ++++++-
 kortex_description/robots/kinova.urdf.xacro      | 9 ++++++++-
 kortex_description/robots/kortex_robot.xacro     | 4 +++-
 5 files changed, 26 insertions(+), 4 deletions(-)
 create mode 100644 kortex_description/config/initial_positions.yaml

diff --git a/kortex_description/CMakeLists.txt b/kortex_description/CMakeLists.txt
index 4813a4cc..2bab1505 100644
--- a/kortex_description/CMakeLists.txt
+++ b/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}
 )
 
diff --git a/kortex_description/config/initial_positions.yaml b/kortex_description/config/initial_positions.yaml
new file mode 100644
index 00000000..be3f79b7
--- /dev/null
+++ b/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
diff --git a/kortex_description/robots/gen3.xacro b/kortex_description/robots/gen3.xacro
index a08710c3..d6d36e51 100644
--- a/kortex_description/robots/gen3.xacro
+++ b/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>
 
diff --git a/kortex_description/robots/kinova.urdf.xacro b/kortex_description/robots/kinova.urdf.xacro
index 4a5500c2..371abc81 100644
--- a/kortex_description/robots/kinova.urdf.xacro
+++ b/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>
 
diff --git a/kortex_description/robots/kortex_robot.xacro b/kortex_description/robots/kortex_robot.xacro
index 80216217..0d94550b 100644
--- a/kortex_description/robots/kortex_robot.xacro
+++ b/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>