smooth movement of the gripper

Added mapping routine for ps5 controller

.idea/vcs.xml

@@ -4,6 +4,5 @@
     <mapping directory="$PROJECT_DIR$" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
     <mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
-    <mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
   </component>
 </project>

README.md

@@ -88,7 +88,7 @@ After launching you need to change the controller to enable the servo mode.\
 Terminal 4:
 ```bash
 ros2 control switch_controllers --deactivate joint_trajectory_controller
-ros2 control switch_controllers --deactivate robotiq_gripper_joint_trajectory_controller
+ros2 control switch_controllers --deactivate robotiq_gripper_controller
 ros2 control switch_controllers --activate forward_position_controller
 ros2 control switch_controllers --activate forward_gripper_position_controller
 ```

src/Universal_Robots_ROS2_Description

@@ -0,0 +1 @@
+Subproject commit f5394b8e7439632d26719cd7b47ee9102a921078

src/Universal_Robots_ROS2_Driver

@@ -0,0 +1 @@
+Subproject commit ca5422994c410b3b8c512100144e758e1499705a

src/robotiq_2f/README.md

@@ -0,0 +1,60 @@
+**robotiq_2f Repository** 
+
+***Overview***
+
+This repository provides a ROS2 integration for controlling the Robotiq 2F-85 and 2F-140 grippers. It combines elements from:
+
+* **ros2_robotiq_gripper:** [https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main](https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main)
+* **robotiq_2f_urcap_adapter:** [https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main](https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main)
+
+It leverages the low-level socket communication provided by the robotiq_2f_urcap_adapter to create a new ROS2-compatible hardware interface.
+
+***Features***
+
+* **robotiq_2f_controllers:**
+    * ForwardController
+    * GripperCommand
+    * RobotiqActivationController (inherited by ros2_robotiq_gripper) 
+* **robotiq_2f_description:**
+    * Configuration files for 85mm and 140mm grippers
+    * Controller configuration files
+    * Launch files for RViz visualization and controller activation
+    * Meshes for visualization and collision detection
+    * Modified URDF files
+* **robotiq_2f_interface:**
+    * ROS2 hardware interface 
+    * Robotiq2fSocketAdapter 
+    * MockRobotiq2fSocketAdapter
+* **robotiq_2f_msgs:**
+    * GripperCommand.action
+    * ForwardCommand.msg
+
+***Socket Communication***
+
+Socket communication details: [https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python](https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python)
+
+***Robotiq Resources***
+
+* **Robotiq UR Cap:** [https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button](https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button)
+* **Quick Start Guide:** [https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf](https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf)
+
+***Parameters***
+
+* `robot_ip`: IP address of the robot controller
+* `robot_port`: Port for robot controller communication
+* `use_fake_hardware`: Start gripper with fake hardware mirroring command to its states.
+* `use_forward_controller`: Use forward controller instead of gripper command controller(action controller).
+* `prefix`: In case multiple grippers are used on the same UR Cap.
+* `description_file`: The type of the Gripper (85/140) is decided here.
+
+**robotiq_2f_gripper (config file)**
+
+* `min_position`: 0.0 (fully closed)
+* `max_position`: 0.14 (fully open)
+* `max_angle`: 0.695 (radians, closed)
+* `min_speed`: 0.02 
+* `max_speed`: 0.15 
+* `min_force`: 20.0 
+* `max_force`: 235.0 
+
+

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_controllers)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(rclcpp REQUIRED)
+find_package(controller_interface REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(robotiq_2f_msgs REQUIRED)
+find_package(realtime_tools REQUIRED)
+find_package(std_srvs REQUIRED)
+
+include_directories(include)
+
+add_library(robotiq_2f_controllers SHARED
+  src/robotiq_forward_controller.cpp
+  src/robotiq_gripper_command_controller.cpp
+  src/robotiq_activation_controller.cpp
+)
+ament_target_dependencies(${PROJECT_NAME}
+  rclcpp
+  controller_interface
+  hardware_interface
+  pluginlib
+  robotiq_2f_msgs
+  realtime_tools
+  std_srvs
+)
+pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
+
+
+# Install targets
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Install header files
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Install plugin description file
+install(FILES
+  controller_plugin.xml
+  DESTINATION share/${PROJECT_NAME}
+)
+ament_package()

src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml

@@ -0,0 +1,17 @@
+<library path="robotiq_2f_controllers">
+    <class name="robotiq_2f_controllers/ForwardController" type="robotiq_2f_controllers::RobotiqForwardController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Forward controller for Robotiq 2F Gripper.
+        </description>
+    </class>
+        <class name="robotiq_2f_controllers/GripperCommand" type="robotiq_2f_controllers::RobotiqGripperCommandController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            Gripper Command controller for Robotiq 2F Gripper.
+        </description>
+    </class>
+    <class name="robotiq_2f_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            This controller provides an interface to (re-)activate the Robotiq gripper.
+        </description>
+    </class>
+</library>

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp

@@ -0,0 +1,100 @@
+
+
+#ifndef ROBOTIQ_FORWARD_CONTROLLER_HPP
+#define ROBOTIQ_FORWARD_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include "rclcpp/qos.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+#include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/loaned_command_interface.hpp"
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/msg/forward_command.hpp"
+
+namespace robotiq_2f_controllers
+{
+class RobotiqForwardController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqForwardController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
+
+    // communication interface
+    bool new_msg_ = false;
+    rclcpp::Subscription<robotiq_2f_msgs::msg::ForwardCommand>::SharedPtr command_subscriber_;
+    std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> command_msg_;
+    realtime_tools::RealtimeBuffer<std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand>>
+    msg_external_ptr_;
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    double desired_position_;
+    double desired_max_velocity_;
+    double desired_max_effort_;
+
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp

@@ -0,0 +1,143 @@
+
+
+#ifndef ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+#define ROBOTIQ_GRIPPER_COMMAND_CONTROLLER_HPP
+
+#include <stddef.h>
+#include <memory>
+#include <string>
+#include <vector>
+#include <queue>
+#include <mutex>
+
+#include <stdexcept> // For std::runtime_error
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp/callback_group.hpp>
+
+#include <controller_interface/controller_interface.hpp>
+#include <hardware_interface/loaned_command_interface.hpp>
+#include <hardware_interface/types/hardware_interface_type_values.hpp>
+
+#include "realtime_tools/realtime_buffer.h"
+
+#include "robotiq_2f_msgs/action/gripper_command.hpp"
+
+using GripperGoalHandle = rclcpp_action::ServerGoalHandle<robotiq_2f_msgs::action::GripperCommand>;
+
+namespace robotiq_2f_controllers
+{
+class RobotiqGripperCommandController : public controller_interface::ControllerInterface
+{
+public:
+    CONTROLLER_INTERFACE_PUBLIC
+    RobotiqGripperCommandController();
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_init() override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_configure(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_activate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_deactivate(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_cleanup(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_error(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+    CONTROLLER_INTERFACE_PUBLIC
+    controller_interface::CallbackReturn on_shutdown(
+    const rclcpp_lifecycle::State & previous_state) override;
+
+protected:
+    std::mutex mutex_;
+    std::string joint_name_;
+    std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
+
+    // Checker Methods
+    bool command_interface_checker_();
+    bool is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    bool is_stalled(double current_position);
+    bool is_stopped();
+
+    // Action Interface
+    std::queue<std::shared_ptr<GripperGoalHandle>> goal_queue_;
+    std::shared_ptr<GripperGoalHandle> current_goal_handle_;
+    rclcpp_action::Server<robotiq_2f_msgs::action::GripperCommand>::SharedPtr action_server_;
+
+    rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+         std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
+    rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle);
+    void process_next_goal();
+    void execute_goal(const std::shared_ptr<GripperGoalHandle> goal_handle);
+
+    void publish_feedback();
+
+    // Error Related
+    bool stop_motion(double current_position);
+
+    // Command Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    joint_position_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_velocity_command_interface_;
+    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    max_effort_command_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    command_interface_map_ = {
+      {"position", &joint_position_command_interface_},
+      {"max_velocity", &max_velocity_command_interface_},
+      {"max_effort", &max_effort_command_interface_}};
+
+    // State Interfaces
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
+    joint_position_state_interface_;
+
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> *>
+    state_interface_map_ = {
+      {"position", &joint_position_state_interface_}};
+
+    // Addtional State variables
+    double max_position_ = 0.0;
+    double target_position_ = -1.0;
+    double current_position_ = -1.0;
+    double last_position_ = -1.0;
+    int stall_counter_ = 0;
+    bool cancel_requested_ = false;
+
+    // Const
+    const int MAX_STALL_COUNT = 200;
+    const double POSITION_TOLERANCE = 0.0015;
+    const double MIN_SPEED = 0.0;
+    const double MIN_EFFORT = 0.0;
+
+};
+} // end namespace robotiq_2f_controllers
+#endif

src/robotiq_2f/robotiq_2f_controllers/package.xml

@@ -1,17 +1,20 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>robotiq_controllers</name>
-  <version>0.0.1</version>
-  <description>Controllers for the Robotiq gripper.</description>
-  <maintainer email="alex.moriarty@picknik.ai">Alex Moriarty</maintainer>
-  <maintainer email="marq.rasmussen@picknik.ai">Marq Rasmussen</maintainer>
-  <license>BSD 3-Clause</license>
-  <author>Cory Crean</author>
+  <name>robotiq_2f_controllers</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>rclcpp</depend>
   <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>robotiq_2f_msgs</depend>
+  <depend>realtime_tools</depend>
   <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_activation_controller.cpp

@@ -26,7 +26,7 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include "robotiq_controllers/robotiq_activation_controller.hpp"
+#include "robotiq_2f_controllers/robotiq_activation_controller.hpp"
 
 namespace robotiq_controllers
 {

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp

@@ -0,0 +1,137 @@
+#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqForwardController::RobotiqForwardController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    command_interface_types_ =
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
+
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::NONE;
+    return config;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
+{
+    auto callback =
+        [this](const std::shared_ptr<robotiq_2f_msgs::msg::ForwardCommand> cmd_msg) -> void
+        {
+            msg_external_ptr_.writeFromNonRT(cmd_msg);
+            new_msg_ = true;
+        };
+
+    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
+        "~/command", rclcpp::SystemDefaultsQoS(), callback);
+
+    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
+    for (const auto& interface : command_interfaces_) {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    // clear out vectors in case of restart
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    if (new_msg_)
+    {
+        command_msg_ = *msg_external_ptr_.readFromRT();
+        desired_position_ = command_msg_->position;
+        desired_max_velocity_ = command_msg_->max_velocity;
+        desired_max_effort_ = command_msg_->max_effort;
+    }
+    // Set the hardware interface commands
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
+        return controller_interface::return_type::ERROR;
+    }
+
+    joint_position_command_interface_[0].get().set_value(desired_position_);
+    max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
+    max_effort_command_interface_[0].get().set_value(desired_max_effort_);
+
+    return controller_interface::return_type::OK;
+}
+
+
+controller_interface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State &)
+{
+  release_interfaces();
+
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_cleanup(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_error(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  return CallbackReturn::SUCCESS;
+}
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp

@@ -0,0 +1,389 @@
+#include "robotiq_2f_controllers/robotiq_gripper_command_controller.hpp"
+
+
+using config_type = controller_interface::interface_configuration_type;
+
+namespace robotiq_2f_controllers
+{
+RobotiqGripperCommandController::RobotiqGripperCommandController() : controller_interface::ControllerInterface() {}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
+{
+    joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    max_position_ = auto_declare<double>("max_gap", max_position_);
+    command_interface_types_ =
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_DEBUG(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::command_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqGripperCommandController::state_interface_configuration() const
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of state interfaces.");
+
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
+
+    for (const auto& interface_map_entry : state_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
+    }
+
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of state interfaces.");
+    return config;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
+{
+    action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
+        get_node(),
+        "~/gripper_command",
+        [this](const std::array<unsigned char, 16>& uuid, std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+            return this->handle_goal(uuid, goal);
+        },
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {return this->handle_cancel(goal_handle);},
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);}
+    );
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_activate(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_DEBUG(get_node()->get_logger(), "Available command interfaces:");
+    for (const auto& interface : command_interfaces_) {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
+    }
+
+    // clear out vectors in case of restart
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
+
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    // assign state interface
+        for (auto & interface : state_interfaces_)
+    {
+        state_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    RCLCPP_DEBUG(get_node()->get_logger(), "Checking if command interfaces are initialized corretly...");
+    if (!command_interface_checker_())
+    {
+        return CallbackReturn::ERROR;
+    }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Command interfaces correctly initialized!");
+
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::return_type RobotiqGripperCommandController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+{
+    // Check if there is an active goal
+    if (current_goal_handle_) {
+
+        // Retrieve the current position of the gripper
+        try
+        {
+            if (joint_position_state_interface_.empty())
+            {
+                throw std::runtime_error("State interface is not correctly initialized");
+            }
+
+            current_position_ = joint_position_state_interface_[0].get().get_value();
+        } catch (const std::exception& e) {
+            RCLCPP_FATAL(get_node()->get_logger(), "Failed to access state interface: %s", e.what());
+            return controller_interface::return_type::ERROR;
+        }
+
+
+        // Check if the gripper is stalled
+        if (is_stalled(current_position_)) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position_;
+            result->reached_goal = false;
+            result->stalled = true;
+            current_goal_handle_->abort(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
+        }
+
+        // Check if the gripper has stopped after a cancel request
+        if (cancel_requested_ && is_stopped()) {
+        auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+        result->final_position = current_position_;
+        result->reached_goal = false;
+        result->stalled = false;
+        current_goal_handle_->canceled(result);  // Notify that the goal was canceled successfully
+        cancel_requested_ = false;  // Reset the cancel requested flag
+        current_goal_handle_.reset();  // Clear the current goal handle
+        process_next_goal();  // Proceed to the next goal if available
+
+        RCLCPP_INFO(get_node()->get_logger(), "Gripper Action was canceled and motion has stopped!");
+        }
+
+        // Publish feedback about the current position
+        publish_feedback();
+
+        // Check if the target position is reached within a tolerance
+        if (std::abs(current_position_ - target_position_) < POSITION_TOLERANCE) {
+            auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
+            result->final_position = current_position_;
+            result->reached_goal = true;
+            result->stalled = false;
+            current_goal_handle_->succeed(result);
+            current_goal_handle_.reset();  // Clear the current goal handle
+            process_next_goal();  // Process the next goal, if any
+            target_position_ = -1.0;
+        }
+    }
+
+    return controller_interface::return_type::OK;
+}
+
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_INFO(get_node()->get_logger(), "Deactivating and releasing interfaces.");
+    release_interfaces();  // Ensure this function properly cleans up resources
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State & )
+{
+  return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_error(const rclcpp_lifecycle::State & )
+{
+    RCLCPP_ERROR(get_node()->get_logger(), "An error occurred, attempting to reset the controller");
+
+    // Attempt to stop any ongoing gripper motion as a safety precaution
+    try {
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Assume stop_motion sets the gripper commands to a safe state
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to stop the gripper during error handling: %s", e.what());
+        return controller_interface::CallbackReturn::ERROR;  // Fail if stopping motion fails
+    }
+
+    // Log that the controller is attempting to reactivate
+    RCLCPP_INFO(get_node()->get_logger(), "Attempting to reactivate the controller");
+
+    // Directly call the on_activate method to reset and reactivate the controller
+    return CallbackReturn::SUCCESS;
+}
+
+controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdown(const rclcpp_lifecycle::State & )
+{
+  return CallbackReturn::SUCCESS;
+}
+
+// Action Handling
+rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
+     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal)
+{
+    (void)goal_uuid;  // Suppress unused variable warning
+    RCLCPP_INFO(get_node()->get_logger(), "Received goal request with position: %f", goal->desired_position);
+
+    if (!is_valid_goal(goal)) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal is invalid");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
+    return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
+}
+
+rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+    if (current_goal_handle_ == goal_handle) {
+        RCLCPP_INFO(get_node()->get_logger(), "Received request to cancel goal");
+        stop_motion(joint_position_state_interface_[0].get().get_value());  // Attempt to stop the gripper immediately
+        cancel_requested_ = true;  // Set a flag indicating cancellation has been requested
+        return rclcpp_action::CancelResponse::ACCEPT;  // Accept the cancellation, pending completion
+    }
+    return rclcpp_action::CancelResponse::REJECT;  // Reject cancellation if it's not the current goal
+}
+
+void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle)
+{
+
+    goal_queue_.push(goal_handle);
+    if (!current_goal_handle_) {  // Only start a new goal if one isn't already active
+        process_next_goal();
+    }
+}
+
+void RobotiqGripperCommandController::publish_feedback()
+{
+    if (!current_goal_handle_) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal handle is null, cannot publish feedback.");
+        return;
+    }
+
+    if (current_goal_handle_->is_canceling()) {
+        RCLCPP_WARN(get_node()->get_logger(), "Goal is being canceled, not publishing feedback.");
+        return;
+    }
+
+    auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
+    feedback->current_position = current_position_;
+
+    try {
+        current_goal_handle_->publish_feedback(feedback);
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to publish feedback: %s", e.what());
+    }
+}
+
+void RobotiqGripperCommandController::process_next_goal()
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (goal_queue_.empty()) {
+        current_goal_handle_.reset();
+        RCLCPP_DEBUG(get_node()->get_logger(), "No more goals to process.");
+        return;
+    }
+    current_goal_handle_ = goal_queue_.front();
+    goal_queue_.pop();
+    current_goal_handle_->execute();
+
+    if (current_goal_handle_) {
+        auto goal = current_goal_handle_->get_goal();
+        if (!goal) {
+            RCLCPP_ERROR(get_node()->get_logger(), "Received a null goal in queue, skipping...");
+            process_next_goal();  // Recursively handle the next goal
+        }
+        else {
+            try {
+                auto goal = current_goal_handle_->get_goal();
+                target_position_ = goal->desired_position;
+                double desired_max_velocity = goal->max_velocity;
+                double desired_max_effort = goal->max_effort;
+
+                if (!command_interface_checker_())
+                {
+                    throw std::runtime_error("At least one interface is not initialized!");
+                }
+
+                joint_position_command_interface_[0].get().set_value(target_position_);
+                max_velocity_command_interface_[0].get().set_value(desired_max_velocity);
+                max_effort_command_interface_[0].get().set_value(desired_max_effort);
+            }
+            catch (const std::exception& e) {
+                RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what());
+            }
+        }
+    }
+}
+
+// Utility
+bool RobotiqGripperCommandController::command_interface_checker_()
+{
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position command interfaces have been successfully initialized.");
+    }
+
+    if (max_velocity_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No velocity command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Velocity command interfaces have been successfully initialized.");
+    }
+
+    if (max_effort_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No effort command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Effort command interfaces have been successfully initialized.");
+    }
+
+    if (joint_position_state_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position state interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position stateinterfaces have been successfully initialized.");
+    }
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+    // Add your validation logic here
+    return goal->desired_position >= 0 && goal->desired_position <= max_position_;
+}
+
+bool RobotiqGripperCommandController::is_stalled(double current_position)
+{
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (std::abs(current_position - last_position_) < POSITION_TOLERANCE) {
+        stall_counter_++;
+    } else {
+        stall_counter_ = 0;  // Reset the stall counter if there is movement
+    }
+    last_position_ = current_position;
+
+    return stall_counter_ > MAX_STALL_COUNT;
+}
+
+bool RobotiqGripperCommandController::stop_motion(double current_position)
+{
+    joint_position_command_interface_[0].get().set_value(current_position);
+    max_velocity_command_interface_[0].get().set_value(MIN_SPEED);
+    max_effort_command_interface_[0].get().set_value(MIN_EFFORT);
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_stopped()
+{
+    double current_position = joint_position_state_interface_[0].get().get_value();
+    bool stopped = std::abs(current_position - last_position_) < POSITION_TOLERANCE;
+    last_position_ = current_position;
+    return stopped;
+}
+
+
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqGripperCommandController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml

@@ -1,7 +1,8 @@
 robotiq_2f_gripper:
   # Physical limits
   min_position: 0.0     # Meters (fully closed)
-  max_position: 0.14    # Meters (fully open)
+  max_position: 0.1426    # Meters (fully open)
+  max_angle: 0.695      # radiants (closed)
   min_speed: 0.02       # Meters per second
   max_speed: 0.15      # Meters per second
   min_force: 20.0       # Newtons

src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml

@@ -3,17 +3,21 @@ controller_manager:
     update_rate: 500  # Hz
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
+    forward_gripper_position_controller:
+      type: robotiq_2f_controllers/ForwardController
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
+      type: robotiq_2f_controllers/GripperCommand
     robotiq_activation_controller:
-      type: robotiq_controllers/RobotiqActivationController
+      type: robotiq_2f_controllers/RobotiqActivationController
+
+forward_gripper_position_controller:
+  ros__parameters:
+    joint: $(var prefix)gripper_gap
 
 robotiq_gripper_controller:
   ros__parameters:
-    default: true
-    joint: finger_joint
-    use_effort_interface: true
-    use_speed_interface: true
+    joint: $(var prefix)gripper_gap
+    max_gap: 0.14
 
 robotiq_activation_controller:
   ros__parameters:

src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py

@@ -1,120 +1,139 @@
-# Copyright (c) 2022 PickNik, Inc.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions are met:
-#
-#    * Redistributions of source code must retain the above copyright
-#      notice, this list of conditions and the following disclaimer.
-#
-#    * Redistributions in binary form must reproduce the above copyright
-#      notice, this list of conditions and the following disclaimer in the
-#      documentation and/or other materials provided with the distribution.
-#
-#    * Neither the name of the {copyright_holder} nor the names of its
-#      contributors may be used to endorse or promote products derived from
-#      this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
+from launch_ros.substitutions import FindPackageShare
 
-import launch
-from launch.substitutions import (
-    Command,
-    FindExecutable,
-    LaunchConfiguration,
-    PathJoinSubstitution,
-)
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition
-import launch_ros
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
 import os
 
-
 def generate_launch_description():
-    description_pkg_share = launch_ros.substitutions.FindPackageShare(
-        package="robotiq_description"
-    ).find("robotiq_2f_description")
-    default_model_path = os.path.join(
-        description_pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="robotiq_2f_description",
+            description="Description package with gripper URDF/XACRO files.",
         )
-    default_rviz_config_path = os.path.join(
-        description_pkg_share, "rviz", "view_urdf.rviz"
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="robotiq_2f_140.urdf.xacro",
+            description="URDF/XACRO description file with the robot gripper.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start gripper with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip", description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_port",
+            default_value="63352",
+            description="Remote port for UR Cap.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="false", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("use_forward_controller",
+                              default_value="false",
+                              description="Use forward controller?")
+    )
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
 
-    args = []
-    args.append(
-        launch.actions.DeclareLaunchArgument(
-            name="model",
-            default_value=default_model_path,
-            description="Absolute path to gripper URDF file",
-        )
-    )
-    args.append(
-        launch.actions.DeclareLaunchArgument(
-            name="rvizconfig",
-            default_value=default_rviz_config_path,
-            description="Absolute path to rviz config file",
-        )
-    )
-    args.append(
-        launch.actions.DeclareLaunchArgument(
-            name="launch_rviz", default_value="true", description="Launch RViz?"
-        )
-    )
+
+def launch_setup(context, *args, **kwargs):
+
+    robot_ip = LaunchConfiguration("robot_ip")
+    description_package = LaunchConfiguration("description_package")
+    description_file = LaunchConfiguration("description_file")
+    prefix = LaunchConfiguration("prefix")
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
+    robot_port = LaunchConfiguration("robot_port")
+    launch_rviz = LaunchConfiguration("launch_rviz")
+    use_forward_controller = LaunchConfiguration("use_forward_controller")
 
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
-            LaunchConfiguration("model"),
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "robot_port:=",
+            robot_port,
             " ",
-            "use_fake_hardware:=true",
         ]
     )
