diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index cf700166..e130aa29 100644 --- 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 index 19806b3c..e44be79d 100644 --- a/bindings/python/constraints/velocity.hpp +++ b/bindings/python/constraints/velocity.hpp @@ -4,8 +4,6 @@ #include #include #include -#include - 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_, OpenSoT::Constraint>(m, "CollisionAvoidance") - .def(py::init(), - 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 new file mode 100644 index 00000000..42494e56 --- /dev/null +++ b/bindings/python/constraints/velocity_collision.hpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include + + +namespace py = pybind11; +using namespace OpenSoT::constraints::velocity; + +void pyVelocityCollisionAvoidance(py::module& m) { + py::class_, OpenSoT::Constraint>(m, "CollisionAvoidance") + .def(py::init(), + 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 index 50d71008..4caed7d8 100644 --- 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 index 8e327028..439cca17 100644 --- 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 new file mode 100644 index 00000000..8150e29e --- /dev/null +++ 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 new file mode 100644 index 00000000..d40c6301 --- /dev/null +++ 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 index 5b1be6f2..771baed8 100644 --- a/bindings/python/solver.hpp +++ b/bindings/python/solver.hpp @@ -5,7 +5,6 @@ #include #include #include -#include #include namespace py = pybind11; @@ -102,13 +101,3 @@ void pynHQP(py::module& m) { .def("setPerformSelectiveNullSpaceRegularization", py::overload_cast(&solvers::nHQP::setPerformSelectiveNullSpaceRegularization)); } -void pyHCOD(py::module& m) { - py::class_, OpenSoT::Solver>(m, "HCOD") - .def(py::init()) - .def(py::init::Stack&, OpenSoT::Solver::ConstraintPtr, const double>()) - .def("solve", solve) - .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 new file mode 100644 index 00000000..b1744f98 --- /dev/null +++ b/bindings/python/solver_hcod.hpp @@ -0,0 +1,39 @@ +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace OpenSoT; + +template +class pySolverTrampoline : public Solver { +public: + using Solver::Solver; + typedef Solver SMV; + + bool solve(VectorType& solution) override { + PYBIND11_OVERLOAD_PURE(bool, SMV, solve, solution); + } +}; + +template +VectorType solve(Solver& solver) +{ + VectorType solution; + solver.solve(solution); + return solution; +} + + +void pyHCOD(py::module& m) { + py::class_, OpenSoT::Solver>(m, "HCOD") + .def(py::init()) + .def(py::init::Stack&, OpenSoT::Solver::ConstraintPtr, const double>()) + .def("solve", solve) + .def("setDisableWeightsComputation", &solvers::HCOD::setDisableWeightsComputation) + .def("getDisableWeightsComputation", &solvers::HCOD::getDisableWeightsComputation) + .def("setDamping", &solvers::HCOD::setDamping) + .def("printSOT", &solvers::HCOD::printSOT); +}