Pull request #91: feat: Add Gripper example controller with launch co…

…nfiguration

Merge in MOCTRL/franka_ros2 from BFFTRAC-2008 to humble

* commit '6b1ef58a9a5105fcdfb74cb2bbc672d489e6ca9e':
  Consolidate changes for gripper example controller.
  feat: Add Gripper example controller with launch configuration

.gitignore

@@ -8,3 +8,4 @@ cmake-build-*/
 build/
 install/
 log/
+libfranka

Dockerfile

@@ -17,6 +17,7 @@ RUN --mount=type=cache,target=/var/cache/apt \
     apt-get install -y --no-install-recommends \
         bash-completion \
         curl \
+        gdb \
         git \
         nano \
         openssh-client \

franka_bringup/config/controllers.yaml

@@ -38,6 +38,9 @@ controller_manager:
     model_example_controller:
       type: franka_example_controllers/ModelExampleController
 
+    gripper_example_controller:
+      type: franka_example_controllers/GripperExampleController      
+
     joint_trajectory_controller:
       type: joint_trajectory_controller/JointTrajectoryController
 

franka_bringup/launch/gripper_example_controller.launch.py

@@ -0,0 +1,85 @@
+#  Copyright (c) 2025 Franka Robotics GmbH
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_ip_parameter_name = 'robot_ip'
+    arm_id_parameter_name = 'arm_id'
+    load_gripper_parameter_name = 'load_gripper'
+    use_fake_hardware_parameter_name = 'use_fake_hardware'
+    fake_sensor_commands_parameter_name = 'fake_sensor_commands'
+    use_rviz_parameter_name = 'use_rviz'
+
+    robot_ip = LaunchConfiguration(robot_ip_parameter_name)
+    arm_id = LaunchConfiguration(arm_id_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)
+    use_rviz = LaunchConfiguration(use_rviz_parameter_name)
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            robot_ip_parameter_name,
+            description='Hostname or IP address of the robot.'),
+        DeclareLaunchArgument(
+            arm_id_parameter_name,
+            default_value='fr3',
+            description='ID of the type of arm used. Supported values: fer, fr3, fp3'),
+        DeclareLaunchArgument(
+            use_rviz_parameter_name,
+            default_value='false',
+            description='Visualize the robot in Rviz'),
+        DeclareLaunchArgument(
+            use_fake_hardware_parameter_name,
+            default_value='false',
+            description='Use fake hardware'),
+        DeclareLaunchArgument(
+            fake_sensor_commands_parameter_name,
+            default_value='false',
+            description="Fake sensor commands. Only valid when '{}' is true".format(
+                use_fake_hardware_parameter_name)),
+        DeclareLaunchArgument(
+            load_gripper_parameter_name,
+            default_value='true',
+            description='Use Franka Gripper as an end-effector, otherwise, the robot is loaded '
+                        'without an end-effector.'),
+
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([PathJoinSubstitution(
+                [FindPackageShare('franka_bringup'), 'launch', 'franka.launch.py'])]),
+            launch_arguments={robot_ip_parameter_name: robot_ip,
+                              arm_id_parameter_name: arm_id,
+                              load_gripper_parameter_name: load_gripper,
+                              use_fake_hardware_parameter_name: use_fake_hardware,
+                              fake_sensor_commands_parameter_name: fake_sensor_commands,
+                              use_rviz_parameter_name: use_rviz
+                              }.items(),
+        ),
+
+        Node(
+            package='controller_manager',
+            executable='spawner',
+            arguments=['gripper_example_controller'],
+            output='screen',
+        ),
+    ])

franka_example_controllers/CMakeLists.txt

