[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/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/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/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

@@ -7,15 +7,10 @@ from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamics
 import rclpy
 from rclpy.node import Node
 from rclpy.client import Client
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
 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
-import threading
-from queue import Queue
-import time
-import struct
 
 MOVE_TOPIC_SUFFIX = "move"
 GRASP_TOPIC_SUFFIX = "grasp"
@@ -27,16 +22,14 @@ class FrankaGripperInterface:
     Non blocking interface to control the Franka gripper
     """
 
-    def __init__(self, node: Node, robot_prefix):
+    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._moving = False
         self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
-        self._grasping = False
         self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
-        self._homing = False
         self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
                                                           self._status_callback, 10)
 
@@ -50,9 +43,9 @@ class FrankaGripperInterface:
         self.spin_handler = self._default_spin_handler
 
     def _default_spin_handler(self):
-        rclpy.spin_once(self.node)
+        rclpy.spin_once(self.node, timeout_sec=0.01)
 
-    def _is_service_ready(self, service: Client):
+    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)
@@ -61,7 +54,7 @@ class FrankaGripperInterface:
             self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
             return False
 
-    def is_enabled(self):
+    def is_enabled(self) -> bool:
         if self.state_width is None:
             return False
         if not self._is_service_ready(self.move_service):
@@ -75,7 +68,11 @@ class FrankaGripperInterface:
         self.state_max_width = msg.max_width
         self.state_temperature = msg.temperature
         self.state_is_grasped = msg.is_grasped
-    def home(self):
+
+    #############################################################################
+    # Public methods to control the gripper (actions)
+    #############################################################################
+    def home(self) -> rclpy.Future:
         """
         Reinitialize the gripper
         :return: None
@@ -84,7 +81,7 @@ class FrankaGripperInterface:
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
         self._home()
 
-    def move(self, width, speed=0):
+    def move(self, width, speed=0) -> rclpy.Future:
         """
         Move the gripper to a specific width
         :param width: width in meters
@@ -93,9 +90,9 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._move(width, speed)
+        return self._move(width, speed)
 
-    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
+    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0) -> rclpy.Future:
         """
         Grasp an object with the gripper
         :param width:
@@ -107,49 +104,59 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
+        return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
 
-    def get_max_width(self):
+    #############################################################################
+    # 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):
+    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):
+    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):
+    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._moving
+        return self._current_action == "moving"
 
     def is_grasping(self):
         """ Check if the gripper is currently grasping """
-        return self._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._moving or self._grasping or self.service_future is not None
+        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 False
+            return True
         else:
             if self.service_future is not None:
                 if self.service_future.done():
@@ -158,6 +165,9 @@ class FrankaGripperInterface:
             else:
                 return True
 
+    ##############################################################################
+    # Public methods to get the result of the last action
+    ##############################################################################
     def get_result(self):
         """
         Get the result of the last action
@@ -167,11 +177,11 @@ class FrankaGripperInterface:
             if self.service_future.done():
                 response = self.service_future.result()
                 if isinstance(response, Move.Response):
-                    self._moving = False
+                    self._current_action = None
                 elif isinstance(response, Grasp.Response):
-                    self._grasping = False
+                    self._current_action = None
                 elif isinstance(response, Trigger.Response):
-                    self._homing = False
+                    self._current_action = None
                 else:
                     raise Exception("Invalid response type")
                 self.service_future = None
@@ -181,6 +191,7 @@ class FrankaGripperInterface:
                 raise Exception("No result available")
         else:
             raise Exception("No result available")
+
     def get_last_message(self):
         return self.last_message
 
@@ -189,31 +200,36 @@ class FrankaGripperInterface:
         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()
-            time.sleep(0.01)
+            rate.sleep()
-    def _home(self):
+        return self.get_result()
-        self._homing = True
+
+    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):
+    def _move(self, width, speed) -> rclpy.Future:
-        self._moving = True
+        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):
+    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer) -> rclpy.Future:
-        self._grasping = True
+        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)
@@ -223,7 +239,7 @@ class FrankaGripperInterface:
         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/src/hand/qros_effector_driver_franka_hand.cpp

@@ -122,7 +122,7 @@ namespace qros
         return;
 #endif
         RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
+        gripper_sptr_.reset();
         gripper_sptr_ = nullptr;
     }
 

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