Added more bindings related to tasks::velocity

ADVRHumanoids · May 3, 2024 · 18aea35 · 18aea35
1 parent aa71a95
commit 18aea35
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 16 deletions.
diff --git a/bindings/python/pyopensot.cpp b/bindings/python/pyopensot.cpp
@@ -33,6 +33,9 @@ PYBIND11_MODULE(pyopensot, m) {
     pyVelocityCartesian(m_tv);
     pyVelocityAngularMomentum(m_tv);
     pyVelocityCoM(m_tv);
+    pyVelocityGaze(m_tv);
+    pyVelocityManipulability(m_tv);
+    pyVelocityMinimumEffort(m_tv);
 
     auto m_c = m.def_submodule("constraints");
 

diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp
@@ -5,6 +5,9 @@
 #include <OpenSoT/tasks/velocity/Cartesian.h>
 #include <OpenSoT/tasks/velocity/AngularMomentum.h>
 #include <OpenSoT/tasks/velocity/CoM.h>
+#include <OpenSoT/tasks/velocity/Gaze.h>
+#include <OpenSoT/tasks/velocity/Manipulability.h>
+#include <OpenSoT/tasks/velocity/MinimumEffort.h>
 
 namespace py = pybind11;
 using namespace OpenSoT::tasks::velocity;
@@ -55,7 +58,7 @@ void pyVelocityCartesian(py::module& m) {
 }
 
 void pyVelocityAngularMomentum(py::module& m) {
-py::class_<AngularMomentum, std::shared_ptr<AngularMomentum>, Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "AngularMomentum")
+    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_))
@@ -71,7 +74,7 @@ std::tuple<Eigen::Vector3d, Eigen::Vector3d> com_get_reference(const CoM& com)
 }
 
 void pyVelocityCoM(py::module& m) {
-    py::class_<CoM, std::shared_ptr<CoM>,  OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CoM")
+    py::class_<CoM, std::shared_ptr<CoM>,  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))
@@ -86,4 +89,41 @@ void pyVelocityCoM(py::module& m) {
         .def("reset", &CoM::reset);
 }
 
+void pyVelocityGaze(py::module& m) {
+    py::class_<Gaze, std::shared_ptr<Gaze>, Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "Gaze")
+        .def(py::init<std::string, XBot::ModelInterface&, std::string, std::string>())
+        .def("setGaze", py::overload_cast<const Eigen::Affine3d&>(&Gaze::setGaze))
+        .def("setOrientationErrorGain", &Gaze::setOrientationErrorGain)
+        .def("getOrientationErrorGain", &Gaze::getOrientationErrorGain)
+        .def("setWeight", &Gaze::setWeight)
+        .def("getConstraints", &Gaze::getConstraints, py::return_value_policy::reference)
+        .def("getTaskSize", &Gaze::getTaskSize)
+        .def("getActiveJointsMask", &Gaze::getActiveJointsMask)
+        .def("setActiveJointsMask", &Gaze::setActiveJointsMask)
+        .def("getDistalLink", &Gaze::getDistalLink)
+        .def("setLambda", &Gaze::setLambda)
+        .def("setBaseLink", &Gaze::setBaseLink);
+}
+
+void pyVelocityManipulability(py::module& m) {
+    py::class_<Manipulability, std::shared_ptr<Manipulability>, Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "Manipulability")
+        .def(py::init<const XBot::ModelInterface&, const Cartesian::Ptr, const double>(),
+             py::arg(), py::arg(), py::arg("step") = 1E-3)
+        .def(py::init<const XBot::ModelInterface&, const CoM::Ptr, const double>(),
+             py::arg(), py::arg(), py::arg("step") = 1E-3)
+        .def("ComputeManipulabilityIndex", &Manipulability::ComputeManipulabilityIndex)
+        .def("setW", &Manipulability::setW)
+        .def("getW", &Manipulability::getW)
+        .def("setLambda", &Manipulability::setLambda);
+}
+
+void pyVelocityMinimumEffort(py::module& m) {
+    py::class_<MinimumEffort, std::shared_ptr<MinimumEffort>, Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "MinimumEffort")
+        .def(py::init<const XBot::ModelInterface&, const double>(), py::arg(), py::arg("step") = 1E-3)
+        .def("ComputeEffort", &MinimumEffort::computeEffort)
+        .def("setW", &MinimumEffort::setW)
+        .def("getW", &MinimumEffort::getW)
+        .def("setLambda", &MinimumEffort::setLambda);
+}
+
 
diff --git a/include/OpenSoT/tasks/velocity/Gaze.h b/include/OpenSoT/tasks/velocity/Gaze.h
@@ -86,10 +86,6 @@ class Gaze: public OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>
         @return the number of rows of A */
     virtual const unsigned int getTaskSize() const;
 
-    /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
-        @param x variable state at the current step (input) */
-    virtual void _update();
-
     /**
      * @brief getActiveJointsMask return a vector of length NumberOfDOFs.
      * If an element is false the corresponding column of the task jacobian is set to 0.
@@ -138,6 +134,11 @@ class Gaze: public OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>
     Eigen::Affine3d _tmpEigenM;
     Eigen::Affine3d _tmpEigenM2;
 
+    /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices
+        @param x variable state at the current step (input) */
+    virtual void _update();
+
+
 
 
 };

diff --git a/include/OpenSoT/tasks/velocity/Manipulability.h b/include/OpenSoT/tasks/velocity/Manipulability.h
@@ -47,8 +47,6 @@ namespace OpenSoT {
 
                 ~Manipulability();
 
-                void _update();
-
                 /**
                  * @brief ComputeManipulabilityIndex
                  * @return the manipulability index at the actual configuration q (ast from latest update(q))
@@ -82,6 +80,7 @@ namespace OpenSoT {
                 const XBot::ModelInterface& _model;
                 Eigen::VectorXd _q;
 
+                void _update();
 
                 /**
                  * @brief The ComputeManipulabilityIndexGradient class implements a worker class to computes

diff --git a/include/OpenSoT/tasks/velocity/MinimumEffort.h b/include/OpenSoT/tasks/velocity/MinimumEffort.h
@@ -44,6 +44,16 @@
             protected:
                 const XBot::ModelInterface& _model;
                 Eigen::VectorXd _q;
+
+                /**
+                 * @brief _update updates the minimum effort gradient.
+                 * @detail It also updates the state on the internal robot model so that successive calls to the
+                 * computeEffort() function will take into account the updated posture of the robot.
+                 * @param x the actual posture of the robot
+                 */
+                void _update();
+
+
                 /**
                  * @brief The ComputeGTauGradient class implements a worker class to computes the effort for a certain configuration.
                  * It will take into account the robot as standing on a flat floor, and while computing the gradient,
@@ -101,14 +111,6 @@
 
                 ~MinimumEffort();
 
-                /**
-                 * @brief _update updates the minimum effort gradient.
-                 * @detail It also updates the state on the internal robot model so that successive calls to the
-                 * computeEffort() function will take into account the updated posture of the robot.
-                 * @param x the actual posture of the robot
-                 */
-                void _update();
-
                 /**
                  * @brief computeEffort
                  * @return the effort at the actual configuration q (ast from latest update(q))