-    robot_description_param = {
-        "robot_description": launch_ros.parameter_descriptions.ParameterValue(
-            robot_description_content, value_type=str
-        )
-    }
+
+    robot_description = {"robot_description": robot_description_content}
 
     controllers_file = "robotiq_controllers.yaml"
     initial_joint_controllers = PathJoinSubstitution(
-        [description_pkg_share, "config", controllers_file]
+        [FindPackageShare(description_package), "config", controllers_file]
     )
 
-    control_node = launch_ros.actions.Node(
+    control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[
-            robot_description_param,
-            initial_joint_controllers,
+            robot_description,
+            ParameterFile(initial_joint_controllers, allow_substs=True),
         ],
+        output='screen',
     )
 
-    robot_state_publisher_node = launch_ros.actions.Node(
+    robot_state_publisher_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
-        parameters=[robot_description_param],
+        parameters=[robot_description],
+        output='screen',
     )
 
-    rviz_node = launch_ros.actions.Node(
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "rviz", "view_robot.rviz"]
+    )
+
+
+    rviz_node = Node(
         package="rviz2",
         executable="rviz2",
         name="rviz2",
         output="log",
-        arguments=["-d", LaunchConfiguration("rvizconfig")],
-        condition=IfCondition(LaunchConfiguration("launch_rviz")),
+        arguments=["-d", rviz_config_file],
+        condition=IfCondition(launch_rviz),
     )
 
-    joint_state_broadcaster_spawner = launch_ros.actions.Node(
+    joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=[
@@ -122,27 +141,46 @@ def generate_launch_description():
             "--controller-manager",
             "/controller_manager",
         ],
+        output='screen',
     )
 
-    robotiq_gripper_controller_spawner = launch_ros.actions.Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["robotiq_gripper_controller", "-c", "/controller_manager"],
-    )
-
-    robotiq_activation_controller_spawner = launch_ros.actions.Node(
+    robotiq_activation_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
+        output='screen',
     )
 
+    def controller_spawner(name, active=True):
+        inactive_flags = ["--inactive"] if not active else []
+        return Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=[
+                          name,
+                          "--controller-manager",
+                          "/controller_manager",
+                      ] + inactive_flags,
+            output='screen',
+        )
+
+    if use_forward_controller.perform(context) == 'false':
+        controller_spawner_names = ['robotiq_gripper_controller']
+        controller_spawner_inactive_names = ["forward_gripper_position_controller"]
+    else:
+        controller_spawner_names = ["forward_gripper_position_controller"]
+        controller_spawner_inactive_names = ['robotiq_gripper_controller']
+
+    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
+        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
+    ]
+
     nodes = [
         control_node,
         robot_state_publisher_node,
         joint_state_broadcaster_spawner,
-        robotiq_gripper_controller_spawner,
         robotiq_activation_controller_spawner,
         rviz_node,
-    ]
+    ] + controller_spawners
 
-    return launch.LaunchDescription(args + nodes)
+    return nodes

src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py

@@ -14,9 +14,9 @@ def generate_launch_description():
         package="robotiq_2f_description"
     ).find("robotiq_2f_description")
     default_model_path = os.path.join(
-        pkg_share, "urdf", "robotiq_2f_85.urdf.xacro"
+        pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
     )
-    default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz")
+    default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_robot.rviz")
 
     args = []
     args.append(

src/robotiq_2f/robotiq_2f_description/package.xml

@@ -17,9 +17,12 @@
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <exec_depend>robotiq_2f_msgs</exec_depend>
+
   <exec_depend>ros2_control</exec_depend>
   <exec_depend>ros2_controllers</exec_depend>
 
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
 

src/robotiq_2f/robotiq_2f_description/rviz/view_robot.rviz

@@ -4,8 +4,10 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
-      Splitter Ratio: 0.6264705657958984
+        - /RobotModel1
+      Splitter Ratio: 0.5
     Tree Height: 555
   - Class: rviz_common/Selection
     Name: Selection
@@ -56,7 +58,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: robot_description
+        Value: /robot_description
       Enabled: true
       Links:
         All Links Enabled: true
@@ -64,78 +66,70 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        arm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         dummy_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        end_effector_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        forearm_link:
+          Value: true
+        robotiq_140_base_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        gripper_base_link:
+        robotiq_140_left_inner_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        left_finger_dist_link:
+        robotiq_140_left_inner_finger_pad:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        left_finger_prox_link:
+        robotiq_140_left_inner_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        lower_wrist_link:
+        robotiq_140_left_outer_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        right_finger_dist_link:
+        robotiq_140_left_outer_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        right_finger_prox_link:
+        robotiq_140_right_inner_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        shoulder_link:
+        robotiq_140_right_inner_finger_pad:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        table:
+        robotiq_140_right_inner_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        tool_frame:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        upper_wrist_link:
+        robotiq_140_right_outer_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        robotiq_140_right_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         world:
           Alpha: 1
           Show Axes: false
@@ -194,25 +188,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.1567115783691406
+      Distance: 1.01347017288208
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.09681572020053864
-        Y: -0.10843408107757568
-        Z: 0.1451336145401001
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Pitch: 0.3253982663154602
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Yaw: 1.3853975534439087
     Saved: ~
 Window Geometry:
   Displays:
@@ -220,7 +214,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -230,5 +224,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1200
-  X: 1989
-  Y: 261
+  X: 70
+  Y: 60

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro

@@ -10,6 +10,7 @@
         robot_port
         min_position
         max_position
+        max_angle
         min_speed
         max_speed
         min_force
@@ -18,19 +19,24 @@
         <ros2_control name="${name}" type="system">
             <!-- Plugins -->
             <hardware>
-                <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
                  <xacro:if value="${use_fake_hardware}">
-                    <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                    <param name="mock_sensor_commands">${fake_sensor_commands}</param>
                     <param name="state_following_offset">0.0</param>
                 </xacro:if>
+                <xacro:unless value="${use_fake_hardware}">
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
                     <param name="gripper_closed_position">0.695</param>
                     <param name="robot_ip">${robot_ip}</param>
                     <param name="robot_port">${robot_port}</param>
                     <param name="use_fake_hardware">${use_fake_hardware}</param>
+                </xacro:unless>
                 <param name="gripper_speed_multiplier">1.0</param>
                 <param name="gripper_force_multiplier">0.5</param>
                 <param name="min_position">${min_position}</param>
                 <param name="max_position">${max_position}</param>
+                <param name="max_angle">${max_angle}</param>
                 <param name="min_speed">${min_speed}</param>
                 <param name="max_speed">${max_speed}</param>
                 <param name="min_force">${min_force}</param>
@@ -39,15 +45,42 @@
 
             <!-- Joint interfaces -->
             <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
-            <joint name="${prefix}finger_joint">
+            <joint name="${prefix}gripper_gap">
                 <command_interface name="position" />
                 <state_interface name="position">
-                    <param name="initial_value">0.695</param>
+                    <param name="initial_value">0.0</param>
                 </state_interface>
                 <state_interface name="velocity"/>
             </joint>
+            <joint name="${prefix}finger_joint">
+                <state_interface name="position">
+                    <param name="initial_value">0.695</param>
+                </state_interface>
+            </joint>
             <!-- When simulating we need to include the rest of the gripper joints -->
+            <!-- <xacro:if value="${use_fake_hardware}"> -->
             <xacro:if value="false">
+                <joint name="${prefix}right_outer_knuckle_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}left_outer_finger_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
+                <joint name="${prefix}right_outer_finger_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
                 <joint name="${prefix}left_inner_knuckle_joint">
                     <param name="mimic">${prefix}finger_joint</param>
                     <param name="multiplier">-1</param>
@@ -55,6 +88,13 @@
                     <state_interface name="position"/>
                     <state_interface name="velocity"/>
                 </joint>
+                <joint name="${prefix}right_inner_knuckle_joint">
+                    <param name="mimic">${prefix}finger_joint</param>
+                    <param name="multiplier">-1</param>
+                    <command_interface name="position"/>
+                    <state_interface name="position"/>
+                    <state_interface name="velocity"/>
+                </joint>
                 <joint name="${prefix}left_inner_finger_joint">
                     <param name="mimic">${prefix}finger_joint</param>
                     <param name="multiplier">1</param>
@@ -62,22 +102,8 @@
                     <state_interface name="position"/>
                     <state_interface name="velocity"/>
                 </joint>
-                <joint name="${prefix}right_outer_knuckle_joint">
+                <joint name="${prefix}right_inner_finger_joint">
                     <param name="mimic">${prefix}finger_joint</param>
-                    <param name="multiplier">-1</param>
-                    <command_interface name="position"/>
-                    <state_interface name="position"/>
-                    <state_interface name="velocity"/>
-                </joint>
-                <joint name="${prefix}robotiq_140_left_finger_tip_joint">
-                    <param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
-                    <param name="multiplier">-1</param>
-                    <command_interface name="position"/>
-                    <state_interface name="position"/>
-                    <state_interface name="velocity"/>
-                </joint>
-                <joint name="${prefix}robotiq_140_right_finger_tip_joint">
-                    <param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
                     <param name="multiplier">1</param>
                     <command_interface name="position"/>
                     <state_interface name="position"/>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -8,7 +8,7 @@
         include_ros2_control:=true
         use_fake_hardware:=false
         fake_sensor_commands:=false
-        robot_ip:=192.168.1.1
+        robot_ip:=192.168.1.223
         robot_port:=63352">
 
 
@@ -17,6 +17,7 @@
         <xacro:property name="robotiq_2f_gripper_dict" value="${config_dict['robotiq_2f_gripper']}" />
         <xacro:property name="min_position" value="${robotiq_2f_gripper_dict['min_position']}" />
         <xacro:property name="max_position" value="${robotiq_2f_gripper_dict['max_position']}" />
+        <xacro:property name="max_angle" value="${robotiq_2f_gripper_dict['max_angle']}" />
         <xacro:property name="min_speed" value="${robotiq_2f_gripper_dict['min_speed']}" />
         <xacro:property name="max_speed" value="${robotiq_2f_gripper_dict['max_speed']}" />
         <xacro:property name="min_force" value="${robotiq_2f_gripper_dict['min_force']}" />
@@ -33,6 +34,7 @@
                 robot_port="${robot_port}"
                 min_position="${min_position}"
                 max_position="${max_position}"
+                max_angle="${max_angle}"
                 min_speed="${min_speed}"
                 max_speed="${max_speed}"
                 min_force="${min_force}"
@@ -433,5 +435,34 @@
             <child link="${prefix}robotiq_140_right_inner_finger_pad" />
             <axis xyz="0 0 1" />
         </joint>
+
+        <!-- Dummy link to facilitate the pseudo joint -->
+        <link name="dummy_link">
+            <visual>
+                <geometry>
+                    <box size="0.001 0.001 0.001"/>  <!-- Very small, effectively invisible -->
+                </geometry>
+                <material name="transparent">
+                    <color rgba="0.0 0.0 0.0 0.0"/>  <!-- Transparent color -->
+                </material>
+            </visual>
+        </link>
+
+        <!-- Fixed joint to attach the dummy link to part of the gripper -->
+        <joint name="dummy_to_pad" type="fixed">
+            <parent link="dummy_link"/>
+            <child link="robotiq_140_left_inner_finger_pad"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Position it appropriately -->
+        </joint>
+
+        <!-- Pseudo prismatic joint for controlling the gripper gap using the dummy link -->
+        <joint name="gripper_gap" type="prismatic">
+            <parent link="robotiq_140_base_link"/>  <!-- Adjust the parent link as necessary -->
+            <child link="dummy_link"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Properly position this joint -->
+            <axis xyz="0 1 1"/>  <!-- Ensure the axis aligns with the intended motion -->
+            <limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
+        </joint>
+
     </xacro:macro>
 </robot>

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro

@@ -10,6 +10,7 @@
         robot_port
         min_position
         max_position
+        max_angle
         min_speed
         max_speed
         min_force
@@ -19,8 +20,9 @@
             <!-- Plugins -->
             <hardware>
                  <xacro:if value="${use_fake_hardware}">
-                    <plugin>mock_components/GenericSystem</plugin>
-                    <param name="fake_sensor_commands">${fake_sensor_commands}</param>
+                    <plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
+                    <param name="use_fake_hardware">${use_fake_hardware}</param>
+                    <param name="mock_sensor_commands">${fake_sensor_commands}</param>
                     <param name="state_following_offset">0.0</param>
                 </xacro:if>
                 <xacro:unless value="${use_fake_hardware}">
@@ -29,25 +31,31 @@
                     <param name="robot_ip">${robot_ip}</param>
                     <param name="robot_port">${robot_port}</param>
                     <param name="use_fake_hardware">${use_fake_hardware}</param>
+                </xacro:unless>
                 <param name="gripper_speed_multiplier">1.0</param>
                 <param name="gripper_force_multiplier">0.5</param>
                 <param name="min_position">${min_position}</param>
                 <param name="max_position">${max_position}</param>
+                <param name="max_angle">${max_angle}</param>
                 <param name="min_speed">${min_speed}</param>
                 <param name="max_speed">${max_speed}</param>
                 <param name="min_force">${min_force}</param>
                 <param name="max_force">${max_force}</param>
-                </xacro:unless>
             </hardware>
 
             <!-- Joint interfaces -->
             <!-- With Hardware, they handle mimic joints, so we only need this command interface activated -->
-            <joint name="${prefix}finger_joint">
+            <joint name="${prefix}gripper_gap">
                 <command_interface name="position" />
+                <state_interface name="position">
+                    <param name="initial_value">0.0</param>
+                </state_interface>
+                <state_interface name="velocity"/>
+            </joint>
+            <joint name="${prefix}finger_joint">
                 <state_interface name="position">
                     <param name="initial_value">0.7929</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}">

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro

