diff --git a/CMakeLists.txt b/CMakeLists.txt
index 16cc2b5..31ad27f 100644
--- a/CMakeLists.txt
+++ b/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)
############
diff --git a/include/robot_interface_hand.h b/include/robot_interface_hand.h
new file mode 100644
index 0000000..cdb904f
--- /dev/null
+++ b/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 .
+#
+# ################################################################
+#
+# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+# - Original implementation
+#
+# ################################################################
+*/
+
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+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 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();
+};
+
diff --git a/src/robot_interface_hand.cpp b/src/robot_interface_hand.cpp
new file mode 100644
index 0000000..3ae6227
--- /dev/null
+++ b/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(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();
+ }
+}
diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp
index 620871a..c697911 100644
--- a/src/sas_robot_driver_franka.cpp
+++ b/src/sas_robot_driver_franka.cpp
@@ -72,7 +72,7 @@ namespace sas
robot_driver_interface_sptr_ = std::make_shared(configuration.ip_address,
mode, //None, PositionControl, VelocityControl
- RobotInterfaceFranka::HAND::ON);
+ RobotInterfaceFranka::HAND::OFF);
}
RobotDriverFranka::~RobotDriverFranka()=default;