Forward Controller working

Now 2 states where published:
gripper_gap - which is the actuall gap between the two finger plates
finger_joint - which is the old angle

src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml

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

src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml

@@ -3,42 +3,14 @@ controller_manager:
     update_rate: 500  # Hz
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
-    robotiq_gripper_controller:
-      type: position_controllers/GripperActionController
-    fake_gripper_controller:
-      type: joint_trajectory_controller/JointTrajectoryController
     forward_gripper_position_controller:
       type: robotiq_2f_controllers/RobotiqForwardController
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
-robotiq_gripper_controller:
-  ros__parameters:
-    default: true
-    joint: finger_joint
-    use_effort_interface: true
-    use_speed_interface: true
-
 forward_gripper_position_controller:
   ros__parameters:
-    joint: finger_joint
+    joint: gripper_gap
-
-fake_gripper_controller:
-  ros__parameters:
-    joints:
-     - 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
-      finger_joint: { trajectory: 0.2, goal: 0.1 }
 
 robotiq_activation_controller:
   ros__parameters:

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

@@ -118,13 +118,11 @@ def generate_launch_description():
         )
 
     if not use_fake_hardware_bool:
-        controller_spawner_names = ["robotiq_gripper_controller"]
+        controller_spawner_names = []
-        controller_spawner_inactive_names = ["forward_gripper_position_controller",
+        controller_spawner_inactive_names = ["forward_gripper_position_controller"]
-                                             "fake_gripper_controller"]
     else:
         controller_spawner_names = ["forward_gripper_position_controller"]
-        controller_spawner_inactive_names = ["robotiq_gripper_controller",
+        controller_spawner_inactive_names = []
-                                             "fake_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

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
@@ -35,6 +36,7 @@
                 <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>
@@ -43,12 +45,17 @@
 
             <!-- 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.695</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_140_macro.urdf.xacro

@@ -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,31 @@
             <child link="${prefix}robotiq_140_right_inner_finger_pad" />
             <axis xyz="0 0 1" />
         </joint>
+
+        <!-- Pseudo Link and Joint for gripper gap -->
+        <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>
+
+        <joint name="base_to_dummy" type="fixed">
+            <parent link="${prefix}robotiq_140_base_link"/>
+            <child link="dummy_link"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Adjust origin as needed -->
+        </joint>
+
+        <joint name="gripper_gap" type="prismatic">
+            <parent link="dummy_link"/>
+            <child link="${prefix}robotiq_140_right_inner_finger_pad"/>
+            <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->
+            <axis xyz="0 1 0"/>  <!-- Axis of movement; adjust according to actual alignment -->
+            <limit lower="0.0" upper="0.14" effort="235" velocity="0.15"/>
+        </joint>
     </xacro:macro>
 </robot>

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

@@ -67,8 +67,9 @@ protected: // Likely changes the access to protected from private
     static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::quiet_NaN();
 
     // Conversion Methods
-    double convertRawToPosition(int raw_position);
+    double convertRawToGap(int raw_position);
-    int convertPositionToRaw(double position);
+    double convertRawToAngle(int raw_position);
+    int convertGapToRaw(double gap);
     int convertSpeedToRaw(double speed);
     int convertForceToRaw(double force);
 
@@ -80,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();
@@ -102,6 +104,7 @@ 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_;

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;
 
@@ -96,6 +99,14 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
         }
         RCLCPP_INFO(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_INFO(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 {
@@ -130,49 +141,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(),
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu command interfaces found. 1 expected.", joint_gap.name.c_str(),
-                     joint.command_interfaces.size());
+                     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(),
+        RCLCPP_FATAL(logger_, "Joint '%s' has %s command interfaces found. '%s' expected.", joint_gap.name.c_str(),
-                     joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
+                     joint_gap.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION);
         return CallbackReturn::ERROR;
     }
 
-    // There are two state interfaces: position and velocity.
+    // There are two state interfaces for the gripper gap: position and velocity.
-    if (joint.state_interfaces.size() != 2)
+    if (joint_gap.state_interfaces.size() != 2)
     {
-        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(),
+        RCLCPP_FATAL(logger_, "Joint '%s' has %zu state interface. 2 expected.", joint_gap.name.c_str(),
-                     joint.state_interfaces.size());
+                     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 ||
+        if (!(joint_gap.state_interfaces[i].name == hardware_interface::HW_IF_POSITION ||
-              joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY))
+              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(),
+            RCLCPP_FATAL(logger_, "Joint '%s' has %s state interface. Expected %s or %s.", joint_gap.name.c_str(),
-                       joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION,
+                       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;
 }
 
@@ -207,11 +238,12 @@ 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_gap_ = 0.0;
-        gripper_velocity_ = 0;
+        gripper_angle_ = 0.0;
-        gripper_position_command_ = 0;
+        gripper_velocity_ = 0.0;
+        gripper_gap_command_ = 0.0;
     }
 
     // Activate the gripper.
@@ -272,12 +304,13 @@ 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",
+    RCLCPP_INFO(logger_, "Exporting state interface for joint: %s type: %s",
                 interface.get_name().c_str(), interface.get_interface_name().c_str());
     }
 
@@ -292,7 +325,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_
         )
     );
 
@@ -337,16 +370,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) {
@@ -375,12 +410,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);
 
@@ -425,17 +460,17 @@ void RobotiqGripperHardwareInterface::performRegularOperations()
 {
     try
     {
-        int scaled_position = write_command_.load();
+        int scaled_gap = 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);
+        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.");
         }
 
-        int raw_position = socket_adapter_->position();
+        int raw_gap = socket_adapter_->position();
-        gripper_current_state_.store(convertRawToPosition(raw_position));
+        gripper_current_state_.store(convertRawToGap(raw_gap));
     }
     catch (const std::exception& e)
     {
@@ -457,19 +492,27 @@ 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.");
     }
-    double scaled_position = min_position_ + (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
+    double scaled_position = max_position_ - (static_cast<double>(raw_position) / 255.0) * (max_position_ - min_position_);
     return std::max(min_position_, std::min(scaled_position, max_position_));
 }
 
-int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
+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_angle = (static_cast<double>(raw_position) / 255.0) * max_angle_;
+    return std::max(0.0, std::min(scaled_angle, max_angle_));
+}
+
+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: min_position_ or max_position_ are not configured correctly.");
     }
-    double scaled = (position - min_position_) / (max_position_ - min_position_) * 255.0;
+    double scaled = 255.0 - (gap - min_position_) / (max_position_ - min_position_) * 255.0;
     return static_cast<uint8_t>(std::clamp(scaled, 0.0, 255.0));
 }