@@ -237,13 +237,14 @@
             </inertial>
         </link>
 
+        <!-- Joints-->
         <joint name="${prefix}robotiq_85_base_joint" type="fixed">
             <parent link="${parent}" />
             <child link="${prefix}robotiq_85_base_link" />
             <xacro:insert_block name="origin" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_knuckle_joint" type="revolute">
+        <joint name="${prefix}finger_joint" type="revolute">
             <parent link="${prefix}robotiq_85_base_link" />
             <child link="${prefix}robotiq_85_left_knuckle_link" />
             <axis xyz="0 -1 0" />
@@ -251,57 +252,57 @@
             <limit lower="0.0" upper="0.8" velocity="0.5" effort="50" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_knuckle_joint" type="revolute">
+        <joint name="${prefix}right_outer_knuckle_joint" type="revolute">
             <parent link="${prefix}robotiq_85_base_link" />
             <child link="${prefix}robotiq_85_right_knuckle_link" />
             <axis xyz="0 -1 0" />
             <origin xyz="-0.03060114 0.0 0.05490452" rpy="0 0 0" />
             <limit lower="-0.8" upper="0.0" velocity="0.5" effort="50" />
-            <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_finger_joint" type="fixed">
+        <joint name="${prefix}left_outer_finger_joint" type="fixed">
             <parent link="${prefix}robotiq_85_left_knuckle_link" />
             <child link="${prefix}robotiq_85_left_finger_link" />
             <origin xyz="0.03152616 0.0 -0.00376347" rpy="0 0 0" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_finger_joint" type="fixed">
+        <joint name="${prefix}right_outer_finger_joint" type="fixed">
             <parent link="${prefix}robotiq_85_right_knuckle_link" />
             <child link="${prefix}robotiq_85_right_finger_link" />
             <origin xyz="-0.03152616 0.0 -0.00376347" rpy="0 0 0" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_inner_knuckle_joint" type="continuous">
+        <joint name="${prefix}left_inner_knuckle_joint" type="continuous">
             <parent link="${prefix}robotiq_85_base_link" />
             <child link="${prefix}robotiq_85_left_inner_knuckle_link" />
             <axis xyz="0 -1 0" />
             <origin xyz="0.0127 0.0 0.06142" rpy="0 0 0" />
-            <mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
+            <mimic joint="${prefix}finger_joint" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_inner_knuckle_joint" type="continuous">
+        <joint name="${prefix}right_inner_knuckle_joint" type="continuous">
             <parent link="${prefix}robotiq_85_base_link" />
             <child link="${prefix}robotiq_85_right_inner_knuckle_link" />
             <axis xyz="0 -1 0" />
             <origin xyz="-0.0127 0.0 0.06142" rpy="0 0 0" />
-            <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_left_finger_tip_joint" type="continuous">
+        <joint name="${prefix}left_inner_finger_pad_joint" type="continuous">
             <parent link="${prefix}robotiq_85_left_finger_link" />
             <child link="${prefix}robotiq_85_left_finger_tip_link" />
             <axis xyz="0 -1 0" />
             <origin xyz="0.00563134 0.0 0.04718515" rpy="0 0 0" />
-            <mimic joint="${prefix}robotiq_85_left_knuckle_joint" multiplier="-1" />
+            <mimic joint="${prefix}finger_joint" multiplier="-1" />
         </joint>
 
-        <joint name="${prefix}robotiq_85_right_finger_tip_joint" type="continuous">
+        <joint name="${prefix}right_inner_finger_pad_joint" type="continuous">
             <parent link="${prefix}robotiq_85_right_finger_link" />
             <child link="${prefix}robotiq_85_right_finger_tip_link" />
             <axis xyz="0 -1 0" />
             <origin xyz="-0.00563134 0.0 0.04718515" rpy="0 0 0" />
-            <mimic joint="${prefix}robotiq_85_left_knuckle_joint" />
+            <mimic joint="${prefix}finger_joint" />
         </joint>
     </xacro:macro>
 </robot>

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp

@@ -22,7 +22,7 @@ public:
         }
     }
 
-    bool connect(const std::string& hostname, int port) override {
+    bool connecting(const std::string& hostname, int port) override {
         isConnected.store(true);
         return true; // Simulate successful connection
     }
@@ -48,23 +48,35 @@ public:
     }
 
     void activate(bool autoCalibrate = true) override {
-        isActive.store(true);
+        isActive.store(true);  // Ensure we set active before moving
+        if (!autoCalibrate) {
+            // Simulate a calibration sequence
+            move_and_wait(0, 255, 1);
+        }
     }
 
     void deactivate() override {
         isActive.store(false);
     }
 
-    std::tuple<bool, int> move(int targetPosition, int speed, int force) override{
-        // Ensure only one movement operation is active at a time
+    std::tuple<bool, int> move(int targetPosition, int speed, int force) {
+        // Start a new movement operation in a background thread
         if (movementThread && movementThread->joinable()) {
-            movementThread->join(); // Wait for any ongoing movement to complete
+            movementThread->join(); // Ensure no concurrent movements
         }
 
-        // Start a new movement operation in a background thread
-        movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
+        gripperSpeed.store(speed);  // Simulate immediate speed update
+        gripperForce.store(force);  // Simulate immediate force update
 
-        return std::make_tuple(true, targetPosition); // Immediately return success and the target position
+        movementThread = std::make_unique<std::thread>(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force);
+        return std::make_tuple(true, targetPosition);
+    }
+
+    void move_and_wait(int targetPosition, int speed, int force) {
+        auto [success, pos] = move(targetPosition, speed, force);
+        if (movementThread && movementThread->joinable()) {
+            movementThread->join();  // Wait for movement to complete
+        }
     }
 
 private:
@@ -77,17 +89,15 @@ private:
     std::unique_ptr<std::thread> movementThread;
 
     void simulateMovement(int targetPosition, int speed, int force) {
-        int step = (targetPosition > gripperPosition) ? 1 : -1;
+        int step = (targetPosition > gripperPosition.load()) ? 1 : -1;
 
         // Simulate moving towards the target position
-        while (gripperPosition != targetPosition) {
-            gripperPosition += step;
-            std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step
+        while (gripperPosition.load() != targetPosition) {
+            gripperPosition.fetch_add(step);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Simulate time to move a step
         }
 
-        // Here, you simulate setting the speed and force, though for simplicity, we're just logging it
-        // In a real mock, you might update some internal state to reflect these changes
-        std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
+        // std::cout << "Reached position " << targetPosition << " with speed " << speed << " and force " << force << std::endl;
     }
 };
 } // end robotiq_interface

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp

@@ -25,6 +25,8 @@
 #include <cerrno> // For errno
 #include <iostream>
 
+#include <rclcpp/rclcpp.hpp>
+
 #include "robotiq_2f_interface/SocketAdapterBase.hpp"
 
 namespace robotiq_interface
@@ -64,7 +66,7 @@ public:
      *
      * @throws std::runtime_error if socket creation fails.
      */
-    bool connect(const std::string& hostname, int port) override;
+    bool connecting(const std::string& hostname, int port) override;
     /**
     * Closes the connection to the Robotiq gripper.
     *
@@ -88,7 +90,7 @@ public:
     *
     * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
     */
-    bool setGripperVariables(const std::map<std::string, int>& variableMap);
+    bool setGripperVariables(const std::map<std::string, unsigned int>& variableMap);
     bool setGripperVariables(const std::map<std::string, double>& variableMap);
 
     /**
@@ -104,7 +106,7 @@ public:
     *
     * @throws std::runtime_error if the adapter is not connected to the gripper or if sending the command fails.
     */
-    bool setGripperVariable(const std::string& variable, int value);
+    bool setGripperVariable(const std::string& variable, unsigned int value);
     bool setGripperVariable(const std::string& variable, double value);
 
     /**
@@ -159,7 +161,7 @@ public:
     *         activation command fails, or if the gripper does not reach the expected state within
     *         a reasonable timeframe.
     */
-    void activate(bool autoCalibrate = true) override;
+    void activate(bool autoCalibrate = false) override;
     /**
     * Performs auto-calibration of the gripper.
     *
@@ -241,8 +243,8 @@ private:
     std::mutex socket_mutex_; // Mutex for socket access synchronization
 
     // bounds of the encoded gripper states
-    int min_position_ = 0;
-    int max_position_ = 255;
+    int min_position_ = 3;
+    int max_position_ = 230;
     int min_speed_ = 0;
     int max_speed_ = 255;
     int min_force_ = 0;
@@ -277,7 +279,7 @@ private:
     *
     * @throws std::runtime_error if attempted on a disconnected socket or if a select() call fails.
     */
-    std::string receiveResponse(int timeout_ms=2000);
+    std::string receiveResponse(int timeout_ms=2);
 
 
     /**
@@ -304,7 +306,7 @@ private:
     * @param max_value The maximum allowable value for the variable.
     * @return The clipped value, constrained within the specified min and max bounds.
     */
-    int clip_val(int min_value, int variable, int max_value);
+    unsigned int clip_val(int min_value, int variable, int max_value);
     /**
     * Blocks until the gripper reaches a specified status.
     *
@@ -352,8 +354,11 @@ private:
 
     // Constants buffer sizes
     const size_t BUFFER_SIZE = 1024; // Adjust as necessary
-    const int MAX_RETRIES = 5;
-    const int RETRY_DELAY_MS = 20;
+    const int MAX_RETRIES = 200;
+    const int RETRY_DELAY_MS = 50;
+
+    // Logging
+    const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqSocketAdapter");
 };
 } // end robotiq_interface
 

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp

@@ -12,7 +12,7 @@ class SocketAdapterBase {
 public:
     virtual ~SocketAdapterBase() = default;
 
-    virtual bool connect(const std::string& hostname, int port) = 0;
+    virtual bool connecting(const std::string& hostname, int port) = 0;
     virtual void disconnect() = 0;
     virtual int force() = 0;
     virtual int speed() = 0;

src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp

@@ -62,14 +62,16 @@ protected: // Likely changes the access to protected from private
     std::atomic<bool> communication_thread_is_running_;
     std::mutex resource_mutex_;
     void background_task();
+    void performRegularOperations();
 
     static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
 
     // Conversion Methods
-    double convertRawToPosition(int raw_position);
-    int convertPositionToRaw(double position);
-    int convertSpeedToRaw(double speed);
-    int convertForceToRaw(double force);
+    double convertRawToGap(int raw_position);
+    double convertRawToAngle(int raw_position);
+    unsigned int convertGapToRaw(double gap);
+    unsigned int convertSpeedToRaw(double speed);
+    unsigned int convertForceToRaw(double force);
 
     // Gripper Commands
     std::atomic<uint8_t> write_command_;
@@ -79,9 +81,10 @@ protected: // Likely changes the access to protected from private
 
 
     // Gripper State Tracking
-    double gripper_position_ = 0.0;
+    double gripper_gap_ = 0.0;
+    double gripper_angle_ = 0.0;
     double gripper_velocity_ = 0.0;
-    double gripper_position_command_ = 0.0;
+    double gripper_gap_command_ = 0.0;
 
     rclcpp::Clock::SharedPtr clock_ = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
     rclcpp::Time last_update_time_ = clock_ -> now();
@@ -101,13 +104,20 @@ protected: // Likely changes the access to protected from private
     // Parameters for physical limits and configuration
     double min_position_;
     double max_position_;
+    double max_angle_;
     double min_speed_;
     double max_speed_;
     double min_force_;
     double max_force_;
 
+    // Last Command Values
+    int last_cmd_gap = 0;
+    int last_cmd_speed = 0;
+    int last_cmd_force = 0;
+    int last_raw_gap = 0;
+
     // loop time
-    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
+    const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 2 };
 
     // Logger
     const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");

src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp

@@ -12,49 +12,61 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
 }
 
 // Connection and disconnection
-bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
+bool Robotiq2fSocketAdapter::connecting(const std::string& hostname, int port) {
     std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ >= 0) {
-        std::cerr << "Already connected. Disconnecting to reconnect.\n";
+        // std::cerr << "Already connected. Disconnecting to reconnect.\n";
+        RCLCPP_ERROR(logger_, "Already connected. Disconnecting to reconnect.");
         disconnect();
     }
 
     int sock = socket(AF_INET, SOCK_STREAM, 0);
     if (sock < 0) {
-        std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
+        // std::cerr << "Socket creation failed: " << strerror(errno) << "\n";
+        RCLCPP_ERROR(logger_, "Socket creation failed!");
         return false;
     }
     *sockfd_ = sock;
 
+
+
     sockaddr_in serv_addr{};
     memset(&serv_addr, 0, sizeof(serv_addr));
     serv_addr.sin_family = AF_INET;
     serv_addr.sin_port = htons(port);
 
     if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) {
-        std::cerr << "Invalid address: " << hostname << "\n";
+        // std::cerr << "Invalid address: " << hostname << "\n";
+        RCLCPP_ERROR(logger_, "Invalid address: %s", hostname.c_str());
         close(*sockfd_);
         *sockfd_ = -1;
         return false;
     }
 
-    if (::connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) {
-        std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
+    if (connect(*sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) != 0) {
+        // std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n";
+        RCLCPP_ERROR(logger_, "Connection to  %s: %d failed!", hostname.c_str(), port);
         close(*sockfd_);
         *sockfd_ = -1;
         return false;
     }
-
+    else{
+        RCLCPP_INFO(logger_, "Connection to  %s: %d established!", hostname.c_str(), port);
         return true;
     }
+    RCLCPP_ERROR(logger_, "Unknown connection error in socket adapter connecting!");
+    return false;
+}
 
 void Robotiq2fSocketAdapter::disconnect() {
     std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ >= 0) {
         if (close(*sockfd_) == -1) {
-            std::cerr << "Error closing socket: " << strerror(errno) << "\n";
+            // std::cerr << "Error closing socket: " << strerror(errno) << "\n";
+            RCLCPP_ERROR(logger_, "Error closing socket:  %s", strerror(errno));
         } else {
-            std::cout << "Disconnected successfully.\n";
+            // std::cout << "Disconnected successfully.\n";
+            RCLCPP_INFO(logger_, "Disconnected successfully.");
         }
         *sockfd_ = -1;
     }
@@ -64,13 +76,14 @@ void Robotiq2fSocketAdapter::disconnect() {
 bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
     std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
-        std::cerr << "Attempted to send command on a disconnected socket.\n";
+        // std::cerr << "Attempted to send command on a disconnected socket.\n";
+        RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
         return false;
     }
-
     ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0);
     if (result < 0) {
-        std::cerr << "Failed to send command: " << strerror(errno) << "\n";
+        // std::cerr << "Failed to send command: " << strerror(errno) << "\n";
+        RCLCPP_ERROR(logger_, "Failed to send command: %s", strerror(errno));
         return false;
     }
 
@@ -80,7 +93,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
 std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
     std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
-        std::cerr << "Attempted to receive response on a disconnected socket.\n";
+        // std::cerr << "Attempted to send command on a disconnected socket.\n";
+        RCLCPP_ERROR(logger_, "Attempted to send command on a disconnected socket.");
         return "";
     }
 
@@ -98,35 +112,40 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
         FD_ZERO(&readfds);
         FD_SET(*sockfd_, &readfds);
 
+        select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
+/*
         int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv);
         if (activity == -1) {
-            std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
+            // std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n";
+            RCLCPP_ERROR(logger_, "[receiveResponse] Error in select(): %s", strerror(errno));
             return "";
         } else if (activity == 0) {
-            std::cerr << "Timeout occurred while waiting for response.\n";
+            RCLCPP_ERROR(logger_, "[receiveResponse] Activity is Zero!");
             return "";
-        }
+        }*/
 
         ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0);
         if (bytes_read < 0) {
             if (errno != EAGAIN && errno != EWOULDBLOCK) {
-                std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
+                // std::cerr << "Failed to receive response: " << strerror(errno) << "\n";
+                RCLCPP_ERROR(logger_, "Failed to receive response: %s", strerror(errno));
             }
             return "";
         } else if (bytes_read == 0) {
-            std::cerr << "Connection closed by peer.\n";
+            // std::cerr << "Connection closed by peer.\n";
+            RCLCPP_ERROR(logger_, "Connection closed by peer.");
             return "";
         } else {
             buffer[bytes_read] = '\0';
             result += buffer;
-
             // Check if we have a complete response:
             if (result.find("\n") != std::string::npos) {
                 complete_response = true;
+            } else if (result == "ack"){
+                complete_response = true;
             }
         }
     }
