Fix mock_hardware and enable simulating gen3_lite (#196)

* Add additional guards if user sets use_fake_hardware and use_internal_bus_gripper_comm true
Kinovarobotics · Nov 28, 2023 · 2f3f056 · 2f3f056
1 parent 0e899f0
commit 2f3f056
Show file tree

Hide file tree

Showing 11 changed files with 194 additions and 21 deletions.
diff --git a/README.md b/README.md
@@ -199,6 +199,17 @@ ros2 launch kinova_gen3_6dof_robotiq_2f_85_moveit_config robot.launch.py \
   use_fake_hardware:=true
 ```
 
+Alternatively, if you wish to use the Kinova Gen3_lite's 6 DoF variant:
+
+```bash
+ros2 launch kortex_bringup gen3.launch.py \
+  robot_ip:=yyy.yyy.yyy.yyy \
+  use_fake_hardware:=true \
+  robot_type:=gen3_lite \
+  gripper:=gen3_lite_2f \
+  dof:=6
+```
+
 To simulate the 7dof Kinova Gen3 robot with ignition run the following:
 
 ```bash

diff --git a/kortex_bringup/launch/gen3.launch.py b/kortex_bringup/launch/gen3.launch.py
@@ -23,6 +23,14 @@
 def generate_launch_description():
     # Declare arguments
     declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_type",
+            default_value="gen3",
+            description="Type/series of robot.",
+            choices=["gen3", "gen3_lite"],
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_ip",
@@ -64,8 +72,9 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "gripper",
-            default_value='"robotiq_2f_85"',
+            default_value="robotiq_2f_85",
             description="Name of the gripper attached to the arm",
+            choices=["robotiq_2f_85", "gen3_lite_2f"],
         )
     )
     declared_arguments.append(
@@ -101,6 +110,7 @@ def generate_launch_description():
     )
 
     # Initialize Arguments
+    robot_type = LaunchConfiguration("robot_type")
     robot_ip = LaunchConfiguration("robot_ip")
     dof = LaunchConfiguration("dof")
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
@@ -117,7 +127,7 @@ def generate_launch_description():
     base_launch = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([ThisLaunchFileDir(), "/kortex_control.launch.py"]),
         launch_arguments={
-            "robot_type": "gen3",
+            "robot_type": robot_type,
             "robot_ip": robot_ip,
             "dof": dof,
             "use_fake_hardware": use_fake_hardware,

diff --git a/kortex_bringup/launch/kortex_control.launch.py b/kortex_bringup/launch/kortex_control.launch.py
@@ -56,6 +56,10 @@ def launch_setup(context, *args, **kwargs):
     use_internal_bus_gripper_comm = LaunchConfiguration("use_internal_bus_gripper_comm")
     gripper_joint_name = LaunchConfiguration("gripper_joint_name")
 
+    # if we are using fake hardware then we can't use the internal gripper communications of the hardware
+    if use_fake_hardware.parse:
+        use_internal_bus_gripper_comm = "false"
+
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -107,7 +111,7 @@ def launch_setup(context, *args, **kwargs):
     robot_controllers = PathJoinSubstitution(
         [
             FindPackageShare(description_package),
-            "arms/gen3/" + dof.perform(context) + "dof/config",
+            "arms/" + robot_type.perform(context) + "/" + dof.perform(context) + "dof/config",
             controllers_file,
         ]
     )
@@ -174,13 +178,14 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         arguments=[robot_hand_controller, "-c", "/controller_manager"],
-        condition=IfCondition(use_internal_bus_gripper_comm),
     )
 
+    # only start the fault controller if we are using hardware
     fault_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=[fault_controller, "-c", "/controller_manager"],
