[message interface] combining driver and interface package for better future management

.gitmodules

@@ -1,3 +0,0 @@
-[submodule "constraints_manager"]
-	path = constraints_manager
-	url = https://github.com/juanjqo/constraints_manager.git

README.md

@@ -1,5 +0,0 @@
-![](https://img.shields.io/github/license/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka/master)![](https://img.shields.io/badge/status-experimental-red)
-# sas_robot_driver_franka
-
-
-![ezgif com-video-to-gif (1)](https://github.com/SmartArmStack/sas_robot_driver/assets/23158313/b4e1efa7-8d93-4a67-ab87-a74c41d8f4bc)

sas_robot_driver_franka/CMakeLists.txt

@@ -14,14 +14,17 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(sas_common REQUIRED)
 find_package(sas_core REQUIRED)
+find_package(sas_msgs REQUIRED)
 find_package(sas_conversions REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(sas_robot_driver REQUIRED)
 find_package(Franka REQUIRED)
+find_package(pinocchio REQUIRED)
 find_package(sas_robot_driver_franka_interfaces REQUIRED)  # Add this line
 
 ## To correctly find and link with QT
@@ -90,7 +93,7 @@ install(
 ##### CPP LIBRARY Motion Generation  #####
 
 add_library(MotionGenerator src/generator/motion_generator.cpp)
-target_link_libraries(MotionGenerator Franka::Franka Eigen3::Eigen)
+target_link_libraries(MotionGenerator Franka::Franka pinocchio::pinocchio Eigen3::Eigen)
 
 add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
 target_link_libraries(ConstraintsManager Eigen3::Eigen)
@@ -125,6 +128,7 @@ target_link_libraries(CustomMotionGeneration
 
 add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
 target_link_libraries(robot_interface_franka Franka::Franka
+        pinocchio::pinocchio
         dqrobotics
         MotionGenerator
         ConstraintsManager
@@ -133,6 +137,7 @@ target_link_libraries(robot_interface_franka Franka::Franka
 
 add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
 target_link_libraries(robot_interface_hand Franka::Franka
+        pinocchio::pinocchio
         dqrobotics)
 
 
@@ -152,7 +157,7 @@ target_link_libraries(sas_robot_driver_franka
 add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
 
 ament_target_dependencies(qros_effector_driver_franka_hand
-        rclcpp sas_common sas_core
+        rclcpp sas_common sas_core std_srvs
         sas_robot_driver_franka_interfaces
 )
 
@@ -163,7 +168,9 @@ ament_target_dependencies(qros_effector_driver_franka_hand
 
 target_link_libraries(qros_effector_driver_franka_hand
         dqrobotics
-        Franka::Franka)
+        Franka::Franka
+        pinocchio::pinocchio
+)
 
 #target_include_directories(qros_effector_driver_franka_hand
 #        PUBLIC
@@ -244,22 +251,22 @@ install(TARGETS
 ##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
 ament_python_install_package(${PROJECT_NAME})
 
-pybind11_add_module(_qros_franka_robot_dynamic SHARED
+pybind11_add_module(_qros_franka_robot_dynamics_py SHARED
         src/robot_dynamics/qros_robot_dynamics_py.cpp
         src/robot_dynamics/qros_robot_dynamics_client.cpp
         src/robot_dynamics/qros_robot_dynamics_server.cpp
 )
 
-target_include_directories(_qros_franka_robot_dynamic
+target_include_directories(_qros_franka_robot_dynamics_py
         PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
         $<INSTALL_INTERFACE:include/robot_dynamics>)
 
-target_compile_definitions(_qros_franka_robot_dynamic PRIVATE IS_SAS_PYTHON_BUILD)
+target_compile_definitions(_qros_franka_robot_dynamics_py PRIVATE IS_SAS_PYTHON_BUILD)
 # https://github.com/pybind/pybind11/issues/387
-target_link_libraries(_qros_franka_robot_dynamic PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
+target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
 
-install(TARGETS _qros_franka_robot_dynamic
+install(TARGETS _qros_franka_robot_dynamics_py
         DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
 )
 ##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####

sas_robot_driver_franka/README.md

@@ -0,0 +1,84 @@
+![](https://img.shields.io/github/license/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka/ros2_jazzy)![](https://img.shields.io/badge/status-experimental-red)
+# sas_robot_driver_franka
+![ezgif com-video-to-gif (1)](https://github.com/SmartArmStack/sas_robot_driver/assets/23158313/b4e1efa7-8d93-4a67-ab87-a74c41d8f4bc)
+Robot driver for the Franka Research 3 Robot. 
+
+## Original Contributors
+- [Juan José Quiroz Omaña](https://github.com/juanjqo/sas_robot_driver_franka)
+
+## dependencies
+SAS: 
+- sas_common
+- sas_core
+- sas_msgs
+- sas_conversions
+- sas_robot_driver
+- [sas_robot_driver_franka_interfaces](https://www.github.com/qlin960618/sas_robot_driver_franka_interfaces)
+
+Misc: 
+- geometry_msgs
+- std_msgs
+- std_srvs
+- tf2_ros
+- tf2
+
+Franka Related
+
+- Franka (libfranka)
+- pinocchio
+
+## Components
+
+### sas_robot_driver_franka
+Kernel-space real-time robot driver for the SmartArm compatible control
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.004  # control sampling time (1.0/loop_rate)
+"q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895]  # joint limit minimum, if not define will obtain from [ROBOT_JSON]
+"q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895]  # joint limit maximum, if not define will obtain from [ROBOT_JSON]
+"force_upper_limits_scaling_factor": 4.0  # force and torque error limit scaling factor from default, (if any of the below is not defined)
+"upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # joint torque safety threshold, [j1, j2, ..., j7]
+"upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # end-effector frame safety force threshold [x, y, z, rx, ry, rz] 
+"robot_mode": "VelocityControl"  # control mode [VelocityControl/PositionControl]: currently PositionControl can be unstable 
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```
+
+### sas_robot_driver_franka_hand
+Franka Hand driver for basic control of gripping functionality.
+
+#### special dependencies
+- sas_robot_driver_franka_interfaces
+  - <sas_robot_driver_franka_interfaces/srv/grasp.hpp>
+  - <sas_robot_driver_franka_interfaces/srv/move.hpp>
+  - <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.02  # control sampling time, (1.0/status update rate)
+"default_force": 2.0  # default gripping force (overridable from service call)
+"default_speed": 0.07  # default gripping speed (overridable from service call)
+"default_epsilon_inner": 0.007  # default grip call position epsilon (overridable from service call)
+"default_epsilon_outer": 0.007  # default grip call position epsilon (overridable from service call)
+```
+
+
+### sas_robot_driver_coppelia_node
+Digital twin mirroring node for driver node to CoppeliaSim
+
+
+#### node parameters:
+```yaml
+"thread_sampling_time_sec": 0.008  # control sampling time (1.0/loop_rate)
+"vrep_ip": os.environ["VREP_IP"]  # ip of the vrep computer
+"vrep_port": 20012  # vrep port
+"vrep_dynamically_enabled": True  # if vrep scene is dynamically enabled
+"using_real_robot": True  # if node should read-only from real robot driver, False for simulation mode
+"vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"]  # coppelia scene joint names
+"robot_topic_prefix": "/arm3"  # robot driver namespace
+"robot_mode": "PositionControl"  # only use when in simulation node, aka using_real_robot==False, for simulating velocity control mode. (TODO: currently VelocityControl is not stable)
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```

sas_robot_driver_franka/__init__.py

@@ -1,203 +0,0 @@
-"""
-"""
-from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClient, RobotDynamicsServer
-
-import rclpy
-from rclpy.node import Node
-# from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
-from sas_robot_driver_franka_interfaces.srv import Move, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response
-from sas_robot_driver_franka_interfaces.msg import GripperState
-import os
-import threading
-from queue import Queue
-import time
-import struct
-
-MOVE_TOPIC_SUFFIX = "move"
-GRASP_TOPIC_SUFFIX = "grasp"
-STATUS_TOPIC_SUFFIX = "gripper_status"
-
-
-class FrankaGripperInterface:
-    """
-    Non blocking interface to control the Franka gripper
-    """
-
-    def __init__(self, node: Node, robot_prefix):
-        self.robot_prefix = robot_prefix
-        self.node = node
-        self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
-        self._moving = False
-        self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
-        self._grasping = False
-        self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
-                                                          self._status_callback, 5)
-
-        self.result_queue = Queue()
-
-        # gripper state
-        self.state_width = None
-        self.state_max_width = None
-        self.state_temperature = None
-        self.state_is_grasped = None
-        self.spin_handler = self._default_spin_handler
-
-    def _default_spin_handler(self):
-        rclpy.spin_once(self.node)
-
-    def set_spin_handler(self, spin_handler):
-        self.spin_handler = spin_handler
-
-    def _is_service_ready(self, service):
-        try:
-            self.node.get_logger().info("Waiting for service: " + service.service_name)
-            service.wait_for_service(timeout_sec=0.1)
-            return True
-        except Exception as e:
-            self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
-            return False
-
-    def is_enabled(self):
-        if self.state_width is None:
-            return False
-        if not self._is_service_ready(self.move_service):
-            return False
-        if not self._is_service_ready(self.grasp_service):
-            return False
-        return True
-
-    def _status_callback(self, msg: GripperState):
-        self.state_width = msg.width
-        self.state_max_width = msg.max_width
-        self.state_temperature = msg.temperature
-        self.state_is_grasped = msg.is_grasped
-
-    def move(self, width, speed=0):
-        """
-        Move the gripper to a specific width
-        :param width: width in meters
-        :param speed: speed in meters per second
-        :return: None
-        """
-        if self.is_busy():
-            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        threading.Thread(target=self._move, args=(width, speed)).start()
-
-    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
-        """
-        Grasp an object with the gripper
-        :param width:
-        :param force:
-        :param speed:
-        :param epsilon_inner:
-        :param epsilon_outer:
-        :return:
-        """
-        if self.is_busy():
-            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
-
-    def get_max_width(self):
-        """ Get the maximum width of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_max_width
-
-    def get_width(self):
-        """ Get the current width of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_width
-
-    def get_temperature(self):
-        """ Get the temperature of the gripper """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_temperature
-
-    def is_grasped(self):
-        """ Check if an object is grasped """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_is_grasped
-
-    def is_moving(self):
-        """ Check if the gripper is currently moving """
-        return self._moving
-
-    def is_grasping(self):
-        """ Check if the gripper is currently grasping """
-        return self._grasping
-
-    def is_busy(self):
-        """ Check if the gripper is currently moving or grasping """
-        return self._moving or self._grasping
-
-    def is_done(self):
-        """ Check if the gripper is done moving or grasping """
-        if not self.is_busy():
-            self.node.get_logger().warn("Gripper is not moving or grasping")
-            return False
-        else:
-            if self.result_queue.empty():
-                return False
-            else:
-                return True
-
-    def get_result(self):
-        """
-        Get the result of the last action
-        :return:
-        """
-        if not self.result_queue.empty():
-            response = self.result_queue.get()
-            self._moving = False
-            self._grasping = False
-            return response.success
-        else:
-            raise Exception("No result available")
-
-    def wait_until_done(self):
-        """
-        Wait until the gripper is done moving or grasping
-        :return: success
-        """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        if not self._moving and not self._grasping:
-            raise Exception("Gripper is not moving or grasping")
-        if not self._moving or self._grasping:
-            raise Exception("Gripper is not moving or grasping")
-        while self._moving or self._grasping:
-            self.spin_handler()
-            time.sleep(0.1)
-            if not self.result_queue.empty():
-                response = self.result_queue.get()
-                if isinstance(response, Move_Response):
-                    self._moving = False
-                elif isinstance(response, Grasp_Response):
-                    self._grasping = False
-                else:
-                    raise Exception("Invalid response type")
-                break
-
-        return response.success
-
-    def _move(self, width, speed):
-        self._moving = True
-        request = Move_Request()
-        request.width = width
-        request.speed = speed
-        response = self.move_service.call(request)
-        self.result_queue.put(response)
-
-    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
-        self._grasping = True
-        request = Grasp_Request()
-        request.width = width
-        request.force = force
-        request.speed = speed
-        request.epsilon_inner = epsilon_inner
-        request.epsilon_outer = epsilon_outer
-        response = self.grasp_service.call(request)
-        self.result_queue.put(response)

sas_robot_driver_franka/constraints_manager/LICENSE

@@ -0,0 +1,165 @@
+                   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.

sas_robot_driver_franka/constraints_manager/README.md

@@ -0,0 +1,2 @@
+![](https://img.shields.io/github/license/juanjqo/constraints_manager)![](https://img.shields.io/github/contributors/juanjqo/constraints_manager)![](https://img.shields.io/github/last-commit/juanjqo/constraints_manager/main)![](https://img.shields.io/github/commit-activity/t/juanjqo/constraints_manager/main)
+# constraints_manager

sas_robot_driver_franka/constraints_manager/constraints_manager_example/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(constraints_manager_example LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+FIND_PACKAGE(Eigen3 REQUIRED)
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+ADD_COMPILE_OPTIONS(-Werror=return-type -Wall -Wextra -Wmissing-declarations -Wredundant-decls -Woverloaded-virtual)
+
+include_directories(
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+
+    )
+
+
+add_library(ConstraintsManager ${CMAKE_CURRENT_SOURCE_DIR}/../src/constraints_manager.cpp)
+add_executable(constraints_manager_example main.cpp)
+target_link_libraries(constraints_manager_example
+                      ConstraintsManager)
+

sas_robot_driver_franka/constraints_manager/constraints_manager_example/main.cpp

@@ -0,0 +1,26 @@
+#include <iostream>
+#include "constraints_manager.h"
+
+using namespace Eigen;
+
+int main()
+{
+    auto manager = ConstraintsManager(3);
+    auto A1 = MatrixXd::Zero(3,3);
+    auto b1 = VectorXd::Zero(3);
+    auto A2 = MatrixXd::Ones(1,3);
+    auto b2 = VectorXd::Ones(1);
+
+    manager.add_inequality_constraint(A1, b1);
+    manager.add_inequality_constraint(A2, b2);
+
+    MatrixXd A;
+    VectorXd b;
+
+    std::tie(A,b) = manager.get_inequality_constraints();
+    std::cout<<"A: "<<std::endl;
+    std::cout<<A<<std::endl;
+    std::cout<<"b: "<<std::endl;
+    std::cout<<b<<std::endl;
+
+}

sas_robot_driver_franka/constraints_manager/include/constraints_manager.h

@@ -0,0 +1,67 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    constraints_manager 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.
+#
+#    constraints_manager 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 constraints_manager.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################*/
+
+#pragma once
+#include <Eigen/Dense>
+#include <vector>
+
+
+using namespace Eigen;
+
+class ConstraintsManager
+{
+protected:
+    int dim_configuration_;
+
+    VectorXd q_dot_min_ = VectorXd::Zero(0);
+    VectorXd q_dot_max_ = VectorXd::Zero(0);
+    VectorXd q_min_ = VectorXd::Zero(0);
+    VectorXd q_max_ = VectorXd::Zero(0);
+
+    MatrixXd equality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    VectorXd equality_constraint_vector_ = VectorXd::Zero(0);
+    MatrixXd inequality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    VectorXd inequality_constraint_vector_ = VectorXd::Zero(0);
+
+    MatrixXd _raw_add_matrix_constraint(const MatrixXd& A0, const MatrixXd& A);
+    VectorXd _raw_add_vector_constraint(const VectorXd& b0, const VectorXd& b);
+    void _check_matrix_and_vector_sizes(const MatrixXd& A, const VectorXd& b);
+
+    void _check_vectors_size(const VectorXd& q1, const VectorXd& q2, const std::string &msg);
+    void _check_vector_initialization(const VectorXd& q, const std::string &msg);
+    MatrixXd _create_matrix(const MatrixXd& A);
+
+public:
+    ConstraintsManager() = delete;
+    ConstraintsManager(const int& dim_configuration);
+
+    void add_equality_constraint(const MatrixXd& A, const VectorXd& b);
+    void add_inequality_constraint(const MatrixXd& A, const VectorXd& b);
+
+    std::tuple<MatrixXd, VectorXd> get_equality_constraints();
+    std::tuple<MatrixXd, VectorXd> get_inequality_constraints();
+
+    void set_joint_position_limits(const VectorXd& q_lower_bound, const VectorXd& q_upper_bound);
+    void set_joint_velocity_limits(const VectorXd& q_dot_lower_bound, const VectorXd& q_dot_upper_bound);
+};
+
+

sas_robot_driver_franka/constraints_manager/src/constraints_manager.cpp

@@ -0,0 +1,259 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    constraints_manager 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.
+#
+#    constraints_manager 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 constraints_manager.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################*/
+
+
+#include "constraints_manager.h"
+
+/**
+ * @brief Constructor
+ * @param int dim_configuration Dimension of the robot configuration
+ */
+ConstraintsManager::ConstraintsManager(const int& dim_configuration):dim_configuration_(dim_configuration)
+{
+
+}
+
+/**
+ * @brief The method _create_matrix returns a matrix containing the matrix A
+ *        and taking into acount the dimension of the robot configuration. If matrix A has lower columns than
+ *        dim_configuration then _create_matrix completes the correct size with zeros.
+ * @param MatrixXd A Input matrix
+ * @return
+ */
+MatrixXd ConstraintsManager::_create_matrix(const MatrixXd& A)
+{
+    MatrixXd constraint_matrix = MatrixXd::Zero(A.rows(), dim_configuration_);
+    constraint_matrix.block(0,0, A.rows(), A.cols()) = A;
+    return constraint_matrix;
+}
+
+
+/**
+ * @brief The method _check_matrix_and_vector_sizes(A,b) check the if the rows of Matrix A
+ * has the same dimension of Vector b.
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::_check_matrix_and_vector_sizes(const MatrixXd& A, const VectorXd& b)
+{
+    int m = A.rows();
+    int n = A.cols();
+    int nb = b.size();
+    if (m != nb)
+    {
+        throw std::runtime_error(std::string("Incompatible sizes. The rows of Matrix A must have the same dimension of Vector b. ")
+                               + std::string("But you used A ")+ std::to_string(m)+ std::string("x")+ std::to_string(n)
+                               + std::string(" and b ")+ std::to_string(nb) + std::string("x1"));
+    }
+    if (n != dim_configuration_)
+    {
+        throw std::runtime_error(std::string("Incompatible sizes. The cols of Matrix A must have dimension ") + std::to_string(dim_configuration_)
+                                 + std::string(". But you used A ")+ std::to_string(m)+ std::string("x")+ std::to_string(n)
+                                 + std::string(" and b ")+ std::to_string(nb) + std::string("x1"));
+    }
+
+}
+
+/**
+ * @brief ConstraintsManager::_check_vectors_size() checks the dimensions of two vectors.
+ * @param VectorXd q1 First vector to be checked.
+ * @param VectorXd q2 Second vector to be checked.
+ * @param std::string msg Desired error message if vectors q1 and q2 have different size.
+ */
+void ConstraintsManager::_check_vectors_size(const VectorXd& q1, const VectorXd& q2, const std::string &msg)
+{
+    if (q1.size() != q2.size() )
+    {
+       throw std::runtime_error(msg);
+    }
+}
+
+/**
+ * @brief ConstraintsManager::_check_vector_initialization()
+ * @param VectorXd q
+ * @param std::string msg
+ */
+void ConstraintsManager::_check_vector_initialization(const VectorXd& q, const std::string &msg)
+{
+    if (q.size() == 0)
+    {
+        throw std::runtime_error(msg);
+    }
+}
+
+
+/**
+ * @brief The method _raw_add_matrix_constraint(A0, A) returns a constraint_matrix, which is given as
+ *         constraint_matrix = [A0
+ *                              A]
+ * @param MatrixXd A0
+ * @param MatrixXd A
+ * @return MatrixXd constraint_matrix
+ */
+MatrixXd ConstraintsManager::_raw_add_matrix_constraint(const MatrixXd& A0, const MatrixXd& A)
+{
+        int m = A.rows();
+        int n = A.cols();
+        int m0 = A0.rows();
+        int n0 = A0.cols();
+        MatrixXd constraint_matrix = MatrixXd::Zero(m0+m, dim_configuration_);
+        if(n != n0)
+        {
+            throw std::runtime_error(std::string("Incompatible sizes. The equality matrix must be ")
+                                   + std::string("m x ")+ std::to_string(n0)
+                                   + std::string(". But you used m x ")+ std::to_string(n));
+        }
+        constraint_matrix.block(0,0, m0, n0) = A0;
+        constraint_matrix.block(m0,0, m, n) = A;
+        return constraint_matrix;
+}
+
+
+/**
+ * @brief The method _raw_add_vector_constraint(b0, b) returns the vector constraint_vector,
+ *        which is given as
+ *        constraint_vector = [b0
+ *                             b];
+ * @param VectorXd b0
+ * @param VectorXd b
+ * @return VectorXd constraint_vector
+ */
+VectorXd ConstraintsManager::_raw_add_vector_constraint(const VectorXd& b0, const VectorXd& b)
+{
+    int nb = b.size();
+    int nb0 = b0.size();
+    VectorXd constraint_vector =  VectorXd::Zero(nb0+nb);
+    constraint_vector.head(nb0) = b0;
+    constraint_vector.segment(nb0, nb) = b;
+    return constraint_vector;
+}
+
+
+/**
+ * @brief The method add_equality_constraint(A, b) adds the equality constraint A*x = b
+ *        to the set of equality constraints.
+ *
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::add_equality_constraint(const MatrixXd& A, const VectorXd& b)
+{
+    _check_matrix_and_vector_sizes(A, b);
+
+    if (equality_constraint_matrix_.size() == 0)
+    {
+        equality_constraint_matrix_ = _create_matrix(A);
+        equality_constraint_vector_ = b;
+    }else
+    {
+        equality_constraint_matrix_ = _raw_add_matrix_constraint(equality_constraint_matrix_, A);
+        equality_constraint_vector_ = _raw_add_vector_constraint(equality_constraint_vector_, b);
+    }
+}
+
+
+
+/**
+ * @brief The method add_inequality_constraint(A, b) adds the inequality constraint A*x <= b
+ *        to the set of ineequality constraints.
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::add_inequality_constraint(const MatrixXd& A, const VectorXd& b)
+{
+    _check_matrix_and_vector_sizes(A, b);
+
+    if (inequality_constraint_matrix_.size() == 0)
+    {
+        inequality_constraint_matrix_ = _create_matrix(A);
+        inequality_constraint_vector_ = b;
+    }else
+    {
+        inequality_constraint_matrix_ = _raw_add_matrix_constraint(inequality_constraint_matrix_, A);
+        inequality_constraint_vector_ = _raw_add_vector_constraint(inequality_constraint_vector_, b);
+    }
+}
+
+
+/**
+ * @brief The method get_equality_constraints() returns the set of equality constraints
+ *        composed of the Matrix A and the vector b, where
+ *        A*x = b.
+ *        Warning: This method deletes all the equality constraints stored.
+ * @return std::tuple<MatrixXd A, VectorXd b>
+ */
+std::tuple<MatrixXd, VectorXd> ConstraintsManager::get_equality_constraints()
+{
+    MatrixXd A = equality_constraint_matrix_;
+    equality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    return std::make_tuple(A, equality_constraint_vector_);
+}
+
+
+/**
+ * @brief The method get_inequality_constraints() returns the set of inequality constraints
+ *        composed of the Matrix A and the vector b, where
+ *        A*x <= b
+ *        Warning: This method deletes all the inequality constraints stored.
+ * @return
+ */
+std::tuple<MatrixXd, VectorXd> ConstraintsManager::get_inequality_constraints()
+{
+    MatrixXd A = inequality_constraint_matrix_;
+    inequality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    return std::make_tuple(A, inequality_constraint_vector_);
+}
+
+
+/**
+ * @brief ConstraintsManager::set_joint_position_limits
+ * @param VectorXd q_lower_bound
+ * @param VectorXd q_upper_bound
+ */
+void ConstraintsManager::set_joint_position_limits(const VectorXd& q_lower_bound, const VectorXd& q_upper_bound)
+{
+    _check_vectors_size(q_lower_bound, q_upper_bound,
+                          std::string("The sizes are incompatibles. q_lower_bound has size ") + std::to_string(q_lower_bound.size())
+                          + std::string(" and q_upper_bound has size ") + std::to_string(q_upper_bound.size()));
+    _check_vectors_size(q_lower_bound, VectorXd::Zero(dim_configuration_),
+                          std::string("The sizes are incompatibles. The joint limits have size ") + std::to_string(q_lower_bound.size())
+                          + std::string(" and the robot configuration has size ") + std::to_string(dim_configuration_));
+    q_min_ = q_lower_bound;
+    q_max_ = q_upper_bound;
+}
+
+/**
+ * @brief ConstraintsManager::set_joint_velocity_limits
+ * @param q_dot_lower_bound
+ * @param q_dot_upper_bound
+ */
+void ConstraintsManager::set_joint_velocity_limits(const VectorXd& q_dot_lower_bound, const VectorXd& q_dot_upper_bound)
+{
+    _check_vectors_size(q_dot_lower_bound, q_dot_upper_bound,
+                          std::string("The sizes are incompatibles. q_dot_lower_bound has size ") + std::to_string(q_dot_lower_bound.size())
+                          + std::string(" and q_dot_upper_bound has size ") + std::to_string(q_dot_upper_bound.size()));
+    _check_vectors_size(q_dot_lower_bound, VectorXd::Zero(dim_configuration_),
+                          std::string("The sizes are incompatibles. The joint limits have size ") + std::to_string(q_dot_lower_bound.size())
+                          + std::string(" and the robot configuration has size ") + std::to_string(dim_configuration_));
+    q_dot_min_ = q_dot_lower_bound;
+    q_dot_max_ = q_dot_upper_bound;
+}

sas_robot_driver_franka/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp

@@ -56,7 +56,7 @@
 #include <sas_robot_driver_franka_interfaces/srv/move.hpp>
 #include <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
 #endif
-
+#include <std_srvs/srv/trigger.hpp>
 
 // using namespace DQ_robotics;
 // using namespace Eigen;
@@ -69,6 +69,7 @@ namespace qros {
 struct EffectorDriverFrankaHandConfiguration
 {
     std::string robot_ip;
+    bool initialize_with_homing = true;
     double thread_sampeling_time_s = 1e8;  // 10Hz
     double default_force = 3.0;
     double default_speed = 0.1;
@@ -99,19 +100,25 @@ private:
     std::mutex gripper_in_use_;
     Service<srv::Grasp>::SharedPtr grasp_srv_;
     Service<srv::Move>::SharedPtr move_srv_;
+    Service<std_srvs::srv::Trigger>::SharedPtr homing_srv_;
 
 public:
 
     bool _grasp_srv_callback(
-        const std::shared_ptr<srv::Grasp::Request> req,
+        const std::shared_ptr<srv::Grasp::Request> &req,
         std::shared_ptr<srv::Grasp::Response> res
         );
 
     bool _move_srv_callback(
-        const std::shared_ptr<srv::Move::Request> req,
+        const std::shared_ptr<srv::Move::Request> &req,
         std::shared_ptr<srv::Move::Response> res
         );
 
+    bool _home_srv_callback(
+        const std::shared_ptr<std_srvs::srv::Trigger::Request> &req,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> res
+        );
+
     EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
     EffectorDriverFrankaHand()=delete;
     ~EffectorDriverFrankaHand();

sas_robot_driver_franka/include/sas_robot_driver_franka/sas_robot_driver_franka.hpp

@@ -113,7 +113,7 @@ public:
     VectorXd get_joint_velocities() override;
     void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
 
-    VectorXd get_joint_forces() override;
+    VectorXd get_joint_torques() override;
 
     void connect() override;
     void disconnect() override;

sas_robot_driver_franka/package.xml

@@ -2,9 +2,9 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>sas_robot_driver_franka</name>
-  <version>0.0.0</version>
+  <version>0.0.15</version>
   <description>The sas_driver_franka package</description>
-  <maintainer email="moonshot@todo.todo">moonshot</maintainer>
+  <maintainer email="qlin1806@g.ecc.u-tokyo.ac.jp">qlin</maintainer>
 
   <license>LGPLv3</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -17,7 +17,7 @@
   <depend>sas_core</depend>
   <depend>sas_robot_driver</depend>
   <depend>sas_common</depend>
-  <depend>sas_robot_driver_franka_interface</depend>
+  <depend>sas_robot_driver_franka_interfaces</depend>
 
 
   <test_depend>ament_lint_auto</test_depend>

sas_robot_driver_franka/sas_robot_driver_franka/__init__.py

@@ -0,0 +1,245 @@
+"""
+"""
+from typing import Union
+
+from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
+
+import rclpy
+from rclpy.node import Node
+from rclpy.client import Client
+from sas_robot_driver_franka_interfaces.srv import Move, Grasp
+from std_srvs.srv import Trigger
+from sas_robot_driver_franka_interfaces.msg import GripperState
+import os
+
+MOVE_TOPIC_SUFFIX = "move"
+GRASP_TOPIC_SUFFIX = "grasp"
+STATUS_TOPIC_SUFFIX = "gripper_status"
+
+
+class FrankaGripperInterface:
+    """
+    Non blocking interface to control the Franka gripper
+    """
+
+    def __init__(self, node: Node, robot_prefix: str):
+        self.last_message = None
+        self.robot_prefix = robot_prefix
+        self.node = node
+        self._current_action = None  # between moving, grasping, and homing
+        self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
+        self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
+        self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
+        self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
+                                                          self._status_callback, 10)
+
+        self.service_future: Union[rclpy.Future, None] = None
+
+        # gripper state
+        self.state_width = None
+        self.state_max_width = None
+        self.state_temperature = None
+        self.state_is_grasped = None
+        self.spin_handler = self._default_spin_handler
+
+    def _default_spin_handler(self):
+        rclpy.spin_once(self.node, timeout_sec=0.01)
+
+    def _is_service_ready(self, service: Client) -> bool:
+        try:
+            # self.node.get_logger().info("Waiting for service: " + service.service_name)
+            ret = service.wait_for_service(timeout_sec=0.1)
+            return ret
+        except Exception as e:
+            self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
+            return False
+
+    def is_enabled(self) -> bool:
+        if self.state_width is None:
+            return False
+        if not self._is_service_ready(self.move_service):
+            return False
+        if not self._is_service_ready(self.grasp_service):
+            return False
+        return True
+
+    def _status_callback(self, msg: GripperState):
+        self.state_width = msg.width
+        self.state_max_width = msg.max_width
+        self.state_temperature = msg.temperature
+        self.state_is_grasped = msg.is_grasped
+
+    #############################################################################
+    # Public methods to control the gripper (actions)
+    #############################################################################
+    def home(self) -> rclpy.Future:
+        """
+        Reinitialize the gripper
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        self._home()
+
+    def move(self, width, speed=0) -> rclpy.Future:
+        """
+        Move the gripper to a specific width
+        :param width: width in meters
+        :param speed: speed in meters per second
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        return self._move(width, speed)
+
+    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0) -> rclpy.Future:
+        """
+        Grasp an object with the gripper
+        :param width:
+        :param force:
+        :param speed:
+        :param epsilon_inner:
+        :param epsilon_outer:
+        :return:
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
+
+    #############################################################################
+    # Public methods to get the gripper state, from ros topics
+    #############################################################################
+    def get_max_width(self) -> float:
+        """ Get the maximum width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_max_width
+
+    def get_width(self) -> float:
+        """ Get the current width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_width
+
+    def get_temperature(self) -> float:
+        """ Get the temperature of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_temperature
+
+    def is_grasped(self) -> bool:
+        """ Check if an object is grasped """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_is_grasped
+
+    #############################################################################
+    # relating to action state
+    #############################################################################
+    def is_moving(self):
+        """ Check if the gripper is currently moving """
+        return self._current_action == "moving"
+
+    def is_grasping(self):
+        """ Check if the gripper is currently grasping """
+        return self._current_action == "grasping"
+
+    def is_homing(self):
+        """ Check if the gripper is currently homing """
+        return self._current_action == "homing"
+
+    def is_busy(self):
+        """ Check if the gripper is currently moving or grasping """
+        return self._current_action is not None
+
+    def is_done(self):
+        """ Check if the gripper is done moving or grasping """
+        if not self.is_busy():
+            self.node.get_logger().warn("Gripper is not moving or grasping")
+            return True
+        else:
+            if self.service_future is not None:
+                if self.service_future.done():
+                    return True
+                return False
+            else:
+                return True
+
+    ##############################################################################
+    # Public methods to get the result of the last action
+    ##############################################################################
+    def get_result(self):
+        """
+        Get the result of the last action
+        :return:
+        """
+        if self.service_future is not None:
+            if self.service_future.done():
+                response = self.service_future.result()
+                if isinstance(response, Move.Response):
+                    self._current_action = None
+                elif isinstance(response, Grasp.Response):
+                    self._current_action = None
+                elif isinstance(response, Trigger.Response):
+                    self._current_action = None
+                else:
+                    raise Exception("Invalid response type")
+                self.service_future = None
+                self.last_message = hasattr(response, "message") and response.message or ""
+                return response.success
+            else:
+                raise Exception("No result available")
+        else:
+            raise Exception("No result available")
+
+    def get_last_message(self):
+        return self.last_message
+
+    def wait_until_done(self):
+        """
+        Wait until the gripper is done moving or grasping
+        :return: success
+        """
+        rate = self.node.create_rate(100)  # 100 Hz
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        if not self.is_busy():
+            return
+        while not self.is_done():
+            self.spin_handler()
+            rate.sleep()
+        return self.get_result()
+
+    def _home(self) -> rclpy.Future:
+        self._current_action = "homing"
+        # self.node.get_logger().info("Homing gripper")
+        request = Trigger.Request()
+        # self.node.get_logger().info("Calling home service")
+        self.service_future = self.home_service.call_async(request)
+        return self.service_future
+
+    def _move(self, width, speed) -> rclpy.Future:
+        self._current_action = "moving"
+        # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
+        request = Move.Request()
+        request.width = float(width)
+        request.speed = float(speed)
+        # self.node.get_logger().info("Calling move service")
+        self.service_future = self.move_service.call_async(request)
+        return self.service_future
+
+    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer) -> rclpy.Future:
+        self._current_action = "grasping"
+        # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
+        request = Grasp.Request()
+        request.width = float(width)
+        request.force = float(force)
+        request.speed = float(speed)
+        request.epsilon_inner = float(epsilon_inner)
+        request.epsilon_outer = float(epsilon_outer)
+        # self.node.get_logger().info("Calling grasp service")
+        self.service_future = self.grasp_service.call_async(request)
+        return self.service_future
+
+    def set_spin_handler(self, spin_handler):
+        self.spin_handler = spin_handler

sas_robot_driver_franka/scripts/example_gripper_control.py

@@ -1,6 +1,5 @@
 import threading
 
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
 import rclpy
 from sas_robot_driver_franka import FrankaGripperInterface
 import time
@@ -14,13 +13,11 @@ def main_loop(node):
 
     while not hand1.is_enabled():
         logger.info("Waiting for gripper to be enabled...")
-        rclcpp_spin_some(node)
         time.sleep(0.1)
     rclpy.node.get_logger(node_name).info("Gripper enabled!")
 
     def spin_all(node_):
         while rclpy.ok():
-            rclcpp_spin_some(node_)
             rclpy.spin_once(node_, timeout_sec=0.1)
             time.sleep(0.1)
             

sas_robot_driver_franka/scripts/publish_dumy_robot_dynmaics.py

@@ -0,0 +1,46 @@
+import time
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
+import dqrobotics as dql
+import numpy as np
+
+
+
+
+if __name__ == "__main__":
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_server_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_server")
+    dynam_provider = RobotDynamicsServer(sas_node, "/franka1")
+
+    t = dql.DQ([0, 0, 1])
+    r = dql.DQ([1, 0, 0, 0])
+    base_pose = r + 0.5 * dql.E_ * t * r
+    dynam_provider.set_world_to_base_tf(base_pose)
+    t_ = 0
+    node.get_logger().info("Dummy robot dynamics server started")
+    r = dql.DQ([0, 0, 0, 1])
+    rate = node.create_rate(100)
+    dummy_force = np.random.rand(3) * 100
+    dummy_torque = np.random.rand(3) * 10
+    try:
+        while rclpy.ok():
+            t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
+            ee_pose = r + 0.5 * dql.E_ * t * r
+            dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
+            dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
+            dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
+            rclcpp_spin_some(sas_node)
+            rclpy.spin_once(node)
+            t_ += 0.01
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
+

sas_robot_driver_franka/scripts/subscribe_dummy_dynamic.py

@@ -0,0 +1,53 @@
+import time
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
+import dqrobotics as dql
+import numpy as np
+
+from dqrobotics.interfaces.vrep import DQ_VrepInterface
+
+# vrep = DQ_VrepInterface()
+# vrep.connect("192.168.10.103", 19997, 100, 10)
+vrep = None
+
+if __name__ == "__main__":
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_client_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_client")
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin_once(timeout_sec=0.1)
+        dynam_provider = RobotDynamicsClient(sas_node, "/franka1")
+        rclcpp_spin_some(sas_node)
+        while not dynam_provider.is_enabled():
+            node.get_logger().info("Waiting for robot dynamics provider to be enabled")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            node.get_logger().info("retrying...")
+            time.sleep(0.5)
+
+        node.get_logger().info("Dummy robot dynamics client started")
+        while rclpy.ok():
+            force = dynam_provider.get_stiffness_force()
+            torque = dynam_provider.get_stiffness_torque()
+            ee_pose = dynam_provider.get_stiffness_frame_pose()
+            if vrep is not None:
+                vrep.set_object_pose("xd1", ee_pose)
+            node.get_logger().info(f"EE Pose: {ee_pose}")
+            node.get_logger().info(f"Force: {force}")
+            node.get_logger().info(f"Torque: {torque}")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            time.sleep(0.5)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        if vrep is not None:
+            vrep.disconnect()
+        rclpy.shutdown()

sas_robot_driver_franka/src/hand/qros_effector_driver_franka_hand.cpp

@@ -63,6 +63,8 @@ namespace qros
             std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2));
         move_srv_ = node->create_service<srv::Move>(driver_node_prefix_ + "/move",
             std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
+        homing_srv_ = node->create_service<std_srvs::srv::Trigger>(driver_node_prefix_ + "/homing",
+            std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2));
         gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
             driver_node_prefix_ + "/gripper_status", 1);
     }
@@ -78,19 +80,18 @@ namespace qros
     }
 
 
-    void EffectorDriverFrankaHand::start_control_loop()
-    {
+    void EffectorDriverFrankaHand::start_control_loop() {
         sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
         clock.init();
-        RCLCPP_INFO_STREAM(node_->get_logger(),
-            "["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
         while (!(*break_loops_))
         {
             if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
 
             if (!status_loop_running_)
             {
-                RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
+                RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
                 break_loops_->store(true);
                 break;
             }
@@ -98,14 +99,14 @@ namespace qros
             clock.update_and_sleep();
             spin_some(node_);
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
     }
 
 
     void EffectorDriverFrankaHand::connect()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
         return;
 #endif
         gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
@@ -117,11 +118,11 @@ namespace qros
     void EffectorDriverFrankaHand::disconnect() noexcept
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
         return;
 #endif
-        RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnecting...");
+        gripper_sptr_.reset();
         gripper_sptr_ = nullptr;
     }
 
@@ -131,7 +132,7 @@ namespace qros
     void EffectorDriverFrankaHand::gripper_homing()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
         return;
 #endif
         if (!_is_connected()) throw std::runtime_error(
@@ -141,22 +142,32 @@ namespace qros
         {
             throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
     }
 
     void EffectorDriverFrankaHand::initialize()
     {
         if (!_is_connected()) throw std::runtime_error(
             "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
-        gripper_homing();
+        if (configuration_.initialize_with_homing) {
+            gripper_homing();
+        }
         // start gripper status loop
         status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
+        // check status loop with timeout
+        auto time_now = std::chrono::system_clock::now();
+        auto time_out = time_now + std::chrono::seconds(5);
+        while (!status_loop_running_)
+        {
+            if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
     }
 
     void EffectorDriverFrankaHand::deinitialize()
     {
 #ifdef IN_TESTING
-        RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
         return;
 #endif
         if (_is_connected())
@@ -170,9 +181,9 @@ namespace qros
     }
 
 
-    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> req, std::shared_ptr<srv::Grasp::Response> res)
+    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> &req, std::shared_ptr<srv::Grasp::Response> res)
     {
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
         auto force = req->force;
         auto speed = req->speed;
         auto epsilon_inner = req->epsilon_inner;
@@ -181,61 +192,91 @@ namespace qros
         if (speed <= 0.0) speed = configuration_.default_speed;
         if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
         if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req->width));
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req->width));
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
         bool ret = false;
         bool function_ret = true;
-        gripper_in_use_.lock();
 #ifdef IN_TESTING
         ret = true;
         std::this_thread::sleep_for(std::chrono::milliseconds(2000));
 #else
         try
         {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
             ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer);
         }catch(franka::CommandException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + std::string(e.what()));
             function_ret = false;
         }catch(franka::NetworkException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + std::string(e.what()));
             function_ret = false;
         }
 #endif
-        gripper_in_use_.unlock();
         res->set__success(ret);
         return function_ret;
     }
 
 
-    bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> req, std::shared_ptr<srv::Move::Response> res)
+    bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> &req, std::shared_ptr<srv::Move::Response> res)
     {
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
         auto speed = req->speed;
         if (speed <= 0.0) speed = configuration_.default_speed;
-        RCLCPP_DEBUG_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
         bool ret = false;
         bool function_ret = true;
-        gripper_in_use_.lock();
 #ifdef IN_TESTING
         ret = true;
         std::this_thread::sleep_for(std::chrono::milliseconds(2000));
 #else
         try
         {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
             ret = gripper_sptr_->move(req->width, speed);
         }catch(franka::CommandException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + std::string(e.what()));
             function_ret = false;
         }catch(franka::NetworkException& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + std::string(e.what()));
             function_ret = false;
         }
 #endif
-        gripper_in_use_.unlock();
+        res->set__success(ret);
+        return function_ret;
+    }
+
+
+    bool EffectorDriverFrankaHand::_home_srv_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> &req, std::shared_ptr<std_srvs::srv::Trigger::Response> res)
+    {
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::Homing...");
+        bool ret = false;
+        bool function_ret = true;
+#ifdef IN_TESTING
+        ret = true;
+        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+#else
+        try
+        {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->homing();
+        }catch(franka::CommandException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::CommandException::" + std::string(e.what()));
+            res->set__message("_home_srv_callback::CommandException::" + std::string(e.what()));
+            function_ret = false;
+            ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::NetworkException::" + std::string(e.what()));
+            res->set__message("_home_srv_callback::NetworkException::" + std::string(e.what()));
+            function_ret = false;
+            ret = false;
+        }
+#endif
         res->set__success(ret);
         return function_ret;
     }
@@ -243,49 +284,38 @@ namespace qros
 
     void EffectorDriverFrankaHand::_gripper_status_loop()
     {
-        status_loop_running_ = true;
         sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
-        ;
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
         clock.init();
         try
         {
+            status_loop_running_ = true;
             while (status_loop_running_)
             {
-#ifdef BLOCK_READ_IN_USED
-                bool should_unlock = false;
-#endif
-                if (!_is_connected()) throw std::runtime_error(
-                    "[" + std::string(node_->get_name())+
-                    "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
+                if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
                 try
                 {
 #ifdef IN_TESTING
-                    RCLCPP_WARN_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                    RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
                     throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
-#endif
-#ifdef BLOCK_READ_IN_USED
-                    gripper_in_use_.lock();
-                    should_unlock = true;
-#endif
-                    franka::GripperState gripper_state = gripper_sptr_->readOnce();
-#ifdef BLOCK_READ_IN_USED
-                    gripper_in_use_.unlock();
 #endif
                     msg::GripperState msg;
-                    msg.set__width(static_cast<float>(gripper_state.width));
-                    msg.set__max_width(static_cast<float>(gripper_state.max_width));
-                    msg.set__is_grasped(gripper_state.is_grasped);
-                    msg.set__temperature(gripper_state.temperature);
-                    msg.set__duration_ms(gripper_state.time.toMSec());
+                    {
+#ifdef BLOCK_READ_IN_USED
+                        std::lock_guard<std::mutex> lock(gripper_in_use_);
+#endif
+                        franka::GripperState gripper_state = gripper_sptr_->readOnce();
+                        msg.set__width(static_cast<float>(gripper_state.width));
+                        msg.set__max_width(static_cast<float>(gripper_state.max_width));
+                        msg.set__is_grasped(gripper_state.is_grasped);
+                        msg.set__temperature(gripper_state.temperature);
+                        msg.set__duration_ms(gripper_state.time.toMSec());
+                    }
                     gripper_status_publisher_->publish(msg);
                 }
                 catch (...)
                 {
-#ifdef BLOCK_READ_IN_USED
-                    if (should_unlock) gripper_in_use_.unlock();
-#endif
-                    RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
+                    RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
                     msg::GripperState msg;
                     msg.width = 0.0;
                     gripper_status_publisher_->publish(msg);
@@ -297,16 +327,14 @@ namespace qros
         }
         catch (std::exception& e)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
-                what());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + std::string(e.what()));
             status_loop_running_ = false;
         }
         catch (...)
         {
-            RCLCPP_ERROR_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+
-                "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
             status_loop_running_ = false;
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
     }
 } // qros

sas_robot_driver_franka/src/hand/robot_interface_hand.cpp

@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include "sas_robot_driver_franka/robot_interface_hand.hpp"
+#include <sas_robot_driver_franka/robot_interface_hand.hpp>
 
 
 

sas_robot_driver_franka/src/joint/sas_robot_driver_franka.cpp

@@ -50,7 +50,7 @@ namespace sas
    {
         joint_positions_.resize(7);
         joint_velocities_.resize(7);
-        joint_forces_.resize(7);
+        joint_torques_.resize(7);
         //end_effector_pose_.resize(7);
         //joint_positions_buffer_.resize(8,0);
         //end_effector_pose_euler_buffer_.resize(7,0);
@@ -193,7 +193,7 @@ namespace sas
      */
     void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
     {
-        desired_joint_velocities_ = desired_joint_velocities;
+        // desired_joint_velocities_ = desired_joint_velocities;
         robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
         if(robot_driver_interface_sptr_->get_err_state()) {
             RCLCPP_ERROR_STREAM(node_->get_logger(),
@@ -207,10 +207,10 @@ namespace sas
      * @brief RobotDriverFranka::get_joint_forces
      * @return
      */
-    VectorXd RobotDriverFranka::get_joint_forces()
+    VectorXd RobotDriverFranka::get_joint_torques()
     {
-        joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
-        return joint_forces_;
+        joint_torques_ = robot_driver_interface_sptr_->get_joint_forces();
+        return joint_torques_;
     }
 
 }

sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_py.cpp

@@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient;
 using RDS = qros::RobotDynamicsServer;
 
 
-PYBIND11_MODULE(_qros_franka_robot_dynamic, m)
+PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
 {
     py::class_<RDC>(m, "RobotDynamicsClient")
         .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())

sas_robot_driver_franka/src/sas_robot_driver_franka_hand_node.cpp

@@ -53,10 +53,10 @@ void sig_int_handler(int)
 template<typename T>
 void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
 {
-    if(node->has_parameter(param_name))
+    try
     {
         sas::get_ros_parameter(node,param_name,param);
-    }else
+    }catch (const std::exception& e)
     {
         RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
     }
@@ -81,6 +81,7 @@ int main(int argc, char **argv)
     RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
 
     qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+    get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing);
     sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
     sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s);
     sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);

sas_robot_driver_franka/src/sas_robot_driver_franka_node.cpp

@@ -118,35 +118,33 @@ int main(int argc, char **argv)
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
     }
 
-    try{node->declare_parameter<std::vector<double>>("upper_torque_threshold");}catch (...){}
-    if(node->has_parameter("upper_torque_threshold")) {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
+    try {
         std::vector<double> upper_torque_threshold_std_vec;
         sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
         franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
-    }else {
+    }catch(...) {
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
     }
-    try{node->declare_parameter<std::vector<double>>("upper_force_threshold");}catch (...){}
-    if(node->has_parameter("upper_force_threshold")) {
+    try {
+        std::vector<double> upper_force_threshold_std_vec;
+        sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
-        std::vector<double> upper_torque_threshold_std_vec;
-        sas::get_ros_parameter(node,"upper_force_threshold",upper_torque_threshold_std_vec);
-        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
-    }else {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold not set. Using default with value scalling.");
+        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
     }
-    try{node->declare_parameter<std::string>("robot_parameter_file_path");}catch (...){}
-    if(node->has_parameter("robot_parameter_file_path"))
-    {
+    try {
         std::string robot_parameter_file_path;
         sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
         const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
         robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
-    }else{RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");}
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");
+    }
 
     robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
 
@@ -167,7 +165,7 @@ int main(int argc, char **argv)
     try
         {
             RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
-            auto robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
+            std::shared_ptr<sas::RobotDriverFranka> robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
                 node,
                 robot_dynamic_provider_ptr,
                 robot_driver_franka_configuration,
@@ -177,7 +175,7 @@ int main(int argc, char **argv)
             robot_driver_franka -> set_joint_limits(joint_limits);
             RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
             sas::RobotDriverROS robot_driver_ros(node,
-                                                robot_driver_franka, 
+                                                robot_driver_franka,
                                                 robot_driver_ros_configuration,
                                                 &kill_this_process);
             robot_driver_ros.control_loop();

sas_robot_driver_franka_interfaces/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(sas_robot_driver_franka_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/GripperState.msg"
+        "srv/Move.srv"
+        "srv/Grasp.srv"
+)
+
+install(
+        DIRECTORY msg
+        DESTINATION share/${PROJECT_NAME}/msg
+)
+
+install(
+        DIRECTORY srv
+        DESTINATION share/${PROJECT_NAME}/srv
+)
+
+
+
+
+ament_package()

sas_robot_driver_franka_interfaces/msg/GripperState.msg

@@ -0,0 +1,5 @@
+float32 width
+float32 max_width
+bool is_grasped
+uint16 temperature
+uint64 duration_ms

sas_robot_driver_franka_interfaces/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sas_robot_driver_franka_interfaces</name>
+  <version>0.0.0</version>
+  <description>sas_driver_franka interfaces package</description>
+  <maintainer email="qlin1806@gmail.com">qlin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sas_robot_driver_franka_interfaces/srv/Grasp.srv

@@ -0,0 +1,7 @@
+float32 width
+float32 speed
+float32 force
+float32 epsilon_inner
+float32 epsilon_outer
+---
+bool success

sas_robot_driver_franka_interfaces/srv/Move.srv

@@ -0,0 +1,4 @@
+float32 width
+float32 speed
+---
+bool success

scripts/publish_dumy_robot_dynmaics.py

@@ -1,29 +0,0 @@
-import rospy
-from sas_robot_driver_franka import RobotDynamicsServer
-import dqrobotics as dql
-import numpy as np
-
-
-if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_provider")
-
-    dynam_provider = RobotDynamicsProvider("/franka1")
-    t = dql.DQ([0, 0, 1])
-    r = dql.DQ([1, 0, 0, 0])
-    base_pose = r + 0.5 * dql.E_ * t * r
-    dynam_provider.set_world_to_base_tf(base_pose)
-    t_ = 0
-    rospy.loginfo("Publishing dummy robot dynamics")
-    r = dql.DQ([0, 0, 0, 1])
-    rate = rospy.Rate(100)
-    dummy_force = np.random.rand(3) * 100
-    dummy_torque = np.random.rand(3) * 10
-
-    while not rospy.is_shutdown():
-        t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
-        ee_pose = r + 0.5 * dql.E_ * t * r
-        dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
-        dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
-        dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
-        rate.sleep()
-        t_ += 0.01

scripts/subscribe_dummy_dynamic.py

@@ -1,30 +0,0 @@
-import rospy
-from sas_robot_driver_franka import RobotDynamicsInterface
-import dqrobotics as dql
-import numpy as np
-
-from dqrobotics.interfaces.vrep import DQ_VrepInterface
-
-vrep = DQ_VrepInterface()
-vrep.connect("192.168.10.103", 19997, 100, 10)
-
-if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_subscriber")
-
-    dynam_provider = RobotDynamicsInterface("/franka1")
-    while not dynam_provider.is_enabled():
-        rospy.loginfo("Waiting for robot dynamics provider to be enabled")
-        rospy.sleep(1)
-
-    rospy.loginfo("Subscribing to dummy robot dynamics")
-    rate = rospy.Rate(50)
-
-    while not rospy.is_shutdown():
-        force = dynam_provider.get_stiffness_force()
-        torque = dynam_provider.get_stiffness_torque()
-        ee_pose = dynam_provider.get_stiffness_frame_pose()
-        vrep.set_object_pose("xd1", ee_pose)
-        rospy.loginfo("EE Pose: %s", ee_pose)
-        rospy.loginfo("Force: %s", force)
-        rospy.loginfo("Torque: %s", torque)
-        rate.sleep()