fixed bindings operators

ADVRHumanoids · May 9, 2024 · f497579 · f497579
1 parent be91104
commit f497579
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 8 deletions.
diff --git a/bindings/python/autostack.hpp b/bindings/python/autostack.hpp
@@ -8,7 +8,7 @@ using namespace OpenSoT;
 
 void pyAutostack(py::module& m) {
     // Expose AutoStack class
-    py::class_<AutoStack, AutoStack::Ptr>(m, "AutoStack")
+    py::class_<AutoStack, std::shared_ptr<AutoStack>>(m, "AutoStack")
         .def(py::init<const int>())
         .def(py::init<OpenSoT::tasks::Aggregated::TaskPtr>())
         .def(py::init<OpenSoT::tasks::Aggregated::TaskPtr, std::list<OpenSoT::constraints::Aggregated::ConstraintPtr>>())
@@ -23,8 +23,15 @@ void pyAutostack(py::module& m) {
         .def("getRegularisationTask", &AutoStack::getRegularisationTask)
         .def("setBoundsAggregationPolicy", &AutoStack::setBoundsAggregationPolicy)
         .def("getBounds", &AutoStack::getBounds)
-        .def("getTask", &AutoStack::getTask);
-
+        .def("getTask", &AutoStack::getTask)
+        .def("__lshift__", [](OpenSoT::AutoStack::Ptr stack, OpenSoT::constraints::Aggregated::ConstraintPtr bound) {
+            return stack << bound;})
+        .def("__lshift__", [](OpenSoT::AutoStack::Ptr stack, OpenSoT::tasks::Aggregated::TaskPtr constraint) {
+            return stack << constraint;})
+        .def("__truediv__", [](const OpenSoT::AutoStack::Ptr stack, const OpenSoT::tasks::Aggregated::TaskPtr task ) {
+            return stack / task;})
+        .def("__truediv__", [](OpenSoT::AutoStack::Ptr stack1, OpenSoT::AutoStack::Ptr stack2) {
+            return stack1 / stack2;});
 
     m.def("mul", [](const Eigen::MatrixXd& W, OpenSoT::tasks::Aggregated::TaskPtr task) {
         return W * task;

diff --git a/bindings/python/basic.hpp b/bindings/python/basic.hpp
@@ -1,8 +1,10 @@
 #include <pybind11/pybind11.h>
 #include <pybind11/eigen.h>
+#include <pybind11/numpy.h>
 #include <pybind11/stl.h>
 #include <OpenSoT/Task.h>
 #include <OpenSoT/Constraint.h>
+#include <OpenSoT/utils/AutoStack.h>
 
 using namespace OpenSoT;
 namespace py = pybind11;
@@ -47,7 +49,26 @@ void pyTask(py::module& m, const std::string& className) {
         .def("setActiveJointsMask", &Task<MatrixType, VectorType>::setActiveJointsMask)
         .def("log", &Task<MatrixType, VectorType>::log)
         .def("computeCost", &Task<MatrixType, VectorType>::computeCost)
-        .def("checkConsistency", &Task<MatrixType, VectorType>::checkConsistency);
+        .def("checkConsistency", &Task<MatrixType, VectorType>::checkConsistency)
+        .def("__add__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task1, const std::shared_ptr<Task<MatrixType, VectorType>> task2) {
+            return task1 + task2;})
+        .def("__mod__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task, const std::list<unsigned int>& rowIndices) {
+            return task % rowIndices;})
+        .def("__rmul__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task, const float& w) {
+            return w * task;})
+        .def("__truediv__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task1,
+             const std::shared_ptr<Task<MatrixType, VectorType>> task2) {
+            return task1 / task2;})
+        .def("__truediv__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task, const OpenSoT::AutoStack::Ptr stack) {
+            return task / stack;})
+        .def("__lshift__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task, const std::shared_ptr<Constraint<MatrixType, VectorType>> constraint) {
+            return task << constraint;})
+        .def("__lshift__", [](const std::shared_ptr<Task<MatrixType, VectorType>> task1,
+             const std::shared_ptr<Task<MatrixType, VectorType>> task2) {
+            return task1 << task2;
+    });
+
+
 }
 
 template<typename MatrixType, typename VectorType>
@@ -72,7 +93,9 @@ void pyConstraint(py::module& m, const std::string& className) {
             .def("getConstraintID", &Constraint<MatrixType, VectorType>::getConstraintID)
             .def("update", &Constraint<MatrixType, VectorType>::update)
             .def("log", &Constraint<MatrixType, VectorType>::log)
-            .def("checkConsistency", &Constraint<MatrixType, VectorType>::checkConsistency);
+            .def("checkConsistency", &Constraint<MatrixType, VectorType>::checkConsistency)
+            .def("__mod__", [](const std::shared_ptr<Constraint<MatrixType, VectorType>> constraint, const std::list<unsigned int>& rowIndices) {
+                return constraint % rowIndices;});
 }
 
 

diff --git a/bindings/python/constraints/force.hpp b/bindings/python/constraints/force.hpp
@@ -16,7 +16,6 @@ void pyForceCoP(py::module& m) {
 
     py::class_<CoPs, std::shared_ptr<CoPs>, OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CoPs")
         .def(py::init<const std::vector<AffineHelper>&, const std::vector<std::string>&, XBot::ModelInterface&, const std::vector<Eigen::Vector2d>&, const std::vector<Eigen::Vector2d>&>())
-        .def("getCoP", &CoPs::getCoP)
         .def("update", &CoPs::update);
 }
 

diff --git a/include/OpenSoT/constraints/force/CoP.h b/include/OpenSoT/constraints/force/CoP.h
@@ -85,8 +85,6 @@ namespace OpenSoT {
                 const std::vector<Eigen::Vector2d>& X_Lims,
                 const std::vector<Eigen::Vector2d>& Y_Lims);
 
-           CoP::Ptr getCoP(const std::string& contact_name);
-
            void update();