Merge branch 'issue#216' into 4.0-devel

ADVRHumanoids · Oct 10, 2024 · 74fbec0 · 74fbec0
2 parents 3e114c4 + 2622e6a
commit 74fbec0
Show file tree

Hide file tree

Showing 9 changed files with 137 additions and 52 deletions.
diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt
@@ -14,20 +14,23 @@ if(${pybind11_FOUND})
 
     message(STATUS "compiling python bindings")
 
+    set(OPENSOT_BINDINGS_SOURCES    pyopensot.cpp
+                                    variables.hpp
+                                    autostack.hpp
+                                    sub.hpp
+                                    aggregated.hpp
+                                    generic.hpp
+                                    basic.hpp
+                                    solver.hpp
+                                    tasks/velocity.hpp
+                                    tasks/acceleration.hpp
+                                    constraints/velocity.hpp
+                                    constraints/acceleration.hpp
+                                    constraints/force.hpp)
+
     include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-    pybind11_add_module(pyopensot pyopensot.cpp
-                                  variables.hpp
-                                  autostack.hpp
-                                  sub.hpp
-                                  aggregated.hpp
-                                  generic.hpp
-                                  basic.hpp
-                                  solver.hpp
-                                  tasks/velocity.hpp
-                                  tasks/acceleration.hpp
-                                  constraints/velocity.hpp
-                                  constraints/acceleration.hpp
-                                  constraints/force.hpp)
+    pybind11_add_module(pyopensot ${OPENSOT_BINDINGS_SOURCES})
+
     target_link_libraries(pyopensot PUBLIC OpenSoT)
 
 
@@ -39,6 +42,42 @@ if(${pybind11_FOUND})
         ARCHIVE DESTINATION "lib"
         RUNTIME DESTINATION "bin")
 
