Added a new interface class to handle the franka hand

CMakeLists.txt

@@ -76,6 +76,9 @@ target_link_libraries(robot_interface_franka Franka::Franka
                       QuadraticProgramMotionGenerator
                       CustomMotionGeneration)
 
+add_library(robot_interface_hand src/robot_interface_hand.cpp)
+target_link_libraries(robot_interface_hand Franka::Franka
+                      dqrobotics)
 
 
 ############

include/robot_interface_hand.h

@@ -0,0 +1,78 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Original implementation
+#
+# ################################################################
+*/
+
+
+#pragma once
+
+#include <dqrobotics/DQ.h>
+#include <memory.h>
+#include <tuple>
+#include <exception>
+#include <vector>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <franka/exception.h>
+#include <thread>
+#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
+#include <atomic>
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+
+
+class RobotInterfaceHand
+{
+protected:
+    double speed_gripper_ = 0.02;
+    std::string ip_ = "172.16.0.2";
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+    void _check_if_hand_is_connected();
+public:
+    enum GRIPPER_MODE_STATES{WIDTH=0, MAX_WIDTH=1};
+    RobotInterfaceHand() = delete;
+    RobotInterfaceHand(const RobotInterfaceHand&) = delete;
+    RobotInterfaceHand& operator= (const RobotInterfaceHand&) = delete;
+    RobotInterfaceHand(const std::string &ROBOT_IP);
+
+    double read_gripper(const GRIPPER_MODE_STATES& gripper_state = WIDTH);
+    bool read_grasped_status();
+    void set_gripper(const double& width);
+    void gripper_homing();
+    bool grasp_object(const double& width,
+                      const double& speed,
+                      const double& force,
+                      double 	epsilon_inner = 0.005,
+                      double 	epsilon_outer = 0.005);
+    void release_object();
+    VectorXd get_home_robot_configuration();
+};
+

src/robot_interface_hand.cpp

@@ -0,0 +1,108 @@
+#include "robot_interface_hand.h"
+
+
+
+RobotInterfaceHand::RobotInterfaceHand(const std::string &ROBOT_IP):ip_(ROBOT_IP)
+{
+    gripper_sptr_ = std::make_shared<franka::Gripper>(ip_);
+}
+
+/**
+  * @brief robot_driver_franka::_check_if_hand_is_connected
+  */
+void RobotInterfaceHand::_check_if_hand_is_connected()
+{
+    if(!gripper_sptr_)
+        throw std::runtime_error("Invalid hand pointer. You must connect(), then initialize(). "
+                                 + std::string("Example:  robot_driver_franka(IP, robot_driver_franka::MODE::None, robot_driver_franka::HAND::ON)" ));
+}
+
+
+double RobotInterfaceHand::read_gripper(const GRIPPER_MODE_STATES& gripper_state)
+{
+    _check_if_hand_is_connected();
+    franka::GripperState state = gripper_sptr_->readOnce();
+    switch (gripper_state) {
+    case RobotInterfaceHand::GRIPPER_MODE_STATES::WIDTH:
+        return state.width;
+        break;
+    case RobotInterfaceHand::GRIPPER_MODE_STATES::MAX_WIDTH:
+        return state.max_width;
+        break;
+    default:
+        throw std::runtime_error(std::string("Wrong argument in sas_robot_driver_franka::read_gripper. "));
+        break;
+    }
+}
+
+/**
+ * @brief robot_driver_franka::read_grasped_status
+ * @return
+ */
+bool RobotInterfaceHand::read_grasped_status()
+{
+    _check_if_hand_is_connected();
+    franka::GripperState state = gripper_sptr_->readOnce();
+    return state.is_grasped;
+}
+
+
+/**
+ * @brief robot_driver_franka::set_gripper
+ * @param width
+ */
+void RobotInterfaceHand::set_gripper(const double& width)
+{
+    _check_if_hand_is_connected();
+    auto gripper_max_width = read_gripper(RobotInterfaceHand::GRIPPER_MODE_STATES::MAX_WIDTH);
+    if (width > gripper_max_width)
+    {
+        throw std::runtime_error(
+            std::string("You used a width = ") +
+            std::to_string(width) +
+            std::string(". Maximum width allowed is ") +
+            std::to_string(gripper_max_width)
+            );
+    }
+    gripper_sptr_->move(width, speed_gripper_);
+}
+
+
+/**
+ * @brief robot_driver_franka::gripper_homing
+ */
+void RobotInterfaceHand::gripper_homing()
+{
+    _check_if_hand_is_connected();
+    gripper_sptr_->homing();
+}
+
+bool RobotInterfaceHand::grasp_object(const double &width, const double &speed, const double &force, double epsilon_inner, double epsilon_outer)
+{
+    // Check for the maximum grasping width.
+    _check_if_hand_is_connected();
+    franka::GripperState gripper_state = gripper_sptr_->readOnce();
+    if (gripper_state.max_width < width) {
+        std::cout << "Object is too large for the current fingers on the gripper." << std::endl;
+        return false;
+    }
+    // Grasp the object.
+    if (!gripper_sptr_->grasp(width, 0.1, 20)) {
+        std::cout << "Failed to grasp object." << std::endl;
+        return false;
+    }else{
+        return true;
+    }
+
+
+}
+
+void RobotInterfaceHand::release_object()
+{
+    _check_if_hand_is_connected();
+    franka::GripperState gripper_state = gripper_sptr_->readOnce();
+    if(gripper_state.is_grasped)
+    {
+        gripper_sptr_->stop();
+    }
+}

src/sas_robot_driver_franka.cpp

@@ -72,7 +72,7 @@ namespace sas
 
         robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
                                                                         mode, //None, PositionControl, VelocityControl
-                                                                        RobotInterfaceFranka::HAND::ON);
+                                                                        RobotInterfaceFranka::HAND::OFF);
     }
 
     RobotDriverFranka::~RobotDriverFranka()=default;