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 }} diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index bf25a47b3..59a4354cc 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -70,9 +70,13 @@ void pyVelocityCartesianAdmittance(py::module & m) { py::class_, Cartesian>(m, "CartesianAdmittance") .def(py::init()) .def("getCartesianCompliance",py::overload_cast<>(&CartesianAdmittance::getCartesianCompliance)) + .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("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 cf6e1c95e..179612154 100644 --- a/include/OpenSoT/tasks/velocity/CartesianAdmittance.h +++ b/include/OpenSoT/tasks/velocity/CartesianAdmittance.h @@ -273,6 +273,12 @@ namespace OpenSoT { std::string base_link, XBot::ForceTorqueSensor::ConstPtr ft_sensor); + /** + * @brief getFilterOmega + * @return the filter omega vector + */ + const Eigen::Vector6d& getFilterOmega(); + /** * @brief getCartesianCompliance * @return the actual Compliance matrix @@ -345,6 +351,14 @@ namespace OpenSoT { const Eigen::Matrix6d getDamping(); double getFilterTimeStep(); + + void setFilterTimeStep(const double time_step); + + /** + * @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 2066d35b6..a01d860fb 100644 --- a/src/tasks/velocity/CartesianAdmittance.cpp +++ b/src/tasks/velocity/CartesianAdmittance.cpp @@ -109,21 +109,44 @@ 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(); return _tmp_mat6; } +double CartesianAdmittance::getLambda() +{ + return _lambda; +} + void CartesianAdmittance::getCartesianCompliance(Eigen::Matrix6d& C) { C = _C.asDiagonal(); } - 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) @@ -166,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, @@ -198,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, @@ -238,6 +257,7 @@ void CartesianAdmittance::setImpedanceParams(const Eigen::Vector6d& K, _D = D; _K = K; _dt = dt; + setFilterTimeStep(_dt); setFilterOmega(_w); } } @@ -269,12 +289,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; @@ -284,7 +301,6 @@ bool OpenSoT::tasks::velocity::CartesianAdmittance::setRawParams(const Eigen::Ve _w = omega; _dt = dt; - return true; } @@ -294,6 +310,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++)