+    if(${OPENSOT_COMPILE_COLLISION})
+        set(OPENSOT_BINDINGS_COLLISION_SOURCES pyopensot_collision.cpp constraints/velocity_collision.hpp)
+
+        include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+        pybind11_add_module(pyopensot_collision ${OPENSOT_BINDINGS_COLLISION_SOURCES})
+
+        target_link_libraries(pyopensot_collision PUBLIC OpenSoT)
+
+
+        find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
+
+        install(TARGETS pyopensot_collision
+            COMPONENT python
+            LIBRARY DESTINATION "~/.local/lib/python${Python3_VERSION_MAJOR}.${Python3_VERSION_MINOR}/site-packages"
+            ARCHIVE DESTINATION "lib"
+            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()
     message(STATUS "pybind not found")

diff --git a/bindings/python/constraints/velocity.hpp b/bindings/python/constraints/velocity.hpp
@@ -4,8 +4,6 @@
 #include <OpenSoT/constraints/velocity/JointLimits.h>
 #include <OpenSoT/constraints/velocity/VelocityLimits.h>
 #include <OpenSoT/constraints/velocity/OmniWheels4X.h>
-#include <OpenSoT/constraints/velocity/CollisionAvoidance.h>
-
 
 namespace py = pybind11;
 using namespace OpenSoT::constraints::velocity;
@@ -35,27 +33,3 @@ void pyVelocityOmniWheels4X(py::module& m) {
         .def("update", &OmniWheels4X::update);
 
 }
-
-void pyVelocityCollisionAvoidance(py::module& m) {
-    py::class_<CollisionAvoidance, std::shared_ptr<CollisionAvoidance>, OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CollisionAvoidance")
-        .def(py::init<const XBot::ModelInterface&, int, urdf::ModelConstSharedPtr, srdf::ModelConstSharedPtr>(),
-             py::arg(), py::arg("max_pairs") = -1, py::arg("collision_urdf") = nullptr, py::arg("collision_srdf") = nullptr)
-        .def("getLinkPairThreshold", &CollisionAvoidance::getLinkPairThreshold)
-        .def("getDetectionThreshold", &CollisionAvoidance::getDetectionThreshold)
-        .def("setLinkPairThreshold", &CollisionAvoidance::setLinkPairThreshold)
-        .def("setDetectionThreshold", &CollisionAvoidance::setDetectionThreshold)
-        .def("update", &CollisionAvoidance::update)
-        .def("setMaxPairs", &CollisionAvoidance::setMaxPairs)
-        .def("setCollisionList", &CollisionAvoidance::setCollisionList)
-        .def("collisionModelUpdated", &CollisionAvoidance::collisionModelUpdated)
-        .def("addCollisionShape", &CollisionAvoidance::addCollisionShape)
-        .def("moveCollisionShape", &CollisionAvoidance::moveCollisionShape)
-        .def("setBoundScaling", &CollisionAvoidance::setBoundScaling)
-        .def("setLinksVsEnvironment", &CollisionAvoidance::setLinksVsEnvironment)
-        .def("getCollisionModel", (const XBot::Collision::CollisionModel& (CollisionAvoidance::*)() const) &CollisionAvoidance::getCollisionModel)
-        .def("getOrderedWitnessPointVector", &CollisionAvoidance::getOrderedWitnessPointVector)
-        .def("getOrderedLinkPairVector", &CollisionAvoidance::getOrderedLinkPairVector)
-        .def("getOrderedDistanceVector", &CollisionAvoidance::getOrderedDistanceVector)
-        .def("getCollisionJacobian", &CollisionAvoidance::getCollisionJacobian);
-}
-
diff --git a/bindings/python/constraints/velocity_collision.hpp b/bindings/python/constraints/velocity_collision.hpp
@@ -0,0 +1,31 @@
+#include <pybind11/pybind11.h>
+#include <pybind11/eigen.h>
+#include <pybind11/stl.h>
+#include <OpenSoT/constraints/velocity/CollisionAvoidance.h>
+
+
+namespace py = pybind11;
+using namespace OpenSoT::constraints::velocity;
+
+void pyVelocityCollisionAvoidance(py::module& m) {
+    py::class_<CollisionAvoidance, std::shared_ptr<CollisionAvoidance>, OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "CollisionAvoidance")
+    .def(py::init<const XBot::ModelInterface&, int, urdf::ModelConstSharedPtr, srdf::ModelConstSharedPtr>(),
+         py::arg(), py::arg("max_pairs") = -1, py::arg("collision_urdf") = nullptr, py::arg("collision_srdf") = nullptr)
+        .def("getLinkPairThreshold", &CollisionAvoidance::getLinkPairThreshold)
+        .def("getDetectionThreshold", &CollisionAvoidance::getDetectionThreshold)
+        .def("setLinkPairThreshold", &CollisionAvoidance::setLinkPairThreshold)
+        .def("setDetectionThreshold", &CollisionAvoidance::setDetectionThreshold)
+        .def("update", &CollisionAvoidance::update)
+        .def("setMaxPairs", &CollisionAvoidance::setMaxPairs)
+        .def("setCollisionList", &CollisionAvoidance::setCollisionList)
+        .def("collisionModelUpdated", &CollisionAvoidance::collisionModelUpdated)
+        .def("addCollisionShape", &CollisionAvoidance::addCollisionShape)
+        .def("moveCollisionShape", &CollisionAvoidance::moveCollisionShape)
+        .def("setBoundScaling", &CollisionAvoidance::setBoundScaling)
+        .def("setLinksVsEnvironment", &CollisionAvoidance::setLinksVsEnvironment)
+        .def("getCollisionModel", (const XBot::Collision::CollisionModel& (CollisionAvoidance::*)() const) &CollisionAvoidance::getCollisionModel)
+        .def("getOrderedWitnessPointVector", &CollisionAvoidance::getOrderedWitnessPointVector)
+        .def("getOrderedLinkPairVector", &CollisionAvoidance::getOrderedLinkPairVector)
+        .def("getOrderedDistanceVector", &CollisionAvoidance::getOrderedDistanceVector)
+        .def("getCollisionJacobian", &CollisionAvoidance::getCollisionJacobian);
+}
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");
 
@@ -54,7 +53,6 @@ PYBIND11_MODULE(pyopensot, m) {
     pyVelocityJointLimits(m_cv);
     pyVelocityLimits(m_cv);
     pyVelocityOmniWheels4X(m_cv);
-    pyVelocityCollisionAvoidance(m_cv);
 
     auto m_ca = m_c.def_submodule("acceleration");
     pyAccelerationJointLimits(m_ca);

diff --git a/bindings/python/pyopensot_collision.cpp b/bindings/python/pyopensot_collision.cpp
@@ -0,0 +1,7 @@
+#include "constraints/velocity_collision.hpp"
+
+PYBIND11_MODULE(pyopensot_collision, m) {
+    auto m_c = m.def_submodule("constraints");
+    auto m_cv = m_c.def_submodule("velocity");
+    pyVelocityCollisionAvoidance(m_cv);
+}
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);
+}