-
     return result;
 }
 
@@ -134,8 +153,9 @@ bool Robotiq2fSocketAdapter::isAck(const std::string& data) {
     return data == ACK_MESSAGE;
 }
 
-int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
-    return std::max(min_value, std::min(value, max_value));
+unsigned int Robotiq2fSocketAdapter::clip_val(int min_value, int value, int max_value) {
+    int clipped = std::max(min_value, std::min(value, max_value));
+    return static_cast<unsigned int>(clipped);
 }
 
 void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus) {
@@ -153,8 +173,31 @@ void Robotiq2fSocketAdapter::waitForGripperStatus(GripperStatus expectedStatus)
 
 
 // Gripper variable Getter and Setter
-bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, int>& variableMap) {
-    return setGripperVariables(reinterpret_cast<const std::map<std::string, double>&>(variableMap));
+bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, unsigned int>& variableMap) {
+    // Build the command string
+    std::string cmd = SET_COMMAND;
+    for (const auto& [variable, value] : variableMap) {
+        std::stringstream ss;
+        ss << value;
+        cmd += " " + variable + " " + ss.str();
+    }
+    cmd += "\n";
+
+    // Send the command and receive the response
+
+    if (*sockfd_ < 0) {
+        throw std::runtime_error("Cannot set variables on a disconnected socket.");
+        return false;
+    }
+
+    if (!sendCommand(cmd)) {
+        throw std::runtime_error("[setGripperVariables] Sending command failed.");
+        return false;
+    }
+
+    std::string response = receiveResponse();
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
 }
 
 bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, double>& variableMap) {
@@ -168,7 +211,7 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
     cmd += "\n";
 
     // Send the command and receive the response
-    std::lock_guard<std::mutex> lock(socket_mutex_);
+
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -180,20 +223,16 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map<std::string, dou
     }
 
     std::string response = receiveResponse();
+    RCLCPP_DEBUG(logger_, "[setGripperVariables]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
-bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, int value) {
-    return setGripperVariable(variable, static_cast<double>(value)); // Convert int to double
-}
-
-bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, unsigned int value) {
     std::stringstream ss;
     ss << value;  // Convert the value to a string
 
     std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
 
-    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot set variables on a disconnected socket.");
         return false;
@@ -205,30 +244,52 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
     }
 
     std::string response = receiveResponse();
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
+    return isAck(response);
+}
+
+bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, double value) {
+    std::stringstream ss;
+    ss << value;  // Convert the value to a string
+
+    std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
+
+    if (*sockfd_ < 0) {
+        throw std::runtime_error("Cannot set variables on a disconnected socket.");
+        return false;
+    }
+
+    if (!sendCommand(cmd)) {
+        throw std::runtime_error("[setGripperVariable] Sending command failed.");
+        return false; // Sending command failed
+    }
+
+    std::string response = receiveResponse();
+    RCLCPP_DEBUG(logger_, "[setGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     return isAck(response);
 }
 
 int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
     std::string cmd = GET_COMMAND + " " + variable + "\n";
 
-    std::lock_guard<std::mutex> lock(socket_mutex_);
     if (*sockfd_ < 0) {
         throw std::runtime_error("Cannot get variables on a disconnected socket.");
         return -1;
     }
-
     if (!sendCommand(cmd)) {
         throw std::runtime_error("[getGripperVariable] Sending command failed.");
         return -1; // Error sending command
     }
-
     std::string response = receiveResponse();
-
-    // Parse the response (assuming format: "variable_name value")
+    RCLCPP_DEBUG(logger_, "[getGripperVariable]Command: %s and Response: %s", cmd.c_str(), response.c_str());
     std::istringstream iss(response);
     std::string var_name, value_str;
 
-    if (!(iss >> var_name >> value_str)) {
+    if (response == ACK_MESSAGE) {
+        RCLCPP_DEBUG(logger_, "[getGripperVariable]Gripper still responding with ACK_MESSAGE");
+        return -1;
+    } else if (!(iss >> var_name >> value_str)) {
+        RCLCPP_ERROR(logger_, "Invalid gripper response format: %s for cmd: %s", response.c_str(), cmd.c_str());
         throw std::runtime_error("Invalid gripper response format.");
         return -1;
     }
@@ -241,7 +302,8 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
     try {
         return std::stoi(value_str);
     } catch (const std::exception& ex) {
-        std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
+        // std::cerr << "Failed to parse gripper response: " << ex.what() << std::endl;
+        RCLCPP_ERROR(logger_, "Failed to parse gripper response: %s",  ex.what());
         return -1;
     }
 }
@@ -249,17 +311,18 @@ int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
 // Activation, deactivation and reset of the Gripper
 void Robotiq2fSocketAdapter::activate(bool autoCalibrate) {
     reset(); // Always start with a reset to ensure a known state
-    setGripperVariable(CMD_ACT, 1); // Activate the gripper
+    setGripperVariable(CMD_ACT, static_cast<unsigned int>(1)); // Activate the gripper
     waitForGripperStatus(GripperStatus::ACTIVE); // Wait until active
 
-    if (autoCalibrate) {
+    if (!autoCalibrate) {
         auto_calibrate();
     }
+    RCLCPP_DEBUG(logger_, "Socket adapter activated.");
 }
 
 void Robotiq2fSocketAdapter::reset() {
-    setGripperVariable(CMD_ACT, 0);
-    setGripperVariable(CMD_ATR, 0);
+    setGripperVariable(CMD_ACT, static_cast<unsigned int>(0));
+    setGripperVariable(CMD_ATR, static_cast<unsigned int>(0));
     waitForGripperStatus(GripperStatus::RESET);
 }
 
@@ -290,42 +353,45 @@ int Robotiq2fSocketAdapter::position(){
 
 // Movement Methods
 std::tuple<bool, int> Robotiq2fSocketAdapter::move(int position, int speed, int force) {
-    int clippedPosition = clip_val(min_position_, position, max_position_);
-    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
-    int clippedForce = clip_val(min_force_, force, max_force_);
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
+
+    RCLCPP_DEBUG(logger_, "Current min/max positions: [%d, %d]", min_position_, max_position_);
+    RCLCPP_DEBUG(logger_, "Clipped command: %d", clippedPosition);
 
     // Create a map for gripper variables (similar to Python's OrderedDict)
-    std::map<std::string, int> variableMap = {
+    std::map<std::string, unsigned int> variableMap = {
         { CMD_POS, clippedPosition },
         { CMD_SPE, clippedSpeed },
         { CMD_FOR, clippedForce },
-        { CMD_GTO, 1 }
+        { CMD_GTO, static_cast<unsigned int>(1) }
     };
-
     bool setResult = setGripperVariables(variableMap);
+    if (!setResult)
+    {
+        std::string result;
+        for (const auto& pair : variableMap) {
+            result += "{" + pair.first + ", " + std::to_string(pair.second) + "}, ";
+        }
+        RCLCPP_ERROR(logger_, "[move] failed for CMD Map: %s", result.c_str());
+    }
     return std::make_tuple(setResult, clippedPosition);
 }
 
 std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) {
-    int clippedPosition = clip_val(min_position_, position, max_position_);
-    int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
-    int clippedForce = clip_val(min_force_, force, max_force_);
+    unsigned int clippedPosition = clip_val(min_position_, position, max_position_);
+    unsigned int clippedSpeed = clip_val(min_speed_, speed, max_speed_);
+    unsigned int clippedForce = clip_val(min_force_, force, max_force_);
 
-    // Set gripper variables to initiate the move
-    std::map<std::string, int> variableMap = {
-        { CMD_POS, clippedPosition },
-        { CMD_SPE, clippedSpeed },
-        { CMD_FOR, clippedForce },
-        { CMD_GTO, 1 }
-    };
-    bool setResult = setGripperVariables(variableMap);
-    if (!setResult) {
+    auto move_result = move(clippedPosition, clippedSpeed, clippedForce);
+    if (!std::get<0>(move_result)) {
         // Handle the error case, e.g., throw an exception
-        throw std::runtime_error("Failed to set variables for move.");
+        RCLCPP_ERROR(logger_, "Failed to set variables for move.");
     }
 
     // Wait until position is acknowledged:
-    while(getGripperVariable(CMD_PRE) != clippedPosition) {
+    while(getGripperVariable(CMD_PRE) != std::get<1>(move_result)) {
         std::this_thread::sleep_for(std::chrono::milliseconds(10)); // Adjust sleep as needed
     }
 
@@ -345,6 +411,7 @@ std::tuple<int, ObjectStatus> Robotiq2fSocketAdapter::move_and_wait_for_pos(int
 
 void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
     // Open in case we are holding an object
+    RCLCPP_INFO(logger_, "Calibration started");
     std::tuple<int, ObjectStatus> result = move_and_wait_for_pos(open_position_, 64, 1);
     if (std::get<1>(result) != ObjectStatus::AT_DEST) {
         throw std::runtime_error("Calibration failed opening to start.");
@@ -365,8 +432,9 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log) {
     min_position_ = std::get<0>(result); // Update min position
 
     if (log) {
-        std::cout << "Gripper auto-calibrated to ["
-                  << min_position_ << ", " << max_position_ << "]\n";
+        // std::cout << "Gripper auto-calibrated to ["
+        //           << min_position_ << ", " << max_position_ << "]\n";
+        RCLCPP_INFO(logger_, "Gripper auto-calibrated to [%d, %d]", min_position_, max_position_);
     }
 }
 } // end robotiq_interface

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -18,6 +18,9 @@ const double min_position_default = 0.01;
 const std::string max_position_parameter_name = "max_position";
 const double max_position_default = 0.0;
 
+const std::string max_angle_parameter_name = "max_angle";
+const double max_angle_default = 0.5;
+
 const std::string min_speed_parameter_name = "min_speed";
 const double min_speed_default = 0.0;
 
@@ -33,7 +36,7 @@ const double max_force_default = 0.0;
 RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
   : communication_thread_is_running_(false)
 {
-RCLCPP_INFO(logger_, "Constructor");
+RCLCPP_DEBUG(logger_, "Constructor");
 }
 
 RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
@@ -62,6 +65,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) {
             robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name);
         } else {
+            RCLCPP_DEBUG(logger_, "Falling back on default ip!");
             robot_ip_ = robot_ip_default;
         }
         RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str());
@@ -71,7 +75,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         } else {
             robot_port_ = robot_port_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_);
 
         std::string param_value;
         if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) {
@@ -80,49 +84,57 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         } else {
             use_mock_hardware_ = use_mock_hardware_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str());
+        RCLCPP_DEBUG(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str());
 
         if (info.hardware_parameters.count(min_position_parameter_name) > 0) {
             min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name));
         } else {
             min_position_ = min_position_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: min position, Value: %f", min_position_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min position, Value: %f", min_position_);
 
         if (info.hardware_parameters.count(max_position_parameter_name) > 0) {
             max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name));
         } else {
             max_position_ = max_position_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max position, Value: %f", max_position_);
+
+        if (info.hardware_parameters.count(max_angle_parameter_name) > 0) {
+            max_angle_ = std::stod(info.hardware_parameters.at(max_angle_parameter_name));
+        } else {
+            RCLCPP_WARN(logger_, "max angle not found falling back to default value!");
+            max_angle_ = max_angle_default;
+        }
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max angle , Value: %f", max_angle_);
 
         if (info.hardware_parameters.count(min_speed_parameter_name) > 0) {
             min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name));
         } else {
             min_speed_ = min_speed_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: min speed, Value: %f", min_speed_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min speed, Value: %f", min_speed_);
 
         if (info.hardware_parameters.count(max_speed_parameter_name) > 0) {
             max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name));
         } else {
             max_speed_ = max_speed_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: max speed, Value: %f", max_speed_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max speed, Value: %f", max_speed_);
 
                 if (info.hardware_parameters.count(min_force_parameter_name) > 0) {
             min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name));
         } else {
             min_force_ = robot_port_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: min force, Value: %f", min_force_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: min force, Value: %f", min_force_);
 
         if (info.hardware_parameters.count(max_force_parameter_name) > 0) {
             max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name));
         } else {
             max_force_ = max_force_default;
         }
-        RCLCPP_INFO(logger_, "Accessing parameter: max force, Value: %f", max_force_);
+        RCLCPP_DEBUG(logger_, "Accessing parameter: max force, Value: %f", max_force_);
     }
     catch (const std::exception& e)
     {
@@ -130,49 +142,69 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         return CallbackReturn::ERROR;
     }
 
-    gripper_position_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_gap_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_angle_ = std::numeric_limits<double>::quiet_NaN();
     gripper_velocity_ = std::numeric_limits<double>::quiet_NaN();
-    gripper_position_command_ = std::numeric_limits<double>::quiet_NaN();
+    gripper_gap_command_ = std::numeric_limits<double>::quiet_NaN();
     reactivate_gripper_cmd_ = NO_NEW_CMD_;
     reactivate_gripper_async_cmd_.store(false);
 
-    const hardware_interface::ComponentInfo& joint = info_.joints[0];
+    const hardware_interface::ComponentInfo& joint_gap = info_.joints[0];
+    const hardware_interface::ComponentInfo& joint_angle = info_.joints[1];
 
     // There is one command interface: position.
-    if (joint.command_interfaces.size() != 1)
+    if (joint_gap.command_interfaces.size() != 1)
     {
-        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(),
-                     joint.command_interfaces.size());
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint_gap.name.c_str(),
+                     joint_gap.command_interfaces.size());
         return CallbackReturn::ERROR;
     }
 
