Fixed the Gazebo Fortress Simulation + Separated the arm and gripper …

…control for Gen3_Lite + Fixed bugs (#252) * Separated the arm and gripper controllers for Gen3_Lite in Kortex_bringup * Modified the Gen3_Lite related description and ros2_control files * Modified the Gen3_Lite Moveit Package for arm-gripper control separation
Kinovarobotics · Nov 6, 2024 · 5e9067c · 5e9067c
1 parent c0af316
commit 5e9067c
Show file tree

Hide file tree

Showing 30 changed files with 740 additions and 153 deletions.
diff --git a/kortex_bringup/launch/gen3_lite.launch.py b/kortex_bringup/launch/gen3_lite.launch.py
@@ -30,6 +30,13 @@ def generate_launch_description():
             description="Type/series of robot.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_hand_controller",
+            default_value="gen3_lite_2f_gripper_controller",
+            description="Gripper controller for the Gen3 Lite",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_ip",
@@ -54,7 +61,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_controller",
-            default_value="joint_trajectory_controller",
+            default_value="gen3_lite_joint_trajectory_controller",
             description="Robot controller to start.",
         )
     )
@@ -119,6 +126,7 @@ def generate_launch_description():
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
     fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
     robot_controller = LaunchConfiguration("robot_controller")
+    robot_hand_controller = LaunchConfiguration("robot_hand_controller")
     gripper = LaunchConfiguration("gripper")
     use_internal_bus_gripper_comm = LaunchConfiguration("use_internal_bus_gripper_comm")
     gripper_max_velocity = LaunchConfiguration("gripper_max_velocity")
@@ -137,6 +145,7 @@ def generate_launch_description():
             "use_fake_hardware": use_fake_hardware,
             "fake_sensor_commands": fake_sensor_commands,
             "robot_controller": robot_controller,
+            "robot_hand_controller": robot_hand_controller,
             "gripper": gripper,
             "use_internal_bus_gripper_comm": use_internal_bus_gripper_comm,
             "gripper_max_velocity": gripper_max_velocity,

diff --git a/kortex_bringup/launch/kortex_control.launch.py b/kortex_bringup/launch/kortex_control.launch.py
@@ -62,11 +62,6 @@ def launch_setup(context, *args, **kwargs):
     if use_fake_hardware_value == "true":
         use_internal_bus_gripper_comm = "false"
 
-    robot_model = robot_type.perform(context)
-    is_gen3_lite = "false"
-    if robot_model == "gen3_lite":
-        is_gen3_lite = "true"
-
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -188,9 +183,7 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         arguments=[robot_hand_controller, "-c", "/controller_manager"],
-        condition=IfCondition(
-            PythonExpression(["'", gripper, "' != '' and '", is_gen3_lite, "' == 'false'"])
-        ),
+        condition=IfCondition(PythonExpression(["'", gripper, "' != ''"])),
     )
 
     # only start the fault controller if we are using hardware
@@ -210,7 +203,7 @@ def launch_setup(context, *args, **kwargs):
         robot_pos_controller_spawner,
         fault_controller_spawner,
     ]
-    start_robot_hand_controller = gripper.perform(context) != "" and is_gen3_lite != "true"
+    start_robot_hand_controller = gripper.perform(context) != ""
     # Conditionally add robot_hand_controller_spawner
     if start_robot_hand_controller:
         nodes_to_start.append(robot_hand_controller_spawner)

diff --git a/kortex_bringup/launch/kortex_sim_control.launch.py b/kortex_bringup/launch/kortex_sim_control.launch.py
@@ -51,6 +51,7 @@ def launch_setup(context, *args, **kwargs):
     robot_traj_controller = LaunchConfiguration("robot_controller")
     robot_pos_controller = LaunchConfiguration("robot_pos_controller")
     robot_hand_controller = LaunchConfiguration("robot_hand_controller")
+    robot_lite_hand_controller = LaunchConfiguration("robot_lite_hand_controller")
     launch_rviz = LaunchConfiguration("launch_rviz")
     use_sim_time = LaunchConfiguration("use_sim_time")
     gripper = LaunchConfiguration("gripper")
@@ -170,6 +171,13 @@ def launch_setup(context, *args, **kwargs):
         condition=UnlessCondition(is_gen3_lite),
     )
 
+    robot_hand_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[robot_lite_hand_controller, "-c", "/controller_manager"],
+        condition=IfCondition(is_gen3_lite),
+    )
+
     # Bridge
     bridge = Node(
         package="ros_gz_bridge",
@@ -361,7 +369,7 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "robot_controller",
-            default_value="joint_trajectory_controller",
+            default_value="gen3_lite_joint_trajectory_controller",
             description="Robot controller to start.",
         )
     )
@@ -379,6 +387,13 @@ def generate_launch_description():
             description="Robot hand controller to start.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_lite_hand_controller",
+            default_value="gen3_lite_2f_gripper_controller",
+            description="Robot hand controller to start for Gen3_Lite.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_sim_time",

diff --git a/kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml b/kortex_description/arms/gen3_lite/6dof/config/ros2_controllers.yaml
@@ -5,7 +5,7 @@ controller_manager:
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
-    joint_trajectory_controller:
+    gen3_lite_joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 
     twist_controller:
@@ -14,7 +14,10 @@ controller_manager:
     fault_controller:
       type: picknik_reset_fault_controller/PicknikResetFaultController
 
-joint_trajectory_controller:
+    gen3_lite_2f_gripper_controller:
+      type: position_controllers/GripperActionController
+
+gen3_lite_joint_trajectory_controller:
   ros__parameters:
     joints:
       - joint_1
