binded AngularMomentum and Com (velocity). Small fixes in task files

ADVRHumanoids · May 2, 2024 · aa71a95 · aa71a95
1 parent 2bb8d32
commit aa71a95
Show file tree

Hide file tree

Showing 5 changed files with 46 additions and 3 deletions.
diff --git a/bindings/python/pyopensot.cpp b/bindings/python/pyopensot.cpp
@@ -31,6 +31,8 @@ PYBIND11_MODULE(pyopensot, m) {
     auto m_tv = m_t.def_submodule("velocity");
     pyVelocityPostural(m_tv);
     pyVelocityCartesian(m_tv);
+    pyVelocityAngularMomentum(m_tv);
+    pyVelocityCoM(m_tv);
 
     auto m_c = m.def_submodule("constraints");
 

diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp
@@ -3,6 +3,8 @@
 #include <pybind11/stl.h>
 #include <OpenSoT/tasks/velocity/Postural.h>
 #include <OpenSoT/tasks/velocity/Cartesian.h>
+#include <OpenSoT/tasks/velocity/AngularMomentum.h>
+#include <OpenSoT/tasks/velocity/CoM.h>
 
 namespace py = pybind11;
 using namespace OpenSoT::tasks::velocity;
@@ -52,4 +54,36 @@ void pyVelocityCartesian(py::module& m) {
           .def_property_readonly("baseLinkIsWorld", &Cartesian::baseLinkIsWorld);
 }
 
+void pyVelocityAngularMomentum(py::module& m) {
+py::class_<AngularMomentum, std::shared_ptr<AngularMomentum>, Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "AngularMomentum")
+        .def(py::init<XBot::ModelInterface&>())
+        .def("setReference", py::overload_cast<const Eigen::Vector3d&>(&AngularMomentum::setReference))
+        .def("getReference", py::overload_cast<Eigen::Vector3d&>(&AngularMomentum::getReference, py::const_))
+        .def("getBaseLink", &AngularMomentum::getBaseLink)
+        .def("getDistalLink", &AngularMomentum::getDistalLink);
+}
+
+std::tuple<Eigen::Vector3d, Eigen::Vector3d> com_get_reference(const CoM& com)
+{
+    Eigen::Vector3d pos_ref, vel_ref;
+    com.getReference(pos_ref, vel_ref);
+    return std::make_tuple(pos_ref, vel_ref);
+}
+
+void pyVelocityCoM(py::module& m) {
+    py::class_<CoM, std::shared_ptr<CoM>,  OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CoM")
+        .def(py::init<XBot::ModelInterface&, const std::string&>(),
+             py::arg(), py::arg("id") = "CoM")
+        .def("setReference", py::overload_cast<const Eigen::Vector3d&>(&CoM::setReference))
+        .def("setReference", py::overload_cast<const Eigen::Vector3d&, const Eigen::Vector3d&>(&CoM::setReference))
+        .def("getReference", com_get_reference)
+        .def("getCachedVelocityReference", &CoM::getCachedVelocityReference, py::return_value_policy::reference)
+        .def("getActualPosition", &CoM::getActualPosition, py::return_value_policy::reference)
+        .def("getBaseLink", &CoM::getBaseLink, py::return_value_policy::reference)
+        .def("getDistalLink", &CoM::getDistalLink, py::return_value_policy::reference)
+        .def("setLambda", &CoM::setLambda)
+        .def("getError", &CoM::getError, py::return_value_policy::reference)
+        .def("reset", &CoM::reset);
+}
+
 
diff --git a/include/OpenSoT/tasks/velocity/AngularMomentum.h b/include/OpenSoT/tasks/velocity/AngularMomentum.h
@@ -76,6 +76,7 @@ namespace OpenSoT {
             */
            void getReference(Eigen::Vector3d& desiredAngularMomentum) const;
            void getReference(KDL::Vector& desiredAngularMomentum) const;
+           const Eigen::Vector3d& getReference() const;
 
            /**
             * @brief getBaseLink

diff --git a/include/OpenSoT/tasks/velocity/CoM.h b/include/OpenSoT/tasks/velocity/CoM.h
@@ -54,6 +54,10 @@
                 std::string _base_link;
                 std::string _distal_link;
 
+                virtual void _log(XBot::MatLogger2::Ptr logger);
+                virtual void _update();
+
+
             public:
 
 
@@ -69,7 +73,6 @@
 
                 ~CoM();
 
-                virtual void _update();
 
                 /**
                  * @brief setReference sets a new reference for the CoM task.
@@ -156,8 +159,6 @@
                  */
                 bool reset();
 
-                virtual void _log(XBot::MatLogger2::Ptr logger);
-
                 static bool isCoM(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task);
 
                 static OpenSoT::tasks::velocity::CoM::Ptr asCoM(OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>::TaskPtr task);

diff --git a/src/tasks/velocity/AngularMomentum.cpp b/src/tasks/velocity/AngularMomentum.cpp
@@ -67,6 +67,11 @@ void AngularMomentum::getReference(Eigen::Vector3d& desiredAngularMomentum) cons
     desiredAngularMomentum = _desiredAngularMomentum;
 }
 
+const Eigen::Vector3d& AngularMomentum::getReference() const
+{
+    return _desiredAngularMomentum;
+}
+
 void AngularMomentum::getReference(KDL::Vector& desiredAngularMomentum) const
 {
     desiredAngularMomentum[0] = _desiredAngularMomentum[0];