-    if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
+    if (joint_gap.command_interfaces[0].name != hardware_interface::HW_IF_POSITION)
     {
-        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(),
-                     joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint_gap.name.c_str(),
+                     joint_gap.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
         return CallbackReturn::ERROR;
     }
 
-    // There are two state interfaces: position and velocity.
-    if (joint.state_interfaces.size() != 2)
+    // There are two state interfaces for the gripper gap: position and velocity.
+    if (joint_gap.state_interfaces.size() != 2)
     {
-        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
-                     joint.state_interfaces.size());
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint_gap.name.c_str(),
+                     joint_gap.state_interfaces.size());
         return CallbackReturn::ERROR;
     }
 
     for (int i = 0; i < 2; ++i)
     {
-        if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
-              joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
+        if (!(joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
+              joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
         {
-            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(),
-                       joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
+            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_gap.name.c_str(),
+                       joint_gap.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
                        hardware_interface::HW_IF_VELOCITY);
             return CallbackReturn::ERROR;
         }
     }
 
+        // There are two state interfaces for the gripper angle: position.
+    if (joint_angle.state_interfaces.size() != 1)
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 1 expected.", joint_angle.name.c_str(),
+                     joint_angle.state_interfaces.size());
+        return CallbackReturn::ERROR;
+    }
+
+    if (!(joint_angle.state_interfaces[0].name == hardware_interface::HW_IF_POSITION))
+    {
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_angle.name.c_str(),
+                   joint_angle.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION,
+                   hardware_interface::HW_IF_VELOCITY);
+        return CallbackReturn::ERROR;
+    }
+
+
+
     return CallbackReturn::SUCCESS;
 }
 
@@ -187,11 +219,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure
         }
 
         configureAdapter(use_mock_hardware_);
-        if (!socket_adapter_->connect(robot_ip_, robot_port_))
-        {
-            RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
-            return CallbackReturn::ERROR;
-        }
     }
     catch (const std::exception& e)
     {
@@ -207,21 +234,29 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(
     RCLCPP_DEBUG(logger_, "on_activate");
 
     // set some default values for joints
-    if (std::isnan(gripper_position_))
+    if (std::isnan(gripper_gap_))
     {
-        gripper_position_ = 0;
-        gripper_velocity_ = 0;
-        gripper_position_command_ = 0;
+        gripper_gap_ = 0.0;
+        gripper_angle_ = 0.0;
+        gripper_velocity_ = 0.0;
+        gripper_gap_command_ = 0.0;
     }
 
     // Activate the gripper.
     try
     {
+        RCLCPP_INFO(logger_, "Starting Socket connection!");
+        if (!socket_adapter_->connecting(robot_ip_, robot_port_))
+        {
+            RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_);
+            return CallbackReturn::ERROR;
+        }
         socket_adapter_->deactivate();
         socket_adapter_->activate();
 
         communication_thread_is_running_.store(true);
         communication_thread_ = std::thread([this] { this->background_task(); });
+        RCLCPP_INFO(logger_, "Background task thread started.");
     }
     catch (const std::exception& e)
     {
@@ -229,9 +264,15 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate(
         return CallbackReturn::ERROR;
     }
 
+    if (!socket_adapter_->is_active()) {
+        RCLCPP_ERROR(logger_, "Attempted to activate while gripper is not active!");
+        return CallbackReturn::ERROR;
+    }
+    else{
         RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!");
         return CallbackReturn::SUCCESS;
     }
+}
 
 hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
 {
@@ -264,10 +305,11 @@ std::vector<hardware_interface::StateInterface> RobotiqGripperHardwareInterface:
     std::vector<hardware_interface::StateInterface> state_interfaces;
 
     state_interfaces.emplace_back(
-        hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_));
+        hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_));
     state_interfaces.emplace_back(
         hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_));
-
+    state_interfaces.emplace_back(
+        hardware_interface::StateInterface(info_.joints[1].name, hardware_interface::HW_IF_POSITION, &gripper_angle_));
     for (const auto& interface : state_interfaces) {
     RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s",
                 interface.get_name().c_str(), interface.get_interface_name().c_str());
@@ -284,7 +326,7 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
 
     command_interfaces.emplace_back(
         hardware_interface::CommandInterface(
-        info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_
+        info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_gap_command_
         )
     );
 
@@ -303,9 +345,9 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
     gripper_force_ = gripper_force_multiplier;
 
     command_interfaces.emplace_back(
-        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
+        hardware_interface::CommandInterface(info_.joints[0].name, "max_velocity", &gripper_speed_));
     command_interfaces.emplace_back(
-        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
+        hardware_interface::CommandInterface(info_.joints[0].name, "max_effort", &gripper_force_));
     command_interfaces.emplace_back(
         hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
     command_interfaces.emplace_back(
@@ -329,16 +371,18 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
 
     try {
         int raw_position = socket_adapter_->position();
-        double new_position = convertRawToPosition(raw_position);
+        double new_gap = convertRawToGap(raw_position);
+        double new_angle = convertRawToAngle(raw_position);
 
-        if (!std::isnan(gripper_position_)) {
+        if (!std::isnan(gripper_gap_)) {
             double time_difference = (current_time_ - last_update_time_).seconds();
             if (time_difference > 0) {
-                gripper_velocity_ = (new_position - gripper_position_) / time_difference;
+                gripper_velocity_ = (new_gap - gripper_gap_) / time_difference;
             }
         }
 
-        gripper_position_ = new_position;
+        gripper_gap_ = new_gap;
+        gripper_angle_ = new_angle;
         last_update_time_ = current_time_;
 
     } catch (const std::exception& e) {
@@ -347,9 +391,17 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc
     }
 
     // Process asynchronous reactivation response if available
-    if (reactivate_gripper_async_response_.load().has_value()) {
+    if (!std::isnan(reactivate_gripper_cmd_))
+    {
+        RCLCPP_INFO(logger_, "Sending gripper reactivation request.");
+        reactivate_gripper_async_cmd_.store(true);
+        reactivate_gripper_cmd_ = NO_NEW_CMD_;
+    }
+
+    if (reactivate_gripper_async_response_.load().has_value())
+    {
         reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value();
-        reactivate_gripper_async_response_.store(std::nullopt);  // Reset response
+        reactivate_gripper_async_response_.store(std::nullopt);
     }
 
     return hardware_interface::return_type::OK;
@@ -359,12 +411,12 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl
 {
     try {
         // Use the conversion methods to scale the command values to the range expected by the hardware
-        int scaled_position = convertPositionToRaw(gripper_position_command_);
+        int scaled_gap = convertGapToRaw(gripper_gap_command_);
         int scaled_speed = convertSpeedToRaw(gripper_speed_);
         int scaled_force = convertForceToRaw(gripper_force_);
 
         // Store the scaled values into atomic variables
-        write_command_.store(scaled_position);
+        write_command_.store(scaled_gap);
         write_speed_.store(scaled_speed);
         write_force_.store(scaled_force);
 
@@ -381,38 +433,54 @@ void RobotiqGripperHardwareInterface::background_task()
     while (communication_thread_is_running_.load())
     {
         std::lock_guard<std::mutex> guard(resource_mutex_);
-        try
-        {
-            // Re-activate the gripper
-            // This can be used, for example, to re-run the auto-calibration.
         if (reactivate_gripper_async_cmd_.load())
         {
+            try
+            {
+                // Execute the reactivation sequence
+                RCLCPP_INFO(logger_, "Reactivating gripper.");
                 socket_adapter_->deactivate();
-                socket_adapter_->activate();
-                reactivate_gripper_async_cmd_.store(false);
-                reactivate_gripper_async_response_.store(true);
-            }
-
-            // Write the latest command to the gripper.
-            int scaled_position = write_command_.load();
-            int scaled_speed = write_speed_.load();
-            int scaled_force = write_force_.load();
-
-            auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
-            if (!std::get<0>(result)) {
-                throw std::runtime_error("Failed to send move command to Robotiq gripper.");
-            }
-
-            // Read the state of the gripper.
-            int raw_position = socket_adapter_->position();
-            gripper_current_state_.store(convertRawToPosition(raw_position));
+                socket_adapter_->activate(false);
+                reactivate_gripper_async_cmd_.store(false);  // Reset the command flag
+                reactivate_gripper_async_response_.store(true);  // Store the success of the operation
             }
             catch (const std::exception& e)
             {
-            RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
+                RCLCPP_ERROR(logger_, "Failed to reactivate gripper: %s", e.what());
+                reactivate_gripper_async_response_.store(false);  // Store the failure of the operation
+            }
         }
 
-        std::this_thread::sleep_for(gripperCommsLoopPeriod);  // Sleep for a predefined period
+        // Additional periodic operations
+        performRegularOperations();
+        std::this_thread::sleep_for(gripperCommsLoopPeriod);  // Sleep for the duration of the communication loop period
+    }
+}
+
+void RobotiqGripperHardwareInterface::performRegularOperations()
+{
+    try
+    {
+        int scaled_gap = write_command_.load();
+        int scaled_speed = write_speed_.load();
+        int scaled_force = write_force_.load();
+
+        if (scaled_gap != last_cmd_gap || scaled_speed != last_cmd_speed || scaled_force != last_cmd_force){
+            RCLCPP_DEBUG(logger_, "New move cmd: POS: %d, SPE: %d, FOR: %d", scaled_gap, scaled_speed , scaled_force);
+            auto result = socket_adapter_->move(scaled_gap, scaled_speed, scaled_force);
+            if (!std::get<0>(result)) {
+                throw std::runtime_error("Failed to send move command to Robotiq gripper.");
+            }
+            last_cmd_gap = scaled_gap;
+            last_cmd_speed = scaled_speed;
+            last_cmd_force = scaled_force;
+        }
+        int raw_gap = socket_adapter_->position();
+        gripper_current_state_.store(convertRawToGap(raw_gap));
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_ERROR(logger_, "Regular operation error: %s", e.what());
     }
 }
 
@@ -430,36 +498,44 @@ void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) {
 
 
 // Conversion methods
-double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
+double RobotiqGripperHardwareInterface::convertRawToGap(int raw_position) {
     if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
-        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
+        throw std::runtime_error("Invalid gripper position limits.");
     }
-    double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
+    double scaled_position = min_position_ + ((230 - static_cast<double>(raw_position)) / 227.0) * (max_position_ - min_position_);
     return std::max(min_position_, std::min(scaled_position, max_position_));
 }
 
-int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
-    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
-        throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
+double RobotiqGripperHardwareInterface::convertRawToAngle(int raw_position) {
+    if (std::isnan(max_angle_) || 0.0 >= max_angle_) {
+        throw std::runtime_error("Invalid gripper angle limits: max_angle is not configured correctly.");
     }
-    double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
-    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+    double scaled_angle = ((230 - static_cast<double>(raw_position)) / 227.0) * max_angle_;
+    return std::max(0.0, std::min(scaled_angle, max_angle_));
 }
 
-int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
+unsigned int RobotiqGripperHardwareInterface::convertGapToRaw(double gap) {
+    if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
+        throw std::runtime_error("Invalid gripper position limits.");
+    }
+    unsigned int raw = static_cast<unsigned int>(230 - ((gap - min_position_) / (max_position_ - min_position_)) * 227);
+    return std::clamp(raw, 3u, 230u); // Ensuring the output is within the valid range
+}
+
+unsigned int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
     if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
         throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
     }
     double scaled = (speed - min_speed_) / (max_speed_ - min_speed_) * 255.0;
-    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+    return static_cast<unsigned int>(std::clamp(scaled, 0.0, 255.0));
 }
 
-int RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
+unsigned int RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
     if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
         throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
     }
     double scaled = (force - min_force_) / (max_force_ - min_force_) * 255.0;
-    return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
+    return static_cast<unsigned int>(std::clamp(scaled, 0.0, 255.0));
 }
 
 }  // namespace robotiq_interface

