From bd03390ac9da75e561697947f3c327e0fafcd9b2 Mon Sep 17 00:00:00 2001
From: Marius Winkelmeier <marius.winkelmeier@franka.de>
Date: Thu, 19 Sep 2024 19:02:08 +0200
Subject: [PATCH] fix: joint_impedance_with_ik works with and without gripper

---
 franka_bringup/launch/franka.launch.py                     | 1 +
 .../joint_impedance_with_ik_example_controller.hpp         | 1 +
 .../src/joint_impedance_with_ik_example_controller.cpp     | 7 +++++--
 franka_fr3_moveit_config/launch/move_group.launch.py       | 7 +++++--
 franka_fr3_moveit_config/srdf/fr3_arm.srdf.xacro           | 2 +-
 5 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/franka_bringup/launch/franka.launch.py b/franka_bringup/launch/franka.launch.py
index 1046b312..57720df5 100644
--- a/franka_bringup/launch/franka.launch.py
+++ b/franka_bringup/launch/franka.launch.py
@@ -76,6 +76,7 @@ def robot_description_dependent_nodes_spawner(
             parameters=[franka_controllers,
                         {'robot_description': robot_description},
                         {'arm_id': arm_id},
+                        {'load_gripper': load_gripper},
                         ],
             remappings=[('joint_states', 'franka/joint_states')],
             output={
diff --git a/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp b/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp
index 3871dd8e..ca62d4c0 100644
--- a/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp
+++ b/franka_example_controllers/include/franka_example_controllers/joint_impedance_with_ik_example_controller.hpp
@@ -98,6 +98,7 @@ class JointImpedanceWithIKExampleController : public controller_interface::Contr
   bool initialization_flag_{true};
 
   std::string arm_id_;
+  bool is_gripper_loaded_ = true;
 
   double elapsed_time_{0.0};
   std::unique_ptr<franka_semantic_components::FrankaRobotModel> franka_robot_model_;
diff --git a/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp
index ac8a996a..7b0ee3e5 100644
--- a/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp
+++ b/franka_example_controllers/src/joint_impedance_with_ik_example_controller.cpp
@@ -111,8 +111,9 @@ JointImpedanceWithIKExampleController::create_ik_service_request(
   service_request->ik_request.robot_state.joint_state.velocity = joint_velocities_current;
   service_request->ik_request.robot_state.joint_state.effort = joint_efforts_current;
 
-  // If Franka Hand is not connected, the following line should be commented out.
-  service_request->ik_request.ik_link_name = arm_id_ + "_hand_tcp";
+  if (is_gripper_loaded_) {
+    service_request->ik_request.ik_link_name = arm_id_ + "_hand_tcp";
+  }
   return service_request;
 }
 
@@ -189,6 +190,8 @@ CallbackReturn JointImpedanceWithIKExampleController::on_init() {
 
 bool JointImpedanceWithIKExampleController::assign_parameters() {
   arm_id_ = get_node()->get_parameter("arm_id").as_string();
+  is_gripper_loaded_ = get_node()->get_parameter("load_gripper").as_bool();
+
   auto k_gains = get_node()->get_parameter("k_gains").as_double_array();
   auto d_gains = get_node()->get_parameter("d_gains").as_double_array();
   if (k_gains.empty()) {
diff --git a/franka_fr3_moveit_config/launch/move_group.launch.py b/franka_fr3_moveit_config/launch/move_group.launch.py
index dd409677..47e49cda 100644
--- a/franka_fr3_moveit_config/launch/move_group.launch.py
+++ b/franka_fr3_moveit_config/launch/move_group.launch.py
@@ -39,10 +39,12 @@ def load_yaml(package_name, file_path):
 
 def generate_launch_description():
     robot_ip_parameter_name = 'robot_ip'
+    load_gripper_parameter_name = 'load_gripper'
     use_fake_hardware_parameter_name = 'use_fake_hardware'
     fake_sensor_commands_parameter_name = 'fake_sensor_commands'
 
     robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    load_gripper = LaunchConfiguration(load_gripper_parameter_name)
     use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
     fake_sensor_commands = LaunchConfiguration(
         fake_sensor_commands_parameter_name)
@@ -62,7 +64,8 @@ def generate_launch_description():
             ' ',
             franka_xacro_file,
             ' ros2_control:=false',
-            ' hand:=true',
+            ' hand:=',
+            load_gripper,
             ' arm_id:=fr3',
             ' robot_ip:=',
             robot_ip,
@@ -83,7 +86,7 @@ def generate_launch_description():
 
     robot_description_semantic_command = Command(
         [FindExecutable(name='xacro'), ' ',
-         franka_semantic_xacro_file, ' hand:=true']
+         franka_semantic_xacro_file, ' hand:=', load_gripper]
     )
 
     # Use ParameterValue here as well if needed
diff --git a/franka_fr3_moveit_config/srdf/fr3_arm.srdf.xacro b/franka_fr3_moveit_config/srdf/fr3_arm.srdf.xacro
index 9c30c888..97ffe57e 100644
--- a/franka_fr3_moveit_config/srdf/fr3_arm.srdf.xacro
+++ b/franka_fr3_moveit_config/srdf/fr3_arm.srdf.xacro
@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fr3">
 
   <xacro:arg name="arm_id" default="fr3"/>
-  <xacro:arg name="hand" default="true"/> <!-- Should a franka_gripper be mounted at the flange? (Currently does not work without it) -->
+  <xacro:arg name="hand" default="true"/>
 
   <xacro:property name="arm_id" value="$(arg arm_id)"/>