From 87024fb996eba528409d98c4ca909c2ef9199169 Mon Sep 17 00:00:00 2001 From: Juancho Date: Tue, 11 Jul 2023 08:37:41 +0900 Subject: [PATCH] Added a new interface class to handle the franka hand --- CMakeLists.txt | 3 + include/robot_interface_hand.h | 78 +++++++++++++++++++++++ src/robot_interface_hand.cpp | 108 ++++++++++++++++++++++++++++++++ src/sas_robot_driver_franka.cpp | 2 +- 4 files changed, 190 insertions(+), 1 deletion(-) create mode 100644 include/robot_interface_hand.h create mode 100644 src/robot_interface_hand.cpp 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;