src/robotiq_2f/robotiq_2f_msgs/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.8)
+project(robotiq_2f_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+find_package(action_msgs REQUIRED)
+
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ForwardCommand.msg"
+  "action/GripperCommand.action"
+  # Add more messages here
+  DEPENDENCIES builtin_interfaces action_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+
+ament_package()

src/robotiq_2f/robotiq_2f_msgs/action/GripperCommand.action

@@ -0,0 +1,12 @@
+# Goal definition
+float64 desired_position    # in m
+float64 max_velocity        # in m/s
+float64 max_effort          # in Newtons
+---
+# Result definition
+float64 final_position      # The final gripper gap size (in meters)
+bool stalled                # True if the gripper is exerting max effort and not moving
+bool reached_goal           # True if the gripper position has reached the commanded setpoint
+---
+# Feedback definition
+float64 current_position    # in m

src/robotiq_2f/robotiq_2f_msgs/msg/ForwardCommand.msg

@@ -0,0 +1,3 @@
+float64 position
+float64 max_velocity
+float64 max_effort

src/robotiq_2f/robotiq_2f_msgs/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>robotiq_2f_msgs</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nikolaus.feith@unileoben.ac.at">niko</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msgs</depend>
+  <depend>builtin_interfaces</depend>
+  <depend>action_msgs</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/robotiq_2f/robotiq_controllers/CHANGELOG.rst

@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package robotiq_controllers
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.0.1 (2023-07-17)
-------------------
-* Initial ROS 2 release of robotiq_controllers
-  * This package is not supported by Robotiq but is being maintained by PickNik Robotics
-* Contributors: Alex Moriarty, Cory Crean

src/robotiq_2f/robotiq_controllers/CMakeLists.txt

@@ -1,84 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(robotiq_controllers)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(controller_interface REQUIRED)
-find_package(std_srvs REQUIRED)
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  controller_interface
-  std_srvs
-)
-
-include_directories(include)
-
-add_library(${PROJECT_NAME} SHARED
-  src/robotiq_activation_controller.cpp
-)
-
-target_include_directories(${PROJECT_NAME} PRIVATE
-  include
-)
-
-ament_target_dependencies(${PROJECT_NAME}
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
-)
-
-pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
-
-# # INSTALL
-install(
-  TARGETS ${PROJECT_NAME}
-  EXPORT export_${PROJECT_NAME}
-  ARCHIVE DESTINATION lib/${PROJECT_NAME}
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-install(
-  DIRECTORY include/
-  DESTINATION include
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-
-  # the following skips uncrustify
-  # ament_uncrustify and ament_clang_format cannot both be satisfied
-  set(ament_cmake_uncrustify_FOUND TRUE)
-
-  # the following skips xmllint
-  # ament_xmllint requires network and can timeout if on throttled networks
-  set(ament_cmake_xmllint_FOUND TRUE)
-
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-# # EXPORTS
-ament_export_include_directories(
-  include
-)
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
-ament_export_targets(
-  export_${PROJECT_NAME}
-)
-ament_export_dependencies(
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
-)
-
-ament_package()

src/robotiq_2f/robotiq_controllers/controller_plugins.xml

@@ -1,7 +0,0 @@
-<library path="robotiq_controllers">
-    <class name="robotiq_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
-        <description>
-            This controller provides an interface to (re-)activate the Robotiq gripper.
-        </description>
-    </class>
-</library>

src/ur_robotiq_description/config/ur_robotiq_calibration.yaml

@@ -0,0 +1,44 @@
+kinematics:
+  shoulder:
+    x: 0
+    y: 0
+    z: 0.1519161231793138
+    roll: -0
+    pitch: 0
+    yaw: -8.3688888041777445e-08
+  upper_arm:
+    x: -9.8255319984929057e-05
+    y: 0
+    z: 0
+    roll: 1.5713473529266058
+    pitch: 0
+    yaw: 4.5372573280303068e-06
+  forearm:
+    x: -0.24361145101605494
+    y: 0
+    z: 0
+    roll: 3.138959939545904
+    pitch: 3.141417818890341
+    yaw: 3.1415861917625292
+  wrist_1:
+    x: -0.21328351386382516
+    y: -0.00058695912107010507
+    z: 0.13086135281159214
+    roll: 0.0044853210843803608
+    pitch: 0.00071590512460444685
+    yaw: -2.0286176909971606e-06
+  wrist_2:
+    x: 9.0086536104922084e-05
+    y: -0.085314349728116412
+    z: -6.1195228207677147e-05
+    roll: 1.5715136178239859
+    pitch: 0
+    yaw: 1.3915445690113049e-06
+  wrist_3:
+    x: -0.00014076969524819022
+    y: 0.092309359181826575
+    z: -0.00011628039579221433
+    roll: 1.5695366459205147
+    pitch: 3.1415926535897931
+    yaw: 3.1415925648453848
+  hash: calib_2535041911463403862

src/ur_robotiq_description/config/ur_robotiq_cartesian_controllers.yaml

@@ -1,233 +0,0 @@
-controller_manager:
-  ros__parameters:
-    force_torque_sensor_broadcaster:
-      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
-
-    scaled_joint_trajectory_controller:
-      type: ur_controllers/ScaledJointTrajectoryController
-
-    joint_state_broadcaster:
-      type: joint_state_broadcaster/JointStateBroadcaster
-
-    cartesian_compliance_controller:
-      type: cartesian_compliance_controller/CartesianComplianceController
-
-    cartesian_force_controller:
-      type: cartesian_force_controller/CartesianForceController
-
-    cartesian_motion_controller:
-      type: cartesian_motion_controller/CartesianMotionController
-
-    motion_control_handle:
-      type: cartesian_controller_handles/MotionControlHandle
-
-    joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
-
-
-force_torque_sensor_broadcaster:
-  ros__parameters:
-    sensor_name: tcp_fts_sensor
-    state_interface_names:
-      - force.x
-      - force.y
-      - force.z
-      - torque.x
-      - torque.y
-      - torque.z
-    frame_id: tool0
-    topic_name: wrench
-
-scaled_joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_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
-      shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 }
-      shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 }
-      elbow_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_1_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_2_joint: { trajectory: 0.2, goal: 0.1 }
-      wrist_3_joint: { trajectory: 0.2, goal: 0.1 }
-
-cartesian_compliance_controller:
-  ros__parameters:
-
-    # This is the tip of the robot tool that you usually use for your task.
-    # For instance, it could be the drilling bit of a screwdriver or a grinding
-    # tool.  When you specify a target_wrench, i.e. some additional forces that
-    # your robot should apply to its environment, that target_wrench gets
-    # applied in this frame.
-    end_effector_link: "tool0"
-
-    # This is usually the link directly before the first actuated joint.  All
-    # controllers will build a kinematic chain from this link up to
-    # end_effector_link.  It's also the reference frame for the superposition
-    # of error components in all controllers.
-    robot_base_link: "base_link"
-
-    # This is the URDF link of your sensor. Sensor signals are assumed to be
-    # given in this frame.  It's important that this link is located somewhere
-    # between end_effector_link and robot_base_link.  If that's not the case,
-    # the controllers won't initialize and will emit an error message.
-    ft_sensor_ref_link: "tool0"
-
-    # This is the link that the robot feels compliant about. It does not need
-    # to coincide with the end_effector_link, but for many use cases, this
-    # configuration is handy. When working with a screwdriver, for instance,
-    # setting compliance_ref_link == end_effector_link makes it easy to specify
-    # downward pushing forces without generating unwanted offset moments.
-    # On the other hand, an application could benefit from yielding a little in
-    # the robot's wrist while drawing a line on a surface with a pen.
-    compliance_ref_link: "tool0"
-
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # Choose between position or velocity.  In fact, the controllers allow to
-    # set both at the same time, but not all drivers will support this.
-    # In general, position control is a little smoother due to the double
-    # time-integrated commands from the solver. If available on a robot, it
-    # should be the default.  On some drivers, the velocity interface provides
-    # faster control cycles, and hence could improve stability in
-    # contact-dominated tasks.  A drawback is that we lose one time integration
-    # step here and obtain noisier command signals in comparison to the
-    # position interface.  It's probably suitable to test both on a new robot
-    # and decide for what works best.
-    command_interfaces:
-      - position
-        #- velocity
-
-    stiffness:  # w.r.t. compliance_ref_link coordinates
-        trans_x: 500.0
-        trans_y: 500.0
-        trans_z: 500.0
-        rot_x: 20.0
-        rot_y: 20.0
-        rot_z: 20.0
-
-    solver:
-        error_scale: 0.5
-        iterations: 1
-        publish_state_feedback: True
-
-    # For all controllers, these gains are w.r.t. the robot_base_link coordinates.
-    pd_gains:
-        trans_x: {p: 0.05, d: 0.005}
-        trans_y: {p: 0.05, d: 0.005}
-        trans_z: {p: 0.05, d: 0.005}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_force_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 0.5
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 0.05}
-        trans_y: {p: 0.05}
-        trans_z: {p: 0.05}
-        rot_x: {p: 1.5}
-        rot_y: {p: 1.5}
-        rot_z: {p: 1.5}
-
-cartesian_motion_controller:
-  ros__parameters:
-
-    # See the cartesian_compliance_controller
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-    # See the cartesian_compliance_controller
-    command_interfaces:
-      - position
-        #- velocity
-
-    solver:
-        error_scale: 1.0
-        iterations: 10
-        publish_state_feedback: True
-
-    pd_gains:
-        trans_x: {p: 1.0}
-        trans_y: {p: 1.0}
-        trans_z: {p: 1.0}
-        rot_x: {p: 0.5}
-        rot_y: {p: 0.5}
-        rot_z: {p: 0.5}
-
-motion_control_handle:
-  ros__parameters:
-    end_effector_link: "tool0"
-    robot_base_link: "base_link"
-    ft_sensor_ref_link: "tool0"
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-
-joint_trajectory_controller:
-  ros__parameters:
-    joints:
-      - shoulder_pan_joint
-      - shoulder_lift_joint
-      - elbow_joint
-      - wrist_1_joint
-      - wrist_2_joint
-      - wrist_3_joint
-    command_interfaces:
-      - position
-    state_interfaces:
-      - position
-      - velocity

src/ur_robotiq_description/config/ur_robotiq_controllers.yaml

@@ -25,16 +25,13 @@ controller_manager:
       type: position_controllers/JointGroupPositionController
 
     forward_gripper_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: robotiq_2f_controllers/ForwardController
 
     robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
-
-    robotiq_gripper_joint_trajectory_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
+      type: robotiq_2f_controllers/GripperCommand
 
     robotiq_activation_controller:
-      type: robotiq_controllers/RobotiqActivationController
+      type: robotiq_2f_controllers/RobotiqActivationController
 
 speed_scaling_state_broadcaster:
   ros__parameters:
@@ -138,32 +135,12 @@ forward_position_controller:
 
 forward_gripper_position_controller:
   ros__parameters:
-    joints:
-      - $(var tf_prefix)finger_joint
-
+    joint: $(var prefix)gripper_gap
 
 robotiq_gripper_controller:
   ros__parameters:
-    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 }
+    joint: $(var prefix)gripper_gap
+    max_gap: 0.14
 
 robotiq_activation_controller:
   ros__parameters:

src/ur_robotiq_description/launch/ur_robotiq_control.launch.py

@@ -165,6 +165,9 @@ def launch_setup(context, *args, **kwargs):
             "trajectory_port:=",
             trajectory_port,
             " ",
+            "prefix:=",
+            tf_prefix,
+            " ",
         ]
     )
 
@@ -214,13 +217,6 @@ def launch_setup(context, *args, **kwargs):
         package="controller_manager",
         executable="spawner",
         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(
@@ -240,20 +236,6 @@ def launch_setup(context, *args, **kwargs):
         parameters=[{"robot_ip": robot_ip}],
     )
 
-    tool_communication_node = Node(
-        package="ur_robot_driver",
-        condition=IfCondition(use_tool_communication),
-        executable="tool_communication.py",
-        name="ur_tool_comm",
-        output="screen",
-        parameters=[
-            {
-                "robot_ip": robot_ip,
-                "tcp_port": tool_tcp_port,
-                "device_name": tool_device_name,
-            }
-        ],
-    )
 
     controller_stopper_node = Node(
         package="ur_robot_driver",
@@ -358,7 +340,6 @@ def launch_setup(context, *args, **kwargs):
                          initial_joint_controller_spawner_stopped,
                          initial_joint_controller_spawner_started,
                          robotiq_gripper_controller_spawner,
-                         robotiq_gripper_joint_trajectory_controller_spawner,
                          robotiq_activation_controller_spawner,
                      ] + controller_spawners
 
@@ -451,6 +432,15 @@ def generate_launch_description():
         have to be updated.",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "use_fake_hardware",

src/ur_robotiq_description/package.xml

@@ -23,9 +23,9 @@
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>ur_controllers</exec_depend>
-  <exec_depend>robotiq_description</exec_depend>
-  <exec_depend>robotiq_driver</exec_depend>
-  <exec_depend>robotiq_controllers</exec_depend>
+  <exec_depend>robotiq_2f_description</exec_depend>
+  <exec_depend>robotiq_2f_interface</exec_depend>
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
 

src/ur_robotiq_description/urdf/ur_robotiq.urdf.xacro

@@ -5,8 +5,7 @@
 	<xacro:arg name="name" default="ur_manipulator"/>
 	<!-- import main macro -->
 	<xacro:include filename="$(find ur_description)/urdf/ur_macro.xacro"/>
-	<xacro:include filename="$(find ur_robotiq_description)/urdf/ur_to_robotiq_adapter.urdf.xacro" />
-	<xacro:include filename="$(find robotiq_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
+	<xacro:include filename="$(find robotiq_2f_description)/urdf/robotiq_2f_140_macro.urdf.xacro" />
 
 	<!-- possible 'ur_type' values: ur3, ur3e, ur5, ur5e, ur10, ur10e, ur16e, ur20 -->
 	<!-- the default value should raise an error in case this was called without defining the type -->
@@ -34,15 +33,9 @@
 	<xacro:arg name="script_sender_port" default="50002"/>
 	<xacro:arg name="trajectory_port" default="50003"/>
 	<!--   tool communication related parameters-->
-	<xacro:arg name="use_tool_communication" default="false" />
-	<xacro:arg name="tool_voltage" default="0" />
-	<xacro:arg name="tool_parity" default="0" />
-	<xacro:arg name="tool_baud_rate" default="115200" />
-	<xacro:arg name="tool_stop_bits" default="1" />
-	<xacro:arg name="tool_rx_idle_chars" default="1.5" />
-	<xacro:arg name="tool_tx_idle_chars" default="3.5" />
-	<xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
-	<xacro:arg name="tool_tcp_port" default="54321" />
+	<xacro:arg name="use_tool_communication" default="true" />
+	<xacro:arg name="tool_voltage" default="24" />
+	<xacro:arg name="tool_tcp_port" default="63352" />
 
 	<!-- Simulation parameters -->
 	<xacro:arg name="use_fake_hardware" default="false" />
@@ -79,12 +72,6 @@
      initial_positions="${xacro.load_yaml(initial_positions_file)}"
      use_tool_communication="$(arg use_tool_communication)"
      tool_voltage="$(arg tool_voltage)"
-     tool_parity="$(arg tool_parity)"
-     tool_baud_rate="$(arg tool_baud_rate)"
-     tool_stop_bits="$(arg tool_stop_bits)"
-     tool_rx_idle_chars="$(arg tool_rx_idle_chars)"
-     tool_tx_idle_chars="$(arg tool_tx_idle_chars)"
-     tool_device_name="$(arg tool_device_name)"
      tool_tcp_port="$(arg tool_tcp_port)"
      robot_ip="$(arg robot_ip)"
      script_filename="$(arg script_filename)"
@@ -103,6 +90,8 @@
     prefix="$(arg tf_prefix)"
     parent="$(arg tf_prefix)tool0"
     use_fake_hardware="$(arg use_fake_hardware)"
+    robot_ip="$(arg robot_ip)"
+    robot_port="$(arg tool_tcp_port)"
     >
     <origin xyz="0 0 0" rpy="0 0 0" />
     </xacro:robotiq_gripper>

src/ur_robotiq_description/urdf/ur_to_robotiq_adapter.urdf.xacro

@@ -1,39 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<robot xmlns:xacro="http://ros.org/wiki/xacro" name="ur_robotiq_adapter">
-  <xacro:macro name="ur_to_robotiq" params="prefix parent child rotation:=^|${0.0}">
-
-    <joint name="${prefix}ur_to_robotiq_joint" type="fixed">
-      <!-- The parent link must be read from the robot model it is attached to. -->
-      <parent link="${parent}"/>
-      <child link="${prefix}ur_to_robotiq_link"/>
-      <origin xyz="0 0 0" rpy="0 0 ${rotation}"/>
-    </joint>
-
-    <link name="${prefix}ur_to_robotiq_link">
-        <visual>
-          <origin xyz="0 0 0" rpy="0 0 0" />
-          <geometry>
-            <mesh filename="package://robotiq_description/meshes/visual/2f_85/ur_to_robotiq_adapter.dae"/>
-          </geometry>
-        </visual>
-        <collision>
-          <origin xyz="0 0 0" rpy="0 0 0"/>
-          <geometry>
-            <mesh filename="package://robotiq_description/meshes/collision/2f_85/ur_to_robotiq_adapter.stl"/>
-          </geometry>
-        </collision>
-        <inertial>
-          <mass value="0.01" />
-          <origin xyz="0 0 0" />
-          <inertia ixx="0.000044" ixy="0.0" ixz="0.0" iyy="0.000027" iyz="0.0" izz="0.000027" />
-        </inertial>
-    </link>
-
-    <joint name="${prefix}gripper_side_joint" type="fixed">
-      <parent link="${prefix}ur_to_robotiq_link"/>
-      <child link="${child}"/>
-      <!-- <origin xyz="0 0 0.011" rpy="0 ${-pi/2} ${pi/2}"/> -->
-      <origin xyz="0 0 0.011" rpy="0 0 0"/>
-    </joint>
-  </xacro:macro>
-</robot>

src/ur_robotiq_moveit_config/bash/servo_startup.sh

@@ -0,0 +1,51 @@
+#!/bin/bash
+
+# Create a Tilix layout file in JSON format
+layout_file=$(mktemp)
+cat <<EOL > $layout_file
+{
+  "type": "Window",
+  "tabs": [
+    {
+      "type": "Terminal",
+      "children": [
+        {
+          "type": "Terminal",
+          "children": [
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            },
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            }
+          ],
+          "orientation": "vertical"
+        },
+        {
+          "type": "Terminal",
+          "children": [
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            },
+            {
+              "type": "Terminal",
+              "command": "bash -c 'cd ~/PycharmProjects/UR_Robotiq && . install/setup.bash; exec bash'"
+            }
+          ],
+          "orientation": "vertical"
+        }
+      ],
+      "orientation": "horizontal"
+    }
+  ]
+}
+EOL
+
+# Open Tilix with the specified layout
+tilix --session $layout_file
+
+# Remove the temporary layout file
+rm $layout_file

src/ur_robotiq_moveit_config/config/initial_positions.yaml

@@ -4,4 +4,4 @@ elbow_joint: 1.57
 wrist_1_joint: -1.57
 wrist_2_joint: -1.57
 wrist_3_joint: 0.0
-finger_joint: 20.0
+gripper_gap: 0.0

src/ur_robotiq_moveit_config/config/joint_limits.yaml

@@ -1,50 +1,90 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-
-# For beginners, we downscale velocity and acceleration limits.
-# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
-default_velocity_scaling_factor: 0.1
-default_acceleration_scaling_factor: 0.1
-
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
+# Joints limits
+#
+# Sources:
+#
+#  - Universal Robots e-Series, User Manual, UR3e, Version 5.8
+#    https://s3-eu-west-1.amazonaws.com/ur-support-site/69043/99403_UR3e_User_Manual_en_Global.pdf
+#  - Support > Articles > UR articles > Max. joint torques
+#    https://www.universal-robots.com/articles/ur-articles/max-joint-torques
+#    retrieved: 2020-06-16, last modified: 2020-06-09
 joint_limits:
-  elbow_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
-  left_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  right_inner_knuckle_joint:
-    has_velocity_limits: true
-    max_velocity: 2
-    has_acceleration_limits: false
-    max_acceleration: 0
-  shoulder_lift_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
-    has_acceleration_limits: false
-    max_acceleration: 0
   shoulder_pan_joint:
-    has_velocity_limits: true
-    max_velocity: 3.1415926535897931
+    # acceleration limits are not publicly available
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  shoulder_lift_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 56.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -360.0
+  elbow_joint:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 28.0
+    # we artificially limit this joint to half its actual joint position limit
+    # to avoid (MoveIt/OMPL) planning problems, as due to the physical
+    # construction of the robot, it's impossible to rotate the 'elbow_joint'
+    # over more than approx +- 1 pi (the shoulder lift joint gets in the way).
+    #
+    # This leads to planning problems as the search space will be divided into
+    # two sections, with no connections from one to the other.
+    #
+    # Refer to https://github.com/ros-industrial/universal_robot/issues/265 for
+    # more information.
+    max_position: !degrees  180.0
+    max_velocity: !degrees  180.0
+    min_position: !degrees -180.0
   wrist_1_joint:
-    has_velocity_limits: true
-    max_velocity: 6.2831853071795862
+    # acceleration limits are not publicly available
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_2_joint:
-    has_velocity_limits: true
-    max_velocity: 6.2831853071795862
+    # acceleration limits are not publicly available
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
   wrist_3_joint:
-    has_velocity_limits: true
-    max_velocity: 6.2831853071795862
+    # acceleration limits are not publicly available
     has_acceleration_limits: false
