Merge remote-tracking branch 'gitea/ros2_jazzy' into ros2_jazzy

.gitmodules

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

README.md

@@ -1,5 +1,3 @@
 ![](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)

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.

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

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)
+

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;
+
+}

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);
+};
+
+

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;
+}