@@ -15,33 +15,37 @@ option(CHECK_TIDY "Adds clang-tidy tests" OFF)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(rclcpp REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 find_package(controller_interface REQUIRED)
-find_package(pluginlib REQUIRED)
-find_package(rclcpp_lifecycle REQUIRED)
-find_package(hardware_interface REQUIRED)
 find_package(franka_msgs REQUIRED)
-find_package(Eigen3 REQUIRED)
 find_package(franka_semantic_components REQUIRED)
+find_package(hardware_interface REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_msgs REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 add_library(
         ${PROJECT_NAME}
         SHARED
-        src/gravity_compensation_example_controller.cpp
-        src/joint_impedance_example_controller.cpp
-        src/joint_velocity_example_controller.cpp
-        src/joint_position_example_controller.cpp
-        src/cartesian_velocity_example_controller.cpp
-        src/cartesian_pose_example_controller.cpp
-        src/joint_impedance_with_ik_example_controller.cpp
         src/cartesian_elbow_example_controller.cpp
         src/cartesian_orientation_example_controller.cpp
+        src/cartesian_pose_example_controller.cpp
+        src/cartesian_velocity_example_controller.cpp
         src/elbow_example_controller.cpp
+        src/gravity_compensation_example_controller.cpp
+        src/gripper_example_controller.cpp
+        src/joint_impedance_example_controller.cpp
+        src/joint_impedance_with_ik_example_controller.cpp
+        src/joint_position_example_controller.cpp
+        src/joint_velocity_example_controller.cpp
         src/model_example_controller.cpp
-        src/move_to_start_example_controller.cpp
-        src/motion_generator.cpp)
+        src/motion_generator.cpp
+        src/move_to_start_example_controller.cpp)
 target_include_directories(
         ${PROJECT_NAME}
         PUBLIC
@@ -51,13 +55,16 @@ target_include_directories(
 ament_target_dependencies(
         ${PROJECT_NAME}
         controller_interface
+        franka_msgs
+        franka_semantic_components
         hardware_interface
+        moveit_core
+        moveit_msgs
+        pinocchio
         pluginlib
         rclcpp
         rclcpp_lifecycle
-        franka_semantic_components
-        moveit_core
-        moveit_msgs
+        std_srvs
 )
 
 pluginlib_export_plugin_description_file(

franka_example_controllers/franka_example_controllers.xml

@@ -71,4 +71,10 @@
             The franka model example controller evaluates and prints the dynamic model ofFranka Robotics Robots.
         </description>
     </class>
+    <class name="franka_example_controllers/GripperExampleController"
+           type="franka_example_controllers::GripperExampleController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            The Gripper example controller demonstrates the use of the Robot gripper.
+        </description>
+    </class>
 </library>

franka_example_controllers/include/franka_example_controllers/gripper_example_controller.hpp

@@ -0,0 +1,111 @@
+// Copyright (c) 2025 Franka Robotics GmbH
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <string>
+
+#include <controller_interface/controller_interface.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
+
+#include "franka_example_controllers/robot_utils.hpp"
+#include "franka_msgs/action/grasp.hpp"
+#include "franka_msgs/action/move.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace franka_example_controllers {
+
+/**
+ * The Gripper Example Controller
+ *
+ * Assumptions:
+ * - The Franka Hand ("Gripper") is correctly attached to the Franka FR3 Robotic Arm.
+ * - The Robotic Arm is powered on, unlocked, and in FCI (Franka Control Interface) mode.
+ * - The Arm is positioned and ready to test only the Gripper functionality,
+ *   with no movement of other joints involved.
+ *
+ * Purpose:
+ * This controller demonstrates the Franka action interface for controlling the gripper.
+ * It uses two hard-coded goals:
+ *
+ * 1. Grasp Goal (see: GripperExampleController::graspGripper()):
+ *    - Target width: 0.015 meters (e.g., to grasp a "Magic Marker").
+ *    - Tolerances:
+ *      - epsilon.inner: 0.005 meters (inner tolerance for success).
+ *      - epsilon.outer: 0.010 meters (outer tolerance for success).
+ *    - Grasping force: 100.0 N (a very firm grip).
+ *
+ * 2. Move Goal (see: GripperExampleController::openGripper()):
+ *    - Opens the gripper to a width of 0.080 meters.
+ *
+ * Object Size Examples:
+ * - Magic Marker: 15 mm diameter - Within tolerance: success.
+ * - Bic Pen: ~8 mm diameter - Below tolerance: fail.
+ * - Mini Flashlight: ~30 mm diameter - Exceeds tolerance: fail.
+ * - No Object: ~0 mm (fingers touch) - Below tolerance: fail.
+ *
+ * Behavior:
+ * The controller repeatedly opens and closes the gripper and evaluates
+ * whether the grasp is successful or failed based on the object's size
+ * and the defined tolerances.
+ */
+
+class GripperExampleController : public controller_interface::ControllerInterface {
+ public:
+  [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration()
+      const override;
+  [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration()
+      const override;
+
+  CallbackReturn on_init() override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::return_type update(const rclcpp::Time& time,
+                                           const rclcpp::Duration& period) override;
+
+ private:
+  // Close Gripper if Open, Open Gripper if Closed
+  void toggleGripperState();
+  // Issues the Move Goal to open the Gripper
+  bool openGripper();
+  // Issues the Grasp Goal to close the Gripper around an object.
+  void graspGripper();
+  // Populates the callbacks for the Move Goal
+  void assignMoveGoalOptionsCallbacks();
+  // Populates the callbacks for the Grasp Goal
+  void assignGraspGoalOptionsCallbacks();
+
+  std::shared_ptr<rclcpp_action::Client<franka_msgs::action::Grasp>> gripper_grasp_action_client_;
+  std::shared_ptr<rclcpp_action::Client<franka_msgs::action::Move>> gripper_move_action_client_;
+  std::shared_ptr<rclcpp::Client<std_srvs::srv::Trigger>> gripper_stop_client_;
+
+  /**
+   * The struct SendGoalOptions is used solely for setting goal callbacks.
+   * In this example, the callbacks are tied to the lifetime of the
+   * GripperExampleController instance, so the SendGoalOptions objects
+   * are stored as members of the class.
+   */
+  rclcpp_action::Client<franka_msgs::action::Move>::SendGoalOptions move_goal_options_;
+  rclcpp_action::Client<franka_msgs::action::Grasp>::SendGoalOptions grasp_goal_options_;
+
+  std::string arm_id_;
+};
+
+}  // namespace franka_example_controllers

franka_example_controllers/package.xml

@@ -9,25 +9,29 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>rclcpp</depend>
   <depend>controller_interface</depend>
-  <depend>pluginlib</depend>
-  <depend>rclcpp_lifecycle</depend>
   <depend>franka_msgs</depend>
   <depend>franka_semantic_components</depend>
+  <depend>pinocchio</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
 
 
-  <test_depend>hardware_interface_testing</test_depend>
+  <test_depend>ament_cmake</test_depend>
   <test_depend>ament_cmake_clang_format</test_depend>
+  <test_depend>ament_cmake_clang_tidy</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>
   <test_depend>ament_cmake_cppcheck</test_depend>
   <test_depend>ament_cmake_flake8</test_depend>
+  <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_cmake_lint_cmake</test_depend>
   <test_depend>ament_cmake_pep257</test_depend>
   <test_depend>ament_cmake_xmllint</test_depend>
-  <test_depend>ament_cmake_gmock</test_depend>
-  <test_depend>ament_cmake_clang_tidy</test_depend>
   <test_depend>controller_manager</test_depend>
+  <test_depend>hardware_interface_testing</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>