-    max_acceleration: 0
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 12.0
+    max_position: !degrees  360.0
+    max_velocity: !degrees  360.0
+    min_position: !degrees -360.0
+  gripper_gap:
+    # acceleration limits are not publicly available
+    has_acceleration_limits: false
+    has_effort_limits: true
+    has_position_limits: true
+    has_velocity_limits: true
+    max_effort: 235.0
+    max_position: 0.14
+    max_velocity: 0.15
+    min_position: 0.0

src/ur_robotiq_moveit_config/config/moveit_controllers.yaml

@@ -1,10 +1,7 @@
-
 controller_names:
   - scaled_joint_trajectory_controller
   - joint_trajectory_controller
   - robotiq_gripper_controller
-  - robotiq_gripper_joint_trajectory_controller
-  - combined_joint_trajectory_controller
 
 
 scaled_joint_trajectory_controller:
@@ -32,16 +29,10 @@ joint_trajectory_controller:
     - wrist_2_joint
     - wrist_3_joint
 
+
 robotiq_gripper_controller:
   action_ns: gripper_cmd
   type: GripperCommand
   default: true
   joints:
-    - finger_joint
-
-robotiq_gripper_joint_trajectory_controller:
-  action_ns: follow_joint_trajectory
-  type: FollowJointTrajectory
-  default: false
-  joints:
-    - finger_joint
+    - gripper_gap

src/ur_robotiq_moveit_config/config/ur_servo.yaml

@@ -2,15 +2,15 @@
 # Modify all parameters related to servoing here
 ###############################################
 use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
-use_intra_process_comms : true
+use_intra_process_comms : True
 ## Properties of incoming commands
 command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
 scale:
   # Scale parameters are only used if command_in_type=="unitless"
-  linear:  0.00001  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
-  rotational:  0.001 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  linear:  0.4  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational:  3.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
   # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
-  joint: 0.01
+  joint: 0.5
 # This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
 # controller performance. It essentially increases the timestep when calculating the target pose, to move the target
 # pose farther away. [seconds]
@@ -38,6 +38,7 @@ low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the
 ## MoveIt properties
 move_group_name:  ur_manipulator  # Often 'manipulator' or 'arm'
 planning_frame: world  # The MoveIt planning frame. Often 'base_link' or 'world'
+is_primary_planning_scene_monitor: false
 
 ## Other frames
 ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose

src/ur_robotiq_moveit_config/launch/ur_robotiq_moveit.launch.py

@@ -40,17 +40,17 @@ def launch_setup(context, *args, **kwargs):
     launch_servo = LaunchConfiguration("launch_servo")
 
     # File and package names
-    ur_description_package = "ur_description"
-    ur_robotiq_description_package = "ur_robotiq_description"
-    ur_robotiq_description_file = "ur_robotiq.urdf.xacro"
-    moveit_config_package = "ur_robotiq_moveit_config"
-    moveit_config_file = "ur_robotiq.srdf.xacro"
+    ur_description_package = LaunchConfiguration("ur_description_package")
+    ur_robotiq_description_package = LaunchConfiguration("ur_robotiq_description_package")
+    ur_robotiq_description_file = LaunchConfiguration("ur_robotiq_description_file")
+    moveit_config_package = LaunchConfiguration("moveit_config_package")
+    moveit_config_file = LaunchConfiguration("moveit_config_file")
 
     joint_limit_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "joint_limits.yaml"]
     )
     kinematics_params = PathJoinSubstitution(
-        [FindPackageShare(ur_description_package), "config", ur_type, "default_kinematics.yaml"]
+        [FindPackageShare(ur_robotiq_description_package), "config", "ur_robotiq_calibration.yaml"]
     )
     physical_params = PathJoinSubstitution(
         [FindPackageShare(ur_description_package), "config", ur_type, "physical_parameters.yaml"]
@@ -59,6 +59,8 @@ def launch_setup(context, *args, **kwargs):
         [FindPackageShare(ur_description_package), "config", ur_type, "visual_parameters.yaml"]
     )
 
+
+
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
@@ -126,7 +128,7 @@ def launch_setup(context, *args, **kwargs):
 
     robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
 
-    robot_description_kinematics_params = load_yaml(moveit_config_package, "config/kinematics.yaml")
+    robot_description_kinematics_params = load_yaml("ur_robotiq_moveit_config", "config/kinematics.yaml")
     robot_description_kinematics = {"robot_description_kinematics": robot_description_kinematics_params}
     # robot_description_planning = {
     #     "robot_description_planning": load_yaml(ur_moveit_config_package, "config/joint_limits.yaml")
@@ -140,20 +142,17 @@ def launch_setup(context, *args, **kwargs):
             "start_state_max_bounds_error": 0.1,
         }
     }
-
-    ompl_planning_yaml = load_yaml(moveit_config_package, "config/ompl_planning.yaml")
+    ompl_planning_yaml = load_yaml("ur_robotiq_moveit_config", "config/ompl_planning.yaml")
     ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
 
     # Trajectory Execution Configuration
     # 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")
+    controllers_yaml = load_yaml("ur_robotiq_moveit_config", "config/moveit_controllers.yaml")
 
-    if use_fake_hw == "true":
+    if use_fake_hw == "true" or use_fake_hw == "True":
         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,
@@ -221,7 +220,7 @@ def launch_setup(context, *args, **kwargs):
 
     # Servo node for realtime control
 
-    servo_yaml = load_yaml(moveit_config_package, "config/ur_servo.yaml")
+    servo_yaml = load_yaml("ur_robotiq_moveit_config", "config/ur_servo.yaml")
     servo_params = {"moveit_servo": servo_yaml}
     servo_node = Node(
         package="moveit_servo",
@@ -243,7 +242,46 @@ def launch_setup(context, *args, **kwargs):
 def generate_launch_description():
     # Declare the Launch arguments
     declared_arguments = []
-    # UR specific arguments
+
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_description_package",
+            description="Description package with robot URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_package",
+            description="Description package with robot and gripper URDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom description.",
+            default_value="ur_robotiq_description",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "ur_robotiq_description_file",
+            description="URDF/XACRO description file with the robot.",
+            default_value="ur_robotiq.urdf.xacro",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_package",
+            description="MoveIt config package with robot and gripper SRDF/XACRO files. Usually the argument \
+        is not set, it enables use of a custom moveit config.",
+            default_value="ur_robotiq_moveit_config",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "moveit_config_file",
+            description="MoveIt SRDF/XACRO description file with the robot and gripper.",
+            default_value="ur_robotiq.srdf.xacro",
+        )
+    )
+
     declared_arguments.append(
         DeclareLaunchArgument(
             "ur_type",

src/ur_robotiq_moveit_config/package.xml

@@ -45,9 +45,9 @@
   <exec_depend>ur_description</exec_depend>
   <exec_depend>ur_robot_driver</exec_depend>
   <exec_depend>ur_controllers</exec_depend>
-  <exec_depend>robotiq_description</exec_depend>
-  <exec_depend>robotiq_driver</exec_depend>
-  <exec_depend>robotiq_controllers</exec_depend>
+  <exec_depend>robotiq_2f_description</exec_depend>
+  <exec_depend>robotiq_2f_interface</exec_depend>
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
 
   <depend>control_msgs</depend>

src/ur_robotiq_moveit_config/srdf/old_ur_robotiq_macro.srdf.xacro

@@ -1,134 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro" name="$(arg name)">
-  <xacro:macro name="ur_robotiq_srdf" params="name prefix">
-    <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
-    <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
-    <!--JOINTS - When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
-    <!--CHAINS - When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
-    <!--SUBGROUPS - Groups can also be formed by referencing to already defined group names-->
-    <group name="${prefix}${name}_manipulator">
-      <chain base_link="${prefix}base_link" tip_link="${prefix}tool0" />
-    </group>
-    <group name="${prefix}gripper">
-        <joint name="${prefix}finger_joint"/>
-        <joint name="${prefix}left_outer_finger_joint" />
-        <joint name="${prefix}left_inner_knuckle_joint" />
-        <joint name="${prefix}left_inner_finger_joint" />
-        <joint name="${prefix}right_outer_knuckle_joint" />
-        <joint name="${prefix}right_outer_finger_joint" />
-        <joint name="${prefix}right_inner_knuckle_joint" />
-        <joint name="${prefix}right_inner_finger_joint" />
-    </group>
-    <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="0" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}up" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="0" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.5707" />
-      <joint name="${prefix}shoulder_pan_joint" value="0" />
-      <joint name="${prefix}wrist_1_joint" value="-1.5707" />
-      <joint name="${prefix}wrist_2_joint" value="0" />
-      <joint name="${prefix}wrist_3_joint" value="0" />
-    </group_state>
-    <group_state name="${prefix}test_configuration" group="${prefix}${name}_manipulator">
-      <joint name="${prefix}elbow_joint" value="1.4" />
-      <joint name="${prefix}shoulder_lift_joint" value="-1.62" />
-      <joint name="${prefix}shoulder_pan_joint" value="1.54" />
-      <joint name="${prefix}wrist_1_joint" value="-1.2" />
-      <joint name="${prefix}wrist_2_joint" value="-1.6" />
-      <joint name="${prefix}wrist_3_joint" value="-0.11" />
-    </group_state>
-        <group_state name="open" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0" />
-    </group_state>
-    <group_state name="closed" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0.698132" />
-    </group_state>
-    <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
-    <end_effector name="${prefix}gripper" parent_link="${prefix}tool0" group="${prefix}gripper" parent_group="${prefix}${name}_manipulator" />
-    <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
-    <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link" />
-    <!--PASSIVE JOINT - Purpose - this element is used to mark joints that are not actuated-->
-    <passive_joint name="${prefix}left_outer_finger_joint" />
-    <passive_joint name="${prefix}left_inner_knuckle_joint" />
-    <passive_joint name="${prefix}left_inner_finger_joint" />
-    <passive_joint name="${prefix}right_outer_knuckle_joint" />
-    <passive_joint name="${prefix}right_outer_finger_joint" />
-    <passive_joint name="${prefix}right_inner_knuckle_joint" />
-    <passive_joint name="${prefix}right_inner_finger_joint" />
-    <!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
-    <!--UR-Robot-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_1_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_2_link" reason="Never" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <!--Robotiq-Gripper-->
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-
-    <!-- Weird Bug-->
-    <disable_collisions link1="${prefix}base" link2="${prefix}base" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link" link2="${prefix}base_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}base_link_inertia" reason="Adjacent" />
-    <disable_collisions link1="${prefix}shoulder_link" link2="${prefix}shoulder_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}upper_arm_link" link2="${prefix}upper_arm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}forearm_link" link2="${prefix}forearm_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_1_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_2_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}wrist_3_link" link2="${prefix}wrist_3_link" reason="Adjacent" />
-    <disable_collisions link1="${prefix}flange" link2="${prefix}flange" reason="Adjacent" />
-    <disable_collisions link1="${prefix}ft_frame" link2="${prefix}ft_frame" reason="Adjacent" />
-    <disable_collisions link1="${prefix}tool0" link2="${prefix}tool0" reason="Adjacent" />
-
-    <disable_collisions link1="${prefix}robotiq_base_link" link2="${prefix}robotiq_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}robotiq_140_base_link" link2="robotiq_140_base_link" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_finger_pad" reason="Default"/>
-  </xacro:macro>
-</robot>

src/ur_robotiq_moveit_config/srdf/ur_robotiq_macro.srdf.xacro

@@ -14,6 +14,7 @@
         <chain base_link="${prefix}base_link" tip_link="${prefix}tool0"/>
     </group>
     <group name="${prefix}gripper">
+        <joint name="${prefix}gripper_gap"/>
         <joint name="${prefix}robotiq_base_joint"/>
         <joint name="${prefix}robotiq_140_base_joint"/>
         <joint name="${prefix}finger_joint"/>
@@ -27,16 +28,14 @@
         <joint name="${prefix}right_inner_finger_joint"/>
         <joint name="${prefix}right_inner_finger_pad_joint"/>
     </group>
-    <group name="${prefix}${name}_manipulator_combined">
-        <group name="${prefix}${name}_manipulator"/>
-        <group name="${prefix}gripper"/>
-    </group>
     <!--GROUP STATES Purpose Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
-    <group_state name="${prefix}close" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0.7"/>
+    <group_state name="open" group="${prefix}gripper">
+        <joint name="${prefix}gripper_gap" value="0.14"/>
+        <joint name="${prefix}finger_joint" value="0.0"/>
     </group_state>
-    <group_state name="${prefix}open" group="${prefix}gripper">
-        <joint name="${prefix}finger_joint" value="0"/>
+    <group_state name="closed" group="${prefix}gripper">
+        <joint name="${prefix}gripper_gap" value="0.0"/>
+        <joint name="${prefix}finger_joint" value="0.695"/>
     </group_state>
     <group_state name="${prefix}home" group="${prefix}${name}_manipulator">
       <joint name="${prefix}elbow_joint" value="0" />
@@ -67,6 +66,7 @@
     <!--VIRTUAL JOINT Purpose this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="${prefix}base_link"/>
     <!--PASSIVE JOINT Purpose this element is used to mark joints that are not actuated-->
+    <passive_joint name="${prefix}finger_joint"/>
     <passive_joint name="${prefix}left_inner_knuckle_joint"/>
     <passive_joint name="${prefix}left_outer_finger_joint"/>
     <passive_joint name="${prefix}left_inner_finger_joint"/>
@@ -81,90 +81,90 @@
     <disable_collisions link1="${prefix}base_link_inertia" link2="${prefix}wrist_2_link" reason="Never"/>
     <disable_collisions link1="${prefix}forearm_link" link2="${prefix}upper_arm_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}forearm_link" link2="${prefix}wrist_1_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}left_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}left_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}left_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_finger_pad" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}left_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_finger_pad" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_inner_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_finger" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_inner_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}right_outer_knuckle" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_finger" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}right_outer_knuckle" reason="Default"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}right_outer_knuckle" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
-    <disable_collisions link1="${prefix}right_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_inner_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_left_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_left_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_left_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_inner_finger_pad" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_outer_finger" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_inner_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_finger_pad" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_right_outer_finger" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Default"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_inner_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}robotiq_140_right_outer_knuckle" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}robotiq_140_base_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_finger" link2="${prefix}wrist_3_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_1_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_2_link" reason="Never"/>
+    <disable_collisions link1="${prefix}robotiq_140_right_outer_knuckle" link2="${prefix}wrist_3_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_1_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_2_link" reason="Never"/>
     <disable_collisions link1="${prefix}robotiq_140_base_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
@@ -174,5 +174,7 @@
     <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_2_link" reason="Adjacent"/>
     <disable_collisions link1="${prefix}wrist_1_link" link2="${prefix}wrist_3_link" reason="Never"/>
     <disable_collisions link1="${prefix}wrist_2_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}dummy_link" link2="${prefix}robotiq_140_base_link" reason="Adjacent"/>
+    <disable_collisions link1="${prefix}dummy_link" link2="${prefix}wrist_3_link" reason="Adjacent"/>
   </xacro:macro>
 </robot>

src/ur_robotiq_servo/bash/mapping_routine.sh

@@ -0,0 +1,20 @@
+#!/bin/bash
+
+# Specify the input device (adjust the device path if needed)
+DEVICE="/dev/input/js0"
+
+# Specify the exact path to the ROS 2 workspace
+ROS2_WS_PATH=~/PycharmProjects/UR_Robotiq
+
+# Source the ROS 2 setup script
+source /opt/ros/$ROS_DISTRO/setup.bash
+source ~/ros2_ws/install/setup.bash
+
+# Launch the joy_node in a new terminal with the specified input device
+gnome-terminal -- bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; source $ROS2_WS_PATH/install/setup.bash; ros2 run joy joy_node --ros-args --param dev:=$DEVICE; exec bash"
+
+# Wait for the joy_node to initialize
+sleep 2
+
+# Launch the ps5_mapping_script in another new terminal
+gnome-terminal -- bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; source $ROS2_WS_PATH/install/setup.bash; ros2 run ur_robotiq_servo ps5_mapping; exec bash"

src/ur_robotiq_servo/config/joy-params.yaml

@@ -1,7 +1,7 @@
 joy_node:
     ros__parameters:
         device_id: 0
-        device_name: "PS5 Controller"
+        device_name: "Wireless Controller"
         deadzone: 0.5
         autorepeat_rate: 20.0
         sticky_buttons: false

src/ur_robotiq_servo/config/ps5-controller-mapping.yaml

@@ -0,0 +1,20 @@
+axes:
+  DPadHorizontal: 6
+  DPadVertical: 7
+  L2: 2
+  LeftStickHorizontal: 0
+  LeftStickVertical: 1
+  R2: 5
+  RightStickHorizontal: 3
+  RightStickVertical: 4
+buttons:
+  Circle: 1
+  L1: 4
+  L2: 6
+  Options: 9
+  R1: 5
+  R2: 7
+  Share: 8
+  Square: 3
+  Triangle: 2
+  X: 0

