begin work on gripper node using rosservice

CMakeLists.txt

@@ -102,6 +102,11 @@ target_link_libraries(sas_robot_driver_franka
                       dqrobotics
                       robot_interface_franka)
 
+add_library(sas_robot_driver_franka_hand src/sas_robot_driver_franka_hand.cpp)
+target_link_libraries(sas_robot_driver_franka_hand
+        dqrobotics
+        Franka::Franka)
+
 
 add_library(sas_robot_driver_coppelia src/sas_robot_driver_coppelia.cpp)
 target_link_libraries(sas_robot_driver_coppelia
@@ -122,6 +127,11 @@ target_link_libraries(sas_robot_driver_franka_node
                       ${catkin_LIBRARIES})
 
 
+add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
+target_link_libraries(sas_robot_driver_franka_hand_node
+        sas_robot_driver_franka_hand
+        ${catkin_LIBRARIES})
+
 
 
 add_executable(JuankaEmika

cfg/sas_robot_driver_franka_hand_1.yaml

@@ -0,0 +1,7 @@
+"robot_ip_address": "172.16.0.2"
+"robot_mode": "PositionControl"
+"robot_speed": 20.0
+"thread_sampling_time_nsec": 40000000
+"q_min": [0.00]
+"q_max": [0.04]
+

launch/sas_robot_driver_franka_hand_1.launch

@@ -0,0 +1,17 @@
+<launch>
+
+
+
+<node pkg="sas_robot_driver_franka" type="sas_robot_driver_franka_hand_node" name="franka_hand_1">
+<rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_franka_hand_1.yaml" command="load"
+/>
+
+
+
+</node>
+
+
+
+
+
+</launch>

src/sas_robot_driver_franka_hand.cpp

@@ -0,0 +1,127 @@
+//
+// Created by qlin on 7/17/24.
+//
+
+#include "sas_robot_driver_franka_hand.h"
+
+namespace sas {
+
+    //const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
+
+    RobotDriverFrankaHand::~RobotDriverFrankaHand()
+    {
+        if(_is_connected())
+        {
+            disconnect();
+        }
+    }
+
+    RobotDriverFrankaHand::RobotDriverFrankaHand(
+        const RobotDriverFrankaHandConfiguration& configuration,
+        const RobotDriverROSConfiguration& ros_configuration,
+        std::atomic_bool* break_loops):
+    configuration_(configuration), ros_configuration_(ros_configuration), break_loops_(break_loops)
+    {
+        gripper_sptr_ = nullptr;
+
+    }
+
+    bool RobotDriverFrankaHand::_is_connected() const
+    {
+        if(gripper_sptr_ == nullptr) return false;
+        if(!gripper_sptr_) return false;
+        else return true;
+    }
+
+    VectorXd RobotDriverFrankaHand::get_joint_positions()
+    {
+        return joint_positions_;
+
+    }
+    void RobotDriverFrankaHand::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
+    {
+
+    }
+
+    VectorXd RobotDriverFrankaHand::get_joint_velocities()
+    {
+        return VectorXd::Zero(1);
+    }
+    void RobotDriverFrankaHand::set_target_joint_velocities(const VectorXd& desired_joint_velocities)
+    {
+
+    }
+
+    VectorXd RobotDriverFrankaHand::get_joint_forces()
+    {
+        return VectorXd::Zero(1);
+    }
+
+    void RobotDriverFrankaHand::start_control_loop()
+    {
+
+        Clock clock = Clock(ros_configuration_.thread_sampling_time_nsec);
+        clock.init();
+        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Starting control loop.");
+        while(!(*break_loops_))
+        {
+            if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Robot is not connected.");
+
+
+            clock.update_and_sleep();
+        }
+        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Exiting control loop.");
+
+    }
+
+
+    void RobotDriverFrankaHand::connect()
+    {
+        gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
+        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::connect::Could not connect to the robot.");
+
+    }
+    void RobotDriverFrankaHand::disconnect() noexcept
+    {
+        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::disconnecting...");
+        gripper_sptr_->~Gripper();
+        gripper_sptr_ = nullptr;
+    }
+
+    /**
+     * @brief robot_driver_franka::gripper_homing
+     */
+    void RobotDriverFrankaHand::gripper_homing()
+    {
+        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Robot is not connected.");
+        auto ret = gripper_sptr_->homing();
+        if(!ret)
+        {
+            throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
+        }
+        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Gripper homed.");
+    }
+
+    void RobotDriverFrankaHand::initialize()
+    {
+        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::initialize::Robot is not connected.");
+        gripper_homing();
+    }
+
+    void RobotDriverFrankaHand::deinitialize()
+    {
+        if(_is_connected())
+        {
+            franka::GripperState gripper_state = gripper_sptr_->readOnce();
+            if(gripper_state.is_grasped)
+            {
+                gripper_sptr_->stop();
+            }
+        }
+    }
+
+
+
+
+} // sas

src/sas_robot_driver_franka_hand.h

@@ -0,0 +1,88 @@
+#pragma once
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+// #include <thread>
+
+#include <dqrobotics/DQ.h>
+
+#include <sas_robot_driver/sas_robot_driver.h>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include <sas_clock/sas_clock.h>
+#include <ros/common.h>
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+
+namespace sas {
+
+struct RobotDriverFrankaHandConfiguration
+{
+    std::string robot_ip;
+
+};
+
+class RobotDriverFrankaHand{
+private:
+    RobotDriverFrankaHandConfiguration configuration_;
+    RobotDriverROSConfiguration ros_configuration_;
+
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+
+    //Joint positions
+    VectorXd joint_positions_;
+    //VectorXd joint_velocities_;
+    //VectorXd end_effector_pose_;
+
+
+    // std::thread control_loop_thread_;
+    std::atomic_bool* break_loops_;
+
+    bool _is_connected() const;
+
+public:
+    //const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
+
+    RobotDriverFrankaHand(const RobotDriverFrankaHand&)=delete;
+    RobotDriverFrankaHand()=delete;
+    ~RobotDriverFrankaHand();
+
+    RobotDriverFrankaHand(
+        const RobotDriverFrankaHandConfiguration& configuration,
+        const RobotDriverROSConfiguration& ros_configuration,
+        std::atomic_bool* break_loops);
+
+    void start_control_loop();
+
+
+
+    VectorXd get_joint_positions();
+    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad);
+
+    VectorXd get_joint_velocities();
+    void set_target_joint_velocities(const VectorXd& desired_joint_velocities);
+
+    VectorXd get_joint_forces();
+
+    void gripper_homing();
+
+
+    void connect() ;
+    void disconnect() noexcept;
+
+    void initialize() ;
+    void deinitialize() ;
+
+    //bool set_end_effector_pose_dq(const DQ& pose);
+    //DQ get_end_effector_pose_dq();
+
+};
+
+} // sas
+

src/sas_robot_driver_franka_hand_node.cpp

@@ -0,0 +1,116 @@
+/*
+# 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. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
+#         - Adapted from sas_robot_driver_franka
+# ################################################################
+*/
+
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <sas_common/sas_common.h>
+#include <sas_conversions/eigen3_std_conversions.h>
+#include <sas_robot_driver/sas_robot_driver_ros.h>
+#include "sas_robot_driver_franka_hand.h"
+
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+static std::atomic_bool kill_this_process(false);
+void sig_int_handler(int)
+{
+    kill_this_process = true;
+}
+
+int main(int argc, char **argv)
+{
+    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
+    {
+        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
+    }
+    ros::init(argc, argv, "sas_robot_driver_franka_hand_node", ros::init_options::NoSigintHandler);
+    ROS_WARN("=====================================================================");
+    ROS_WARN("---------------------------Quentin Lin-------------------------------");
+    ROS_WARN("=====================================================================");
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
+
+
+    ros::NodeHandle nh;
+    sas::RobotDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+
+    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
+
+
+    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
+
+    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
+    bool q_lim_override = false;
+    if(nh.hasParam("q_min") || nh.hasParam("q_max"))
+    {
+        sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
+        sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
+        q_lim_override = true;
+    }else
+    {
+
+    }
+    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
+
+    sas::RobotDriverFrankaHand franka_hand_driver(
+        robot_driver_franka_hand_configuration,
+        robot_driver_ros_configuration,
+        &kill_this_process
+    );
+    try
+    {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka hand.");
+        franka_hand_driver.connect();
+        franka_hand_driver.initialize();
+
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating control loop.");
+        franka_hand_driver.start_control_loop();
+
+    }
+    catch (const std::exception& e)
+    {
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
+    }catch (...)
+    {
+        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::Unknown exception.");
+    }
+    ROS_INFO_STREAM(ros::this_node::getName()+"::Exiting...");
+    franka_hand_driver.deinitialize();
+    ROS_INFO_STREAM(ros::this_node::getName()+"::deinitialized.");
+    franka_hand_driver.disconnect();
+    ROS_INFO_STREAM(ros::this_node::getName()+"::disconnected.");
+
+
+  return 0;
+}