From 3625554298c13186a9a0fac1b3c311e0a199c01c Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Tue, 15 Apr 2025 08:04:57 +0000 Subject: [PATCH 1/7] add admittance bindings and getters --- bindings/python/tasks/velocity.hpp | 2 ++ include/OpenSoT/tasks/velocity/CartesianAdmittance.h | 6 ++++++ src/tasks/velocity/CartesianAdmittance.cpp | 5 +++++ 3 files changed, 13 insertions(+) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index bf25a47b3..3b542f145 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -70,6 +70,8 @@ void pyVelocityCartesianAdmittance(py::module & m) { py::class_, Cartesian>(m, "CartesianAdmittance") .def(py::init()) .def("getCartesianCompliance",py::overload_cast<>(&CartesianAdmittance::getCartesianCompliance)) + .def("getFilterOmega",py::overload_cast<>(&CartesianAdmittance::getFilterOmega)) + .def("getFilterTimeStep",py::overload_cast<>(&CartesianAdmittance::getFilterTimeStep)) .def("setWrenchReference", py::overload_cast(&CartesianAdmittance::setWrenchReference)) .def("getWrenchReference", py::overload_cast<>(&CartesianAdmittance::getWrenchReference)) .def("setFilterDamping", &CartesianAdmittance::setFilterDamping) diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h index cf6e1c95e..319f2dda9 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -274,6 +274,12 @@ namespace OpenSoT { XBot::ForceTorqueSensor::ConstPtr ft_sensor); /** + * @brief getFilterOmega + * @return the filter omega vector + */ + const Eigen::Vector6d& getFilterOmega(); + + /** * @brief getCartesianCompliance * @return the actual Compliance matrix */ diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index 2066d35b6..1589e2633 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -109,6 +109,11 @@ OpenSoT::tasks::velocity::CartesianAdmittance::Ptr CartesianAdmittance::asCartes return std::dynamic_pointer_cast(task); } +const Eigen::Vector6d& CartesianAdmittance::getFilterOmega() +{ + return _w; +} + const Eigen::Matrix6d& CartesianAdmittance::getCartesianCompliance() { _tmp_mat6 = _C.asDiagonal(); From 7e62fecafb9076d63576d705da824e1c346c97ed Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Tue, 15 Apr 2025 11:23:54 +0000 Subject: [PATCH 2/7] expose lambda and fix getFilterTimeStep --- bindings/python/tasks/velocity.hpp | 1 + .../tasks/velocity/CartesianAdmittance.h | 6 +++++ src/tasks/velocity/CartesianAdmittance.cpp | 22 +++++++++++++++++-- 3 files changed, 27 insertions(+), 2 deletions(-) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 3b542f145..13102cb3a 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -74,6 +74,7 @@ void pyVelocityCartesianAdmittance(py::module & m) { .def("getFilterTimeStep",py::overload_cast<>(&CartesianAdmittance::getFilterTimeStep)) .def("setWrenchReference", py::overload_cast(&CartesianAdmittance::setWrenchReference)) .def("getWrenchReference", py::overload_cast<>(&CartesianAdmittance::getWrenchReference)) + .def("getLambda", py::overload_cast<>(&CartesianAdmittance::getLambda)) .def("setFilterDamping", &CartesianAdmittance::setFilterDamping) .def("setImpedanceParams", &CartesianAdmittance::setImpedanceParams) .def("setRawParams",&CartesianAdmittance::setRawParams) diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h index 319f2dda9..639e6d67f 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -351,6 +351,12 @@ namespace OpenSoT { const Eigen::Matrix6d getDamping(); double getFilterTimeStep(); + + /** + * @brief getLambda + * @return lambda of Cartesian Task + */ + double getLambda(); void setFilterDamping(const double damping); diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index 1589e2633..46f6602fb 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -120,6 +120,11 @@ const Eigen::Matrix6d& CartesianAdmittance::getCartesianCompliance() return _tmp_mat6; } +double CartesianAdmittance::getLambda() +{ + return _lambda; +} + void CartesianAdmittance::getCartesianCompliance(Eigen::Matrix6d& C) { C = _C.asDiagonal(); @@ -128,7 +133,21 @@ void CartesianAdmittance::getCartesianCompliance(Eigen::Matrix6d& C) double CartesianAdmittance::getFilterTimeStep() { - return _dt; + int channel = 0; + for(unsigned int i = 1; i < _filter.getNumberOfChannels(); ++i) + { + if(_filter.getTimeStep(i) != _filter.getTimeStep(channel)) + { + XBot::Logger::error("Filter time step is not the same for all channels! \nChannel %d has time step %f, while channel %d has time step %f\n", i, _filter.getTimeStep(i), channel, _filter.getTimeStep(channel)); + return -1; + } + } + if(_filter.getTimeStep(channel) != _dt) + { + XBot::Logger::error("Filter time step is not equal to Admittance Task dt! \nFilter time step: %f, Admittance Task dt: %f\n", _filter.getTimeStep(channel), _dt); + return -1; + } + return _filter.getTimeStep(channel); } void CartesianAdmittance::setFilterDamping(const double damping) @@ -289,7 +308,6 @@ bool OpenSoT::tasks::velocity::CartesianAdmittance::setRawParams(const Eigen::Ve _w = omega; _dt = dt; - return true; } From 7ae04c3db70b7be662788704ccc8792c70faf7d2 Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Tue, 15 Apr 2025 12:06:26 +0000 Subject: [PATCH 3/7] fix ImpedanceParams and expose setFilterTimeStep --- bindings/python/tasks/velocity.hpp | 1 + .../OpenSoT/tasks/velocity/CartesianAdmittance.h | 4 +++- src/tasks/velocity/CartesianAdmittance.cpp | 16 ++++++++++------ 3 files changed, 14 insertions(+), 7 deletions(-) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 13102cb3a..53a07660b 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -76,6 +76,7 @@ void pyVelocityCartesianAdmittance(py::module & m) { .def("getWrenchReference", py::overload_cast<>(&CartesianAdmittance::getWrenchReference)) .def("getLambda", py::overload_cast<>(&CartesianAdmittance::getLambda)) .def("setFilterDamping", &CartesianAdmittance::setFilterDamping) + .def("setFilterTimeStep", &CartesianAdmittance::setFilterTimeStep) .def("setImpedanceParams", &CartesianAdmittance::setImpedanceParams) .def("setRawParams",&CartesianAdmittance::setRawParams) .def("setDeadZone",&CartesianAdmittance::setDeadZone) diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h index 639e6d67f..00e9eba02 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -352,13 +352,15 @@ namespace OpenSoT { double getFilterTimeStep(); + void setFilterTimeStep(const double time_step); + /** * @brief getLambda * @return lambda of Cartesian Task */ double getLambda(); - void setFilterDamping(const double damping); + void setFilterDamping(const double damping); /** * @brief Set impedance parameters to be emulated via admittance control. diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index 46f6602fb..dc2c5ef07 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -262,6 +262,7 @@ void CartesianAdmittance::setImpedanceParams(const Eigen::Vector6d& K, _D = D; _K = K; _dt = dt; + setFilterTimeStep(_dt); // should be added here - probably ommited by mistake setFilterOmega(_w); } } @@ -293,12 +294,9 @@ bool OpenSoT::tasks::velocity::CartesianAdmittance::setRawParams(const Eigen::Ve } _C = C; - - for(unsigned int i = 0; i < _filter.getNumberOfChannels(); ++i) - { - _filter.setTimeStep(dt, i); - _filter.setOmega(omega[i], i); - } + + setFilterTimeStep(dt); + setFilterOmega(omega); _lambda = lambda; @@ -317,6 +315,12 @@ void CartesianAdmittance::setFilterOmega(const Eigen::Vector6d& w) _filter.setOmega(w[i], i); } +void CartesianAdmittance::setFilterTimeStep(const double dt) +{ + for(unsigned int i = 0; i < CHANNELS; ++i) + _filter.setTimeStep(dt, i); +} + void CartesianAdmittance::apply_deadzone(Eigen::Vector6d& data) { for(int i = 0; i < 6; i++) From 554e75bb365c2578cf25fc28b4f80620a2dfcd90 Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Wed, 16 Apr 2025 11:20:02 +0000 Subject: [PATCH 4/7] fix bindings and comments --- bindings/python/tasks/velocity.hpp | 6 +++--- src/tasks/velocity/CartesianAdmittance.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 53a07660b..59a4354cc 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -70,11 +70,11 @@ void pyVelocityCartesianAdmittance(py::module & m) { py::class_, Cartesian>(m, "CartesianAdmittance") .def(py::init()) .def("getCartesianCompliance",py::overload_cast<>(&CartesianAdmittance::getCartesianCompliance)) - .def("getFilterOmega",py::overload_cast<>(&CartesianAdmittance::getFilterOmega)) - .def("getFilterTimeStep",py::overload_cast<>(&CartesianAdmittance::getFilterTimeStep)) + .def("getFilterOmega",&CartesianAdmittance::getFilterOmega) + .def("getFilterTimeStep",&CartesianAdmittance::getFilterTimeStep) + .def("getLambda", &CartesianAdmittance::getLambda) .def("setWrenchReference", py::overload_cast(&CartesianAdmittance::setWrenchReference)) .def("getWrenchReference", py::overload_cast<>(&CartesianAdmittance::getWrenchReference)) - .def("getLambda", py::overload_cast<>(&CartesianAdmittance::getLambda)) .def("setFilterDamping", &CartesianAdmittance::setFilterDamping) .def("setFilterTimeStep", &CartesianAdmittance::setFilterTimeStep) .def("setImpedanceParams", &CartesianAdmittance::setImpedanceParams) diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index dc2c5ef07..d305db45a 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -262,7 +262,7 @@ void CartesianAdmittance::setImpedanceParams(const Eigen::Vector6d& K, _D = D; _K = K; _dt = dt; - setFilterTimeStep(_dt); // should be added here - probably ommited by mistake + setFilterTimeStep(_dt); setFilterOmega(_w); } } From 6e6233e28a9d5a8fd11fb60927c9760f7a316e52 Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Wed, 16 Apr 2025 11:34:40 +0000 Subject: [PATCH 5/7] update CI --- .github/workflows/sphinx.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/sphinx.yml b/.github/workflows/sphinx.yml index de0f3d729..6d1b3c56d 100644 --- a/.github/workflows/sphinx.yml +++ b/.github/workflows/sphinx.yml @@ -9,7 +9,7 @@ jobs: - name: Doxygen:Install run: sudo apt-get install -y libboost-all-dev libeigen3-dev doxygen - name: Doxygen:CheckOut - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Doxygen:cmake continue-on-error: true run: | @@ -20,12 +20,12 @@ jobs: - name: Sphinx:BuildHTML uses: ammaraskar/sphinx-action@master - name: Sphinx:UploadArtifacts - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: html-docs path: docs/build/html/ - name: Sphinx:Deploy - uses: peaceiris/actions-gh-pages@v3 + uses: peaceiris/actions-gh-pages@v4 if: github.ref == 'refs/heads/4.0-devel' with: github_token: ${{ secrets.GITHUB_TOKEN }} From a68d70a42914ad99892363fe13ec722d37c0afc0 Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Wed, 16 Apr 2025 12:07:11 +0000 Subject: [PATCH 6/7] fix indentation --- include/OpenSoT/tasks/velocity/CartesianAdmittance.h | 10 +++++----- src/tasks/velocity/CartesianAdmittance.cpp | 5 ----- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h index 00e9eba02..751b05b59 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -279,7 +279,7 @@ namespace OpenSoT { */ const Eigen::Vector6d& getFilterOmega(); - /** + /** * @brief getCartesianCompliance * @return the actual Compliance matrix */ @@ -352,15 +352,15 @@ namespace OpenSoT { double getFilterTimeStep(); - void setFilterTimeStep(const double time_step); + void setFilterTimeStep(const double time_step); - /** + /** * @brief getLambda * @return lambda of Cartesian Task */ - double getLambda(); + double getLambda(); - void setFilterDamping(const double damping); + void setFilterDamping(const double damping); /** * @brief Set impedance parameters to be emulated via admittance control. diff --git a/src/tasks/velocity/CartesianAdmittance.cpp b/src/tasks/velocity/CartesianAdmittance.cpp index d305db45a..a01d860fb 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -130,7 +130,6 @@ void CartesianAdmittance::getCartesianCompliance(Eigen::Matrix6d& C) C = _C.asDiagonal(); } - double CartesianAdmittance::getFilterTimeStep() { int channel = 0; @@ -190,8 +189,6 @@ const Eigen::Matrix6d CartesianAdmittance::getDamping() return _D.asDiagonal(); } - - bool CartesianAdmittance::computeParameters(const Eigen::Vector6d& K, const Eigen::Vector6d& D, const double lambda, @@ -222,8 +219,6 @@ bool CartesianAdmittance::computeParameters(const Eigen::Vector6d& K, return true; } - - void CartesianAdmittance::setImpedanceParams(const Eigen::Vector6d& K, const Eigen::Vector6d& D, const double lambda, From c25ca242dbba102abe8df0b47c9ec00b57c75838 Mon Sep 17 00:00:00 2001 From: Anna Tuma Date: Wed, 16 Apr 2025 12:08:26 +0000 Subject: [PATCH 7/7] fix indentation --- include/OpenSoT/tasks/velocity/CartesianAdmittance.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h index 751b05b59..179612154 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -277,7 +277,7 @@ namespace OpenSoT { * @brief getFilterOmega * @return the filter omega vector */ - const Eigen::Vector6d& getFilterOmega(); + const Eigen::Vector6d& getFilterOmega(); /** * @brief getCartesianCompliance