src/ur_robotiq_servo/config/ps5-params.yaml

@@ -1,6 +1,7 @@
 ps5_servo_node:
     ros__parameters:
-      linear_speed_multiplier: 0.2
-      gripper_speed_multiplier: 0.02
+      linear_speed_multiplier: 0.5
+      gripper_position_multiplier: 0.01
+      gripper_cycle_time: 0.25
       gripper_lower_limit: 0.0
-      gripper_upper_limit: 0.70
+      gripper_upper_limit: 0.14

src/ur_robotiq_servo/launch/ps5_servo.launch.py

@@ -10,6 +10,8 @@ def generate_launch_description():
         'config')
     joy_params = os.path.join(config_directory, 'joy-params.yaml')
     ps5_params = os.path.join(config_directory, 'ps5-params.yaml')
+    ps5_controller_mapping = os.path.join(config_directory, 'ps5-controller-mapping.yaml')
+    print(ps5_controller_mapping)
     return LaunchDescription([
         Node(
             package='joy',
@@ -23,7 +25,8 @@ def generate_launch_description():
             executable='ps5_servo',
             name='ps5_servo_node',
             output='screen',
-            parameters=[ps5_params],
+            parameters=[ps5_params,
+                        {'yaml_file_path': ps5_controller_mapping}],
             arguments=[],
         ),
     ])

src/ur_robotiq_servo/package.xml

@@ -11,6 +11,7 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>robotiq_2f_msgs</depend>
   <depend>std_srvs</depend>
   <depend>joy</depend>
   <depend>moveit_servo</depend>

src/ur_robotiq_servo/setup.py

@@ -24,7 +24,8 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'ps5_servo = ur_robotiq_servo.ps5_control:main'
+            'ps5_servo = ur_robotiq_servo.ps5_control:main',
+            'ps5_mapping = ur_robotiq_servo.ps5_mapping_script:main',
         ],
     },
 )

src/ur_robotiq_servo/ur_robotiq_servo/ps5_control.py

@@ -4,16 +4,36 @@ from sensor_msgs.msg import Joy, JointState
 from geometry_msgs.msg import TwistStamped
 from control_msgs.msg import JointJog
 from std_srvs.srv import Trigger
-from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout
+from robotiq_2f_msgs.msg import ForwardCommand
+import yaml
+import os
 
+def load_yaml(file_path):
+    with open(file_path, 'r') as file:
+        return yaml.safe_load(file)
 
 class PS5ControllerNode(Node):
     def __init__(self):
         super().__init__('ps5_controller_node')
+
+        # Load the YAML file path from parameters
+        self.yaml_file_path = self.declare_parameter('yaml_file_path', '').get_parameter_value().string_value
+
+        # Validate and load the YAML file
+        if not self.yaml_file_path or not os.path.exists(self.yaml_file_path):
+            self.get_logger().error(f"Invalid YAML file path: {self.yaml_file_path}")
+            rclpy.shutdown()
+            return
+
+        self.mappings = load_yaml(self.yaml_file_path)
+        self.get_logger().info(f"Loaded mappings: {self.mappings}")
+
+
         # States
         self.mode = 'twist'  # Initialize mode to 'twist'. Alternatives: 'twist', 'joint'
         self.last_button_state = 0  # Track the last state of the toggle button to detect presses
-        self.last_finger_joint_angle = 0.0
+        self.last_gripper_gap = 0.0
+        self.gripper_cmd = 0.0
 
         # Parameters
         self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0)
@@ -24,20 +44,26 @@ class PS5ControllerNode(Node):
         self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value
         self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}")
 
-        self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0)
-        self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier')
+        self.gripper_position_multiplier = self.declare_parameter('gripper_position_multiplier', 1.0)
+        self.gripper_position_multiplier = (self.get_parameter('gripper_position_multiplier')
                                          .get_parameter_value().double_value)
-        self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}")
+        self.get_logger().info(f"Gripper position multiplier: {self.gripper_position_multiplier}")
 
-        self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0)
+        self.gripper_cycle_time = self.declare_parameter('gripper_cycle_time', 1.0)
+        self.gripper_cycle_time = (self.get_parameter('gripper_cycle_time')
+                                         .get_parameter_value().double_value)
+        self.get_logger().info(f"Gripper cycle time: {self.gripper_cycle_time}")
+
+        self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 0.0)
         self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit')
                                          .get_parameter_value().double_value)
         self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}")
 
-        self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0)
+        self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 0.14)
         self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit')
                                          .get_parameter_value().double_value)
         self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}")
+
         # Subscriber and Publisher
         self.joint_state_sub = self.create_subscription(
             JointState,
@@ -62,19 +88,21 @@ class PS5ControllerNode(Node):
             10)
 
         self.gripper_cmd_pub = self.create_publisher(
-            Float64MultiArray,
-            '/forward_gripper_position_controller/commands',
+            ForwardCommand,
+            '/forward_gripper_position_controller/command',
             10)
 
         # Services
         self.servo_client = self.create_client(Trigger, '/servo_node/start_servo')
 
-        srv_msg = Trigger.Request()
-        while not self.servo_client.wait_for_service(timeout_sec=1.0):
+        while not self.servo_client.wait_for_service(timeout_sec=5.0):
             self.get_logger().info('/servo_node/start_servo service not available, waiting again...')
 
         self.call_start_servo()
 
+        # Timer for publishing gripper command
+        self.gripper_timer = self.create_timer(self.gripper_cycle_time, self.publish_gripper_command)
+
         self.get_logger().info('ps5_servo_node started!')
 
     def call_start_servo(self):
@@ -89,34 +117,53 @@ class PS5ControllerNode(Node):
 
     def joint_state_callback(self, msg):
         try:
-            index = msg.name.index('finger_joint')
-            self.last_finger_joint_angle = msg.position[index]
+            index = msg.name.index('gripper_gap')
+            self.last_gripper_gap = msg.position[index]
         except ValueError:
-            self.get_logger().error('finger_joint not found in /joint_states msg')
+            self.get_logger().error('gripper_gap not found in /joint_states msg')
+
+    def publish_gripper_command(self):
+        if self.gripper_cmd is not None:
+            self.get_logger().debug(f'Publishing gripper command: {self.gripper_cmd}')
+            new_gripper_position = self.gripper_cmd + self.last_gripper_gap
+            if new_gripper_position >= self.gripper_upper_limit:
+                new_gripper_position = self.gripper_upper_limit
+            elif new_gripper_position <= self.gripper_lower_limit:
+                new_gripper_position = self.gripper_lower_limit
+
+
+            self.gripper_cmd = 0.0
+            gripper_msg = ForwardCommand()
+            gripper_msg.position = new_gripper_position
+            gripper_msg.max_velocity = 0.01
+            gripper_msg.max_effort = 235.0
+            self.gripper_cmd_pub.publish(gripper_msg)
+        else:
+            self.get_logger().warn('Gripper command not available!')
 
     def joy_callback(self, msg):
         # Check for button press to toggle mode
-        # Assuming button 2 (e.g., Triangle on PS5) for toggling mode
-        current_button_state = msg.buttons[16]
+        toggle_button_index = self.mappings['buttons']['Options']  # Update to the correct button index for toggling
+        current_button_state = msg.buttons[toggle_button_index]
         if current_button_state == 1 and self.last_button_state == 0:
             self.mode = 'joint' if self.mode == 'twist' else 'twist'
             self.get_logger().info(f'Mode switched to: {self.mode}')
         self.last_button_state = current_button_state
 
-        left_trigger = (msg.axes[4] - 1) / - 2.0
-        right_trigger = (msg.axes[5] - 1) / - 2.0
+        left_trigger = (msg.axes[self.mappings['axes']['L2']] - 1) / -2.0
+        right_trigger = (msg.axes[self.mappings['axes']['R2']] - 1) / -2.0
 
         # Process control input based on current mode
         if self.mode == 'twist':
             twist_msg = TwistStamped()
             twist_msg.header.frame_id = 'tool0'
             twist_msg.header.stamp = self.get_clock().now().to_msg()
-            twist_msg.twist.linear.x = msg.axes[0] * self.linear_speed_multiplier
-            twist_msg.twist.linear.y = -msg.axes[1] * self.linear_speed_multiplier
+            twist_msg.twist.linear.x = msg.axes[self.mappings['axes']['LeftStickHorizontal']] * self.linear_speed_multiplier
+            twist_msg.twist.linear.y = msg.axes[self.mappings['axes']['LeftStickVertical']] * self.linear_speed_multiplier
             twist_msg.twist.linear.z = (left_trigger - right_trigger) * self.linear_speed_multiplier
-            twist_msg.twist.angular.x = msg.axes[3]
-            twist_msg.twist.angular.y = msg.axes[2]
-            twist_msg.twist.angular.z = (msg.buttons[9] - msg.buttons[10]) * 1.0
+            twist_msg.twist.angular.x = msg.axes[self.mappings['axes']['RightStickHorizontal']]
+            twist_msg.twist.angular.y = msg.axes[self.mappings['axes']['RightStickVertical']]
+            twist_msg.twist.angular.z = (msg.buttons[self.mappings['buttons']['L1']] - msg.buttons[self.mappings['buttons']['R1']]) * 1.0
             self.twist_pub.publish(twist_msg)
         elif self.mode == 'joint':
             joint_msg = JointJog()
@@ -130,31 +177,20 @@ class PS5ControllerNode(Node):
                 "wrist_2_joint",
                 "wrist_3_joint"
             ]
-            joint_msg.velocities = [msg.axes[0],
-                                    msg.axes[1],
-                                    msg.axes[2],
-                                    msg.axes[3],
-                                    (msg.buttons[11] - msg.buttons[12]) * 1.0,
-                                    (msg.buttons[13] - msg.buttons[14]) * 1.0]
+            joint_msg.velocities = [
+                msg.axes[self.mappings['axes']['LeftStickHorizontal']],
+                msg.axes[self.mappings['axes']['LeftStickVertical']],
+                msg.axes[self.mappings['axes']['RightStickHorizontal']],
+                msg.axes[self.mappings['axes']['RightStickVertical']],
+                msg.axes[self.mappings['axes']['DPadHorizontal']],
+                msg.axes[self.mappings['axes']['DPadVertical']]
+            ]
             self.joint_pub.publish(joint_msg)
 
         # Gripper controller
-        new_finger_joint_angle = (self.last_finger_joint_angle +
-                                  (msg.buttons[1] - msg.buttons[0]) * self.gripper_speed_multiplier)
-        if self.gripper_lower_limit > new_finger_joint_angle or self.gripper_upper_limit < new_finger_joint_angle:
-            self.get_logger().debug(f"New finger joint angle out of bounds: {new_finger_joint_angle}")
-            new_finger_joint_angle = self.last_finger_joint_angle
-        gripper_msg = Float64MultiArray()
-        layout_msg = MultiArrayLayout()
-        layout_msg.dim = [MultiArrayDimension()]
-        layout_msg.dim[0].label = "finger_joint"
-        layout_msg.dim[0].size = 1
-        layout_msg.dim[0].stride = 1
-        layout_msg.data_offset = 0
-        gripper_msg.layout = layout_msg
-        gripper_msg.data = [new_finger_joint_angle]
-        self.gripper_cmd_pub.publish(gripper_msg)
-
+        gripper_open_button = self.mappings['buttons']['Circle']
+        gripper_close_button = self.mappings['buttons']['X']
+        self.gripper_cmd  = self.gripper_cmd + int((msg.buttons[gripper_open_button] - msg.buttons[gripper_close_button])) * self.gripper_position_multiplier
 
 def main(args=None):
     rclpy.init(args=args)

src/ur_robotiq_servo/ur_robotiq_servo/ps5_mapping_script.py

@@ -0,0 +1,162 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Joy
+import yaml
+import os
+import time
+from ament_index_python.packages import get_package_share_directory
+import threading
+
+class PS5ControllerMappingNode(Node):
+    def __init__(self):
+        super().__init__('ps5_controller_mapping_node')
+        self.subscription = self.create_subscription(
+            Joy,
+            'joy',
+            self.joy_callback,
+            10)
+        self.subscription  # prevent unused variable warning
+
+        self.button_mapping = {}
+        self.axis_mapping = {}
+        self.joy_msg = None
+        self.previous_buttons = []
+        self.l2_axis_index = None
+        self.r2_axis_index = None
+        self.prompt_thread = threading.Thread(target=self.prompt_user)
+        self.prompt_thread.start()
+
+        self.get_logger().info("Waiting for joystick messages...")
+
+    def joy_callback(self, msg):
+        # Normalize L2 and R2 axis values if indices are known
+        if self.l2_axis_index is not None and self.r2_axis_index is not None:
+            msg.axes[self.l2_axis_index] -= 1.0
+            msg.axes[self.r2_axis_index] -= 1.0
+
+        self.joy_msg = msg
+
+        if not self.previous_buttons:
+            self.previous_buttons = list(msg.buttons)
+
+        for index, (prev, curr) in enumerate(zip(self.previous_buttons, msg.buttons)):
+            if prev != curr and curr == 1:
+                self.get_logger().info(f"Button with index {index} was pressed.")
+
+        self.previous_buttons = list(msg.buttons)  # Update the previous_buttons list
+
+    def prompt_user(self):
+        while self.joy_msg is None:
+            self.get_logger().info("Waiting for joystick messages...")
+            time.sleep(1)  # Allow other ROS 2 operations to run
+
+        self.get_logger().info("Joystick messages received. Please press each button and move each axis one by one.")
+
+        self.prompt_button('X')
+        self.prompt_button('Circle')
+        self.prompt_button('Square')
+        self.prompt_button('Triangle')
+        self.prompt_button('L1')
+        self.prompt_button('R1')
+        self.prompt_button('L2')
+        self.prompt_button('R2')
+        self.prompt_button('Share')
+        self.prompt_button('Options')
+
+        self.prompt_axis('LeftStickHorizontal')
+        self.prompt_axis('LeftStickVertical')
+        self.prompt_trigger_axis('L2')
+
+        self.prompt_axis('RightStickHorizontal')
+        self.prompt_axis('RightStickVertical')
+        self.prompt_trigger_axis('R2')
+
+        self.prompt_axis('DPadHorizontal')
+        self.prompt_axis('DPadVertical')
+
+        self.save_mapping_to_yaml()
+
+    def prompt_button(self, button_name):
+        input(f"Press the {button_name} button and then press Enter.")
+        index = self.get_button_index()
+        self.button_mapping[button_name] = index
+        if button_name == 'L2':
+            self.l2_axis_index = self.get_trigger_axis_index()
+        elif button_name == 'R2':
+            self.r2_axis_index = self.get_trigger_axis_index()
+        time.sleep(1)  # Allow other ROS 2 operations to run
+
+    def prompt_axis(self, axis_name):
+        input(f"Move the {axis_name} and then press Enter.\n")
+        self.get_logger().info(f"Current axis: {self.get_axis_index()} was moved.")
+        self.axis_mapping[axis_name] = self.get_axis_index()
+        time.sleep(1)  # Allow other ROS 2 operations to run
+
+    def prompt_trigger_axis(self, axis_name):
+        input(f"Move the {axis_name} and then press Enter.\n")
+        self.axis_mapping[axis_name] = self.get_trigger_axis_index()
+        self.get_logger().info(f"Current axis: {self.get_axis_index()} was moved.")
+        time.sleep(1)  # Allow other ROS 2 operations to run
+
+    def get_button_index(self):
+        if self.joy_msg:
+            return self.joy_msg.buttons.index(1)
+        else:
+            self.get_logger().warn("Joy message not received yet.")
+            return -1
+
+    def get_axis_index(self):
+        if self.joy_msg:
+            max_val = max(self.joy_msg.axes, key=abs)
+            return self.joy_msg.axes.index(max_val)
+        else:
+            self.get_logger().warn("Joy message not received yet.")
+            return -1
+
+    def get_trigger_axis_index(self):
+        if self.joy_msg:
+            min_val = min(self.joy_msg.axes)
+            return self.joy_msg.axes.index(min_val)
+        else:
+            self.get_logger().warn("Joy message not received yet.")
+            return -1
+
+    def save_mapping_to_yaml(self):
+        package_name = 'ur_robotiq_servo'  # Replace with your package name
+        share_path = get_package_share_directory(package_name)
+
+        # Navigate to the source config directory
+        config_path = os.path.join(share_path, '..', '..', '..', '..', 'src', package_name, 'config')
+        config_path = os.path.realpath(config_path)
+
+        if not os.path.exists(config_path):
+            os.makedirs(config_path)
+
+        file_path = os.path.join(config_path, 'ps5-controller-mapping.yaml')
+
+        mapping = {
+            'buttons': self.button_mapping,
+            'axes': self.axis_mapping
+        }
+
+        with open(file_path, 'w') as file:
+            yaml.dump(mapping, file)
+
+        self.get_logger().info(f"Mapping saved to {file_path}")
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    node = PS5ControllerMappingNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()