+        condition=IfCondition(use_internal_bus_gripper_comm),
     )
 
     nodes_to_start = [

diff --git a/kortex_description/arms/gen3/7dof/urdf/kortex.ros2_control.xacro b/kortex_description/arms/gen3/7dof/urdf/kortex.ros2_control.xacro
@@ -136,13 +136,15 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
-      <xacro:if value="${use_internal_bus_gripper_comm}">
-        <joint name="${prefix}${gripper_joint_name}">
-          <command_interface name="position" />
-          <state_interface name="position"/>
-          <state_interface name="velocity"/>
-        </joint>
-      </xacro:if>
+      <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
+        <xacro:if value="${use_internal_bus_gripper_comm}">
+          <joint name="${prefix}${gripper_joint_name}">
+            <command_interface name="position" />
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+          </joint>
+        </xacro:if>
+      </xacro:unless>
     </ros2_control>
   </xacro:macro>
 

diff --git a/kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml b/kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml
@@ -0,0 +1,56 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    twist_controller:
+      type: picknik_twist_controller/PicknikTwistController
+
+    robotiq_gripper_controller:
+      type: position_controllers/GripperActionController
+
+    fault_controller:
+      type: picknik_reset_fault_controller/PicknikResetFaultController
+
+joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 100.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    constraints:
+      stopped_velocity_tolerance: 0.0
+      goal_time: 0.0
+
+twist_controller:
+  ros__parameters:
+    joint: tcp
+    interface_names:
+      - twist.linear.x
+      - twist.linear.y
+      - twist.linear.z
+      - twist.angular.x
+      - twist.angular.y
+      - twist.angular.z
+
+robotiq_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: right_finger_bottom_joint
+    allow_stalling: true
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
@@ -125,13 +125,15 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
-      <xacro:if value="${use_internal_bus_gripper_comm}">
-      <joint name="${prefix}right_finger_bottom_joint">
-        <command_interface name="position"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
-      </xacro:if>
+      <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
+        <xacro:if value="${use_internal_bus_gripper_comm}">
+          <joint name="${prefix}right_finger_bottom_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+          </joint>
+        </xacro:if>
+      </xacro:unless>
     </ros2_control>
   </xacro:macro>
 

diff --git a/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f.ros2_control.xacro b/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f.ros2_control.xacro
@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="gripper_ros2_control" params="
+        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:=false">
+
+        <ros2_control name="GripperHardwareInterface" type="system">
+            <hardware>
+                <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>
+                    <param name="trigger_joint_command_threshold">0.02</param>
+                </xacro:if>
+                <xacro:if value="${sim_ignition}">
+                    <plugin>ign_ros2_control/IgnitionSystem</plugin>
+                </xacro:if>
+                <xacro:if value="${use_fake_hardware}">
+                    <plugin>mock_components/GenericSystem</plugin>
+                    <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+                    <param name="state_following_offset">0.0</param>
+                </xacro:if>
+            </hardware>
+
+            <!-- Joint interfaces -->
+            <!-- With Ignition or Hardware, they handle mimic joints, so we only need this command interface activated -->
+            <joint name="${prefix}right_finger_bottom_joint">
+                <command_interface name="position" />
+                <state_interface name="position">
+                    <param name="initial_value">0.85</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <!-- When simulating we need to include the rest of the gripper joints -->
+            <xacro:if value="${use_fake_hardware or sim_isaac or sim_ignition}">
+                <joint name="${prefix}right_finger_tip_joint">
+                    <param name="mimic">${prefix}right_finger_bottom_joint</param>
+                    <param name="multiplier">-1</param>
+                    <xacro:unless value="${sim_ignition}">
+                        <command_interface name="position"/>
+                        <state_interface name="position"/>
+                        <state_interface name="velocity"/>
+                    </xacro:unless>
+                </joint>
+                <joint name="${prefix}left_finger_bottom_joint">
+                    <param name="mimic">${prefix}right_finger_bottom_joint</param>
+                    <param name="multiplier">1</param>
+                    <xacro:unless value="${sim_ignition}">
+                        <command_interface name="position"/>
+                        <state_interface name="position"/>
+                        <state_interface name="velocity"/>
+                    </xacro:unless>
+                </joint>
+                <joint name="${prefix}left_finger_tip_joint">
+                    <param name="mimic">${prefix}right_finger_bottom_joint</param>
+                    <param name="multiplier">-1</param>
+                    <xacro:unless value="${sim_ignition}">
+                        <command_interface name="position"/>
+                        <state_interface name="position"/>
+                        <state_interface name="velocity"/>
+                    </xacro:unless>
+                </joint>
+            </xacro:if>
+        </ros2_control>
+    </xacro:macro>
+
+</robot>
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
@@ -12,6 +12,17 @@
     isaac_joint_states:=/isaac_joint_states
     use_internal_bus_gripper_comm:=true">
 
+    <xacro:include filename="$(find kortex_description)/grippers/gen3_lite_2f/urdf/gen3_lite_2f.ros2_control.xacro" />
+    <xacro:gripper_ros2_control
+      prefix="${prefix}"
+      use_fake_hardware="${use_fake_hardware}"
+      fake_sensor_commands="${fake_sensor_commands}"
+      sim_ignition="${sim_ignition}"
+      sim_isaac="${sim_isaac}"
+      isaac_joint_commands="${isaac_joint_commands}"
+      isaac_joint_states="${isaac_joint_states}"
+      use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"/>
+
     <!-- Tool frame used by the arm -->
     <link name="${prefix}tool_frame"/>
     <joint name="${prefix}tool_frame_joint" type="fixed">

diff --git a/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_macro.xacro b/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_2f_140_macro.xacro
@@ -8,7 +8,8 @@
     sim_ignition:=false
     sim_isaac:=false
     isaac_joint_commands:=/isaac_joint_commands
-    isaac_joint_states:=/isaac_joint_states">
+    isaac_joint_states:=/isaac_joint_states
+    use_internal_bus_gripper_comm:=false">
     <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
     <!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->

diff --git a/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_macro.xacro b/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_2f_85_macro.xacro
@@ -9,7 +9,7 @@
     sim_isaac:=false
     isaac_joint_commands:=/isaac_joint_commands
     isaac_joint_states:=/isaac_joint_states
-    use_internal_bus_gripper_comm:=true">
+    use_internal_bus_gripper_comm:=false">
     <xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_85_macro.urdf.xacro" />
 
     <!-- Hardware talks directly to the gripper so we don't need ros2_control unless we are simulating -->

diff --git a/kortex_description/robots/kortex_robot.xacro b/kortex_description/robots/kortex_robot.xacro
@@ -86,7 +86,8 @@
         sim_ignition="${sim_ignition}"
         sim_isaac="${sim_isaac}"
         isaac_joint_commands="${isaac_joint_commands}"
-        isaac_joint_states="${isaac_joint_states}"/>
+        isaac_joint_states="${isaac_joint_states}"
+        use_internal_bus_gripper_comm="${use_internal_bus_gripper_comm}"/>
     </xacro:unless>
 
   </xacro:macro>