From 79aa262153e02c23b3b3f541f6fda9afb43a3546 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Wed, 3 Apr 2024 10:59:07 +0200
Subject: [PATCH] Gripper has now a fake hardware controller
 (joint_trajectory_controller/JointTrajectoryController)

ur_robotiq_control.launch.py and ur_robotiq_moveit.launch.py work as expected
---
 .../config/ur_robotiq_controllers.yaml        | 21 ++++++++++++++++++-
 .../launch/ur_robotiq_control.launch.py       |  7 +++++++
 .../config/moveit_controllers.yaml            |  9 +++++++-
 .../launch/ur_robotiq_moveit.launch.py        |  3 +++
 4 files changed, 38 insertions(+), 2 deletions(-)

diff --git a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
index c108a50..9a50b20 100644
--- a/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
+++ b/src/ur_robotiq_description/config/ur_robotiq_controllers.yaml
@@ -27,6 +27,9 @@ controller_manager:
     robotiq_gripper_controller:
       type: position_controllers/GripperActionController
 
+    robotiq_gripper_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
@@ -134,11 +137,27 @@ forward_position_controller:
 
 robotiq_gripper_controller:
   ros__parameters:
-    default: true
     joint: $(var tf_prefix)finger_joint
     use_effort_interface: true
     use_speed_interface: true
 
+robotiq_gripper_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+     - $(var tf_prefix)finger_joint
+    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.2
+      goal_time: 0.0
+      $(var tf_prefix)finger_joint: { trajectory: 0.2, goal: 0.1 }
+
 robotiq_activation_controller:
   ros__parameters:
     default: true
diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
index b5f0896..7b01ac9 100644
--- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
+++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py
@@ -215,6 +215,12 @@ def launch_setup(context, *args, **kwargs):
         arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
         condition=UnlessCondition(use_fake_hardware),
     )
+    robotiq_gripper_joint_trajectory_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["robotiq_gripper_joint_trajectory_controller", "-c", "/controller_manager"],
+        condition=IfCondition(use_fake_hardware),
+    )
 
     robotiq_activation_controller_spawner = Node(
         package="controller_manager",
@@ -346,6 +352,7 @@ def launch_setup(context, *args, **kwargs):
                          control_node,
                          ur_control_node,
                          robotiq_gripper_controller_spawner,
+                         robotiq_gripper_joint_trajectory_controller_spawner,
                          robotiq_activation_controller_spawner,
                          dashboard_client_node,
                          tool_communication_node,
diff --git a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
index 902312a..158f762 100644
--- a/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
+++ b/src/ur_robotiq_moveit_config/config/moveit_controllers.yaml
@@ -3,6 +3,7 @@ controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller
   - robotiq_gripper_controller
+  - robotiq_gripper_joint_trajectory_controller
 
 
 scaled_joint_trajectory_controller:
@@ -34,7 +35,13 @@ robotiq_gripper_controller:
   action_ns: gripper_cmd
   type: GripperCommand
   default: true
-  controlled_joints:
+  joints:
     - finger_joint
 
+robotiq_gripper_joint_trajectory_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: false
+  joints:
+    - finger_joint
 
diff --git a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
index 0994b43..61d39db 100644
--- a/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
+++ b/src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py
@@ -148,11 +148,14 @@ def launch_setup(context, *args, **kwargs):
     # the scaled_joint_trajectory_controller does not work on fake hardware
     use_fake_hw = use_fake_hardware.perform(context)
     controllers_yaml = load_yaml(moveit_config_package, "config/moveit_controllers.yaml")
+    print(controllers_yaml)
 
     if use_fake_hw == "true":
         print("Using fake hardware")
         controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
         controllers_yaml["joint_trajectory_controller"]["default"] = True
+        controllers_yaml["robotiq_gripper_controller"]["default"] = False
+        controllers_yaml["robotiq_gripper_joint_trajectory_controller"]["default"] = True
 
     moveit_controllers = {
         "moveit_simple_controller_manager": controllers_yaml,