From 40ad138f16caa1d20b6ef201db4ab35ea95113db Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 12:52:59 +0200 Subject: [PATCH 1/6] Changed name of method tthat rotates both Jacobian and b vector to local frame to consistency --- bindings/python/tasks/velocity.hpp | 1 - include/OpenSoT/tasks/velocity/Cartesian.h | 8 ++++---- src/tasks/velocity/Cartesian.cpp | 8 ++++---- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 895542fb..7d81733c 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -50,7 +50,6 @@ void pyVelocityCartesian(py::module& m) { .def("getActualPose", py::overload_cast<>(&Cartesian::getActualPose, py::const_)) .def("getError", &Cartesian::getError) .def("reset", &Cartesian::reset) - .def("setIsBodyJacobian", &Cartesian::setIsBodyJacobian) .def_property("orientationErrorGain", &Cartesian::getOrientationErrorGain, &Cartesian::setOrientationErrorGain) .def_property_readonly("distalLink", &Cartesian::getDistalLink) .def_property_readonly("baseLink", &Cartesian::getBaseLink) diff --git a/include/OpenSoT/tasks/velocity/Cartesian.h b/include/OpenSoT/tasks/velocity/Cartesian.h index dfef2614..7fa53cca 100644 --- a/include/OpenSoT/tasks/velocity/Cartesian.h +++ b/include/OpenSoT/tasks/velocity/Cartesian.h @@ -84,7 +84,7 @@ Eigen::Vector3d positionError; Eigen::Vector3d orientationError; - bool _is_body_jacobian; + bool _rotate_to_local; Eigen::MatrixXd _tmp_A; Eigen::VectorXd _tmp_b; @@ -219,10 +219,10 @@ virtual bool reset(); /** - * @brief setIsBodyJacobian - * @param is_body_jacobian if true jacobians are in body (ee reference) + * @brief rotateToLocal rotates both Jacobian and references to local (ee) distal frame, this is mostly used for local subtasks + * @param rotate_to_local default is false */ - void setIsBodyJacobian(const bool is_body_jacobian); + void rotateToLocal(const bool rotate_to_local); static bool isCartesian(OpenSoT::Task::TaskPtr task); diff --git a/src/tasks/velocity/Cartesian.cpp b/src/tasks/velocity/Cartesian.cpp index 8685024e..f6ed6574 100644 --- a/src/tasks/velocity/Cartesian.cpp +++ b/src/tasks/velocity/Cartesian.cpp @@ -30,7 +30,7 @@ Cartesian::Cartesian(std::string task_id, Task(task_id, robot.getNv()), _robot(robot), _distal_link(distal_link), _base_link(base_link), _orientationErrorGain(1.0), _is_initialized(false), - _error(6), _is_body_jacobian(false) + _error(6), _rotate_to_local(false) { _error.setZero(6); @@ -90,7 +90,7 @@ void Cartesian::_update() { this->update_b(); //Here we rotate A and b - if(_is_body_jacobian) + if(_rotate_to_local) { _tmp_A = _A; _A = XBot::Utils::adjointFromRotation(_actualPose.linear().transpose())*_tmp_A; @@ -360,7 +360,7 @@ bool Cartesian::reset() return true; } -void Cartesian::setIsBodyJacobian(const bool is_body_jacobian) +void Cartesian::rotateToLocal(const bool rotate_to_local) { - _is_body_jacobian = is_body_jacobian; + _rotate_to_local = rotate_to_local; } From 8438d18ab5d10054fdee25a8fb9fb7f0f6457aa0 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 14:35:47 +0200 Subject: [PATCH 2/6] fix --- src/constraints/velocity/OmniWheels4X.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp index 286cf12c..55f39d12 100644 --- a/src/constraints/velocity/OmniWheels4X.cpp +++ b/src/constraints/velocity/OmniWheels4X.cpp @@ -69,8 +69,9 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r, void OmniWheels4X::update() { - _robot.getPose(_base_link, _w_T_b); - _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); + // _robot.getPose(_base_link, _w_T_b); + // _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); + _Aineq.rightCols(_x_size-6).noalias() = _J.rightCols(_x_size-6); } From 2ca4ef205b5b48f77748563d7f4d84a047453d18 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 14:36:05 +0200 Subject: [PATCH 3/6] added set local velocity reference --- bindings/python/tasks/velocity.hpp | 1 + include/OpenSoT/tasks/velocity/Cartesian.h | 14 +++++++++++++- src/tasks/velocity/Cartesian.cpp | 17 +++++++++++++++-- 3 files changed, 29 insertions(+), 3 deletions(-) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 7d81733c..dcec7d23 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -50,6 +50,7 @@ void pyVelocityCartesian(py::module& m) { .def("getActualPose", py::overload_cast<>(&Cartesian::getActualPose, py::const_)) .def("getError", &Cartesian::getError) .def("reset", &Cartesian::reset) + .def("setVelocityLocalReference", &Cartesian::setVelocityLocalReference) .def_property("orientationErrorGain", &Cartesian::getOrientationErrorGain, &Cartesian::setOrientationErrorGain) .def_property_readonly("distalLink", &Cartesian::getDistalLink) .def_property_readonly("baseLink", &Cartesian::getBaseLink) diff --git a/include/OpenSoT/tasks/velocity/Cartesian.h b/include/OpenSoT/tasks/velocity/Cartesian.h index 7fa53cca..11337a57 100644 --- a/include/OpenSoT/tasks/velocity/Cartesian.h +++ b/include/OpenSoT/tasks/velocity/Cartesian.h @@ -85,10 +85,13 @@ Eigen::Vector3d orientationError; bool _rotate_to_local; + bool _velocity_refs_are_local; Eigen::MatrixXd _tmp_A; Eigen::VectorXd _tmp_b; + Eigen::Vector6d _tmp_twist; + public: /*********** TASK PARAMETERS ************/ @@ -131,7 +134,7 @@ * since THE _update() RESETS THE FEED-FORWARD VELOCITY TERM for safety reasons. * @param desiredPose the \f$R^{4x4}\f$ homogeneous transform matrix describing the desired pose * for the distal_link in the base_link frame of reference. - * @param desireVelocity is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents + * @param desireTwist is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents * a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample, * instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as * setReference(desiredPose, desiredTwist*dt) @@ -143,6 +146,15 @@ void setReference(const KDL::Frame& desiredPose, const KDL::Twist& desiredTwist); + /** + * @brief setVelocityLocalReference permits to set velocity expressed in local (ee) distal frame + * @param desireTwist is a \f$R^{6}\f$ twist describing the desired trajectory velocity, and it represents + * a feed-forward term in the cartesian task computation. NOTICE how the velocities are in units/sample, + * instead of units/s. This means that if you have a twist expressed in SI units, you have to call the function as + * setVelocityLocalReference(desiredTwist*dt) + */ + void setVelocityLocalReference(const Eigen::Vector6d& desiredTwist); + /** * @brief getReference returns the Cartesian task reference * @return the Cartesian task reference \f$R^{4x4}\f$ homogeneous transform matrix describing the desired pose diff --git a/src/tasks/velocity/Cartesian.cpp b/src/tasks/velocity/Cartesian.cpp index f6ed6574..e05c2ef3 100644 --- a/src/tasks/velocity/Cartesian.cpp +++ b/src/tasks/velocity/Cartesian.cpp @@ -30,7 +30,7 @@ Cartesian::Cartesian(std::string task_id, Task(task_id, robot.getNv()), _robot(robot), _distal_link(distal_link), _base_link(base_link), _orientationErrorGain(1.0), _is_initialized(false), - _error(6), _rotate_to_local(false) + _error(6), _rotate_to_local(false), _velocity_refs_are_local(false) { _error.setZero(6); @@ -87,6 +87,12 @@ void Cartesian::_update() { _is_initialized = true; } + if(_velocity_refs_are_local) + { + _tmp_twist = _desiredTwist; + _desiredTwist = XBot::Utils::adjointFromRotation(_actualPose.linear()) * _tmp_twist; + } + this->update_b(); //Here we rotate A and b @@ -100,10 +106,17 @@ void Cartesian::_update() { } this->_desiredTwist.setZero(6); - + _velocity_refs_are_local = false; /**********************************************************************/ } +void Cartesian::setVelocityLocalReference(const Eigen::Vector6d& desiredTwist) +{ + _velocity_refs_are_local = true; + _desiredTwist = desiredTwist; + _desiredTwistRef = _desiredTwist; +} + void Cartesian::setReference(const Eigen::Affine3d& desiredPose) { _desiredPose = desiredPose; From 7c92b15355ec9516a44c94d609ba22c29e77585b Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 27 Sep 2024 15:45:53 +0200 Subject: [PATCH 4/6] fixed constraint ominwheel working on local velocity --- src/constraints/velocity/OmniWheels4X.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp index 55f39d12..76b5d518 100644 --- a/src/constraints/velocity/OmniWheels4X.cpp +++ b/src/constraints/velocity/OmniWheels4X.cpp @@ -71,6 +71,8 @@ void OmniWheels4X::update() { // _robot.getPose(_base_link, _w_T_b); // _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); + + //todo: here we assumes velocity of the floating base are in local frame! _Aineq.rightCols(_x_size-6).noalias() = _J.rightCols(_x_size-6); } From e40997958d06a3c8ee85680629494c005b8f0fc0 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 10 Oct 2024 14:53:49 +0200 Subject: [PATCH 5/6] Added setter/getter of flag for specify if velocity is in local or global for the Moniwheel4X constraint --- bindings/python/constraints/velocity.hpp | 6 ++++-- include/OpenSoT/constraints/velocity/OmniWheels4X.h | 13 +++++++++++++ src/constraints/velocity/OmniWheels4X.cpp | 10 +++++----- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/bindings/python/constraints/velocity.hpp b/bindings/python/constraints/velocity.hpp index 19806b3c..b59665b4 100644 --- a/bindings/python/constraints/velocity.hpp +++ b/bindings/python/constraints/velocity.hpp @@ -26,13 +26,15 @@ void pyVelocityLimits(py::module& m) { .def("setVelocityLimits", py::overload_cast(&VelocityLimits::setVelocityLimits)) .def("setVelocityLimits", py::overload_cast(&VelocityLimits::setVelocityLimits)) .def("getDT", &VelocityLimits::getDT) - .def("update", &VelocityLimits::update);; + .def("update", &VelocityLimits::update); } void pyVelocityOmniWheels4X(py::module& m) { py::class_, OpenSoT::Constraint>(m, "OmniWheels4X") .def(py::init, const std::string, XBot::ModelInterface&>()) - .def("update", &OmniWheels4X::update); + .def("update", &OmniWheels4X::update) + .def("setIsGlobalVelocity", &OmniWheels4X::setIsGlobalVelocity) + .def("getIsGlobalVelocity", &OmniWheels4X::getIsGlobalVelocity); } diff --git a/include/OpenSoT/constraints/velocity/OmniWheels4X.h b/include/OpenSoT/constraints/velocity/OmniWheels4X.h index e313a9c5..9d41eb4b 100644 --- a/include/OpenSoT/constraints/velocity/OmniWheels4X.h +++ b/include/OpenSoT/constraints/velocity/OmniWheels4X.h @@ -75,12 +75,25 @@ namespace OpenSoT { */ virtual void update(); + /** + * @brief setIsGlobalVelocity set the flag to consider the velocity in the global frame + * @param is_global_velocity default is false + */ + void setIsGlobalVelocity(bool is_global_velocity) { _is_global_velocity = is_global_velocity; } + + /** + * @brief getIsGlobalVelocity get the flag to consider the velocity in the global frame + * @return true or false + */ + bool getIsGlobalVelocity() const { return _is_global_velocity; } + private: XBot::ModelInterface& _robot; Eigen::MatrixXd _J; Eigen::Affine3d _w_T_b; const std::string _base_link; + bool _is_global_velocity; }; } diff --git a/src/constraints/velocity/OmniWheels4X.cpp b/src/constraints/velocity/OmniWheels4X.cpp index 76b5d518..9bf34dcb 100644 --- a/src/constraints/velocity/OmniWheels4X.cpp +++ b/src/constraints/velocity/OmniWheels4X.cpp @@ -23,7 +23,7 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r, const std::vector joint_wheels_name, const std::string base_link, XBot::ModelInterface &robot): - Constraint("OmniWheels4X", robot.getNv()), _robot(robot), _base_link(base_link) + Constraint("OmniWheels4X", robot.getNv()), _robot(robot), _base_link(base_link), _is_global_velocity(false) { _J.resize(3, _x_size); _J.setZero(); @@ -69,11 +69,11 @@ OmniWheels4X::OmniWheels4X(const double l1, const double l2, const double r, void OmniWheels4X::update() { - // _robot.getPose(_base_link, _w_T_b); - // _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); + _w_T_b.setIdentity(); - //todo: here we assumes velocity of the floating base are in local frame! - _Aineq.rightCols(_x_size-6).noalias() = _J.rightCols(_x_size-6); + if(_is_global_velocity) + _robot.getPose(_base_link, _w_T_b); + _Aineq.rightCols(_x_size-6).noalias() = _w_T_b.linear() * _J.rightCols(_x_size-6); } From a9fa3d91bc3aececfdf93a19af00f99503b99627 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Thu, 10 Oct 2024 16:36:18 +0200 Subject: [PATCH 6/6] Added _velocity_refs_are_local = false; at every setReference where velocity is set. --- src/tasks/velocity/Cartesian.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/tasks/velocity/Cartesian.cpp b/src/tasks/velocity/Cartesian.cpp index e05c2ef3..cfdcbbdc 100644 --- a/src/tasks/velocity/Cartesian.cpp +++ b/src/tasks/velocity/Cartesian.cpp @@ -149,6 +149,7 @@ void Cartesian::setReference(const KDL::Frame& desiredPose) void Cartesian::setReference(const Eigen::Affine3d& desiredPose, const Eigen::Vector6d& desiredTwist) { + _velocity_refs_are_local = false; _desiredPose = desiredPose; _desiredTwist = desiredTwist; _desiredTwistRef = _desiredTwist; @@ -158,6 +159,7 @@ void Cartesian::setReference(const Eigen::Affine3d& desiredPose, void Cartesian::setReference(const Eigen::Matrix4d &desiredPose, const Eigen::Vector6d &desiredTwist) { + _velocity_refs_are_local = false; _desiredPose.matrix() = desiredPose; _desiredTwist = desiredTwist; _desiredTwistRef = _desiredTwist; @@ -167,6 +169,7 @@ void Cartesian::setReference(const Eigen::Matrix4d &desiredPose, void Cartesian::setReference(const KDL::Frame& desiredPose, const KDL::Twist& desiredTwist) { + _velocity_refs_are_local = false; _desiredPose(0,3) = desiredPose.p.x(); _desiredPose(1,3) = desiredPose.p.y(); _desiredPose(2,3) = desiredPose.p.z();