From 0e899f055cdb4243c4429894457bcbe29b33fff5 Mon Sep 17 00:00:00 2001
From: David Yackzan <dwyackzan@gmail.com>
Date: Tue, 28 Nov 2023 13:31:22 -0500
Subject: [PATCH] Update gen3 lite and gripper macros (#191)

---
 .../gen3_lite/6dof/urdf/gen3_lite_macro.xacro | 15 ++++++-
 .../6dof/urdf/kortex.ros2_control.xacro       | 43 +++++++------------
 .../urdf/gen3_lite_2f_macro.xacro             | 11 ++++-
 3 files changed, 40 insertions(+), 29 deletions(-)

diff --git a/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro
index 9b481db8..2e4b3f4b 100644
--- a/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro
+++ b/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro
@@ -19,10 +19,17 @@
     session_inactivity_timeout_ms:=6000
     connection_inactivity_timeout_ms:=2000
     use_internal_bus_gripper_comm:=false
+    gripper_joint_name
+    gripper_max_velocity:=100.0
+    gripper_max_force:=100.0
     use_fake_hardware:=false
     fake_sensor_commands:=false
     sim_gazebo:=false
     sim_ignition:=false
+    sim_isaac:=false
+    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)}" >
 
     <!-- ros2 control include -->
@@ -42,6 +49,9 @@
       fake_sensor_commands="${fake_sensor_commands}"
       sim_gazebo="${sim_gazebo}"
       sim_ignition="${sim_ignition}"
+      sim_isaac="${sim_isaac}"
+      isaac_joint_commands="${isaac_joint_commands}"
+      isaac_joint_states="${isaac_joint_states}"
       tf_prefix=""
       initial_positions="${initial_positions}"
       robot_ip="${robot_ip}"
@@ -51,7 +61,10 @@
       port_realtime="${port_realtime}"
       session_inactivity_timeout_ms="${session_inactivity_timeout_ms}"
       connection_inactivity_timeout_ms="${connection_inactivity_timeout_ms}"
-      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}" />
+      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"
+      gripper_max_velocity="${gripper_max_velocity}"
+      gripper_max_force="${gripper_max_force}"
+      gripper_joint_name="${gripper_joint_name}"/>
 
     <joint name="${prefix}base_joint" type="fixed">
       <xacro:insert_block name="origin" />
diff --git a/kortex_description/arms/gen3_lite/6dof/urdf/kortex.ros2_control.xacro b/kortex_description/arms/gen3_lite/6dof/urdf/kortex.ros2_control.xacro
index 891ce2bb..62bd82d7 100644
--- a/kortex_description/arms/gen3_lite/6dof/urdf/kortex.ros2_control.xacro
+++ b/kortex_description/arms/gen3_lite/6dof/urdf/kortex.ros2_control.xacro
@@ -8,6 +8,9 @@
     fake_sensor_commands:=false
     sim_gazebo:=false
     sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
     use_internal_bus_gripper_comm:=false
     tf_prefix
     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)}
@@ -17,7 +20,10 @@
     port
     port_realtime
     session_inactivity_timeout_ms
-    connection_inactivity_timeout_ms">
+    connection_inactivity_timeout_ms
+    gripper_joint_name
+    gripper_max_velocity:=100.0
+    gripper_max_force:=100.0">
 
     <ros2_control name="${name}" type="system">
       <hardware>
@@ -27,12 +33,17 @@
         <xacro:if value="${sim_ignition}">
           <plugin>ign_ros2_control/IgnitionSystem</plugin>
         </xacro:if>
+        <xacro:if value="${sim_isaac}">
+          <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
+          <param name="joint_commands_topic">${isaac_joint_commands}</param>
+          <param name="joint_states_topic">${isaac_joint_states}</param>
+        </xacro:if>
         <xacro:if value="${use_fake_hardware}">
           <plugin>fake_components/GenericSystem</plugin>
           <param name="fake_sensor_commands">${fake_sensor_commands}</param>
           <param name="state_following_offset">0.0</param>
         </xacro:if>
-        <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition}">
+        <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
           <plugin>kortex_driver/KortexMultiInterfaceHardware</plugin>
           <param name="robot_ip">${robot_ip}</param>
           <param name="username">${username}</param>
@@ -43,7 +54,9 @@
           <param name="connection_inactivity_timeout_ms">${connection_inactivity_timeout_ms}</param>
           <param name="tf_prefix">"${tf_prefix}"</param>
           <param name="use_internal_bus_gripper_comm">${use_internal_bus_gripper_comm}</param>
-          <param name="gripper_joint_name">${prefix}right_finger_bottom_joint</param>
+          <param name="gripper_joint_name">${gripper_joint_name}</param>
+          <param name="gripper_max_velocity">${gripper_max_velocity}</param>
+          <param name="gripper_max_force">${gripper_max_force}</param>
         </xacro:unless>
       </hardware>
       <joint name="${prefix}joint_1">
@@ -51,10 +64,6 @@
           <param name="min">-2.69</param>
           <param name="max">2.69</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-1.6</param>
-          <param name="max">1.6</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_1']}</param>
         </state_interface>
@@ -66,10 +75,6 @@
           <param name="min">-2.69</param>
           <param name="max">2.36</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-1.6</param>
-          <param name="max">1.6</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_2']}</param>
         </state_interface>
@@ -81,10 +86,6 @@
           <param name="min">-2.69</param>
           <param name="max">2.69</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-1.6</param>
-          <param name="max">1.6</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_3']}</param>
         </state_interface>
@@ -96,10 +97,6 @@
           <param name="min">-2.59</param>
           <param name="max">2.59</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-1.6</param>
-          <param name="max">1.6</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_4']}</param>
         </state_interface>
@@ -111,10 +108,6 @@
           <param name="min">-2.57</param>
           <param name="max">2.57</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-1.6</param>
-          <param name="max">1.6</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_5']}</param>
         </state_interface>
@@ -126,10 +119,6 @@
           <param name="min">-2.59</param>
           <param name="max">2.59</param>
         </command_interface>
-        <command_interface name="velocity">
-          <param name="min">-3.2</param>
-          <param name="max">3.2</param>
-        </command_interface>
         <state_interface name="position">
           <param name="initial_value">${initial_positions['joint_6']}</param>
         </state_interface>
diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
index 804df5a6..39bf3ebd 100644
--- a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
+++ b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro
@@ -1,7 +1,16 @@
 <?xml version="1.0"?>
 
 <robot name="gen3_lite_2f_gripper" xmlns:xacro="http://ros.org/wiki/xacro">
-  <xacro:macro name="load_gripper" params="parent prefix">
+  <xacro:macro name="load_gripper" params="
+    parent
+    prefix
+    use_fake_hardware:=false
+    fake_sensor_commands:=false
+    sim_ignition:=false
+    sim_isaac:=false
+    isaac_joint_commands:=/isaac_joint_commands
+    isaac_joint_states:=/isaac_joint_states
+    use_internal_bus_gripper_comm:=true">
 
     <!-- Tool frame used by the arm -->
     <link name="${prefix}tool_frame"/>