@@ -23,7 +26,6 @@ joint_trajectory_controller:
       - joint_4
       - joint_5
       - joint_6
-      - right_finger_bottom_joint
     command_interfaces:
       - position
     state_interfaces:
@@ -46,3 +48,9 @@ twist_controller:
       - twist.angular.x
       - twist.angular.y
       - twist.angular.z
+
+gen3_lite_2f_gripper_controller:
+  ros__parameters:
+    default: true
+    joint: right_finger_bottom_joint
+    allow_stalling: true
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
@@ -30,19 +30,29 @@
     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)}" >
+    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)}
+    moveit_active" >
 
     <!-- ros2 control include -->
     <xacro:include filename="$(find kortex_description)/arms/gen3_lite/${dof}dof/urdf/kortex.ros2_control.xacro" />
     <xacro:if value="${sim_ignition or sim_gazebo}">
-      <xacro:property name="ros2_control_name" value="IgnitionSystem"/>
+      <xacro:unless value="${moveit_active}">
+        <xacro:property name="ros2_control_name" value="IgnitionSystem"/>
+      </xacro:unless>
     </xacro:if>
      <xacro:if value="${use_fake_hardware}">
-      <xacro:property name="ros2_control_name" value="GenericSystem"/>
+      <xacro:unless value="${moveit_active}">
+        <xacro:property name="ros2_control_name" value="GenericSystem"/>
+      </xacro:unless>
     </xacro:if>
     <xacro:unless value="${use_fake_hardware or sim_ignition or sim_gazebo}">
-      <xacro:property name="ros2_control_name" value="${prefix}KortexMultiInterfaceHardware"/>
+      <xacro:unless value="${moveit_active}">
+        <xacro:property name="ros2_control_name" value="${prefix}KortexMultiInterfaceHardware"/>
+      </xacro:unless>
     </xacro:unless>
+    <xacro:if value="${moveit_active}">
+      <xacro:property name="ros2_control_name" value="${prefix}KortexMultiInterfaceHardware"/>
+    </xacro:if>
     <xacro:kortex_ros2_control
       name="${ros2_control_name}" prefix="${prefix}"
       use_fake_hardware="${use_fake_hardware}"
@@ -64,7 +74,8 @@
       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}"/>
+      gripper_joint_name="${gripper_joint_name}"
+      moveit_active="${moveit_active}"/>
 
     <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
@@ -23,27 +23,53 @@
     connection_inactivity_timeout_ms
     gripper_joint_name
     gripper_max_velocity:=100.0
-    gripper_max_force:=100.0">
+    gripper_max_force:=100.0
+    moveit_active">
 
     <ros2_control name="${name}" type="system">
       <hardware>
         <xacro:if value="${sim_gazebo}">
-          <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+          <xacro:unless value="${moveit_active}">
+            <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+          </xacro:unless>
         </xacro:if>
         <xacro:if value="${sim_ignition}">
-          <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          <xacro:unless value="${moveit_active}">
+            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+          </xacro:unless>
         </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:unless value="${moveit_active}">
+            <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:unless>
         </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:unless value="${moveit_active}">
+            <plugin>mock_components/GenericSystem</plugin>
+            <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+            <param name="state_following_offset">0.0</param>
+          </xacro:unless>
         </xacro:if>
         <xacro:unless value="${use_fake_hardware or sim_gazebo or sim_ignition or sim_isaac}">
+          <xacro:unless value="${moveit_active}">
+            <plugin>kortex_driver/KortexMultiInterfaceHardware</plugin>
+            <param name="robot_ip">${robot_ip}</param>
+            <param name="username">${username}</param>
+            <param name="password">${password}</param>
+            <param name="port">${port}</param>
+            <param name="port_realtime">${port_realtime}</param>
+            <param name="session_inactivity_timeout_ms">${session_inactivity_timeout_ms}</param>
+            <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">${gripper_joint_name}</param>
+            <param name="gripper_max_velocity">${gripper_max_velocity}</param>
+            <param name="gripper_max_force">${gripper_max_force}</param>
+          </xacro:unless>
+        </xacro:unless>
+        <xacro:if value="${moveit_active}">
           <plugin>kortex_driver/KortexMultiInterfaceHardware</plugin>
           <param name="robot_ip">${robot_ip}</param>
           <param name="username">${username}</param>
@@ -57,7 +83,7 @@
           <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>
+        </xacro:if>
       </hardware>
       <joint name="${prefix}joint_1">
         <command_interface name="position">
@@ -125,11 +151,22 @@
         <state_interface name="velocity"/>
         <state_interface name="effort"/>
       </joint>
-      <joint name="${prefix}right_finger_bottom_joint">
-        <command_interface name="position"/>
-        <state_interface name="position"/>
-        <state_interface name="velocity"/>
-      </joint>
+      <xacro:unless value="${use_fake_hardware or sim_isaac or sim_ignition}">
+        <xacro:unless value="${moveit_active}">
+          <joint name="${prefix}right_finger_bottom_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+          </joint>
+        </xacro:unless>
+      </xacro:unless>
+      <xacro:if value="${moveit_active}">
+        <joint name="${prefix}right_finger_bottom_joint">
+          <command_interface name="position"/>
+          <state_interface name="position"/>
+          <state_interface name="velocity"/>
+        </joint>
+      </xacro:if>
     </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
@@ -79,7 +79,7 @@
                 </joint>
                 <joint name="${prefix}left_finger_bottom_joint">
                     <param name="mimic">${prefix}right_finger_bottom_joint</param>
-                    <param name="multiplier">1</param>
+                    <param name="multiplier">-1</param>
                     <xacro:unless value="${sim_ignition}">
                         <command_interface name="position"/>
                         <state_interface name="position"/>