Moved hcod solver outside pyopensot.cpp file in order to make binding…

…s compile when the flag OPENSOT_SOTH_FRONT_END is False. Tested hcod solver in panda_ik.example. HCOD bindings are now in pyopnesot_hcod python lib.
ADVRHumanoids · Oct 7, 2024 · 2622e6a · 2622e6a
1 parent 4dbd5be
commit 2622e6a
Show file tree

Hide file tree

Showing 6 changed files with 64 additions and 12 deletions.
diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
@@ -60,6 +60,23 @@ if(${pybind11_FOUND})
             RUNTIME DESTINATION "bin")
     endif()
 
+    if(${OPENSOT_SOTH_FRONT_END})
+        set(OPENSOT_BINDINGS_HCOD_SOURCES pyopensot_hcod.cpp solver_hcod.hpp)
+
+        include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+        pybind11_add_module(pyopensot_hcod ${OPENSOT_BINDINGS_HCOD_SOURCES})
+
+        target_link_libraries(pyopensot_hcod PUBLIC OpenSoT)
+
+        find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+
+        install(TARGETS pyopensot_hcod
+            COMPONENT python
+            LIBRARY DESTINATION "~/.local/lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
+            ARCHIVE DESTINATION "lib"
+            RUNTIME DESTINATION "bin")
+    endif()
+
 
 
 else()

diff --git a/bindings/python/examples/panda_ik.py b/bindings/python/examples/panda_ik.py
@@ -72,6 +72,9 @@
 
 # Creates iHQP solver with stack (using qpOASES as backend)
 solver = pysot.iHQP(s)
+#import pyopensot_hcod
+#solver = pyopensot_hcod.HCOD(s, 1e-3)
+
 
 # IK loop
 rate = rospy.Rate(1./dt)

diff --git a/bindings/python/pyopensot.cpp b/bindings/python/pyopensot.cpp
@@ -28,7 +28,6 @@ PYBIND11_MODULE(pyopensot, m) {
     pyeHQP(m);
     pyiHQP(m);
     pynHQP(m);
-    pyHCOD(m);
 
     auto m_t = m.def_submodule("tasks");
 

diff --git a/bindings/python/pyopensot_hcod.cpp b/bindings/python/pyopensot_hcod.cpp
@@ -0,0 +1,5 @@
+#include "solver_hcod.hpp"
+
+PYBIND11_MODULE(pyopensot_hcod, m) {
+    pyHCOD(m);
+}
diff --git a/bindings/python/solver.hpp b/bindings/python/solver.hpp
@@ -5,7 +5,6 @@
 #include <OpenSoT/solvers/eHQP.h>
 #include <OpenSoT/solvers/iHQP.h>
 #include <OpenSoT/solvers/nHQP.h>
-#include <OpenSoT/solvers/HCOD.h>
 #include <OpenSoT/solvers/BackEndFactory.h>
 
 namespace py = pybind11;
@@ -102,13 +101,3 @@ void pynHQP(py::module& m) {
        .def("setPerformSelectiveNullSpaceRegularization", py::overload_cast<bool>(&solvers::nHQP::setPerformSelectiveNullSpaceRegularization));
 }
 
-void pyHCOD(py::module& m) {
-    py::class_<solvers::HCOD, std::shared_ptr<solvers::HCOD>, OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>>(m, "HCOD")
-        .def(py::init<OpenSoT::AutoStack&, const double>())
-        .def(py::init<OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>::Stack&, OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>::ConstraintPtr, const double>())
-        .def("solve", solve<Eigen::MatrixXd, Eigen::VectorXd>)
-        .def("setDisableWeightsComputation", &solvers::HCOD::setDisableWeightsComputation)
-        .def("getDisableWeightsComputation", &solvers::HCOD::getDisableWeightsComputation)
-        .def("setDamping", &solvers::HCOD::setDamping)
-        .def("printSOT", &solvers::HCOD::printSOT);
-}
diff --git a/bindings/python/solver_hcod.hpp b/bindings/python/solver_hcod.hpp
@@ -0,0 +1,39 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>
+
+#include <OpenSoT/solvers/HCOD.h>
+
+namespace py = pybind11;
+using namespace OpenSoT;
+
+template <class MatrixType, class VectorType>
+class pySolverTrampoline : public Solver<MatrixType, VectorType> {
+public:
+    using Solver<MatrixType, VectorType>::Solver;
+    typedef Solver<MatrixType, VectorType> SMV;
+
+    bool solve(VectorType& solution) override {
+        PYBIND11_OVERLOAD_PURE(bool, SMV, solve, solution);
+    }
+};
+
+template<typename MatrixType, typename VectorType>
+VectorType solve(Solver<MatrixType, VectorType>& solver)
+{
+    VectorType solution;
+    solver.solve(solution);
+    return solution;
+}
+
+
+void pyHCOD(py::module& m) {
+    py::class_<solvers::HCOD, std::shared_ptr<solvers::HCOD>, OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>>(m, "HCOD")
+    .def(py::init<OpenSoT::AutoStack&, const double>())
+        .def(py::init<OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>::Stack&, OpenSoT::Solver<Eigen::MatrixXd, Eigen::VectorXd>::ConstraintPtr, const double>())
+        .def("solve", solve<Eigen::MatrixXd, Eigen::VectorXd>)
+        .def("setDisableWeightsComputation", &solvers::HCOD::setDisableWeightsComputation)
+        .def("getDisableWeightsComputation", &solvers::HCOD::getDisableWeightsComputation)
+        .def("setDamping", &solvers::HCOD::setDamping)
+        .def("printSOT", &solvers::HCOD::printSOT);
+}