diff --git a/README.md b/README.md
index ec4b544..b30ae3c 100644
--- a/README.md
+++ b/README.md
@@ -1,5 +1,3 @@
# sas_robot_driver_franka
-
-
diff --git a/constraints_manager b/constraints_manager
deleted file mode 160000
index 920afaf..0000000
--- a/constraints_manager
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit 920afaf5ff68ae76a0ac63869dbd4acb1842f15e
diff --git a/constraints_manager/LICENSE b/constraints_manager/LICENSE
new file mode 100644
index 0000000..0a04128
--- /dev/null
+++ b/constraints_manager/LICENSE
@@ -0,0 +1,165 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc.
+ 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.
diff --git a/constraints_manager/README.md b/constraints_manager/README.md
new file mode 100644
index 0000000..d1317d7
--- /dev/null
+++ b/constraints_manager/README.md
@@ -0,0 +1,2 @@
+
+# constraints_manager
diff --git a/constraints_manager/constraints_manager_example/CMakeLists.txt b/constraints_manager/constraints_manager_example/CMakeLists.txt
new file mode 100644
index 0000000..6a5b9b3
--- /dev/null
+++ b/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)
+
diff --git a/constraints_manager/constraints_manager_example/main.cpp b/constraints_manager/constraints_manager_example/main.cpp
new file mode 100644
index 0000000..f3588c5
--- /dev/null
+++ b/constraints_manager/constraints_manager_example/main.cpp
@@ -0,0 +1,26 @@
+#include
+#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: "<.
+#
+# ################################################################
+#
+# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################*/
+
+#pragma once
+#include
+#include
+
+
+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 get_equality_constraints();
+ std::tuple 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);
+};
+
+
diff --git a/constraints_manager/src/constraints_manager.cpp b/constraints_manager/src/constraints_manager.cpp
new file mode 100644
index 0000000..d71507b
--- /dev/null
+++ b/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 .
+#
+# ################################################################
+#
+# 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
+ */
+std::tuple 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 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;
+}