Fix admittance task

.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 }}

bindings/python/tasks/velocity.hpp

@@ -70,9 +70,13 @@ void pyVelocityCartesianAdmittance(py::module & m) {
     py::class_<CartesianAdmittance, std::shared_ptr<CartesianAdmittance>, Cartesian>(m, "CartesianAdmittance")
         .def(py::init<std::string, XBot::ModelInterface&, std::string, XBot::ForceTorqueSensor::ConstPtr>())
         .def("getCartesianCompliance",py::overload_cast<>(&CartesianAdmittance::getCartesianCompliance))
+        .def("getFilterOmega",&CartesianAdmittance::getFilterOmega)
+        .def("getFilterTimeStep",&CartesianAdmittance::getFilterTimeStep)
+        .def("getLambda", &CartesianAdmittance::getLambda)
         .def("setWrenchReference", py::overload_cast<const Eigen::Vector6d&>(&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)

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

src/tasks/velocity/CartesianAdmittance.cpp

@@ -109,21 +109,44 @@ OpenSoT::tasks::velocity::CartesianAdmittance::Ptr CartesianAdmittance::asCartes
     return std::dynamic_pointer_cast<OpenSoT::tasks::velocity::CartesianAdmittance>(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++)