From 6dafac66129ac48b0601068ec5ad47165519406f Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 17:30:58 +0900 Subject: [PATCH 01/13] trying to fixstubgen --- python/bindings/openravepy_global.cpp | 9 ++- python/bindings/openravepy_int.cpp | 77 ++++++++++---------- python/bindings/openravepy_sensor.cpp | 88 +++++++++++------------ python/bindings/openravepy_trajectory.cpp | 40 +++++------ 4 files changed, 110 insertions(+), 104 deletions(-) diff --git a/python/bindings/openravepy_global.cpp b/python/bindings/openravepy_global.cpp index 63bdbfc1dd..992f3c1e3a 100644 --- a/python/bindings/openravepy_global.cpp +++ b/python/bindings/openravepy_global.cpp @@ -1031,6 +1031,11 @@ object pyRaveInvertFileLookup(const std::string& filename) return py::none_(); } +object RaveGlobalState() +{ + return openravepy::toPyUserData(OpenRAVE::RaveGlobalState()); +} + object RaveGetPluginInfo() { py::list plugins; @@ -2163,9 +2168,9 @@ void init_openravepy_global() def("RaveHasInterface",OpenRAVE::RaveHasInterface, PY_ARGS("type","name") DOXY_FN1(RaveHasInterface)); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS - m.def("RaveGlobalState",OpenRAVE::RaveGlobalState,DOXY_FN1(RaveGlobalState)); + m.def("RaveGlobalState",openravepy::RaveGlobalState,DOXY_FN1(RaveGlobalState)); #else - def("RaveGlobalState",OpenRAVE::RaveGlobalState,DOXY_FN1(RaveGlobalState)); + def("RaveGlobalState",openravepy::RaveGlobalState,DOXY_FN1(RaveGlobalState)); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS m.def("RaveClone", openravepy::pyRaveClone, diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index bba935e7f8..125aaef4c6 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3298,6 +3298,45 @@ Because race conditions can pop up when trying to lock the openrave environment ; } +#ifdef USE_PYBIND11_PYTHON_BINDINGS + + + openravepy::init_openravepy_controller(m); + openravepy::init_openravepy_ikparameterization(m); + openravepy::init_openravepy_iksolver(m); + openravepy::init_openravepy_kinbody(m); + openravepy::init_openravepy_robot(m); + openravepy::init_openravepy_module(m); + openravepy::init_openravepy_physicsengine(m); + openravepy::init_openravepy_planner(m); + openravepy::init_openravepy_trajectory(m); + openravepy::init_openravepy_sensor(m); + openravepy::init_openravepy_sensorsystem(m); + openravepy::init_openravepy_spacesampler(m); + openravepy::init_openravepy_viewer(m); + + openravepy::init_openravepy_collisionchecker(m); + openravepy::InitPlanningUtils(m); + openravepy::init_openravepy_global(m); +#else + openravepy::init_openravepy_global(); + openravepy::InitPlanningUtils(); + + openravepy::init_openravepy_collisionchecker(); + openravepy::init_openravepy_controller(); + openravepy::init_openravepy_ikparameterization(); + openravepy::init_openravepy_iksolver(); + openravepy::init_openravepy_kinbody(); + openravepy::init_openravepy_robot(); + openravepy::init_openravepy_module(); + openravepy::init_openravepy_physicsengine(); + openravepy::init_openravepy_planner(); + openravepy::init_openravepy_trajectory(); + openravepy::init_openravepy_sensor(); + openravepy::init_openravepy_sensorsystem(); + openravepy::init_openravepy_spacesampler(); + openravepy::init_openravepy_viewer(); +#endif #ifdef USE_PYBIND11_PYTHON_BINDINGS object environmentbaseinfo = class_ >(m, "EnvironmentBaseInfo", DOXY_CLASS(EnvironmentBase::EnvironmentBaseInfo)) @@ -3786,44 +3825,6 @@ Because race conditions can pop up when trying to lock the openrave environment scope().attr("__pythonbinding__") = "Boost.Python." + std::to_string(BOOST_VERSION); #endif // USE_PYBIND11_PYTHON_BINDINGS -#ifdef USE_PYBIND11_PYTHON_BINDINGS - openravepy::init_openravepy_global(m); - openravepy::InitPlanningUtils(m); - - openravepy::init_openravepy_collisionchecker(m); - openravepy::init_openravepy_controller(m); - openravepy::init_openravepy_ikparameterization(m); - openravepy::init_openravepy_iksolver(m); - openravepy::init_openravepy_kinbody(m); - openravepy::init_openravepy_robot(m); - openravepy::init_openravepy_module(m); - openravepy::init_openravepy_physicsengine(m); - openravepy::init_openravepy_planner(m); - openravepy::init_openravepy_trajectory(m); - openravepy::init_openravepy_sensor(m); - openravepy::init_openravepy_sensorsystem(m); - openravepy::init_openravepy_spacesampler(m); - openravepy::init_openravepy_viewer(m); -#else - openravepy::init_openravepy_global(); - openravepy::InitPlanningUtils(); - - openravepy::init_openravepy_collisionchecker(); - openravepy::init_openravepy_controller(); - openravepy::init_openravepy_ikparameterization(); - openravepy::init_openravepy_iksolver(); - openravepy::init_openravepy_kinbody(); - openravepy::init_openravepy_robot(); - openravepy::init_openravepy_module(); - openravepy::init_openravepy_physicsengine(); - openravepy::init_openravepy_planner(); - openravepy::init_openravepy_trajectory(); - openravepy::init_openravepy_sensor(); - openravepy::init_openravepy_sensorsystem(); - openravepy::init_openravepy_spacesampler(); - openravepy::init_openravepy_viewer(); -#endif - #ifdef USE_PYBIND11_PYTHON_BINDINGS m.def("RaveGetEnvironmentId", openravepy::RaveGetEnvironmentId, DOXY_FN1(RaveGetEnvironmentId)); m.def("RaveGetEnvironment", openravepy::RaveGetEnvironment, DOXY_FN1(RaveGetEnvironment)); diff --git a/python/bindings/openravepy_sensor.cpp b/python/bindings/openravepy_sensor.cpp index 2d088a7d1f..b063bf468c 100644 --- a/python/bindings/openravepy_sensor.cpp +++ b/python/bindings/openravepy_sensor.cpp @@ -737,36 +737,56 @@ void init_openravepy_sensor() OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData1)() = &PySensorBase::GetSensorData; OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData2)(SensorBase::SensorType) = &PySensorBase::GetSensorData; #ifdef USE_PYBIND11_PYTHON_BINDINGS - scope_ sensor = class_, PyInterfaceBase>(m, "Sensor", DOXY_CLASS(SensorBase)) + class_, PyInterfaceBase> sensor(m, "Sensor", DOXY_CLASS(SensorBase)); #else - scope_ sensor = class_, bases >("Sensor", DOXY_CLASS(SensorBase), no_init) + class_, bases > sensor("Sensor", DOXY_CLASS(SensorBase), no_init); #endif + #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("Configure", &PySensorBase::Configure, - "command"_a, - "blocking"_a = false, - DOXY_FN(SensorBase, Configure) - ) -#else - .def("Configure",&PySensorBase::Configure, Configure_overloads(PY_ARGS("command","blocking") DOXY_FN(SensorBase,Configure))) + // SensorType belongs to Sensor + enum_(sensor, "Type", py::arithmetic() DOXY_ENUM(SensorType)) +#else + enum_("Type" DOXY_ENUM(SensorType)) +#endif + .value("Invalid",SensorBase::ST_Invalid) + .value("Laser",SensorBase::ST_Laser) + .value("Camera",SensorBase::ST_Camera) + .value("JointEncoder",SensorBase::ST_JointEncoder) + .value("Force6D",SensorBase::ST_Force6D) + .value("IMU",SensorBase::ST_IMU) + .value("Odometry",SensorBase::ST_Odometry) + .value("Tactile",SensorBase::ST_Tactile) + .value("Actuator",SensorBase::ST_Actuator) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .export_values() #endif - .def("SimulationStep",&PySensorBase::SimulationStep, PY_ARGS("timeelapsed") DOXY_FN(SensorBase,SimulationStep)) - .def("GetSensorData",GetSensorData1, DOXY_FN(SensorBase,GetSensorData)) - .def("GetSensorData",GetSensorData2, DOXY_FN(SensorBase,GetSensorData)) - .def("CreateSensorData",&PySensorBase::CreateSensorData, DOXY_FN(SensorBase,CreateSensorData)) - .def("GetSensorGeometry",&PySensorBase::GetSensorGeometry,DOXY_FN(SensorBase,GetSensorGeometry)) - .def("SetSensorGeometry",&PySensorBase::SetSensorGeometry, DOXY_FN(SensorBase,SetSensorGeometry)) - .def("SetTransform",&PySensorBase::SetTransform, DOXY_FN(SensorBase,SetTransform)) - .def("GetTransform",&PySensorBase::GetTransform, DOXY_FN(SensorBase,GetTransform)) - .def("GetTransformPose",&PySensorBase::GetTransformPose, DOXY_FN(SensorBase,GetTransform)) - .def("GetName",&PySensorBase::GetName, DOXY_FN(SensorBase,GetName)) - .def("SetName",&PySensorBase::SetName, DOXY_FN(SensorBase,SetName)) - .def("Supports",&PySensorBase::Supports, DOXY_FN(SensorBase,Supports)) - .def("__str__",&PySensorBase::__str__) - .def("__unicode__",&PySensorBase::__unicode__) - .def("__repr__",&PySensorBase::__repr__) ; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + sensor.def("Configure", &PySensorBase::Configure, + "command"_a, + "blocking"_a = false, + DOXY_FN(SensorBase, Configure) + ); +#else + sensor.def("Configure",&PySensorBase::Configure, Configure_overloads(PY_ARGS("command","blocking") DOXY_FN(SensorBase,Configure))); +#endif + sensor.def("SimulationStep",&PySensorBase::SimulationStep, PY_ARGS("timeelapsed") DOXY_FN(SensorBase,SimulationStep)); + sensor.def("GetSensorData",GetSensorData1, DOXY_FN(SensorBase,GetSensorData)); + sensor.def("GetSensorData",GetSensorData2, DOXY_FN(SensorBase,GetSensorData)); + sensor.def("CreateSensorData",&PySensorBase::CreateSensorData, DOXY_FN(SensorBase,CreateSensorData)); + sensor.def("GetSensorGeometry",&PySensorBase::GetSensorGeometry,DOXY_FN(SensorBase,GetSensorGeometry)); + sensor.def("SetSensorGeometry",&PySensorBase::SetSensorGeometry, DOXY_FN(SensorBase,SetSensorGeometry)); + sensor.def("SetTransform",&PySensorBase::SetTransform, DOXY_FN(SensorBase,SetTransform)); + sensor.def("GetTransform",&PySensorBase::GetTransform, DOXY_FN(SensorBase,GetTransform)); + sensor.def("GetTransformPose",&PySensorBase::GetTransformPose, DOXY_FN(SensorBase,GetTransform)); + sensor.def("GetName",&PySensorBase::GetName, DOXY_FN(SensorBase,GetName)); + sensor.def("SetName",&PySensorBase::SetName, DOXY_FN(SensorBase,SetName)); + sensor.def("Supports",&PySensorBase::Supports, DOXY_FN(SensorBase,Supports)); + sensor.def("__str__",&PySensorBase::__str__); + sensor.def("__unicode__",&PySensorBase::__unicode__); + sensor.def("__repr__",&PySensorBase::__repr__); + #ifdef USE_PYBIND11_PYTHON_BINDINGS class_ >(sensor, "CameraIntrinsics", DOXY_CLASS(geometry::RaveCameraIntrinsics)) #else @@ -911,26 +931,6 @@ void init_openravepy_sensor() ; } -#ifdef USE_PYBIND11_PYTHON_BINDINGS - // SensorType belongs to Sensor - enum_(sensor, "Type", py::arithmetic() DOXY_ENUM(SensorType)) -#else - enum_("Type" DOXY_ENUM(SensorType)) -#endif - .value("Invalid",SensorBase::ST_Invalid) - .value("Laser",SensorBase::ST_Laser) - .value("Camera",SensorBase::ST_Camera) - .value("JointEncoder",SensorBase::ST_JointEncoder) - .value("Force6D",SensorBase::ST_Force6D) - .value("IMU",SensorBase::ST_IMU) - .value("Odometry",SensorBase::ST_Odometry) - .value("Tactile",SensorBase::ST_Tactile) - .value("Actuator",SensorBase::ST_Actuator) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .export_values() -#endif - ; - #ifdef USE_PYBIND11_PYTHON_BINDINGS enum_(sensor, "ConfigureCommand", py::arithmetic() DOXY_ENUM(ConfigureCommand)) #else diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index 34ec0b7f07..05de6694a9 100644 --- a/python/bindings/openravepy_trajectory.cpp +++ b/python/bindings/openravepy_trajectory.cpp @@ -635,31 +635,31 @@ void init_openravepy_trajectory() void (PyTrajectoryBase::*Insert2)(size_t,object,bool) = &PyTrajectoryBase::Insert; void (PyTrajectoryBase::*Insert3)(size_t,object,PyConfigurationSpecificationPtr) = &PyTrajectoryBase::Insert; void (PyTrajectoryBase::*Insert4)(size_t,object,PyConfigurationSpecificationPtr,bool) = &PyTrajectoryBase::Insert; - void (PyTrajectoryBase::*Insert5)(size_t, object, OPENRAVE_SHARED_PTR) = &PyTrajectoryBase::Insert; - void (PyTrajectoryBase::*Insert6)(size_t, object, OPENRAVE_SHARED_PTR, bool) = &PyTrajectoryBase::Insert; + // void (PyTrajectoryBase::*Insert5)(size_t, object, OPENRAVE_SHARED_PTR) = &PyTrajectoryBase::Insert; + // void (PyTrajectoryBase::*Insert6)(size_t, object, OPENRAVE_SHARED_PTR, bool) = &PyTrajectoryBase::Insert; object (PyTrajectoryBase::*Sample1)(dReal) const = &PyTrajectoryBase::Sample; object (PyTrajectoryBase::*Sample2)(dReal, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::Sample; - object (PyTrajectoryBase::*Sample3)(dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::Sample; + // object (PyTrajectoryBase::*Sample3)(dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::Sample; object (PyTrajectoryBase::*SampleFromPrevious1)(object, dReal, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SampleFromPrevious; - object (PyTrajectoryBase::*SampleFromPrevious2)(object, dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SampleFromPrevious; + // object (PyTrajectoryBase::*SampleFromPrevious2)(object, dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SampleFromPrevious; object (PyTrajectoryBase::*SamplePoints2D1)(object) const = &PyTrajectoryBase::SamplePoints2D; object (PyTrajectoryBase::*SamplePoints2D2)(object, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SamplePoints2D; - object (PyTrajectoryBase::*SamplePoints2D3)(object, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePoints2D; + // object (PyTrajectoryBase::*SamplePoints2D3)(object, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePoints2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D1)(dReal, bool) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D2)(dReal, bool, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; - object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D3)(dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; + // object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D3)(dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*GetWaypoints1)(size_t,size_t) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints2)(size_t,size_t,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoints; - object (PyTrajectoryBase::*GetWaypoints3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints; + // object (PyTrajectoryBase::*GetWaypoints3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints2D1)(size_t,size_t) const = &PyTrajectoryBase::GetWaypoints2D; object (PyTrajectoryBase::*GetWaypoints2D2)(size_t,size_t,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoints2D; - object (PyTrajectoryBase::*GetWaypoints2D3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints2D; + // object (PyTrajectoryBase::*GetWaypoints2D3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints2D; object (PyTrajectoryBase::*GetAllWaypoints2D1)() const = &PyTrajectoryBase::GetAllWaypoints2D; object (PyTrajectoryBase::*GetAllWaypoints2D2)(PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetAllWaypoints2D; - object (PyTrajectoryBase::*GetAllWaypoints2D3)(OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetAllWaypoints2D; + // object (PyTrajectoryBase::*GetAllWaypoints2D3)(OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetAllWaypoints2D; object (PyTrajectoryBase::*GetWaypoint1)(int) const = &PyTrajectoryBase::GetWaypoint; object (PyTrajectoryBase::*GetWaypoint2)(int,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoint; - object (PyTrajectoryBase::*GetWaypoint3)(int, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoint; + // object (PyTrajectoryBase::*GetWaypoint3)(int, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoint; object (PyTrajectoryBase::*__getitem__1)(int) const = &PyTrajectoryBase::__getitem__; object (PyTrajectoryBase::*__getitem__2)(slice) const = &PyTrajectoryBase::__getitem__; @@ -674,37 +674,37 @@ void init_openravepy_trajectory() .def("Insert",Insert2, PY_ARGS("index","data","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; bool")) .def("Insert",Insert3, PY_ARGS("index","data","spec") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification")) .def("Insert",Insert4, PY_ARGS("index","data","spec","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification; bool")) - .def("Insert",Insert5, PY_ARGS("index","data","group") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group")) - .def("Insert",Insert6, PY_ARGS("index","data","group","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group; bool")) + // .def("Insert",Insert5, PY_ARGS("index","data","group") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group")) + // .def("Insert",Insert6, PY_ARGS("index","data","group","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group; bool")) .def("Remove",&PyTrajectoryBase::Remove, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase,Remove)) .def("Sample",Sample1, PY_ARGS("time") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal")) .def("Sample",Sample2, PY_ARGS("time","spec") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification")) - .def("Sample",Sample3, PY_ARGS("time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) + // .def("Sample",Sample3, PY_ARGS("time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) .def("SampleFromPrevious", SampleFromPrevious1, PY_ARGS("data","time","spec") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification")) - .def("SampleFromPrevious", SampleFromPrevious2, PY_ARGS("data","time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) + //.def("SampleFromPrevious", SampleFromPrevious2, PY_ARGS("data","time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) .def("SamplePoints2D",SamplePoints2D1, PY_ARGS("times") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector")) .def("SamplePoints2D",SamplePoints2D2, PY_ARGS("times","spec") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification")) - .def("SamplePoints2D",SamplePoints2D3, PY_ARGS("times","group") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification::Group")) + //.def("SamplePoints2D",SamplePoints2D3, PY_ARGS("times","group") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification::Group")) .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D1, PY_ARGS("deltatime","ensurelastpoint") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool")) .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D2, PY_ARGS("deltatime","ensurelastpoint","spec") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification")) #ifdef USE_PYBIND11_PYTHON_BINDINGS // why boost::python can resolve this overload? - .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D3, PY_ARGS("deltatime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification::Group")) + // .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D3, PY_ARGS("deltatime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification::Group")) #endif .def("GetConfigurationSpecification",&PyTrajectoryBase::GetConfigurationSpecification,DOXY_FN(TrajectoryBase,GetConfigurationSpecification)) .def("GetNumWaypoints",&PyTrajectoryBase::GetNumWaypoints,DOXY_FN(TrajectoryBase,GetNumWaypoints)) .def("GetWaypoints",GetWaypoints1, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetWaypoints",GetWaypoints2, PY_ARGS("startindex","endindex","spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - .def("GetWaypoints",GetWaypoints3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&::Group")) + // .def("GetWaypoints",GetWaypoints3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&::Group")) .def("GetWaypoints2D",GetWaypoints2D1, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetWaypoints2D",GetWaypoints2D2, PY_ARGS("startindex","endindex","spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - .def("GetWaypoints2D",GetWaypoints2D3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) + // .def("GetWaypoints2D",GetWaypoints2D3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) .def("GetAllWaypoints2D",GetAllWaypoints2D1,DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetAllWaypoints2D",GetAllWaypoints2D2, PY_ARGS("spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - .def("GetAllWaypoints2D",GetAllWaypoints2D3, PY_ARGS("group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) + // .def("GetAllWaypoints2D",GetAllWaypoints2D3, PY_ARGS("group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) .def("GetWaypoint",GetWaypoint1, PY_ARGS("index") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector")) .def("GetWaypoint",GetWaypoint2, PY_ARGS("index","spec") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification")) - .def("GetWaypoint",GetWaypoint3, PY_ARGS("index","group") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification::Group")) + // .def("GetWaypoint",GetWaypoint3, PY_ARGS("index","group") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification::Group")) .def("GetFirstWaypointIndexAfterTime",&PyTrajectoryBase::GetFirstWaypointIndexAfterTime, DOXY_FN(TrajectoryBase, GetFirstWaypointIndexAfterTime)) .def("GetDuration",&PyTrajectoryBase::GetDuration,DOXY_FN(TrajectoryBase, GetDuration)) #ifdef USE_PYBIND11_PYTHON_BINDINGS From 38bfdf9691dab7fa9cda8864887cb7585d6d5d7e Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 18:23:41 +0900 Subject: [PATCH 02/13] progress --- python/bindings/openravepy_int.cpp | 17 +- python/bindings/openravepy_planner.cpp | 55 ++-- python/bindings/openravepy_robot.cpp | 426 +++++++++++++------------ 3 files changed, 251 insertions(+), 247 deletions(-) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 125aaef4c6..c1d3687768 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3299,25 +3299,26 @@ Because race conditions can pop up when trying to lock the openrave environment } #ifdef USE_PYBIND11_PYTHON_BINDINGS - - + openravepy::init_openravepy_global(m); openravepy::init_openravepy_controller(m); openravepy::init_openravepy_ikparameterization(m); openravepy::init_openravepy_iksolver(m); openravepy::init_openravepy_kinbody(m); + openravepy::init_openravepy_collisionchecker(m); + openravepy::init_openravepy_sensor(m); + openravepy::init_openravepy_sensorsystem(m); openravepy::init_openravepy_robot(m); + openravepy::init_openravepy_module(m); openravepy::init_openravepy_physicsengine(m); - openravepy::init_openravepy_planner(m); openravepy::init_openravepy_trajectory(m); - openravepy::init_openravepy_sensor(m); - openravepy::init_openravepy_sensorsystem(m); + openravepy::init_openravepy_planner(m); + + openravepy::init_openravepy_spacesampler(m); openravepy::init_openravepy_viewer(m); - - openravepy::init_openravepy_collisionchecker(m); + openravepy::InitPlanningUtils(m); - openravepy::init_openravepy_global(m); #else openravepy::init_openravepy_global(); openravepy::InitPlanningUtils(); diff --git a/python/bindings/openravepy_planner.cpp b/python/bindings/openravepy_planner.cpp index 574bc04772..a3522680e8 100644 --- a/python/bindings/openravepy_planner.cpp +++ b/python/bindings/openravepy_planner.cpp @@ -481,36 +481,12 @@ void init_openravepy_planner() ; { - bool (PyPlannerBase::*InitPlan1)(PyRobotBasePtr, PyPlannerBase::PyPlannerParametersPtr,bool) = &PyPlannerBase::InitPlan; - bool (PyPlannerBase::*InitPlan2)(PyRobotBasePtr, const string &) = &PyPlannerBase::InitPlan; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - scope_ planner = class_, PyInterfaceBase>(m, "Planner", DOXY_CLASS(PlannerBase)) -#else - scope_ planner = class_, bases >("Planner", DOXY_CLASS(PlannerBase), no_init) -#endif #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitPlan", InitPlan1, - "robot"_a, - "params"_a, - "releasegil"_a = false, - DOXY_FN(PlannerBase, InitPlan "RobotBasePtr; PlannerParametersConstPtr") - ) + class_, PyInterfaceBase> planner(m, "Planner", DOXY_CLASS(PlannerBase)); #else - .def("InitPlan",InitPlan1,InitPlan_overloads(PY_ARGS("robot","params","releasegil") DOXY_FN(PlannerBase,InitPlan "RobotBasePtr; PlannerParametersConstPtr"))) + class_, bases > planner("Planner", DOXY_CLASS(PlannerBase), no_init); #endif - .def("InitPlan",InitPlan2, PY_ARGS("robot","xmlparams") DOXY_FN(PlannerBase,InitPlan "RobotBasePtr; std::istream")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("PlanPath", &PyPlannerBase::PlanPath, - "traj"_a, - "releasegil"_a = true, - DOXY_FN(PlannerBase, PlanPath) - ) -#else - .def("PlanPath",&PyPlannerBase::PlanPath,PlanPath_overloads(PY_ARGS("traj","releasegil") DOXY_FN(PlannerBase,PlanPath))) -#endif - .def("GetParameters",&PyPlannerBase::GetParameters, DOXY_FN(PlannerBase,GetParameters)) - .def("RegisterPlanCallback",&PyPlannerBase::RegisterPlanCallback, DOXY_FN(PlannerBase,RegisterPlanCallback)) - ; + #ifdef USE_PYBIND11_PYTHON_BINDINGS // PlannerParameters belongs to Planner class_(planner, "PlannerParameters", DOXY_CLASS(PlannerBase::PlannerParameters)) @@ -576,6 +552,31 @@ void init_openravepy_planner() .def("__ne__",&PyPlannerBase::PyPlannerParameters::__ne__) .def("__hash__",&PyPlannerBase::PyPlannerParameters::__hash__) ; + + bool (PyPlannerBase::*InitPlan1)(PyRobotBasePtr, PyPlannerBase::PyPlannerParametersPtr,bool) = &PyPlannerBase::InitPlan; + bool (PyPlannerBase::*InitPlan2)(PyRobotBasePtr, const string &) = &PyPlannerBase::InitPlan; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + planner.def("InitPlan", InitPlan1, + "robot"_a, + "params"_a, + "releasegil"_a = false, + DOXY_FN(PlannerBase, InitPlan "RobotBasePtr; PlannerParametersConstPtr") + ); +#else + planner.def("InitPlan",InitPlan1,InitPlan_overloads(PY_ARGS("robot","params","releasegil") DOXY_FN(PlannerBase,InitPlan "RobotBasePtr; PlannerParametersConstPtr"))); +#endif + planner.def("InitPlan",InitPlan2, PY_ARGS("robot","xmlparams") DOXY_FN(PlannerBase,InitPlan "RobotBasePtr; std::istream")); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + planner.def("PlanPath", &PyPlannerBase::PlanPath, + "traj"_a, + "releasegil"_a = true, + DOXY_FN(PlannerBase, PlanPath) + ); +#else + planner.def("PlanPath",&PyPlannerBase::PlanPath,PlanPath_overloads(PY_ARGS("traj","releasegil") DOXY_FN(PlannerBase,PlanPath))); +#endif + planner.def("GetParameters",&PyPlannerBase::GetParameters, DOXY_FN(PlannerBase,GetParameters)); + planner.def("RegisterPlanCallback",&PyPlannerBase::RegisterPlanCallback, DOXY_FN(PlannerBase,RegisterPlanCallback)); } #ifdef USE_PYBIND11_PYTHON_BINDINGS diff --git a/python/bindings/openravepy_robot.cpp b/python/bindings/openravepy_robot.cpp index 256054a58e..aac96a7377 100644 --- a/python/bindings/openravepy_robot.cpp +++ b/python/bindings/openravepy_robot.cpp @@ -2536,222 +2536,12 @@ void init_openravepy_robot() ; { - void (PyRobotBase::*psetactivedofs1)(const object&) = &PyRobotBase::SetActiveDOFs; - void (PyRobotBase::*psetactivedofs2)(const object&, int) = &PyRobotBase::SetActiveDOFs; - void (PyRobotBase::*psetactivedofs3)(const object&, int, object) = &PyRobotBase::SetActiveDOFs; - - bool (PyRobotBase::*pgrab1)(PyKinBodyPtr) = &PyRobotBase::Grab; - bool (PyRobotBase::*pgrab3)(PyKinBodyPtr, object) = &PyRobotBase::Grab; - bool (PyRobotBase::*pgrab5)(PyKinBodyPtr, object, object, object) = &PyRobotBase::Grab; - - PyRobotBase::PyManipulatorPtr (PyRobotBase::*setactivemanipulator2)(const std::string&) = &PyRobotBase::SetActiveManipulator; - PyRobotBase::PyManipulatorPtr (PyRobotBase::*setactivemanipulator3)(PyRobotBase::PyManipulatorPtr) = &PyRobotBase::SetActiveManipulator; - - object (PyRobotBase::*GetManipulators1)() = &PyRobotBase::GetManipulators; - object (PyRobotBase::*GetManipulators2)(const string &) = &PyRobotBase::GetManipulators; - bool (PyRobotBase::*setcontroller1)(PyControllerBasePtr,const string &) = &PyRobotBase::SetController; - bool (PyRobotBase::*setcontroller2)(PyControllerBasePtr,object,int) = &PyRobotBase::SetController; - bool (PyRobotBase::*setcontroller3)(PyControllerBasePtr) = &PyRobotBase::SetController; - bool (PyRobotBase::*initrobot)(object, object, object, object, const std::string&) = &PyRobotBase::Init; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - scope_ robot = class_, PyKinBody>(m, "Robot", py::dynamic_attr(), DOXY_CLASS(RobotBase)) -#else - scope_ robot = class_, bases >("Robot", DOXY_CLASS(RobotBase), no_init) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("Init", initrobot, - "linkinfos"_a, - "jointinfos"_a, - "manipinfos"_a, - "attachedsensorinfos"_a, - "uri"_a = "", - DOXY_FN(RobotBase, Init) - ) -#else - .def("Init", initrobot, Init_overloads(PY_ARGS("linkinfos", "jointinfos", "manipinfos", "attachedsensorinfos", "uri") DOXY_FN(RobotBase, Init))) -#endif - .def("GetManipulators",GetManipulators1, DOXY_FN(RobotBase,GetManipulators)) - .def("GetManipulators",GetManipulators2, PY_ARGS("manipname") DOXY_FN(RobotBase,GetManipulators)) - .def("GetManipulator",&PyRobotBase::GetManipulator,PY_ARGS("manipname") "Return the manipulator whose name matches") - .def("SetActiveManipulator",setactivemanipulator2, PY_ARGS("manipname") DOXY_FN(RobotBase,SetActiveManipulator "const std::string")) - .def("SetActiveManipulator",setactivemanipulator3,PY_ARGS("manip") "Set the active manipulator given a pointer") - .def("GetActiveManipulator",&PyRobotBase::GetActiveManipulator, DOXY_FN(RobotBase,GetActiveManipulator)) #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("AddManipulator", &PyRobotBase::AddManipulator, - "manipinfo"_a, - "removeduplicate"_a = false, - DOXY_FN(RobotBase, AddManipulator) - ) + class_, PyKinBody> robot(m, "Robot", py::dynamic_attr(), DOXY_CLASS(RobotBase)); #else - .def("AddManipulator",&PyRobotBase::AddManipulator, AddManipulator_overloads(PY_ARGS("manipinfo", "removeduplicate") DOXY_FN(RobotBase,AddManipulator))) + class_, bases > robot("Robot", DOXY_CLASS(RobotBase), no_init); #endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromRobotInfo", &PyRobotBase::InitFromRobotInfo, - "info"_a, - DOXY_FN(RobotBase, InitFromRobotInfo)) -#else - .def("InitFromRobotInfo",&PyRobotBase::InitFromRobotInfo, DOXY_FN(RobotBase, InitFromRobotInfo)) -#endif - .def("ExtractInfo", &PyRobotBase::ExtractInfo, DOXY_FN(RobotBase, ExtractInfo)) - -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("AddAttachedSensor",&PyRobotBase::AddAttachedSensor, - "attachedsensorinfo"_a, - "removeduplicate"_a = false, - DOXY_FN(RobotBase, AddAttachedSensor) - ) -#else - .def("AddAttachedSensor",&PyRobotBase::AddAttachedSensor, AddAttachedSensor_overloads(PY_ARGS("attachedsensorinfo", "removeduplicate") DOXY_FN(RobotBase,AddAttachedSensor))) -#endif - .def("RemoveAttachedSensor",&PyRobotBase::RemoveAttachedSensor, PY_ARGS("attsensor") DOXY_FN(RobotBase,RemoveAttachedSensor)) - .def("RemoveManipulator",&PyRobotBase::RemoveManipulator, PY_ARGS("manip") DOXY_FN(RobotBase,RemoveManipulator)) - .def("GetAttachedSensors",&PyRobotBase::GetAttachedSensors, DOXY_FN(RobotBase,GetAttachedSensors)) - .def("GetAttachedSensor",&PyRobotBase::GetAttachedSensor,PY_ARGS("sensorname") "Return the attached sensor whose name matches") - .def("GetSensors",&PyRobotBase::GetSensors) - .def("GetSensor",&PyRobotBase::GetSensor,PY_ARGS("sensorname") DOXY_FN(RobotBase, GetSensor)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("AddConnectedBody",&PyRobotBase::AddConnectedBody, - "connectedbodyinfo"_a, - "removeduplicate"_a = false, - DOXY_FN(RobotBase, AddConnectedBody) - ) -#else - .def("AddConnectedBody",&PyRobotBase::AddConnectedBody, AddConnectedBody_overloads(PY_ARGS("connectedbodyinfo", "removeduplicate") DOXY_FN(RobotBase,AddConnectedBody))) -#endif - .def("RemoveConnectedBody",&PyRobotBase::RemoveConnectedBody, PY_ARGS("connectedbody") DOXY_FN(RobotBase,RemoveConnectedBody)) - .def("GetConnectedBodies",&PyRobotBase::GetConnectedBodies, DOXY_FN(RobotBase,GetConnectedBodies)) - .def("GetConnectedBody",&PyRobotBase::GetConnectedBody, PY_ARGS("bodyname") DOXY_FN(RobotBase,GetConnectedBody)) - .def("GetConnectedBodyActiveStates",&PyRobotBase::GetConnectedBodyActiveStates, DOXY_FN(RobotBase,GetConnectedBodyActiveStates)) - .def("SetConnectedBodyActiveStates",&PyRobotBase::SetConnectedBodyActiveStates, DOXY_FN(RobotBase,SetConnectedBodyActiveStates)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("AddGripperInfo",&PyRobotBase::AddGripperInfo, - "gripperInfo"_a, - "removeduplicate"_a = false, - DOXY_FN(RobotBase, AddGripperInfo) - ) -#else - .def("AddGripperInfo",&PyRobotBase::AddGripperInfo, AddGripperInfo_overloads(PY_ARGS("gripperInfo", "removeduplicate") DOXY_FN(RobotBase,AddGripperInfo))) -#endif - .def("RemoveGripperInfo",&PyRobotBase::RemoveGripperInfo, PY_ARGS("name") DOXY_FN(RobotBase,RemoveGripperInfo)) - .def("GetGripperInfo",&PyRobotBase::GetGripperInfo, PY_ARGS("name") DOXY_FN(RobotBase,GetGripperInfo)) - .def("GetGripperInfos",&PyRobotBase::GetGripperInfos, DOXY_FN(RobotBase,GetGripperInfos)) - .def("GetController",&PyRobotBase::GetController, DOXY_FN(RobotBase,GetController)) - .def("SetController",setcontroller1,DOXY_FN(RobotBase,SetController)) - .def("SetController",setcontroller2, PY_ARGS("robot","dofindices","controltransform") DOXY_FN(RobotBase,SetController)) - .def("SetController",setcontroller3,DOXY_FN(RobotBase,SetController)) - .def("SetActiveDOFs",psetactivedofs1, PY_ARGS("dofindices") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int")) - .def("SetActiveDOFs",psetactivedofs2, PY_ARGS("dofindices","affine") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int")) - .def("SetActiveDOFs",psetactivedofs3, PY_ARGS("dofindices","affine","rotationaxis") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int; const Vector")) - .def("GetActiveDOF",&PyRobotBase::GetActiveDOF, DOXY_FN(RobotBase,GetActiveDOF)) - .def("GetAffineDOF",&PyRobotBase::GetAffineDOF, DOXY_FN(RobotBase,GetAffineDOF)) - .def("GetAffineDOFIndex",&PyRobotBase::GetAffineDOFIndex, PY_ARGS("index") DOXY_FN(RobotBase,GetAffineDOFIndex)) - .def("GetAffineRotationAxis",&PyRobotBase::GetAffineRotationAxis, DOXY_FN(RobotBase,GetAffineRotationAxis)) - .def("SetAffineTranslationLimits",&PyRobotBase::SetAffineTranslationLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineTranslationLimits)) - .def("SetAffineRotationAxisLimits",&PyRobotBase::SetAffineRotationAxisLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineRotationAxisLimits)) - .def("SetAffineRotation3DLimits",&PyRobotBase::SetAffineRotation3DLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineRotation3DLimits)) - .def("SetAffineRotationQuatLimits",&PyRobotBase::SetAffineRotationQuatLimits, PY_ARGS("quatangle") DOXY_FN(RobotBase,SetAffineRotationQuatLimits)) - .def("SetAffineTranslationMaxVels",&PyRobotBase::SetAffineTranslationMaxVels, PY_ARGS("vels") DOXY_FN(RobotBase,SetAffineTranslationMaxVels)) - .def("SetAffineRotationAxisMaxVels",&PyRobotBase::SetAffineRotationAxisMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotationAxisMaxVels)) - .def("SetAffineRotation3DMaxVels",&PyRobotBase::SetAffineRotation3DMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotation3DMaxVels)) - .def("SetAffineRotationQuatMaxVels",&PyRobotBase::SetAffineRotationQuatMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotationQuatMaxVels)) - .def("SetAffineTranslationResolution",&PyRobotBase::SetAffineTranslationResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineTranslationResolution)) - .def("SetAffineRotationAxisResolution",&PyRobotBase::SetAffineRotationAxisResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotationAxisResolution)) - .def("SetAffineRotation3DResolution",&PyRobotBase::SetAffineRotation3DResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotation3DResolution)) - .def("SetAffineRotationQuatResolution",&PyRobotBase::SetAffineRotationQuatResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotationQuatResolution)) - .def("SetAffineTranslationWeights",&PyRobotBase::SetAffineTranslationWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineTranslationWeights)) - .def("SetAffineRotationAxisWeights",&PyRobotBase::SetAffineRotationAxisWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotationAxisWeights)) - .def("SetAffineRotation3DWeights",&PyRobotBase::SetAffineRotation3DWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotation3DWeights)) - .def("SetAffineRotationQuatWeights",&PyRobotBase::SetAffineRotationQuatWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotationQuatWeights)) - .def("GetAffineTranslationLimits",&PyRobotBase::GetAffineTranslationLimits, DOXY_FN(RobotBase,GetAffineTranslationLimits)) - .def("GetAffineRotationAxisLimits",&PyRobotBase::GetAffineRotationAxisLimits, DOXY_FN(RobotBase,GetAffineRotationAxisLimits)) - .def("GetAffineRotation3DLimits",&PyRobotBase::GetAffineRotation3DLimits, DOXY_FN(RobotBase,GetAffineRotation3DLimits)) - .def("GetAffineRotationQuatLimits",&PyRobotBase::GetAffineRotationQuatLimits, DOXY_FN(RobotBase,GetAffineRotationQuatLimits)) - .def("GetAffineTranslationMaxVels",&PyRobotBase::GetAffineTranslationMaxVels, DOXY_FN(RobotBase,GetAffineTranslationMaxVels)) - .def("GetAffineRotationAxisMaxVels",&PyRobotBase::GetAffineRotationAxisMaxVels, DOXY_FN(RobotBase,GetAffineRotationAxisMaxVels)) - .def("GetAffineRotation3DMaxVels",&PyRobotBase::GetAffineRotation3DMaxVels, DOXY_FN(RobotBase,GetAffineRotation3DMaxVels)) - .def("GetAffineRotationQuatMaxVels",&PyRobotBase::GetAffineRotationQuatMaxVels, DOXY_FN(RobotBase,GetAffineRotationQuatMaxVels)) - .def("GetAffineTranslationResolution",&PyRobotBase::GetAffineTranslationResolution, DOXY_FN(RobotBase,GetAffineTranslationResolution)) - .def("GetAffineRotationAxisResolution",&PyRobotBase::GetAffineRotationAxisResolution, DOXY_FN(RobotBase,GetAffineRotationAxisResolution)) - .def("GetAffineRotation3DResolution",&PyRobotBase::GetAffineRotation3DResolution, DOXY_FN(RobotBase,GetAffineRotation3DResolution)) - .def("GetAffineRotationQuatResolution",&PyRobotBase::GetAffineRotationQuatResolution, DOXY_FN(RobotBase,GetAffineRotationQuatResolution)) - .def("GetAffineTranslationWeights",&PyRobotBase::GetAffineTranslationWeights, DOXY_FN(RobotBase,GetAffineTranslationWeights)) - .def("GetAffineRotationAxisWeights",&PyRobotBase::GetAffineRotationAxisWeights, DOXY_FN(RobotBase,GetAffineRotationAxisWeights)) - .def("GetAffineRotation3DWeights",&PyRobotBase::GetAffineRotation3DWeights, DOXY_FN(RobotBase,GetAffineRotation3DWeights)) - .def("GetAffineRotationQuatWeights",&PyRobotBase::GetAffineRotationQuatWeights, DOXY_FN(RobotBase,GetAffineRotationQuatWeights)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetActiveDOFValues", &PyRobotBase::SetActiveDOFValues, - "values"_a, - "checklimits"_a = (int) KinBody::CLA_CheckLimits, - DOXY_FN(RobotBase, SetActiveDOFValues) - ) -#else - .def("SetActiveDOFValues",&PyRobotBase::SetActiveDOFValues,SetActiveDOFValues_overloads(PY_ARGS("values","checklimits") DOXY_FN(RobotBase,SetActiveDOFValues))) -#endif - .def("GetActiveDOFValues",&PyRobotBase::GetActiveDOFValues, DOXY_FN(RobotBase,GetActiveDOFValues)) - .def("GetActiveDOFWeights",&PyRobotBase::GetActiveDOFWeights, DOXY_FN(RobotBase,GetActiveDOFWeights)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetActiveDOFVelocities", &PyRobotBase::SetActiveDOFVelocities, - "velocities"_a, - "checklimits"_a = (int) KinBody::CLA_CheckLimits, - DOXY_FN(RobotBase, SetActiveDOFVelocities)) -#else - .def("SetActiveDOFVelocities",&PyRobotBase::SetActiveDOFVelocities, SetActiveDOFVelocities_overloads(PY_ARGS("velocities","checklimits") DOXY_FN(RobotBase,SetActiveDOFVelocities))) -#endif - .def("GetActiveDOFVelocities",&PyRobotBase::GetActiveDOFVelocities, DOXY_FN(RobotBase,GetActiveDOFVelocities)) - .def("GetActiveDOFLimits",&PyRobotBase::GetActiveDOFLimits, DOXY_FN(RobotBase,GetActiveDOFLimits)) - .def("GetActiveDOFMaxVel",&PyRobotBase::GetActiveDOFMaxVel, DOXY_FN(RobotBase,GetActiveDOFMaxVel)) - .def("GetActiveDOFMaxAccel",&PyRobotBase::GetActiveDOFMaxAccel, DOXY_FN(RobotBase,GetActiveDOFMaxAccel)) - .def("GetActiveDOFMaxJerk",&PyRobotBase::GetActiveDOFMaxJerk, DOXY_FN(RobotBase,GetActiveDOFMaxJerk)) - .def("GetActiveDOFHardMaxVel",&PyRobotBase::GetActiveDOFHardMaxVel, DOXY_FN(RobotBase,GetActiveDOFHardMaxVel)) - .def("GetActiveDOFHardMaxAccel",&PyRobotBase::GetActiveDOFHardMaxAccel, DOXY_FN(RobotBase,GetActiveDOFHardMaxAccel)) - .def("GetActiveDOFHardMaxJerk",&PyRobotBase::GetActiveDOFHardMaxJerk, DOXY_FN(RobotBase,GetActiveDOFHardMaxJerk)) - .def("GetActiveDOFResolutions",&PyRobotBase::GetActiveDOFResolutions, DOXY_FN(RobotBase,GetActiveDOFResolutions)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetActiveConfigurationSpecification", &PyRobotBase::GetActiveConfigurationSpecification, - "interpolation"_a = "", - DOXY_FN(RobotBase, GetActiveConfigurationSpecification) - ) -#else - .def("GetActiveConfigurationSpecification",&PyRobotBase::GetActiveConfigurationSpecification, GetActiveConfigurationSpecification_overloads(PY_ARGS("interpolation") DOXY_FN(RobotBase,GetActiveConfigurationSpecification))) -#endif - .def("GetActiveJointIndices",&PyRobotBase::GetActiveJointIndices) - .def("GetActiveDOFIndices",&PyRobotBase::GetActiveDOFIndices, DOXY_FN(RobotBase,GetActiveDOFIndices)) - .def("SubtractActiveDOFValues",&PyRobotBase::SubtractActiveDOFValues, PY_ARGS("values0","values1") DOXY_FN(RobotBase,SubtractActiveDOFValues)) - .def("CalculateActiveJacobian",&PyRobotBase::CalculateActiveJacobian, PY_ARGS("linkindex","offset") DOXY_FN(RobotBase,CalculateActiveJacobian "int; const Vector; std::vector")) - .def("CalculateActiveRotationJacobian",&PyRobotBase::CalculateActiveRotationJacobian, PY_ARGS("linkindex","quat") DOXY_FN(RobotBase,CalculateActiveRotationJacobian "int; const Vector; std::vector")) - .def("CalculateActiveAngularVelocityJacobian",&PyRobotBase::CalculateActiveAngularVelocityJacobian, PY_ARGS("linkindex") DOXY_FN(RobotBase,CalculateActiveAngularVelocityJacobian "int; std::vector")) - .def("Grab",pgrab1, PY_ARGS("body") DOXY_FN(RobotBase,Grab "KinBodyPtr")) - .def("Grab",pgrab3, PY_ARGS("body","grablink") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr")) - .def("Grab",pgrab5, PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr; Linkptr; rapidjson::Document")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, - "linkindex"_a, - "linktrans"_a, - "report"_a = py::none_(), // PyCollisionReportPtr(), - DOXY_FN(RobotBase,CheckLinkSelfCollision) - ) -#else - .def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, CheckLinkSelfCollision_overloads(PY_ARGS("linkindex", "linktrans", "report") DOXY_FN(RobotBase,CheckLinkSelfCollision))) -#endif - .def("WaitForController",&PyRobotBase::WaitForController,PY_ARGS("timeout") "Wait until the robot controller is done") - .def("GetRobotStructureHash",&PyRobotBase::GetRobotStructureHash, DOXY_FN(RobotBase,GetRobotStructureHash)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("CreateRobotStateSaver",&PyRobotBase::CreateRobotStateSaver, - "options"_a = py::none_(), - "Creates an object that can be entered using 'with' and returns a RobotStateSaver" - ) -#else - .def("CreateRobotStateSaver",&PyRobotBase::CreateRobotStateSaver, CreateRobotStateSaver_overloads(PY_ARGS("options") "Creates an object that can be entered using 'with' and returns a RobotStateSaver")[return_value_policy()]) -#endif - .def("__repr__", &PyRobotBase::__repr__) - .def("__str__", &PyRobotBase::__str__) - .def("__unicode__", &PyRobotBase::__unicode__) - ; - robot.attr("DOFAffine") = dofaffine; // deprecated (11/10/04) - robot.attr("ManipulatorInfo") = manipulatorinfo; - robot.attr("AttachedSensorInfo") = attachedsensorinfo; - object (PyRobotBase::PyManipulator::*pmanipik)(object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolution; object (PyRobotBase::PyManipulator::*pmanipikf)(object, object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolution; object (PyRobotBase::PyManipulator::*pmanipiks)(object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolutions; @@ -2995,6 +2785,218 @@ void init_openravepy_robot() .def("__ne__",&PyRobotBase::PyConnectedBody::__ne__) .def("__hash__",&PyRobotBase::PyConnectedBody::__hash__); + void (PyRobotBase::*psetactivedofs1)(const object&) = &PyRobotBase::SetActiveDOFs; + void (PyRobotBase::*psetactivedofs2)(const object&, int) = &PyRobotBase::SetActiveDOFs; + void (PyRobotBase::*psetactivedofs3)(const object&, int, object) = &PyRobotBase::SetActiveDOFs; + + bool (PyRobotBase::*pgrab1)(PyKinBodyPtr) = &PyRobotBase::Grab; + bool (PyRobotBase::*pgrab3)(PyKinBodyPtr, object) = &PyRobotBase::Grab; + bool (PyRobotBase::*pgrab5)(PyKinBodyPtr, object, object, object) = &PyRobotBase::Grab; + + PyRobotBase::PyManipulatorPtr (PyRobotBase::*setactivemanipulator2)(const std::string&) = &PyRobotBase::SetActiveManipulator; + PyRobotBase::PyManipulatorPtr (PyRobotBase::*setactivemanipulator3)(PyRobotBase::PyManipulatorPtr) = &PyRobotBase::SetActiveManipulator; + + object (PyRobotBase::*GetManipulators1)() = &PyRobotBase::GetManipulators; + object (PyRobotBase::*GetManipulators2)(const string &) = &PyRobotBase::GetManipulators; + bool (PyRobotBase::*setcontroller1)(PyControllerBasePtr,const string &) = &PyRobotBase::SetController; + bool (PyRobotBase::*setcontroller2)(PyControllerBasePtr,object,int) = &PyRobotBase::SetController; + bool (PyRobotBase::*setcontroller3)(PyControllerBasePtr) = &PyRobotBase::SetController; + bool (PyRobotBase::*initrobot)(object, object, object, object, const std::string&) = &PyRobotBase::Init; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("Init", initrobot, + "linkinfos"_a, + "jointinfos"_a, + "manipinfos"_a, + "attachedsensorinfos"_a, + "uri"_a = "", + DOXY_FN(RobotBase, Init) + ); +#else + robot.def("Init", initrobot, Init_overloads(PY_ARGS("linkinfos", "jointinfos", "manipinfos", "attachedsensorinfos", "uri") DOXY_FN(RobotBase, Init))); +#endif + robot.def("GetManipulators",GetManipulators1, DOXY_FN(RobotBase,GetManipulators)); + robot.def("GetManipulators",GetManipulators2, PY_ARGS("manipname") DOXY_FN(RobotBase,GetManipulators)); + robot.def("GetManipulator",&PyRobotBase::GetManipulator,PY_ARGS("manipname") "Return the manipulator whose name matches"); + robot.def("SetActiveManipulator",setactivemanipulator2, PY_ARGS("manipname") DOXY_FN(RobotBase,SetActiveManipulator "const std::string")); + robot.def("SetActiveManipulator",setactivemanipulator3,PY_ARGS("manip") "Set the active manipulator given a pointer"); + robot.def("GetActiveManipulator",&PyRobotBase::GetActiveManipulator, DOXY_FN(RobotBase,GetActiveManipulator)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("AddManipulator", &PyRobotBase::AddManipulator, + "manipinfo"_a, + "removeduplicate"_a = false, + DOXY_FN(RobotBase, AddManipulator) + ); +#else + robot.def("AddManipulator",&PyRobotBase::AddManipulator, AddManipulator_overloads(PY_ARGS("manipinfo", "removeduplicate") DOXY_FN(RobotBase,AddManipulator))); +#endif + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("InitFromRobotInfo", &PyRobotBase::InitFromRobotInfo, + "info"_a, + DOXY_FN(RobotBase, InitFromRobotInfo)); +#else + robot.def("InitFromRobotInfo",&PyRobotBase::InitFromRobotInfo, DOXY_FN(RobotBase, InitFromRobotInfo)); +#endif + robot.def("ExtractInfo", &PyRobotBase::ExtractInfo, DOXY_FN(RobotBase, ExtractInfo)); + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("AddAttachedSensor",&PyRobotBase::AddAttachedSensor, + "attachedsensorinfo"_a, + "removeduplicate"_a = false, + DOXY_FN(RobotBase, AddAttachedSensor) + ); +#else + robot.def("AddAttachedSensor",&PyRobotBase::AddAttachedSensor, AddAttachedSensor_overloads(PY_ARGS("attachedsensorinfo", "removeduplicate") DOXY_FN(RobotBase,AddAttachedSensor))); +#endif + robot.def("RemoveAttachedSensor",&PyRobotBase::RemoveAttachedSensor, PY_ARGS("attsensor") DOXY_FN(RobotBase,RemoveAttachedSensor)); + robot.def("RemoveManipulator",&PyRobotBase::RemoveManipulator, PY_ARGS("manip") DOXY_FN(RobotBase,RemoveManipulator)); + robot.def("GetAttachedSensors",&PyRobotBase::GetAttachedSensors, DOXY_FN(RobotBase,GetAttachedSensors)); + robot.def("GetAttachedSensor",&PyRobotBase::GetAttachedSensor,PY_ARGS("sensorname") "Return the attached sensor whose name matches"); + robot.def("GetSensors",&PyRobotBase::GetSensors); + robot.def("GetSensor",&PyRobotBase::GetSensor,PY_ARGS("sensorname") DOXY_FN(RobotBase, GetSensor)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("AddConnectedBody",&PyRobotBase::AddConnectedBody, + "connectedbodyinfo"_a, + "removeduplicate"_a = false, + DOXY_FN(RobotBase, AddConnectedBody) + ); +#else + robot.def("AddConnectedBody",&PyRobotBase::AddConnectedBody, AddConnectedBody_overloads(PY_ARGS("connectedbodyinfo", "removeduplicate") DOXY_FN(RobotBase,AddConnectedBody))); +#endif + robot.def("RemoveConnectedBody",&PyRobotBase::RemoveConnectedBody, PY_ARGS("connectedbody") DOXY_FN(RobotBase,RemoveConnectedBody)); + robot.def("GetConnectedBodies",&PyRobotBase::GetConnectedBodies, DOXY_FN(RobotBase,GetConnectedBodies)); + robot.def("GetConnectedBody",&PyRobotBase::GetConnectedBody, PY_ARGS("bodyname") DOXY_FN(RobotBase,GetConnectedBody)); + robot.def("GetConnectedBodyActiveStates",&PyRobotBase::GetConnectedBodyActiveStates, DOXY_FN(RobotBase,GetConnectedBodyActiveStates)); + robot.def("SetConnectedBodyActiveStates",&PyRobotBase::SetConnectedBodyActiveStates, DOXY_FN(RobotBase,SetConnectedBodyActiveStates)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("AddGripperInfo",&PyRobotBase::AddGripperInfo, + "gripperInfo"_a, + "removeduplicate"_a = false, + DOXY_FN(RobotBase, AddGripperInfo) + ); +#else + robot.def("AddGripperInfo",&PyRobotBase::AddGripperInfo, AddGripperInfo_overloads(PY_ARGS("gripperInfo", "removeduplicate") DOXY_FN(RobotBase,AddGripperInfo))); +#endif + robot.def("RemoveGripperInfo",&PyRobotBase::RemoveGripperInfo, PY_ARGS("name") DOXY_FN(RobotBase,RemoveGripperInfo)); + robot.def("GetGripperInfo",&PyRobotBase::GetGripperInfo, PY_ARGS("name") DOXY_FN(RobotBase,GetGripperInfo)); + robot.def("GetGripperInfos",&PyRobotBase::GetGripperInfos, DOXY_FN(RobotBase,GetGripperInfos)); + robot.def("GetController",&PyRobotBase::GetController, DOXY_FN(RobotBase,GetController)); + robot.def("SetController",setcontroller1,DOXY_FN(RobotBase,SetController)); + robot.def("SetController",setcontroller2, PY_ARGS("robot","dofindices","controltransform") DOXY_FN(RobotBase,SetController)); + robot.def("SetController",setcontroller3,DOXY_FN(RobotBase,SetController)); + robot.def("SetActiveDOFs",psetactivedofs1, PY_ARGS("dofindices") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int")); + robot.def("SetActiveDOFs",psetactivedofs2, PY_ARGS("dofindices","affine") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int")); + robot.def("SetActiveDOFs",psetactivedofs3, PY_ARGS("dofindices","affine","rotationaxis") DOXY_FN(RobotBase,SetActiveDOFs "const std::vector; int; const Vector")); + robot.def("GetActiveDOF",&PyRobotBase::GetActiveDOF, DOXY_FN(RobotBase,GetActiveDOF)); + robot.def("GetAffineDOF",&PyRobotBase::GetAffineDOF, DOXY_FN(RobotBase,GetAffineDOF)); + robot.def("GetAffineDOFIndex",&PyRobotBase::GetAffineDOFIndex, PY_ARGS("index") DOXY_FN(RobotBase,GetAffineDOFIndex)); + robot.def("GetAffineRotationAxis",&PyRobotBase::GetAffineRotationAxis, DOXY_FN(RobotBase,GetAffineRotationAxis)); + robot.def("SetAffineTranslationLimits",&PyRobotBase::SetAffineTranslationLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineTranslationLimits)); + robot.def("SetAffineRotationAxisLimits",&PyRobotBase::SetAffineRotationAxisLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineRotationAxisLimits)); + robot.def("SetAffineRotation3DLimits",&PyRobotBase::SetAffineRotation3DLimits, PY_ARGS("lower","upper") DOXY_FN(RobotBase,SetAffineRotation3DLimits)); + robot.def("SetAffineRotationQuatLimits",&PyRobotBase::SetAffineRotationQuatLimits, PY_ARGS("quatangle") DOXY_FN(RobotBase,SetAffineRotationQuatLimits)); + robot.def("SetAffineTranslationMaxVels",&PyRobotBase::SetAffineTranslationMaxVels, PY_ARGS("vels") DOXY_FN(RobotBase,SetAffineTranslationMaxVels)); + robot.def("SetAffineRotationAxisMaxVels",&PyRobotBase::SetAffineRotationAxisMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotationAxisMaxVels)); + robot.def("SetAffineRotation3DMaxVels",&PyRobotBase::SetAffineRotation3DMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotation3DMaxVels)); + robot.def("SetAffineRotationQuatMaxVels",&PyRobotBase::SetAffineRotationQuatMaxVels, PY_ARGS("velocity") DOXY_FN(RobotBase,SetAffineRotationQuatMaxVels)); + robot.def("SetAffineTranslationResolution",&PyRobotBase::SetAffineTranslationResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineTranslationResolution)); + robot.def("SetAffineRotationAxisResolution",&PyRobotBase::SetAffineRotationAxisResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotationAxisResolution)); + robot.def("SetAffineRotation3DResolution",&PyRobotBase::SetAffineRotation3DResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotation3DResolution)); + robot.def("SetAffineRotationQuatResolution",&PyRobotBase::SetAffineRotationQuatResolution, PY_ARGS("resolution") DOXY_FN(RobotBase,SetAffineRotationQuatResolution)); + robot.def("SetAffineTranslationWeights",&PyRobotBase::SetAffineTranslationWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineTranslationWeights)); + robot.def("SetAffineRotationAxisWeights",&PyRobotBase::SetAffineRotationAxisWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotationAxisWeights)); + robot.def("SetAffineRotation3DWeights",&PyRobotBase::SetAffineRotation3DWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotation3DWeights)); + robot.def("SetAffineRotationQuatWeights",&PyRobotBase::SetAffineRotationQuatWeights, PY_ARGS("weights") DOXY_FN(RobotBase,SetAffineRotationQuatWeights)); + robot.def("GetAffineTranslationLimits",&PyRobotBase::GetAffineTranslationLimits, DOXY_FN(RobotBase,GetAffineTranslationLimits)); + robot.def("GetAffineRotationAxisLimits",&PyRobotBase::GetAffineRotationAxisLimits, DOXY_FN(RobotBase,GetAffineRotationAxisLimits)); + robot.def("GetAffineRotation3DLimits",&PyRobotBase::GetAffineRotation3DLimits, DOXY_FN(RobotBase,GetAffineRotation3DLimits)); + robot.def("GetAffineRotationQuatLimits",&PyRobotBase::GetAffineRotationQuatLimits, DOXY_FN(RobotBase,GetAffineRotationQuatLimits)); + robot.def("GetAffineTranslationMaxVels",&PyRobotBase::GetAffineTranslationMaxVels, DOXY_FN(RobotBase,GetAffineTranslationMaxVels)); + robot.def("GetAffineRotationAxisMaxVels",&PyRobotBase::GetAffineRotationAxisMaxVels, DOXY_FN(RobotBase,GetAffineRotationAxisMaxVels)); + robot.def("GetAffineRotation3DMaxVels",&PyRobotBase::GetAffineRotation3DMaxVels, DOXY_FN(RobotBase,GetAffineRotation3DMaxVels)); + robot.def("GetAffineRotationQuatMaxVels",&PyRobotBase::GetAffineRotationQuatMaxVels, DOXY_FN(RobotBase,GetAffineRotationQuatMaxVels)); + robot.def("GetAffineTranslationResolution",&PyRobotBase::GetAffineTranslationResolution, DOXY_FN(RobotBase,GetAffineTranslationResolution)); + robot.def("GetAffineRotationAxisResolution",&PyRobotBase::GetAffineRotationAxisResolution, DOXY_FN(RobotBase,GetAffineRotationAxisResolution)); + robot.def("GetAffineRotation3DResolution",&PyRobotBase::GetAffineRotation3DResolution, DOXY_FN(RobotBase,GetAffineRotation3DResolution)); + robot.def("GetAffineRotationQuatResolution",&PyRobotBase::GetAffineRotationQuatResolution, DOXY_FN(RobotBase,GetAffineRotationQuatResolution)); + robot.def("GetAffineTranslationWeights",&PyRobotBase::GetAffineTranslationWeights, DOXY_FN(RobotBase,GetAffineTranslationWeights)); + robot.def("GetAffineRotationAxisWeights",&PyRobotBase::GetAffineRotationAxisWeights, DOXY_FN(RobotBase,GetAffineRotationAxisWeights)); + robot.def("GetAffineRotation3DWeights",&PyRobotBase::GetAffineRotation3DWeights, DOXY_FN(RobotBase,GetAffineRotation3DWeights)); + robot.def("GetAffineRotationQuatWeights",&PyRobotBase::GetAffineRotationQuatWeights, DOXY_FN(RobotBase,GetAffineRotationQuatWeights)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("SetActiveDOFValues", &PyRobotBase::SetActiveDOFValues, + "values"_a, + "checklimits"_a = (int) KinBody::CLA_CheckLimits, + DOXY_FN(RobotBase, SetActiveDOFValues) + ); +#else + robot.def("SetActiveDOFValues",&PyRobotBase::SetActiveDOFValues,SetActiveDOFValues_overloads(PY_ARGS("values","checklimits") DOXY_FN(RobotBase,SetActiveDOFValues))); +#endif + robot.def("GetActiveDOFValues",&PyRobotBase::GetActiveDOFValues, DOXY_FN(RobotBase,GetActiveDOFValues)); + robot.def("GetActiveDOFWeights",&PyRobotBase::GetActiveDOFWeights, DOXY_FN(RobotBase,GetActiveDOFWeights)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("SetActiveDOFVelocities", &PyRobotBase::SetActiveDOFVelocities, + "velocities"_a, + "checklimits"_a = (int) KinBody::CLA_CheckLimits, + DOXY_FN(RobotBase, SetActiveDOFVelocities)); +#else + robot.def("SetActiveDOFVelocities",&PyRobotBase::SetActiveDOFVelocities, SetActiveDOFVelocities_overloads(PY_ARGS("velocities","checklimits") DOXY_FN(RobotBase,SetActiveDOFVelocities))); +#endif + robot.def("GetActiveDOFVelocities",&PyRobotBase::GetActiveDOFVelocities, DOXY_FN(RobotBase,GetActiveDOFVelocities)); + robot.def("GetActiveDOFLimits",&PyRobotBase::GetActiveDOFLimits, DOXY_FN(RobotBase,GetActiveDOFLimits)); + robot.def("GetActiveDOFMaxVel",&PyRobotBase::GetActiveDOFMaxVel, DOXY_FN(RobotBase,GetActiveDOFMaxVel)); + robot.def("GetActiveDOFMaxAccel",&PyRobotBase::GetActiveDOFMaxAccel, DOXY_FN(RobotBase,GetActiveDOFMaxAccel)); + robot.def("GetActiveDOFMaxJerk",&PyRobotBase::GetActiveDOFMaxJerk, DOXY_FN(RobotBase,GetActiveDOFMaxJerk)); + robot.def("GetActiveDOFHardMaxVel",&PyRobotBase::GetActiveDOFHardMaxVel, DOXY_FN(RobotBase,GetActiveDOFHardMaxVel)); + robot.def("GetActiveDOFHardMaxAccel",&PyRobotBase::GetActiveDOFHardMaxAccel, DOXY_FN(RobotBase,GetActiveDOFHardMaxAccel)); + robot.def("GetActiveDOFHardMaxJerk",&PyRobotBase::GetActiveDOFHardMaxJerk, DOXY_FN(RobotBase,GetActiveDOFHardMaxJerk)); + robot.def("GetActiveDOFResolutions",&PyRobotBase::GetActiveDOFResolutions, DOXY_FN(RobotBase,GetActiveDOFResolutions)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("GetActiveConfigurationSpecification", &PyRobotBase::GetActiveConfigurationSpecification, + "interpolation"_a = "", + DOXY_FN(RobotBase, GetActiveConfigurationSpecification) + ); +#else + robot.def("GetActiveConfigurationSpecification",&PyRobotBase::GetActiveConfigurationSpecification, GetActiveConfigurationSpecification_overloads(PY_ARGS("interpolation") DOXY_FN(RobotBase,GetActiveConfigurationSpecification))); +#endif + robot.def("GetActiveJointIndices",&PyRobotBase::GetActiveJointIndices); + robot.def("GetActiveDOFIndices",&PyRobotBase::GetActiveDOFIndices, DOXY_FN(RobotBase,GetActiveDOFIndices)); + robot.def("SubtractActiveDOFValues",&PyRobotBase::SubtractActiveDOFValues, PY_ARGS("values0","values1") DOXY_FN(RobotBase,SubtractActiveDOFValues)); + robot.def("CalculateActiveJacobian",&PyRobotBase::CalculateActiveJacobian, PY_ARGS("linkindex","offset") DOXY_FN(RobotBase,CalculateActiveJacobian "int; const Vector; std::vector")); + robot.def("CalculateActiveRotationJacobian",&PyRobotBase::CalculateActiveRotationJacobian, PY_ARGS("linkindex","quat") DOXY_FN(RobotBase,CalculateActiveRotationJacobian "int; const Vector; std::vector")); + robot.def("CalculateActiveAngularVelocityJacobian",&PyRobotBase::CalculateActiveAngularVelocityJacobian, PY_ARGS("linkindex") DOXY_FN(RobotBase,CalculateActiveAngularVelocityJacobian "int; std::vector")); + robot.def("Grab",pgrab1, PY_ARGS("body") DOXY_FN(RobotBase,Grab "KinBodyPtr")); + robot.def("Grab",pgrab3, PY_ARGS("body","grablink") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr")); + robot.def("Grab",pgrab5, PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr; Linkptr; rapidjson::Document")); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, + "linkindex"_a, + "linktrans"_a, + "report"_a = py::none_(), // PyCollisionReportPtr(), + DOXY_FN(RobotBase,CheckLinkSelfCollision) + ); +#else + robot.def("CheckLinkSelfCollision", &PyRobotBase::CheckLinkSelfCollision, CheckLinkSelfCollision_overloads(PY_ARGS("linkindex", "linktrans", "report") DOXY_FN(RobotBase,CheckLinkSelfCollision))); +#endif + robot.def("WaitForController",&PyRobotBase::WaitForController,PY_ARGS("timeout") "Wait until the robot controller is done"); + robot.def("GetRobotStructureHash",&PyRobotBase::GetRobotStructureHash, DOXY_FN(RobotBase,GetRobotStructureHash)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + robot.def("CreateRobotStateSaver",&PyRobotBase::CreateRobotStateSaver, + "options"_a = py::none_(), + "Creates an object that can be entered using 'with' and returns a RobotStateSaver" + ); +#else + robot.def("CreateRobotStateSaver",&PyRobotBase::CreateRobotStateSaver, CreateRobotStateSaver_overloads(PY_ARGS("options") "Creates an object that can be entered using 'with' and returns a RobotStateSaver")[return_value_policy()]); +#endif + robot.def("__repr__", &PyRobotBase::__repr__); + robot.def("__str__", &PyRobotBase::__str__); + robot.def("__unicode__", &PyRobotBase::__unicode__); + + robot.attr("DOFAffine") = dofaffine; // deprecated (11/10/04) + robot.attr("ManipulatorInfo") = manipulatorinfo; + robot.attr("AttachedSensorInfo") = attachedsensorinfo; + #ifdef USE_PYBIND11_PYTHON_BINDINGS class_ >(m, "RobotStateSaver", DOXY_CLASS(Robot::RobotStateSaver)) .def(init(), "robot"_a) From 39375bc2f518faed21958f3c9769950b9457b671 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 18:28:46 +0900 Subject: [PATCH 03/13] splitted init_openravepy_global_functions --- python/bindings/include/openravepy/openravepy_int.h | 2 ++ python/bindings/openravepy_global.cpp | 11 +++++++++++ python/bindings/openravepy_int.cpp | 1 + 3 files changed, 14 insertions(+) diff --git a/python/bindings/include/openravepy/openravepy_int.h b/python/bindings/include/openravepy/openravepy_int.h index 50714d1d8b..6b2b410647 100644 --- a/python/bindings/include/openravepy/openravepy_int.h +++ b/python/bindings/include/openravepy/openravepy_int.h @@ -872,8 +872,10 @@ OPENRAVEPY_API PyConnectedBodyInfoPtr toPyConnectedBodyInfo(const RobotBase::Con OPENRAVEPY_API PyInterfaceBasePtr RaveCreateInterface(PyEnvironmentBasePtr pyenv, InterfaceType type, const std::string& name); #ifdef USE_PYBIND11_PYTHON_BINDINGS void init_openravepy_global(py::module& m); +void init_openravepy_global_functions(py::module& m); #else void init_openravepy_global(); +void init_openravepy_global_functions(); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS void InitPlanningUtils(py::module& m); diff --git a/python/bindings/openravepy_global.cpp b/python/bindings/openravepy_global.cpp index 992f3c1e3a..fad7c453dd 100644 --- a/python/bindings/openravepy_global.cpp +++ b/python/bindings/openravepy_global.cpp @@ -2039,6 +2039,17 @@ void init_openravepy_global() #endif ; } +} + +#ifdef USE_PYBIND11_PYTHON_BINDINGS +void init_openravepy_global_functions(py::module& m) +#else +void init_openravepy_global_functions() +#endif +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + using namespace py::literals; // "..."_a +#endif #ifdef USE_PYBIND11_PYTHON_BINDINGS m.def("RaveSetDebugLevel",openravepy::pyRaveSetDebugLevel, PY_ARGS("level") DOXY_FN1(RaveSetDebugLevel)); #else diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index c1d3687768..d8a0b69a1e 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3319,6 +3319,7 @@ Because race conditions can pop up when trying to lock the openrave environment openravepy::init_openravepy_viewer(m); openravepy::InitPlanningUtils(m); + openravepy::init_openravepy_global_functions(m); #else openravepy::init_openravepy_global(); openravepy::InitPlanningUtils(); From 8f655e8d78efa4fb5164c78c3300fad471dc4c87 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 21:11:36 +0900 Subject: [PATCH 04/13] revert --- python/bindings/openravepy_trajectory.cpp | 40 +++++++++++------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index 05de6694a9..34ec0b7f07 100644 --- a/python/bindings/openravepy_trajectory.cpp +++ b/python/bindings/openravepy_trajectory.cpp @@ -635,31 +635,31 @@ void init_openravepy_trajectory() void (PyTrajectoryBase::*Insert2)(size_t,object,bool) = &PyTrajectoryBase::Insert; void (PyTrajectoryBase::*Insert3)(size_t,object,PyConfigurationSpecificationPtr) = &PyTrajectoryBase::Insert; void (PyTrajectoryBase::*Insert4)(size_t,object,PyConfigurationSpecificationPtr,bool) = &PyTrajectoryBase::Insert; - // void (PyTrajectoryBase::*Insert5)(size_t, object, OPENRAVE_SHARED_PTR) = &PyTrajectoryBase::Insert; - // void (PyTrajectoryBase::*Insert6)(size_t, object, OPENRAVE_SHARED_PTR, bool) = &PyTrajectoryBase::Insert; + void (PyTrajectoryBase::*Insert5)(size_t, object, OPENRAVE_SHARED_PTR) = &PyTrajectoryBase::Insert; + void (PyTrajectoryBase::*Insert6)(size_t, object, OPENRAVE_SHARED_PTR, bool) = &PyTrajectoryBase::Insert; object (PyTrajectoryBase::*Sample1)(dReal) const = &PyTrajectoryBase::Sample; object (PyTrajectoryBase::*Sample2)(dReal, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::Sample; - // object (PyTrajectoryBase::*Sample3)(dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::Sample; + object (PyTrajectoryBase::*Sample3)(dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::Sample; object (PyTrajectoryBase::*SampleFromPrevious1)(object, dReal, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SampleFromPrevious; - // object (PyTrajectoryBase::*SampleFromPrevious2)(object, dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SampleFromPrevious; + object (PyTrajectoryBase::*SampleFromPrevious2)(object, dReal, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SampleFromPrevious; object (PyTrajectoryBase::*SamplePoints2D1)(object) const = &PyTrajectoryBase::SamplePoints2D; object (PyTrajectoryBase::*SamplePoints2D2)(object, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SamplePoints2D; - // object (PyTrajectoryBase::*SamplePoints2D3)(object, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePoints2D; + object (PyTrajectoryBase::*SamplePoints2D3)(object, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePoints2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D1)(dReal, bool) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D2)(dReal, bool, PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; - // object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D3)(dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; + object (PyTrajectoryBase::*SamplePointsSameDeltaTime2D3)(dReal, bool, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::SamplePointsSameDeltaTime2D; object (PyTrajectoryBase::*GetWaypoints1)(size_t,size_t) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints2)(size_t,size_t,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoints; - // object (PyTrajectoryBase::*GetWaypoints3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints; + object (PyTrajectoryBase::*GetWaypoints3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints; object (PyTrajectoryBase::*GetWaypoints2D1)(size_t,size_t) const = &PyTrajectoryBase::GetWaypoints2D; object (PyTrajectoryBase::*GetWaypoints2D2)(size_t,size_t,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoints2D; - // object (PyTrajectoryBase::*GetWaypoints2D3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints2D; + object (PyTrajectoryBase::*GetWaypoints2D3)(size_t, size_t, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoints2D; object (PyTrajectoryBase::*GetAllWaypoints2D1)() const = &PyTrajectoryBase::GetAllWaypoints2D; object (PyTrajectoryBase::*GetAllWaypoints2D2)(PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetAllWaypoints2D; - // object (PyTrajectoryBase::*GetAllWaypoints2D3)(OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetAllWaypoints2D; + object (PyTrajectoryBase::*GetAllWaypoints2D3)(OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetAllWaypoints2D; object (PyTrajectoryBase::*GetWaypoint1)(int) const = &PyTrajectoryBase::GetWaypoint; object (PyTrajectoryBase::*GetWaypoint2)(int,PyConfigurationSpecificationPtr) const = &PyTrajectoryBase::GetWaypoint; - // object (PyTrajectoryBase::*GetWaypoint3)(int, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoint; + object (PyTrajectoryBase::*GetWaypoint3)(int, OPENRAVE_SHARED_PTR) const = &PyTrajectoryBase::GetWaypoint; object (PyTrajectoryBase::*__getitem__1)(int) const = &PyTrajectoryBase::__getitem__; object (PyTrajectoryBase::*__getitem__2)(slice) const = &PyTrajectoryBase::__getitem__; @@ -674,37 +674,37 @@ void init_openravepy_trajectory() .def("Insert",Insert2, PY_ARGS("index","data","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; bool")) .def("Insert",Insert3, PY_ARGS("index","data","spec") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification")) .def("Insert",Insert4, PY_ARGS("index","data","spec","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification; bool")) - // .def("Insert",Insert5, PY_ARGS("index","data","group") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group")) - // .def("Insert",Insert6, PY_ARGS("index","data","group","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group; bool")) + .def("Insert",Insert5, PY_ARGS("index","data","group") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group")) + .def("Insert",Insert6, PY_ARGS("index","data","group","overwrite") DOXY_FN(TrajectoryBase,Insert "size_t; const std::vector; const ConfigurationSpecification::Group; bool")) .def("Remove",&PyTrajectoryBase::Remove, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase,Remove)) .def("Sample",Sample1, PY_ARGS("time") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal")) .def("Sample",Sample2, PY_ARGS("time","spec") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification")) - // .def("Sample",Sample3, PY_ARGS("time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) + .def("Sample",Sample3, PY_ARGS("time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) .def("SampleFromPrevious", SampleFromPrevious1, PY_ARGS("data","time","spec") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification")) - //.def("SampleFromPrevious", SampleFromPrevious2, PY_ARGS("data","time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) + .def("SampleFromPrevious", SampleFromPrevious2, PY_ARGS("data","time","group") DOXY_FN(TrajectoryBase,Sample "std::vector; dReal; const ConfigurationSpecification::Group")) .def("SamplePoints2D",SamplePoints2D1, PY_ARGS("times") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector")) .def("SamplePoints2D",SamplePoints2D2, PY_ARGS("times","spec") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification")) - //.def("SamplePoints2D",SamplePoints2D3, PY_ARGS("times","group") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification::Group")) + .def("SamplePoints2D",SamplePoints2D3, PY_ARGS("times","group") DOXY_FN(TrajectoryBase,SamplePoints2D "std::vector; std::vector; const ConfigurationSpecification::Group")) .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D1, PY_ARGS("deltatime","ensurelastpoint") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool")) .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D2, PY_ARGS("deltatime","ensurelastpoint","spec") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification")) #ifdef USE_PYBIND11_PYTHON_BINDINGS // why boost::python can resolve this overload? - // .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D3, PY_ARGS("deltatime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification::Group")) + .def("SamplePointsSameDeltaTime2D",SamplePointsSameDeltaTime2D3, PY_ARGS("deltatime","ensurelastpoint","group") DOXY_FN(TrajectoryBase,SamplePointsSameDeltaTime2D "dReal; bool; const ConfigurationSpecification::Group")) #endif .def("GetConfigurationSpecification",&PyTrajectoryBase::GetConfigurationSpecification,DOXY_FN(TrajectoryBase,GetConfigurationSpecification)) .def("GetNumWaypoints",&PyTrajectoryBase::GetNumWaypoints,DOXY_FN(TrajectoryBase,GetNumWaypoints)) .def("GetWaypoints",GetWaypoints1, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetWaypoints",GetWaypoints2, PY_ARGS("startindex","endindex","spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - // .def("GetWaypoints",GetWaypoints3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&::Group")) + .def("GetWaypoints",GetWaypoints3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&::Group")) .def("GetWaypoints2D",GetWaypoints2D1, PY_ARGS("startindex","endindex") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetWaypoints2D",GetWaypoints2D2, PY_ARGS("startindex","endindex","spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - // .def("GetWaypoints2D",GetWaypoints2D3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) + .def("GetWaypoints2D",GetWaypoints2D3, PY_ARGS("startindex","endindex","group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) .def("GetAllWaypoints2D",GetAllWaypoints2D1,DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector")) .def("GetAllWaypoints2D",GetAllWaypoints2D2, PY_ARGS("spec") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification&")) - // .def("GetAllWaypoints2D",GetAllWaypoints2D3, PY_ARGS("group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) + .def("GetAllWaypoints2D",GetAllWaypoints2D3, PY_ARGS("group") DOXY_FN(TrajectoryBase, GetWaypoints "size_t; size_t; std::vector, const ConfigurationSpecification::Group&")) .def("GetWaypoint",GetWaypoint1, PY_ARGS("index") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector")) .def("GetWaypoint",GetWaypoint2, PY_ARGS("index","spec") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification")) - // .def("GetWaypoint",GetWaypoint3, PY_ARGS("index","group") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification::Group")) + .def("GetWaypoint",GetWaypoint3, PY_ARGS("index","group") DOXY_FN(TrajectoryBase, GetWaypoint "int; std::vector; const ConfigurationSpecification::Group")) .def("GetFirstWaypointIndexAfterTime",&PyTrajectoryBase::GetFirstWaypointIndexAfterTime, DOXY_FN(TrajectoryBase, GetFirstWaypointIndexAfterTime)) .def("GetDuration",&PyTrajectoryBase::GetDuration,DOXY_FN(TrajectoryBase, GetDuration)) #ifdef USE_PYBIND11_PYTHON_BINDINGS From 974870bbe89d68730b386f217019006247f72844 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 22:26:46 +0900 Subject: [PATCH 05/13] fix Configuration --- python/bindings/openravepy_global.cpp | 294 +++++++++++++------------- 1 file changed, 148 insertions(+), 146 deletions(-) diff --git a/python/bindings/openravepy_global.cpp b/python/bindings/openravepy_global.cpp index f77ff91c96..972461b7eb 100644 --- a/python/bindings/openravepy_global.cpp +++ b/python/bindings/openravepy_global.cpp @@ -1842,154 +1842,11 @@ void init_openravepy_global() ; { - int (PyConfigurationSpecification::*addgroup1)(const std::string&, int, const std::string&) = &PyConfigurationSpecification::AddGroup; - int (PyConfigurationSpecification::*addgroup2)(const ConfigurationSpecification::Group&) = &PyConfigurationSpecification::AddGroup; - - scope_ configurationspecification = -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_(m, "ConfigurationSpecification",DOXY_CLASS(ConfigurationSpecification)) - .def(init<>()) - .def(init(), "pyspec"_a) - .def(init(), "group"_a) - .def(init(), "xmldata"_a) - .def("__copy__", [](const PyConfigurationSpecification& self){ - return self; - }) - .def("__deepcopy__", [](const PyConfigurationSpecification& pyspec, const py::dict& memo) { - return PyConfigurationSpecification(pyspec._spec); - }) - .def("GetGroupFromName", &PyConfigurationSpecification::GetGroupFromName, DOXY_FN(ConfigurationSpecification,GetGroupFromName)) -#else - class_("ConfigurationSpecification",DOXY_CLASS(ConfigurationSpecification)) - .def(init(py::args("spec")) ) - .def(init(py::args("group")) ) - .def(init(py::args("xmldata")) ) - .def("GetGroupFromName",&PyConfigurationSpecification::GetGroupFromName, return_value_policy(), DOXY_FN(ConfigurationSpecification,GetGroupFromName)) -#endif - .def("SerializeJSON", &PyConfigurationSpecification::SerializeJSON, DOXY_FN(ConfigurationSpecification, SerializeJSON)) - .def("DeserializeJSON", &PyConfigurationSpecification::DeserializeJSON, PY_ARGS("obj") DOXY_FN(ConfigurationSpecification, DeserializeJSON)) - .def("FindCompatibleGroup",&PyConfigurationSpecification::FindCompatibleGroup, DOXY_FN(ConfigurationSpecification,FindCompatibleGroup)) - .def("FindTimeDerivativeGroup",&PyConfigurationSpecification::FindTimeDerivativeGroup, DOXY_FN(ConfigurationSpecification,FindTimeDerivativeGroup)) - .def("GetDOF",&PyConfigurationSpecification::GetDOF,DOXY_FN(ConfigurationSpecification,GetDOF)) - .def("IsValid",&PyConfigurationSpecification::IsValid,DOXY_FN(ConfigurationSpecification,IsValid)) - .def("ResetGroupOffsets",&PyConfigurationSpecification::ResetGroupOffsets,DOXY_FN(ConfigurationSpecification,ResetGroupOffsets)) - .def("AddVelocityGroups",&PyConfigurationSpecification::AddVelocityGroups, PY_ARGS("adddeltatime") DOXY_FN(ConfigurationSpecification,AddVelocityGroups)) - .def("AddDerivativeGroups",&PyConfigurationSpecification::AddDerivativeGroups, PY_ARGS("deriv", "adddeltatime") DOXY_FN(ConfigurationSpecification,AddDerivativeGroups)) - .def("AddDeltaTimeGroup",&PyConfigurationSpecification::AddDeltaTimeGroup,DOXY_FN(ConfigurationSpecification,AddDeltaTimeGroup)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("RemoveGroups", &PyConfigurationSpecification::RemoveGroups, - "groupname"_a, - "exactmatch"_a = true, - DOXY_FN(ConfigurationSpecification, RemoveGroups) - ) -#else - .def("RemoveGroups", &PyConfigurationSpecification::RemoveGroups, RemoveGroups_overloads(PY_ARGS("groupname","exactmatch") DOXY_FN(ConfigurationSpecification, RemoveGroups))) -#endif - .def("AddGroup",addgroup1, PY_ARGS("name","dof","interpolation") DOXY_FN(ConfigurationSpecification,AddGroup "const std::string; int; const std::string")) - .def("AddGroup",addgroup2, PY_ARGS("group") DOXY_FN(ConfigurationSpecification,AddGroup "const")) - .def("ConvertToVelocitySpecification",&PyConfigurationSpecification::ConvertToVelocitySpecification,DOXY_FN(ConfigurationSpecification,ConvertToVelocitySpecification)) - .def("ConvertToDerivativeSpecification",&PyConfigurationSpecification::ConvertToDerivativeSpecification, PY_ARGS("timederivative") DOXY_FN(ConfigurationSpecification, ConvertToDerivativeSpecification)) - .def("GetTimeDerivativeSpecification",&PyConfigurationSpecification::GetTimeDerivativeSpecification,DOXY_FN(ConfigurationSpecification,GetTimeDerivativeSpecification)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ExtractTransform", &PyConfigurationSpecification::ExtractTransform, - "transform"_a, - "data"_a, - "body"_a, - "timederivative"_a = 0, - DOXY_FN(ConfigurationSpecification,ExtractTransform) - ) -#else - .def("ExtractTransform",&PyConfigurationSpecification::ExtractTransform,ExtractTransform_overloads(PY_ARGS("transform","data","body","timederivative") DOXY_FN(ConfigurationSpecification,ExtractTransform))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ExtractAffineValues", &PyConfigurationSpecification::ExtractAffineValues, - "data"_a, - "body"_a, - "affinedofs"_a, - "timederivative"_a = 0, - DOXY_FN(ConfigurationSpecification,ExtractAffineValues) - ) -#else - .def("ExtractAffineValues",&PyConfigurationSpecification::ExtractAffineValues,ExtractAffineValues_overloads(PY_ARGS("data","body","affinedofs","timederivative") DOXY_FN(ConfigurationSpecification,ExtractAffineValues))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ExtractIkParameterization", &PyConfigurationSpecification::ExtractIkParameterization, - "data"_a, - "timederivative"_a = 0, - "robotname"_a = "", - "manipulatorname"_a = "", - DOXY_FN(ConfigurationSpecification, ExtractIkParameterization) - ) -#else - .def("ExtractIkParameterization",&PyConfigurationSpecification::ExtractIkParameterization,ExtractIkParameterization_overloads(PY_ARGS("data","timederivative","robotname","manipulatorname") DOXY_FN(ConfigurationSpecification,ExtractIkParameterization))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ExtractJointValues", &PyConfigurationSpecification::ExtractJointValues, - "data"_a, - "body"_a, - "indices"_a, - "timederivative"_a = 0, - DOXY_FN(ConfigurationSpecification,ExtractJointValues) - ) -#else - .def("ExtractJointValues",&PyConfigurationSpecification::ExtractJointValues,ExtractJointValues_overloads(PY_ARGS("data","body","indices","timederivative") DOXY_FN(ConfigurationSpecification,ExtractJointValues))) -#endif - .def("ExtractDeltaTime",&PyConfigurationSpecification::ExtractDeltaTime, PY_ARGS("data") DOXY_FN(ConfigurationSpecification,ExtractDeltaTime)) - .def("InsertDeltaTime",&PyConfigurationSpecification::InsertDeltaTime, PY_ARGS("data","deltatime") DOXY_FN(ConfigurationSpecification,InsertDeltaTime)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InsertJointValues", &PyConfigurationSpecification::InsertJointValues, - "data"_a, - "values"_a, - "body"_a, - "indices"_a, - "timederivative"_a = 0, - DOXY_FN(ConfigurationSpecification, InsertJointValues) - ) -#else - .def("InsertJointValues",&PyConfigurationSpecification::InsertJointValues, PY_ARGS("data","values","body","indices","timederivative") DOXY_FN(ConfigurationSpecification,InsertJointValues)) -#endif - .def("ExtractUsedBodies", &PyConfigurationSpecification::ExtractUsedBodies, PY_ARGS("env") DOXY_FN(ConfigurationSpecification, ExtractUsedBodies)) - .def("ExtractUsedIndices", &PyConfigurationSpecification::ExtractUsedIndices, PY_ARGS("env") DOXY_FN(ConfigurationSpecification, ExtractUsedIndices)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ConvertData", &PyConfigurationSpecification::ConvertData, - "targetspec"_a, - "sourcedata"_a, - "numpoints"_a, - "env"_a, - "filluninitialized"_a = true, - DOXY_FN(ConfigurationSpecification, ConvertData) - ) -#else - .def("ConvertData", &PyConfigurationSpecification::ConvertData, PY_ARGS("targetspec", "sourcedata", "numpoints", "env", "filluninitialized") DOXY_FN(ConfigurationSpecification, ConvertData)) -#endif - .def("ConvertDataFromPrevious", &PyConfigurationSpecification::ConvertDataFromPrevious, PY_ARGS("targetdata", "targetspec", "sourcedata", "numpoints", "env") DOXY_FN(ConfigurationSpecification, ConvertData)) - .def("GetGroups", &PyConfigurationSpecification::GetGroups, /*PY_ARGS("env")*/ "returns a list of the groups") - .def("__eq__",&PyConfigurationSpecification::__eq__) - .def("__ne__",&PyConfigurationSpecification::__ne__) - .def("__add__",&PyConfigurationSpecification::__add__) - .def("__iadd__",&PyConfigurationSpecification::__iadd__) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def(py::pickle( - [](const PyConfigurationSpecification& pyspec) { - std::stringstream ss; - ss << pyspec._spec; - return py::make_tuple(ss.str()); - }, - [](py::tuple state) { - // __setstate__ - if(state.size() != 1) { - throw std::runtime_error("Invalid state"); - } - return PyConfigurationSpecification(state[0].cast()); - } - )) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + class_ configurationspecification(m, "ConfigurationSpecification",DOXY_CLASS(ConfigurationSpecification)); #else - .def_pickle(ConfigurationSpecification_pickle_suite()) + class_ configurationspecification("ConfigurationSpecification",DOXY_CLASS(ConfigurationSpecification)): #endif - .def("__str__",&PyConfigurationSpecification::__str__) - .def("__unicode__",&PyConfigurationSpecification::__unicode__) - .def("__repr__",&PyConfigurationSpecification::__repr__) - ; { #ifdef USE_PYBIND11_PYTHON_BINDINGS @@ -2005,6 +1862,151 @@ void init_openravepy_global() .def_readwrite("dof",&ConfigurationSpecification::Group::dof) ; } + + int (PyConfigurationSpecification::*addgroup1)(const std::string&, int, const std::string&) = &PyConfigurationSpecification::AddGroup; + int (PyConfigurationSpecification::*addgroup2)(const ConfigurationSpecification::Group&) = &PyConfigurationSpecification::AddGroup; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def(init<>()); + configurationspecification.def(init(), "pyspec"_a); + configurationspecification.def(init(), "group"_a); + configurationspecification.def(init(), "xmldata"_a); + configurationspecification.def("__copy__", [](const PyConfigurationSpecification& self){ + return self; + }); + configurationspecification.def("__deepcopy__", [](const PyConfigurationSpecification& pyspec, const py::dict& memo) { + return PyConfigurationSpecification(pyspec._spec); + }); + configurationspecification.def("GetGroupFromName", &PyConfigurationSpecification::GetGroupFromName, DOXY_FN(ConfigurationSpecification,GetGroupFromName)); +#else + configurationspecification.def(init(py::args("spec")) ); + configurationspecification.def(init(py::args("group")) ); + configurationspecification.def(init(py::args("xmldata")) ); + configurationspecification.def("GetGroupFromName",&PyConfigurationSpecification::GetGroupFromName, return_value_policy(), DOXY_FN(ConfigurationSpecification,GetGroupFromName)); +#endif + configurationspecification.def("SerializeJSON", &PyConfigurationSpecification::SerializeJSON, DOXY_FN(ConfigurationSpecification, SerializeJSON)); + configurationspecification.def("DeserializeJSON", &PyConfigurationSpecification::DeserializeJSON, PY_ARGS("obj") DOXY_FN(ConfigurationSpecification, DeserializeJSON)); + configurationspecification.def("FindCompatibleGroup",&PyConfigurationSpecification::FindCompatibleGroup, DOXY_FN(ConfigurationSpecification,FindCompatibleGroup)); + configurationspecification.def("FindTimeDerivativeGroup",&PyConfigurationSpecification::FindTimeDerivativeGroup, DOXY_FN(ConfigurationSpecification,FindTimeDerivativeGroup)); + configurationspecification.def("GetDOF",&PyConfigurationSpecification::GetDOF,DOXY_FN(ConfigurationSpecification,GetDOF)); + configurationspecification.def("IsValid",&PyConfigurationSpecification::IsValid,DOXY_FN(ConfigurationSpecification,IsValid)); + configurationspecification.def("ResetGroupOffsets",&PyConfigurationSpecification::ResetGroupOffsets,DOXY_FN(ConfigurationSpecification,ResetGroupOffsets)); + configurationspecification.def("AddVelocityGroups",&PyConfigurationSpecification::AddVelocityGroups, PY_ARGS("adddeltatime") DOXY_FN(ConfigurationSpecification,AddVelocityGroups)); + configurationspecification.def("AddDerivativeGroups",&PyConfigurationSpecification::AddDerivativeGroups, PY_ARGS("deriv", "adddeltatime") DOXY_FN(ConfigurationSpecification,AddDerivativeGroups)); + configurationspecification.def("AddDeltaTimeGroup",&PyConfigurationSpecification::AddDeltaTimeGroup,DOXY_FN(ConfigurationSpecification,AddDeltaTimeGroup)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("RemoveGroups", &PyConfigurationSpecification::RemoveGroups, + "groupname"_a, + "exactmatch"_a = true, + DOXY_FN(ConfigurationSpecification, RemoveGroups) + ); +#else + configurationspecification.def("RemoveGroups", &PyConfigurationSpecification::RemoveGroups, RemoveGroups_overloads(PY_ARGS("groupname","exactmatch") DOXY_FN(ConfigurationSpecification, RemoveGroups))); +#endif + configurationspecification.def("AddGroup",addgroup1, PY_ARGS("name","dof","interpolation") DOXY_FN(ConfigurationSpecification,AddGroup "const std::string; int; const std::string")); + configurationspecification.def("AddGroup",addgroup2, PY_ARGS("group") DOXY_FN(ConfigurationSpecification,AddGroup "const")); + configurationspecification.def("ConvertToVelocitySpecification",&PyConfigurationSpecification::ConvertToVelocitySpecification,DOXY_FN(ConfigurationSpecification,ConvertToVelocitySpecification)); + configurationspecification.def("ConvertToDerivativeSpecification",&PyConfigurationSpecification::ConvertToDerivativeSpecification, PY_ARGS("timederivative") DOXY_FN(ConfigurationSpecification, ConvertToDerivativeSpecification)); + configurationspecification.def("GetTimeDerivativeSpecification",&PyConfigurationSpecification::GetTimeDerivativeSpecification,DOXY_FN(ConfigurationSpecification,GetTimeDerivativeSpecification)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("ExtractTransform", &PyConfigurationSpecification::ExtractTransform, + "transform"_a, + "data"_a, + "body"_a, + "timederivative"_a = 0, + DOXY_FN(ConfigurationSpecification,ExtractTransform) + ); +#else + configurationspecification.def("ExtractTransform",&PyConfigurationSpecification::ExtractTransform,ExtractTransform_overloads(PY_ARGS("transform","data","body","timederivative") DOXY_FN(ConfigurationSpecification,ExtractTransform))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("ExtractAffineValues", &PyConfigurationSpecification::ExtractAffineValues, + "data"_a, + "body"_a, + "affinedofs"_a, + "timederivative"_a = 0, + DOXY_FN(ConfigurationSpecification,ExtractAffineValues) + ); +#else + configurationspecification.def("ExtractAffineValues",&PyConfigurationSpecification::ExtractAffineValues,ExtractAffineValues_overloads(PY_ARGS("data","body","affinedofs","timederivative") DOXY_FN(ConfigurationSpecification,ExtractAffineValues))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("ExtractIkParameterization", &PyConfigurationSpecification::ExtractIkParameterization, + "data"_a, + "timederivative"_a = 0, + "robotname"_a = "", + "manipulatorname"_a = "", + DOXY_FN(ConfigurationSpecification, ExtractIkParameterization) + ); +#else + configurationspecification.def("ExtractIkParameterization",&PyConfigurationSpecification::ExtractIkParameterization,ExtractIkParameterization_overloads(PY_ARGS("data","timederivative","robotname","manipulatorname") DOXY_FN(ConfigurationSpecification,ExtractIkParameterization))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("ExtractJointValues", &PyConfigurationSpecification::ExtractJointValues, + "data"_a, + "body"_a, + "indices"_a, + "timederivative"_a = 0, + DOXY_FN(ConfigurationSpecification,ExtractJointValues) + ); +#else + configurationspecification.def("ExtractJointValues",&PyConfigurationSpecification::ExtractJointValues,ExtractJointValues_overloads(PY_ARGS("data","body","indices","timederivative") DOXY_FN(ConfigurationSpecification,ExtractJointValues))); +#endif + configurationspecification.def("ExtractDeltaTime",&PyConfigurationSpecification::ExtractDeltaTime, PY_ARGS("data") DOXY_FN(ConfigurationSpecification,ExtractDeltaTime)); + configurationspecification.def("InsertDeltaTime",&PyConfigurationSpecification::InsertDeltaTime, PY_ARGS("data","deltatime") DOXY_FN(ConfigurationSpecification,InsertDeltaTime)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("InsertJointValues", &PyConfigurationSpecification::InsertJointValues, + "data"_a, + "values"_a, + "body"_a, + "indices"_a, + "timederivative"_a = 0, + DOXY_FN(ConfigurationSpecification, InsertJointValues) + ); +#else + configurationspecification.def("InsertJointValues",&PyConfigurationSpecification::InsertJointValues, PY_ARGS("data","values","body","indices","timederivative") DOXY_FN(ConfigurationSpecification,InsertJointValues)); +#endif + configurationspecification.def("ExtractUsedBodies", &PyConfigurationSpecification::ExtractUsedBodies, PY_ARGS("env") DOXY_FN(ConfigurationSpecification, ExtractUsedBodies)); + configurationspecification.def("ExtractUsedIndices", &PyConfigurationSpecification::ExtractUsedIndices, PY_ARGS("env") DOXY_FN(ConfigurationSpecification, ExtractUsedIndices)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def("ConvertData", &PyConfigurationSpecification::ConvertData, + "targetspec"_a, + "sourcedata"_a, + "numpoints"_a, + "env"_a, + "filluninitialized"_a = true, + DOXY_FN(ConfigurationSpecification, ConvertData) + ); +#else + configurationspecification.def("ConvertData", &PyConfigurationSpecification::ConvertData, PY_ARGS("targetspec", "sourcedata", "numpoints", "env", "filluninitialized") DOXY_FN(ConfigurationSpecification, ConvertData)); +#endif + configurationspecification.def("ConvertDataFromPrevious", &PyConfigurationSpecification::ConvertDataFromPrevious, PY_ARGS("targetdata", "targetspec", "sourcedata", "numpoints", "env") DOXY_FN(ConfigurationSpecification, ConvertData)); + configurationspecification.def("GetGroups", &PyConfigurationSpecification::GetGroups, /*PY_ARGS("env")*/ "returns a list of the groups"); + configurationspecification.def("__eq__",&PyConfigurationSpecification::__eq__); + configurationspecification.def("__ne__",&PyConfigurationSpecification::__ne__); + configurationspecification.def("__add__",&PyConfigurationSpecification::__add__); + configurationspecification.def("__iadd__",&PyConfigurationSpecification::__iadd__); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + configurationspecification.def(py::pickle( + [](const PyConfigurationSpecification& pyspec) { + std::stringstream ss; + ss << pyspec._spec; + return py::make_tuple(ss.str()); + }, + [](py::tuple state) { + // __setstate__ + if(state.size() != 1) { + throw std::runtime_error("Invalid state"); + } + return PyConfigurationSpecification(state[0].cast()); + } + )); +#else + configurationspecification.def_pickle(ConfigurationSpecification_pickle_suite()); +#endif + configurationspecification.def("__str__",&PyConfigurationSpecification::__str__); + configurationspecification.def("__unicode__",&PyConfigurationSpecification::__unicode__); + configurationspecification.def("__repr__",&PyConfigurationSpecification::__repr__); } #ifndef USE_PYBIND11_PYTHON_BINDINGS openravepy::spec_from_group(); From 1e39117a987eb8f207ed7339f558a2d01beaa813 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Mon, 12 Jun 2023 23:32:50 +0900 Subject: [PATCH 06/13] fix further --- python/bindings/bindings.cpp | 12 +++ python/bindings/include/openravepy/bindings.h | 4 + python/bindings/openravepy_int.cpp | 1 + python/bindings/openravepy_sensor.cpp | 81 ++++++++++--------- 4 files changed, 58 insertions(+), 40 deletions(-) diff --git a/python/bindings/bindings.cpp b/python/bindings/bindings.cpp index 0a80475c72..5a680a0f77 100644 --- a/python/bindings/bindings.cpp +++ b/python/bindings/bindings.cpp @@ -269,6 +269,18 @@ void init_python_bindings() .def("close",&PyVoidHandle::Close,"deprecated") .def("Close",&PyVoidHandle::Close) ; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + class_ >(m, "VoidHandleConst") + .def(init<>()) + // error: static assertion failed: Holder classes are only supported for custom types, so cannot do + // .def(init>(), "handle"_a) +#else + class_ >("VoidHandleConst") +#endif + .def("close",&PyVoidHandleConst::Close,"deprecated") + .def("Close",&PyVoidHandleConst::Close) + ; } } // namespace openravepy diff --git a/python/bindings/include/openravepy/bindings.h b/python/bindings/include/openravepy/bindings.h index 185a217a95..43f6252d45 100644 --- a/python/bindings/include/openravepy/bindings.h +++ b/python/bindings/include/openravepy/bindings.h @@ -314,7 +314,11 @@ inline std::string GetPyErrorString() } /// should call in the beginning of all BOOST_PYTHON_MODULE +#ifdef USE_PYBIND11_PYTHON_BINDINGS +void init_python_bindings(py::module& m); +#else void init_python_bindings(); +#endif } // namespace openravepy diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index d8a0b69a1e..8866b08cb5 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3112,6 +3112,7 @@ OPENRAVE_PYTHON_MODULE(openravepy_int) #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a + init_python_bindings(m); #else // USE_PYBIND11_PYTHON_BINDINGS #if BOOST_VERSION >= 103500 docstring_options doc_options; diff --git a/python/bindings/openravepy_sensor.cpp b/python/bindings/openravepy_sensor.cpp index b063bf468c..4e6d2e0262 100644 --- a/python/bindings/openravepy_sensor.cpp +++ b/python/bindings/openravepy_sensor.cpp @@ -733,6 +733,7 @@ void init_openravepy_sensor() #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a #endif + { OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData1)() = &PySensorBase::GetSensorData; OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData2)(SensorBase::SensorType) = &PySensorBase::GetSensorData; @@ -742,6 +743,35 @@ void init_openravepy_sensor() class_, bases > sensor("Sensor", DOXY_CLASS(SensorBase), no_init); #endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + // SensorData is inside SensorBase + class_ >(sensor, "SensorData", DOXY_CLASS(SensorBase::SensorData)) +#else + class_ >("SensorData", DOXY_CLASS(SensorBase::SensorData),no_init) +#endif + .def_readonly("type",&PySensorBase::PySensorData::type) + .def_readonly("stamp",&PySensorBase::PySensorData::stamp) + ; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + enum_(sensor, "ConfigureCommand", py::arithmetic() DOXY_ENUM(ConfigureCommand)) +#else + enum_("ConfigureCommand" DOXY_ENUM(ConfigureCommand)) +#endif + .value("PowerOn",SensorBase::CC_PowerOn) + .value("PowerOff",SensorBase::CC_PowerOff) + .value("PowerCheck",SensorBase::CC_PowerCheck) + .value("RenderDataOn",SensorBase::CC_RenderDataOn) + .value("RenderDataOff",SensorBase::CC_RenderDataOff) + .value("RenderDataCheck",SensorBase::CC_RenderDataCheck) + .value("RenderGeometryOn",SensorBase::CC_RenderGeometryOn) + .value("RenderGeometryOff",SensorBase::CC_RenderGeometryOff) + .value("RenderGeometryCheck",SensorBase::CC_RenderGeometryCheck) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .export_values() +#endif + ; + #ifdef USE_PYBIND11_PYTHON_BINDINGS // SensorType belongs to Sensor enum_(sensor, "Type", py::arithmetic() DOXY_ENUM(SensorType)) @@ -762,6 +792,17 @@ void init_openravepy_sensor() #endif ; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + class_>(m, "SensorGeometry", DOXY_CLASS(PySensorGeometry)) + // how to handle pure virtual + // see p.62 (book page) https://buildmedia.readthedocs.org/media/pdf/pybind11/stable/pybind11.pdf + .def("GetType", &PySensorGeometry::GetType) +#else + class_, boost::noncopyable >("SensorGeometry", DOXY_CLASS(PySensorGeometry),no_init) + .def("GetType",py::pure_virtual(&PySensorGeometry::GetType)) +#endif + ; + #ifdef USE_PYBIND11_PYTHON_BINDINGS sensor.def("Configure", &PySensorBase::Configure, "command"_a, @@ -798,16 +839,6 @@ void init_openravepy_sensor() .def_readwrite("focal_length",&PyCameraIntrinsics::focal_length) ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - // SensorData is inside SensorBase - class_ >(sensor, "SensorData", DOXY_CLASS(SensorBase::SensorData)) -#else - class_ >("SensorData", DOXY_CLASS(SensorBase::SensorData),no_init) -#endif - .def_readonly("type",&PySensorBase::PySensorData::type) - .def_readonly("stamp",&PySensorBase::PySensorData::stamp) - ; - #ifdef USE_PYBIND11_PYTHON_BINDINGS // LaserSensorData is inside SensorBase class_, PySensorBase::PySensorData>(sensor, "LaserSensorData", DOXY_CLASS(SensorBase::LaserSensorData)) @@ -930,38 +961,8 @@ void init_openravepy_sensor() #endif ; } - -#ifdef USE_PYBIND11_PYTHON_BINDINGS - enum_(sensor, "ConfigureCommand", py::arithmetic() DOXY_ENUM(ConfigureCommand)) -#else - enum_("ConfigureCommand" DOXY_ENUM(ConfigureCommand)) -#endif - .value("PowerOn",SensorBase::CC_PowerOn) - .value("PowerOff",SensorBase::CC_PowerOff) - .value("PowerCheck",SensorBase::CC_PowerCheck) - .value("RenderDataOn",SensorBase::CC_RenderDataOn) - .value("RenderDataOff",SensorBase::CC_RenderDataOff) - .value("RenderDataCheck",SensorBase::CC_RenderDataCheck) - .value("RenderGeometryOn",SensorBase::CC_RenderGeometryOn) - .value("RenderGeometryOff",SensorBase::CC_RenderGeometryOff) - .value("RenderGeometryCheck",SensorBase::CC_RenderGeometryCheck) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .export_values() -#endif - ; } -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_>(m, "SensorGeometry", DOXY_CLASS(PySensorGeometry)) - // how to handle pure virtual - // see p.62 (book page) https://buildmedia.readthedocs.org/media/pdf/pybind11/stable/pybind11.pdf - .def("GetType", &PySensorGeometry::GetType) -#else - class_, boost::noncopyable >("SensorGeometry", DOXY_CLASS(PySensorGeometry),no_init) - .def("GetType",py::pure_virtual(&PySensorGeometry::GetType)) -#endif - ; - #ifdef USE_PYBIND11_PYTHON_BINDINGS class_, PySensorGeometry>(m, "CameraGeomData", DOXY_CLASS(SensorBase::CameraGeomData)) .def(init<>()) From 8bdb91f97d4a9f856fdab0d28be0c6643cdb45ad Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 10:49:03 +0900 Subject: [PATCH 07/13] Declare PyCollisionCheckerBaseBinderPtr earlier --- .../include/openravepy/openravepy_int.h | 12 +- .../bindings/openravepy_collisionchecker.cpp | 106 ++++++++++-------- python/bindings/openravepy_int.cpp | 3 +- 3 files changed, 69 insertions(+), 52 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_int.h b/python/bindings/include/openravepy/openravepy_int.h index 6b2b410647..a08c1f3ddf 100644 --- a/python/bindings/include/openravepy/openravepy_int.h +++ b/python/bindings/include/openravepy/openravepy_int.h @@ -171,6 +171,12 @@ typedef OPENRAVE_SHARED_PTR PyLinkConstPtr; typedef OPENRAVE_SHARED_PTR PyJointPtr; typedef OPENRAVE_SHARED_PTR PyJointConstPtr; +#ifdef USE_PYBIND11_PYTHON_BINDINGS +typedef py::class_, PyInterfaceBase> PyCollisionCheckerBaseBinder; +#else +typedef py::class_, bases > PyCollisionCheckerBaseBinder; +#endif +typedef OPENRAVE_SHARED_PTR PyCollisionCheckerBaseBinderPtr; inline uint64_t GetMicroTime() { @@ -727,9 +733,11 @@ OPENRAVEPY_API int RaveGetEnvironmentId(PyEnvironmentBasePtr pyenv); OPENRAVEPY_API PyEnvironmentBasePtr RaveGetEnvironment(int id); #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_collisionchecker(py::module& m); +PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(py::module& m); +void init_openravepy_collisionchecker(py::module& m, PyCollisionCheckerBaseBinder& collisionchecker); #else -void init_openravepy_collisionchecker(); +PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(); +void init_openravepy_collisionchecker(PyCollisionCheckerBaseBinder& collisionchecker); #endif OPENRAVEPY_API CollisionCheckerBasePtr GetCollisionChecker(PyCollisionCheckerBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyCollisionChecker(CollisionCheckerBasePtr, PyEnvironmentBasePtr); diff --git a/python/bindings/openravepy_collisionchecker.cpp b/python/bindings/openravepy_collisionchecker.cpp index 874cc8e5c8..dfccbc15d4 100644 --- a/python/bindings/openravepy_collisionchecker.cpp +++ b/python/bindings/openravepy_collisionchecker.cpp @@ -760,7 +760,21 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Reset_overloads, Reset, 0, 1) #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_collisionchecker(py::module& m) +PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(py::module& m) +#else +PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass() +#endif +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + PyCollisionCheckerBaseBinderPtr pCollisionchecker(new PyCollisionCheckerBaseBinder(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase))); +#else + PyCollisionCheckerBaseBinderPtr pCollisionchecker(new PyCollisionCheckerBaseBinder("CollisionChecker", DOXY_CLASS(CollisionCheckerBase))); +#endif + return pCollisionchecker; +} + +#ifdef USE_PYBIND11_PYTHON_BINDINGS +void init_openravepy_collisionchecker(py::module& m, PyCollisionCheckerBaseBinder &collisionchecker) #else void init_openravepy_collisionchecker() #endif @@ -862,57 +876,51 @@ void init_openravepy_collisionchecker() bool (PyCollisionCheckerBase::*pcolobb)(object, object, PyCollisionReportPtr) = &PyCollisionCheckerBase::CheckCollisionOBB; bool (PyCollisionCheckerBase::*pcolobbi)(object, object, object, PyCollisionReportPtr) = &PyCollisionCheckerBase::CheckCollisionOBB; + collisionchecker.def("InitEnvironment", &PyCollisionCheckerBase::InitEnvironment, DOXY_FN(CollisionCheckerBase, InitEnvironment)); + collisionchecker.def("DestroyEnvironment", &PyCollisionCheckerBase::DestroyEnvironment, DOXY_FN(CollisionCheckerBase, DestroyEnvironment)); + collisionchecker.def("InitKinBody", &PyCollisionCheckerBase::InitKinBody, DOXY_FN(CollisionCheckerBase, InitKinBody)); + collisionchecker.def("RemoveKinBody", &PyCollisionCheckerBase::RemoveKinBody, DOXY_FN(CollisionCheckerBase, RemoveKinBody)); + collisionchecker.def("SetGeometryGroup", &PyCollisionCheckerBase::SetGeometryGroup, DOXY_FN(CollisionCheckerBase, SetGeometryGroup)); + collisionchecker.def("SetBodyGeometryGroup", &PyCollisionCheckerBase::SetBodyGeometryGroup, PY_ARGS("body", "groupname") DOXY_FN(CollisionCheckerBase, SetBodyGeometryGroup)); + collisionchecker.def("GetGeometryGroup", &PyCollisionCheckerBase::GetGeometryGroup, DOXY_FN(CollisionCheckerBase, GetGeometryGroup)); + collisionchecker.def("GetBodyGeometryGroup", &PyCollisionCheckerBase::GetBodyGeometryGroup, PY_ARGS("body") DOXY_FN(CollisionCheckerBase, GetBodyGeometryGroup)); + collisionchecker.def("SetCollisionOptions",&PyCollisionCheckerBase::SetCollisionOptions, DOXY_FN(CollisionCheckerBase,SetCollisionOptions "int")); + collisionchecker.def("GetCollisionOptions",&PyCollisionCheckerBase::GetCollisionOptions, DOXY_FN(CollisionCheckerBase,GetCollisionOptions)); + collisionchecker.def("CheckCollision",pcolb, PY_ARGS("body") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolbr, PY_ARGS("body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolbb, PY_ARGS("body1","body2") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolbbr, PY_ARGS("body1","body2","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolyb, PY_ARGS("ray","body") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolybr, PY_ARGS("ray","body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcoly, PY_ARGS("ray") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolyr, PY_ARGS("ray", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcoll, PY_ARGS("link") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcollr, PY_ARGS("link","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolll, PY_ARGS("link1","link2") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBody::LinkConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolllr, PY_ARGS("link1","link2","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBody::LinkConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcollb, PY_ARGS("link","body") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcollbr, PY_ARGS("link","body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolle, PY_ARGS("link","bodyexcluded","linkexcluded") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; const std::vector; const std::vector; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcoller, PY_ARGS("link","bodyexcluded","linkexcluded","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; const std::vector; const std::vector; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolbe, PY_ARGS("body","bodyexcluded","linkexcluded") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; const std::vector; const std::vector; CollisionReportPtr")); + collisionchecker.def("CheckCollision",pcolber, PY_ARGS("body","bodyexcluded","linkexcluded","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; const std::vector; const std::vector; CollisionReportPtr")); + collisionchecker.def("CheckCollisionTriMesh",pcolter, PY_ARGS("trimesh", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const TriMesh; CollisionReportPtr")); + collisionchecker.def("CheckCollisionTriMesh",pcoltbr, PY_ARGS("trimesh", "body", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const TriMesh; KinBodyConstPtr; CollisionReportPtr")); + collisionchecker.def("CheckCollisionOBB", pcolobb, PY_ARGS("aabb", "pose", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const AABB; const Transform; CollisionReport")); + collisionchecker.def("CheckCollisionOBB", pcolobbi, PY_ARGS("aabb", "pose", "bodiesincluded", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const AABB; const Transform; const std::vector; CollisionReport")); + collisionchecker.def("CheckSelfCollision",&PyCollisionCheckerBase::CheckSelfCollision, PY_ARGS("linkbody", "report") DOXY_FN(CollisionCheckerBase,CheckSelfCollision "KinBodyConstPtr, CollisionReportPtr")); #ifdef USE_PYBIND11_PYTHON_BINDINGS - class_, PyInterfaceBase>(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) + collisionchecker.def("CheckCollisionRays", &PyCollisionCheckerBase::CheckCollisionRays, + "rays"_a, + "body"_a, + "front_facing_only"_a = false, + "Check if any rays hit the body and returns their contact points along with a vector specifying if a collision occured or not. Rays is a Nx6 array, first 3 columns are position, last 3 are direction*range. The return value is: (N array of hit points, Nx6 array of hit position and surface normals." + ); #else - class_, bases >("CollisionChecker", DOXY_CLASS(CollisionCheckerBase), no_init) + collisionchecker.def("CheckCollisionRays",&PyCollisionCheckerBase::CheckCollisionRays, + CheckCollisionRays_overloads(PY_ARGS("rays","body","front_facing_only") + "Check if any rays hit the body and returns their contact points along with a vector specifying if a collision occured or not. Rays is a Nx6 array, first 3 columns are position, last 3 are direction*range. The return value is: (N array of hit points, Nx6 array of hit position and surface normals.")); #endif - .def("InitEnvironment", &PyCollisionCheckerBase::InitEnvironment, DOXY_FN(CollisionCheckerBase, InitEnvironment)) - .def("DestroyEnvironment", &PyCollisionCheckerBase::DestroyEnvironment, DOXY_FN(CollisionCheckerBase, DestroyEnvironment)) - .def("InitKinBody", &PyCollisionCheckerBase::InitKinBody, DOXY_FN(CollisionCheckerBase, InitKinBody)) - .def("RemoveKinBody", &PyCollisionCheckerBase::RemoveKinBody, DOXY_FN(CollisionCheckerBase, RemoveKinBody)) - .def("SetGeometryGroup", &PyCollisionCheckerBase::SetGeometryGroup, DOXY_FN(CollisionCheckerBase, SetGeometryGroup)) - .def("SetBodyGeometryGroup", &PyCollisionCheckerBase::SetBodyGeometryGroup, PY_ARGS("body", "groupname") DOXY_FN(CollisionCheckerBase, SetBodyGeometryGroup)) - .def("GetGeometryGroup", &PyCollisionCheckerBase::GetGeometryGroup, DOXY_FN(CollisionCheckerBase, GetGeometryGroup)) - .def("GetBodyGeometryGroup", &PyCollisionCheckerBase::GetBodyGeometryGroup, PY_ARGS("body") DOXY_FN(CollisionCheckerBase, GetBodyGeometryGroup)) - .def("SetCollisionOptions",&PyCollisionCheckerBase::SetCollisionOptions, DOXY_FN(CollisionCheckerBase,SetCollisionOptions "int")) - .def("GetCollisionOptions",&PyCollisionCheckerBase::GetCollisionOptions, DOXY_FN(CollisionCheckerBase,GetCollisionOptions)) - .def("CheckCollision",pcolb, PY_ARGS("body") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolbr, PY_ARGS("body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolbb, PY_ARGS("body1","body2") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolbbr, PY_ARGS("body1","body2","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolyb, PY_ARGS("ray","body") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolybr, PY_ARGS("ray","body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcoly, PY_ARGS("ray") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; CollisionReportPtr")) - .def("CheckCollision",pcolyr, PY_ARGS("ray", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const RAY; CollisionReportPtr")) - .def("CheckCollision",pcoll, PY_ARGS("link") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcollr, PY_ARGS("link","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolll, PY_ARGS("link1","link2") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBody::LinkConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolllr, PY_ARGS("link1","link2","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBody::LinkConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcollb, PY_ARGS("link","body") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcollbr, PY_ARGS("link","body","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollision",pcolle, PY_ARGS("link","bodyexcluded","linkexcluded") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; const std::vector; const std::vector; CollisionReportPtr")) - .def("CheckCollision",pcoller, PY_ARGS("link","bodyexcluded","linkexcluded","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBody::LinkConstPtr; const std::vector; const std::vector; CollisionReportPtr")) - .def("CheckCollision",pcolbe, PY_ARGS("body","bodyexcluded","linkexcluded") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; const std::vector; const std::vector; CollisionReportPtr")) - .def("CheckCollision",pcolber, PY_ARGS("body","bodyexcluded","linkexcluded","report") DOXY_FN(CollisionCheckerBase,CheckCollision "KinBodyConstPtr; const std::vector; const std::vector; CollisionReportPtr")) - .def("CheckCollisionTriMesh",pcolter, PY_ARGS("trimesh", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const TriMesh; CollisionReportPtr")) - .def("CheckCollisionTriMesh",pcoltbr, PY_ARGS("trimesh", "body", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const TriMesh; KinBodyConstPtr; CollisionReportPtr")) - .def("CheckCollisionOBB", pcolobb, PY_ARGS("aabb", "pose", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const AABB; const Transform; CollisionReport")) - .def("CheckCollisionOBB", pcolobbi, PY_ARGS("aabb", "pose", "bodiesincluded", "report") DOXY_FN(CollisionCheckerBase,CheckCollision "const AABB; const Transform; const std::vector; CollisionReport")) - .def("CheckSelfCollision",&PyCollisionCheckerBase::CheckSelfCollision, PY_ARGS("linkbody", "report") DOXY_FN(CollisionCheckerBase,CheckSelfCollision "KinBodyConstPtr, CollisionReportPtr")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("CheckCollisionRays", &PyCollisionCheckerBase::CheckCollisionRays, - "rays"_a, - "body"_a, - "front_facing_only"_a = false, - "Check if any rays hit the body and returns their contact points along with a vector specifying if a collision occured or not. Rays is a Nx6 array, first 3 columns are position, last 3 are direction*range. The return value is: (N array of hit points, Nx6 array of hit position and surface normals." - ) -#else - .def("CheckCollisionRays",&PyCollisionCheckerBase::CheckCollisionRays, - CheckCollisionRays_overloads(PY_ARGS("rays","body","front_facing_only") - "Check if any rays hit the body and returns their contact points along with a vector specifying if a collision occured or not. Rays is a Nx6 array, first 3 columns are position, last 3 are direction*range. The return value is: (N array of hit points, Nx6 array of hit position and surface normals.")) -#endif - ; #ifdef USE_PYBIND11_PYTHON_BINDINGS m.def("RaveCreateCollisionChecker", openravepy::RaveCreateCollisionChecker, PY_ARGS("env","name") DOXY_FN1(RaveCreateCollisionChecker)); diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 8866b08cb5..bf3c5e545e 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3300,12 +3300,13 @@ Because race conditions can pop up when trying to lock the openrave environment } #ifdef USE_PYBIND11_PYTHON_BINDINGS + PyCollisionCheckerBaseBinderPtr pCollisionChecker = openravepy::init_openravepy_collisioncheckerclass(m); openravepy::init_openravepy_global(m); openravepy::init_openravepy_controller(m); openravepy::init_openravepy_ikparameterization(m); openravepy::init_openravepy_iksolver(m); openravepy::init_openravepy_kinbody(m); - openravepy::init_openravepy_collisionchecker(m); + openravepy::init_openravepy_collisionchecker(m, *pCollisionChecker); openravepy::init_openravepy_sensor(m); openravepy::init_openravepy_sensorsystem(m); openravepy::init_openravepy_robot(m); From 7a7df6f2ddc6770c5da0020a24dc9b7772213fb3 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 14:08:22 +0900 Subject: [PATCH 08/13] introduced OpenRAVEPyInitializer --- .../openravepy_collisioncheckerbase.h | 13 +++++++++++++ .../include/openravepy/openravepy_int.h | 14 -------------- .../bindings/openravepy_collisionchecker.cpp | 16 ++++++---------- python/bindings/openravepy_int.cpp | 18 ++++++++++++++++-- 4 files changed, 35 insertions(+), 26 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h b/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h index 414cd57f83..d9be29758c 100644 --- a/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h +++ b/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h @@ -99,5 +99,18 @@ class OPENRAVEPY_API PyCollisionCheckerBase : public PyInterfaceBase virtual bool CheckSelfCollision(object o1, PyCollisionReportPtr pReport); }; +struct CollisionCheckerBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + CollisionCheckerBaseInitializer(py::module& m); + void init_openravepy_collisionchecker(py::module& m); + py::class_, PyInterfaceBase> collisionchecker; +#else + CollisionCheckerBaseInitializer(); + void init_openravepy_collisionchecker(); + py::class_, bases > collisionchecker; +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_COLLISIONCHECKERBASE_H diff --git a/python/bindings/include/openravepy/openravepy_int.h b/python/bindings/include/openravepy/openravepy_int.h index a08c1f3ddf..525b1e2808 100644 --- a/python/bindings/include/openravepy/openravepy_int.h +++ b/python/bindings/include/openravepy/openravepy_int.h @@ -171,13 +171,6 @@ typedef OPENRAVE_SHARED_PTR PyLinkConstPtr; typedef OPENRAVE_SHARED_PTR PyJointPtr; typedef OPENRAVE_SHARED_PTR PyJointConstPtr; -#ifdef USE_PYBIND11_PYTHON_BINDINGS -typedef py::class_, PyInterfaceBase> PyCollisionCheckerBaseBinder; -#else -typedef py::class_, bases > PyCollisionCheckerBaseBinder; -#endif -typedef OPENRAVE_SHARED_PTR PyCollisionCheckerBaseBinderPtr; - inline uint64_t GetMicroTime() { #ifdef _WIN32 @@ -732,13 +725,6 @@ OPENRAVEPY_API void UnlockEnvironment(PyEnvironmentBasePtr); OPENRAVEPY_API int RaveGetEnvironmentId(PyEnvironmentBasePtr pyenv); OPENRAVEPY_API PyEnvironmentBasePtr RaveGetEnvironment(int id); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(py::module& m); -void init_openravepy_collisionchecker(py::module& m, PyCollisionCheckerBaseBinder& collisionchecker); -#else -PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(); -void init_openravepy_collisionchecker(PyCollisionCheckerBaseBinder& collisionchecker); -#endif OPENRAVEPY_API CollisionCheckerBasePtr GetCollisionChecker(PyCollisionCheckerBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyCollisionChecker(CollisionCheckerBasePtr, PyEnvironmentBasePtr); OPENRAVEPY_API CollisionReportPtr GetCollisionReport(py::object); diff --git a/python/bindings/openravepy_collisionchecker.cpp b/python/bindings/openravepy_collisionchecker.cpp index dfccbc15d4..d03fea2c8b 100644 --- a/python/bindings/openravepy_collisionchecker.cpp +++ b/python/bindings/openravepy_collisionchecker.cpp @@ -760,23 +760,19 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Reset_overloads, Reset, 0, 1) #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS -PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass(py::module& m) +CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(py::module& m): + collisionchecker(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) #else -PyCollisionCheckerBaseBinderPtr init_openravepy_collisioncheckerclass() +CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(): + collisionchecker("CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) #endif { -#ifdef USE_PYBIND11_PYTHON_BINDINGS - PyCollisionCheckerBaseBinderPtr pCollisionchecker(new PyCollisionCheckerBaseBinder(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase))); -#else - PyCollisionCheckerBaseBinderPtr pCollisionchecker(new PyCollisionCheckerBaseBinder("CollisionChecker", DOXY_CLASS(CollisionCheckerBase))); -#endif - return pCollisionchecker; } #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_collisionchecker(py::module& m, PyCollisionCheckerBaseBinder &collisionchecker) +void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker(py::module& m) #else -void init_openravepy_collisionchecker() +void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker() #endif { #ifdef USE_PYBIND11_PYTHON_BINDINGS diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index bf3c5e545e..76b85af984 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3096,6 +3096,18 @@ py::object GetCodeStringOpenRAVEException(OpenRAVEException* p) } #endif // USE_PYBIND11_PYTHON_BINDINGS +struct OpenRAVEPyInitializer: + public CollisionCheckerBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + OpenRAVEPyInitializer(py::module& m): CollisionCheckerBaseInitializer(m) +#else + OpenRAVEPyInitializer(): CollisionCheckerBaseInitializer() +#endif + { + } +}; + } // namespace openravepy OPENRAVE_PYTHON_MODULE(openravepy_int) @@ -3300,13 +3312,15 @@ Because race conditions can pop up when trying to lock the openrave environment } #ifdef USE_PYBIND11_PYTHON_BINDINGS - PyCollisionCheckerBaseBinderPtr pCollisionChecker = openravepy::init_openravepy_collisioncheckerclass(m); + OpenRAVEPyInitializer initializer(m); + // PyCollisionCheckerBaseBinderPtr pCollisionChecker = openravepy::init_openravepy_collisioncheckerclass(m); openravepy::init_openravepy_global(m); openravepy::init_openravepy_controller(m); openravepy::init_openravepy_ikparameterization(m); openravepy::init_openravepy_iksolver(m); openravepy::init_openravepy_kinbody(m); - openravepy::init_openravepy_collisionchecker(m, *pCollisionChecker); + // openravepy::init_openravepy_collisionchecker(m, *pCollisionChecker); + initializer.init_openravepy_collisionchecker(m); openravepy::init_openravepy_sensor(m); openravepy::init_openravepy_sensorsystem(m); openravepy::init_openravepy_robot(m); From f3d51e2046a5309b4b6a52796bdb4ee1f2aa6ac5 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 15:12:34 +0900 Subject: [PATCH 09/13] fixed CheckSelfCollision --- .../openravepy/openravepy_collisionreport.h | 13 +++ .../bindings/openravepy_collisionchecker.cpp | 107 ++++++++++-------- python/bindings/openravepy_int.cpp | 8 +- 3 files changed, 80 insertions(+), 48 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_collisionreport.h b/python/bindings/include/openravepy/openravepy_collisionreport.h index f22927df07..ade222d4f3 100644 --- a/python/bindings/include/openravepy/openravepy_collisionreport.h +++ b/python/bindings/include/openravepy/openravepy_collisionreport.h @@ -62,6 +62,19 @@ class PyCollisionReport CollisionReportPtr report; }; +struct CollisionReportInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + CollisionReportInitializer(py::module& m); + void init_openravepy_collisionreport(py::module& m); + py::class_ > collisionreport; +#else + CollisionReportInitializer(); + void init_openravepy_collisionreport(); + py::class_ > collisionreport; +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_COLLISIONREPORT_H diff --git a/python/bindings/openravepy_collisionchecker.cpp b/python/bindings/openravepy_collisionchecker.cpp index d03fea2c8b..8264eea7b4 100644 --- a/python/bindings/openravepy_collisionchecker.cpp +++ b/python/bindings/openravepy_collisionchecker.cpp @@ -764,7 +764,17 @@ CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(py::module& m): collisionchecker(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) #else CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(): - collisionchecker("CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) + collisionchecker("CollisionChecker", DOXY_CLASS(CollisionCheckerBase), no_init) +#endif +{ +} + +#ifdef USE_PYBIND11_PYTHON_BINDINGS +CollisionReportInitializer::CollisionReportInitializer(py::module& m): + collisionreport(m, "CollisionReport", DOXY_CLASS(CollisionReport)) +#else +CollisionReportInitializer::CollisionReportInitializer(): + collisionreport("CollisionReport", DOXY_CLASS(CollisionReport)) #endif { } @@ -805,50 +815,6 @@ void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker() #endif ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - // should this be inside CollisionReport, instead of module "m"? - class_ >(m, "Contact", DOXY_CLASS(CollisionReport::CONTACT)) - .def(init<>()) -#else - class_ >("Contact", DOXY_CLASS(CollisionReport::CONTACT)) -#endif - .def_readonly("pos",&PyCollisionReport::PYCONTACT::pos) - .def_readonly("norm",&PyCollisionReport::PYCONTACT::norm) - .def_readonly("depth",&PyCollisionReport::PYCONTACT::depth) - .def("__str__",&PyCollisionReport::PYCONTACT::__str__) - .def("__unicode__",&PyCollisionReport::PYCONTACT::__unicode__) - ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_ >(m, "CollisionReport", DOXY_CLASS(CollisionReport)) - .def(init<>()) -#else - class_ >("CollisionReport", DOXY_CLASS(CollisionReport)) -#endif - .def_readonly("options",&PyCollisionReport::options) - .def_readonly("plink1",&PyCollisionReport::plink1) - .def_readonly("plink2",&PyCollisionReport::plink2) - .def_readonly("pgeom1",&PyCollisionReport::pgeom1) - .def_readonly("pgeom2",&PyCollisionReport::pgeom2) - .def_readonly("minDistance",&PyCollisionReport::minDistance) - .def_readonly("numWithinTol",&PyCollisionReport::numWithinTol) - .def_readonly("contacts",&PyCollisionReport::contacts) - .def_readonly("vLinkColliding",&PyCollisionReport::vLinkColliding) - .def_readonly("vGeometryContacts",&PyCollisionReport::vGeometryContacts) - .def_readonly("nKeepPrevious", &PyCollisionReport::nKeepPrevious) - .def("__str__",&PyCollisionReport::__str__) - .def("__unicode__",&PyCollisionReport::__unicode__) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("Reset", &PyCollisionReport::Reset, - "coloptions"_a = 0, - "Reset report" - ) -#else - .def("Reset",&PyCollisionReport::Reset, - Reset_overloads(PY_ARGS("coloptions") - "Reset report")) -#endif - ; - bool (PyCollisionCheckerBase::*pcolb)(PyKinBodyPtr) = &PyCollisionCheckerBase::CheckCollision; bool (PyCollisionCheckerBase::*pcolbr)(PyKinBodyPtr, PyCollisionReportPtr) = &PyCollisionCheckerBase::CheckCollision; bool (PyCollisionCheckerBase::*pcolbb)(PyKinBodyPtr,PyKinBodyPtr) = &PyCollisionCheckerBase::CheckCollision; @@ -925,4 +891,55 @@ void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker() #endif } +#ifdef USE_PYBIND11_PYTHON_BINDINGS +void CollisionReportInitializer::init_openravepy_collisionreport(py::module& m) +#else +void CollisionReportInitializer::init_openravepy_collisionreport() +#endif +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + using namespace py::literals; // "..."_a +#endif + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + // should this be inside CollisionReport, instead of module "m"? + class_ >(m, "Contact", DOXY_CLASS(CollisionReport::CONTACT)) + .def(init<>()) +#else + class_ >("Contact", DOXY_CLASS(CollisionReport::CONTACT)) +#endif + .def_readonly("pos",&PyCollisionReport::PYCONTACT::pos) + .def_readonly("norm",&PyCollisionReport::PYCONTACT::norm) + .def_readonly("depth",&PyCollisionReport::PYCONTACT::depth) + .def("__str__",&PyCollisionReport::PYCONTACT::__str__) + .def("__unicode__",&PyCollisionReport::PYCONTACT::__unicode__) + ; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + collisionreport.def(init<>()); +#endif + collisionreport.def_readonly("options",&PyCollisionReport::options); + collisionreport.def_readonly("plink1",&PyCollisionReport::plink1); + collisionreport.def_readonly("plink2",&PyCollisionReport::plink2); + collisionreport.def_readonly("pgeom1",&PyCollisionReport::pgeom1); + collisionreport.def_readonly("pgeom2",&PyCollisionReport::pgeom2); + collisionreport.def_readonly("minDistance",&PyCollisionReport::minDistance); + collisionreport.def_readonly("numWithinTol",&PyCollisionReport::numWithinTol); + collisionreport.def_readonly("contacts",&PyCollisionReport::contacts); + collisionreport.def_readonly("vLinkColliding",&PyCollisionReport::vLinkColliding); + collisionreport.def_readonly("vGeometryContacts",&PyCollisionReport::vGeometryContacts); + collisionreport.def_readonly("nKeepPrevious", &PyCollisionReport::nKeepPrevious); + collisionreport.def("__str__",&PyCollisionReport::__str__); + collisionreport.def("__unicode__",&PyCollisionReport::__unicode__); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + collisionreport.def("Reset", &PyCollisionReport::Reset, + "coloptions"_a = 0, + "Reset report" + ); +#else + collisionreport.def("Reset",&PyCollisionReport::Reset, + Reset_overloads(PY_ARGS("coloptions") + "Reset report")); +#endif +} + } diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 76b85af984..191fd165a0 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3097,12 +3097,13 @@ py::object GetCodeStringOpenRAVEException(OpenRAVEException* p) #endif // USE_PYBIND11_PYTHON_BINDINGS struct OpenRAVEPyInitializer: - public CollisionCheckerBaseInitializer + public CollisionCheckerBaseInitializer, + public CollisionReportInitializer { #ifdef USE_PYBIND11_PYTHON_BINDINGS - OpenRAVEPyInitializer(py::module& m): CollisionCheckerBaseInitializer(m) + OpenRAVEPyInitializer(py::module& m): CollisionCheckerBaseInitializer(m), CollisionReportInitializer(m) #else - OpenRAVEPyInitializer(): CollisionCheckerBaseInitializer() + OpenRAVEPyInitializer(): CollisionCheckerBaseInitializer(), CollisionReportInitializer() #endif { } @@ -3321,6 +3322,7 @@ Because race conditions can pop up when trying to lock the openrave environment openravepy::init_openravepy_kinbody(m); // openravepy::init_openravepy_collisionchecker(m, *pCollisionChecker); initializer.init_openravepy_collisionchecker(m); + initializer.init_openravepy_collisionreport(m); openravepy::init_openravepy_sensor(m); openravepy::init_openravepy_sensorsystem(m); openravepy::init_openravepy_robot(m); From 51a6f609c2bd4be8a8cb3f05f35eb8a099dcb212 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 17:31:11 +0900 Subject: [PATCH 10/13] progress --- .../openravepy_collisioncheckerbase.h | 5 +- .../openravepy/openravepy_collisionreport.h | 5 +- .../openravepy/openravepy_controllerbase.h | 16 ++++- .../openravepy_ikparameterization.h | 12 ++++ .../openravepy/openravepy_iksolverbase.h | 15 ++++- .../include/openravepy/openravepy_int.h | 60 ++++--------------- .../include/openravepy/openravepy_kinbody.h | 12 ++++ .../include/openravepy/openravepy_module.h | 14 ++++- .../openravepy_physicalenginebase.h | 12 ++++ .../openravepy/openravepy_plannerbase.h | 12 ++++ .../include/openravepy/openravepy_robotbase.h | 12 ++++ .../openravepy/openravepy_sensorbase.h | 14 +++++ .../openravepy/openravepy_trajectorybase.h | 12 ++++ .../bindings/openravepy_collisionchecker.cpp | 12 +--- python/bindings/openravepy_controller.cpp | 50 ++++++++-------- .../openravepy_ikparameterization.cpp | 8 ++- python/bindings/openravepy_iksolver.cpp | 8 ++- python/bindings/openravepy_int.cpp | 54 ++++++++++------- python/bindings/openravepy_kinbody.cpp | 8 ++- python/bindings/openravepy_module.cpp | 8 ++- python/bindings/openravepy_physicsengine.cpp | 8 ++- python/bindings/openravepy_planner.cpp | 8 ++- python/bindings/openravepy_robot.cpp | 8 ++- python/bindings/openravepy_sensor.cpp | 15 ++--- python/bindings/openravepy_trajectory.cpp | 8 ++- 25 files changed, 260 insertions(+), 136 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h b/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h index d9be29758c..e52bac9683 100644 --- a/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h +++ b/python/bindings/include/openravepy/openravepy_collisioncheckerbase.h @@ -102,8 +102,9 @@ class OPENRAVEPY_API PyCollisionCheckerBase : public PyInterfaceBase struct CollisionCheckerBaseInitializer { #ifdef USE_PYBIND11_PYTHON_BINDINGS - CollisionCheckerBaseInitializer(py::module& m); - void init_openravepy_collisionchecker(py::module& m); + CollisionCheckerBaseInitializer(py::module& m_); + void init_openravepy_collisionchecker(); + py::module& m; py::class_, PyInterfaceBase> collisionchecker; #else CollisionCheckerBaseInitializer(); diff --git a/python/bindings/include/openravepy/openravepy_collisionreport.h b/python/bindings/include/openravepy/openravepy_collisionreport.h index ade222d4f3..ecf1ae068d 100644 --- a/python/bindings/include/openravepy/openravepy_collisionreport.h +++ b/python/bindings/include/openravepy/openravepy_collisionreport.h @@ -65,8 +65,9 @@ class PyCollisionReport struct CollisionReportInitializer { #ifdef USE_PYBIND11_PYTHON_BINDINGS - CollisionReportInitializer(py::module& m); - void init_openravepy_collisionreport(py::module& m); + CollisionReportInitializer(py::module& m_); + void init_openravepy_collisionreport(); + py::module& m; py::class_ > collisionreport; #else CollisionReportInitializer(); diff --git a/python/bindings/include/openravepy/openravepy_controllerbase.h b/python/bindings/include/openravepy/openravepy_controllerbase.h index 4d543e9c02..8ddb8f453e 100644 --- a/python/bindings/include/openravepy/openravepy_controllerbase.h +++ b/python/bindings/include/openravepy/openravepy_controllerbase.h @@ -73,5 +73,19 @@ class PyMultiControllerBase : public PyControllerBase object GetController(int dof); }; +struct ControllerBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + ControllerBaseInitializer(py::module& m_); + void init_openravepy_controller(); + py::module& m; + py::class_, PyInterfaceBase> controller; +#else + ControllerBaseInitializer(); + void init_openravepy_controller(); + py::class_, bases > controller; +#endif +}; + } // namespace openravepy -#endif // OPENRAVEPY_INTERNAL_CONTROLLERBASE_H \ No newline at end of file +#endif // OPENRAVEPY_INTERNAL_CONTROLLERBASE_H diff --git a/python/bindings/include/openravepy/openravepy_ikparameterization.h b/python/bindings/include/openravepy/openravepy_ikparameterization.h index 22b4b05dd4..d2d373037f 100644 --- a/python/bindings/include/openravepy/openravepy_ikparameterization.h +++ b/python/bindings/include/openravepy/openravepy_ikparameterization.h @@ -121,6 +121,18 @@ class PyIkParameterization void _Update(const IkParameterization& ikparam); }; +struct IkParameterizationInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + IkParameterizationInitializer(py::module& m_); + void init_openravepy_ikparameterization(); + py::module& m; +#else + IkParameterizationInitializer(); + void init_openravepy_ikparameterization(); +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_IKPARAMETERIZATION_H diff --git a/python/bindings/include/openravepy/openravepy_iksolverbase.h b/python/bindings/include/openravepy/openravepy_iksolverbase.h index c6b89d56ef..a7d9d9eafb 100644 --- a/python/bindings/include/openravepy/openravepy_iksolverbase.h +++ b/python/bindings/include/openravepy/openravepy_iksolverbase.h @@ -74,5 +74,18 @@ class PyIkSolverBase : public PyInterfaceBase object RegisterCustomFilter(int priority, object fncallback); }; + +struct IkSolverBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + IkSolverBaseInitializer(py::module& m_); + void init_openravepy_iksolver(); + py::module& m; +#else + IkSolverBaseInitializer(); + void init_openravepy_iksolver(); +#endif +}; + } // namespace openravepy -#endif // OPENRAVEPY_INTERNAL_IKSOLVERBASE_H \ No newline at end of file +#endif // OPENRAVEPY_INTERNAL_IKSOLVERBASE_H diff --git a/python/bindings/include/openravepy/openravepy_int.h b/python/bindings/include/openravepy/openravepy_int.h index 525b1e2808..51c7c8f233 100644 --- a/python/bindings/include/openravepy/openravepy_int.h +++ b/python/bindings/include/openravepy/openravepy_int.h @@ -588,11 +588,7 @@ class OPENRAVEPY_API PyRay OPENRAVEPY_API py::object toPyGraphHandle(const GraphHandlePtr p); OPENRAVEPY_API py::object toPyUserData(UserDataPtr p); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_ikparameterization(py::module& m); -#else -void init_openravepy_ikparameterization(); -#endif + OPENRAVEPY_API py::object toPyAABB(const AABB& ab); /// \brief PyAABB -> AABB OPENRAVEPY_API AABB ExtractAABB(py::object o); @@ -732,27 +728,15 @@ OPENRAVEPY_API CollisionReportPtr GetCollisionReport(PyCollisionReportPtr); OPENRAVEPY_API PyCollisionReportPtr toPyCollisionReport(CollisionReportPtr, PyEnvironmentBasePtr); OPENRAVEPY_API void UpdateCollisionReport(PyCollisionReportPtr, PyEnvironmentBasePtr); OPENRAVEPY_API void UpdateCollisionReport(py::object, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_controller(py::module& m); -#else -void init_openravepy_controller(); -#endif + OPENRAVEPY_API ControllerBasePtr GetController(PyControllerBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyController(ControllerBasePtr, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_iksolver(py::module& m); -#else -void init_openravepy_iksolver(); -#endif + OPENRAVEPY_API IkSolverBasePtr GetIkSolver(py::object); OPENRAVEPY_API IkSolverBasePtr GetIkSolver(PyIkSolverBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyIkSolver(IkSolverBasePtr, PyEnvironmentBasePtr); OPENRAVEPY_API py::object toPyIkSolver(IkSolverBasePtr, py::object); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_kinbody(py::module& m); -#else -void init_openravepy_kinbody(); -#endif + OPENRAVEPY_API KinBodyPtr GetKinBody(py::object); OPENRAVEPY_API KinBodyPtr GetKinBody(PyKinBodyPtr); OPENRAVEPY_API PyEnvironmentBasePtr GetPyEnvFromPyKinBody(py::object okinbody); @@ -767,46 +751,26 @@ OPENRAVEPY_API py::object toPyKinBodyJoint(KinBody::JointPtr pjoint, PyEnvironme OPENRAVEPY_API KinBody::JointPtr GetKinBodyJoint(py::object); OPENRAVEPY_API std::string reprPyKinBodyJoint(py::object); OPENRAVEPY_API std::string strPyKinBodyJoint(py::object); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_module(py::module& m); -#else -void init_openravepy_module(); -#endif + OPENRAVEPY_API ModuleBasePtr GetModule(PyModuleBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyModule(ModuleBasePtr, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_physicsengine(py::module& m); -#else -void init_openravepy_physicsengine(); -#endif + OPENRAVEPY_API PhysicsEngineBasePtr GetPhysicsEngine(PyPhysicsEngineBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyPhysicsEngine(PhysicsEngineBasePtr, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_planner(py::module& m); -#else -void init_openravepy_planner(); -#endif + OPENRAVEPY_API PlannerBasePtr GetPlanner(PyPlannerBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyPlanner(PlannerBasePtr, PyEnvironmentBasePtr); OPENRAVEPY_API PlannerBase::PlannerParametersPtr GetPlannerParameters(py::object); OPENRAVEPY_API PlannerBase::PlannerParametersConstPtr GetPlannerParametersConst(py::object); OPENRAVEPY_API py::object toPyPlannerParameters(PlannerBase::PlannerParametersPtr params); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_robot(py::module& m); -#else -void init_openravepy_robot(); -#endif + OPENRAVEPY_API RobotBasePtr GetRobot(py::object); OPENRAVEPY_API RobotBasePtr GetRobot(PyRobotBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyRobot(RobotBasePtr, PyEnvironmentBasePtr); OPENRAVEPY_API RobotBase::ManipulatorPtr GetRobotManipulator(py::object); OPENRAVEPY_API py::object toPyRobotManipulator(RobotBase::ManipulatorPtr, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_sensor(py::module& m); -#else -void init_openravepy_sensor(); -#endif + OPENRAVEPY_API SensorBasePtr GetSensor(PySensorBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPySensor(SensorBasePtr, PyEnvironmentBasePtr); OPENRAVEPY_API py::object toPySensorData(SensorBasePtr, PyEnvironmentBasePtr); @@ -824,11 +788,7 @@ void init_openravepy_spacesampler(); #endif OPENRAVEPY_API SpaceSamplerBasePtr GetSpaceSampler(PySpaceSamplerBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPySpaceSampler(SpaceSamplerBasePtr, PyEnvironmentBasePtr); -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_trajectory(py::module& m); -#else -void init_openravepy_trajectory(); -#endif + OPENRAVEPY_API TrajectoryBasePtr GetTrajectory(py::object); OPENRAVEPY_API TrajectoryBasePtr GetTrajectory(PyTrajectoryBasePtr); OPENRAVEPY_API PyInterfaceBasePtr toPyTrajectory(TrajectoryBasePtr, PyEnvironmentBasePtr); diff --git a/python/bindings/include/openravepy/openravepy_kinbody.h b/python/bindings/include/openravepy/openravepy_kinbody.h index 1f77753939..b9b38eed51 100644 --- a/python/bindings/include/openravepy/openravepy_kinbody.h +++ b/python/bindings/include/openravepy/openravepy_kinbody.h @@ -362,6 +362,18 @@ class PyKinBody : public PyInterfaceBase template py::object GetCustomParameters(const std::map >& parameters, py::object oname = py::none_(), int index = -1); +struct KinBodyInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + KinBodyInitializer(py::module& m_); + void init_openravepy_kinbody(); + py::module& m; +#else + KinBodyInitializer(); + void init_openravepy_kinbody(); +#endif +}; + } // namespace openravepy #endif diff --git a/python/bindings/include/openravepy/openravepy_module.h b/python/bindings/include/openravepy/openravepy_module.h index 1abe00b322..497cb0337e 100644 --- a/python/bindings/include/openravepy/openravepy_module.h +++ b/python/bindings/include/openravepy/openravepy_module.h @@ -35,5 +35,17 @@ class OPENRAVEPY_API PyModuleBase : public PyInterfaceBase bool SimulationStep(dReal fElapsedTime); }; +struct ModuleBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + ModuleBaseInitializer(py::module& m_); + void init_openravepy_module(); + py::module& m; +#else + ModuleBaseInitializer(); + void init_openravepy_module(); +#endif +}; + } // namespace openravepy -#endif // OPENRAVEPY_INTERNAL_MODULE_H \ No newline at end of file +#endif // OPENRAVEPY_INTERNAL_MODULE_H diff --git a/python/bindings/include/openravepy/openravepy_physicalenginebase.h b/python/bindings/include/openravepy/openravepy_physicalenginebase.h index 0ebe301612..c00e04bdd0 100644 --- a/python/bindings/include/openravepy/openravepy_physicalenginebase.h +++ b/python/bindings/include/openravepy/openravepy_physicalenginebase.h @@ -61,6 +61,18 @@ class PyPhysicsEngineBase : public PyInterfaceBase void SimulateStep(dReal fTimeElapsed); }; +struct PhysicsEngineBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + PhysicsEngineBaseInitializer(py::module& m_); + void init_openravepy_physicsengine(); + py::module& m; +#else + PhysicsEngineBaseInitializer(); + void init_openravepy_physicsengine(); +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_PHYSICSENGINE_H diff --git a/python/bindings/include/openravepy/openravepy_plannerbase.h b/python/bindings/include/openravepy/openravepy_plannerbase.h index ae36fc1346..9cc3a95cf3 100644 --- a/python/bindings/include/openravepy/openravepy_plannerbase.h +++ b/python/bindings/include/openravepy/openravepy_plannerbase.h @@ -146,5 +146,17 @@ class PyPlannerBase : public PyInterfaceBase PlannerBasePtr GetPlanner(); }; +struct PlannerBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + PlannerBaseInitializer(py::module& m_); + void init_openravepy_planner(); + py::module& m; +#else + PlannerBaseInitializer(); + void init_openravepy_planner(); +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_PLANNERBASE_H diff --git a/python/bindings/include/openravepy/openravepy_robotbase.h b/python/bindings/include/openravepy/openravepy_robotbase.h index d18f78ee4e..45cec36f57 100644 --- a/python/bindings/include/openravepy/openravepy_robotbase.h +++ b/python/bindings/include/openravepy/openravepy_robotbase.h @@ -450,6 +450,18 @@ class PyRobotBase : public PyKinBody virtual void __enter__(); }; +struct RobotBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + RobotBaseInitializer(py::module& m_); + void init_openravepy_robot(); + py::module& m; +#else + RobotBaseInitializer(); + void init_openravepy_robot(); +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_ROBOT_H diff --git a/python/bindings/include/openravepy/openravepy_sensorbase.h b/python/bindings/include/openravepy/openravepy_sensorbase.h index ccc6eac369..0facc283df 100644 --- a/python/bindings/include/openravepy/openravepy_sensorbase.h +++ b/python/bindings/include/openravepy/openravepy_sensorbase.h @@ -315,6 +315,20 @@ class PySensorBase : public PyInterfaceBase virtual object __unicode__(); }; +struct SensorBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + SensorBaseInitializer(py::module& m_); + void init_openravepy_sensor(); + py::module& m; + py::class_, PyInterfaceBase> sensor; +#else + SensorBaseInitializer(); + void init_openravepy_sensor(); + py::class_, bases > sensor; +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_SENSORBASE_H diff --git a/python/bindings/include/openravepy/openravepy_trajectorybase.h b/python/bindings/include/openravepy/openravepy_trajectorybase.h index 59c92cb863..6b2ffff477 100644 --- a/python/bindings/include/openravepy/openravepy_trajectorybase.h +++ b/python/bindings/include/openravepy/openravepy_trajectorybase.h @@ -112,5 +112,17 @@ class OPENRAVEPY_API PyTrajectoryBase : public PyInterfaceBase mutable std::vector _vdataCache, _vtimesCache; ///< caches to avoid memory allocation }; +struct TrajectoryBaseInitializer +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + TrajectoryBaseInitializer(py::module& m_); + void init_openravepy_trajectory(); + py::module& m; +#else + TrajectoryBaseInitializer(); + void init_openravepy_trajectory(); +#endif +}; + } // namespace openravepy #endif // OPENRAVEPY_INTERNAL_TRAJECTORYBASE_H diff --git a/python/bindings/openravepy_collisionchecker.cpp b/python/bindings/openravepy_collisionchecker.cpp index 8264eea7b4..831f736d77 100644 --- a/python/bindings/openravepy_collisionchecker.cpp +++ b/python/bindings/openravepy_collisionchecker.cpp @@ -760,7 +760,7 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Reset_overloads, Reset, 0, 1) #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS -CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(py::module& m): +CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(py::module& m_): m(m_), collisionchecker(m, "CollisionChecker", DOXY_CLASS(CollisionCheckerBase)) #else CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(): @@ -770,7 +770,7 @@ CollisionCheckerBaseInitializer::CollisionCheckerBaseInitializer(): } #ifdef USE_PYBIND11_PYTHON_BINDINGS -CollisionReportInitializer::CollisionReportInitializer(py::module& m): +CollisionReportInitializer::CollisionReportInitializer(py::module& m_): m(m_), collisionreport(m, "CollisionReport", DOXY_CLASS(CollisionReport)) #else CollisionReportInitializer::CollisionReportInitializer(): @@ -779,11 +779,7 @@ CollisionReportInitializer::CollisionReportInitializer(): { } -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker(py::module& m) -#else void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker() -#endif { #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a @@ -891,11 +887,7 @@ void CollisionCheckerBaseInitializer::init_openravepy_collisionchecker() #endif } -#ifdef USE_PYBIND11_PYTHON_BINDINGS -void CollisionReportInitializer::init_openravepy_collisionreport(py::module& m) -#else void CollisionReportInitializer::init_openravepy_collisionreport() -#endif { #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a diff --git a/python/bindings/openravepy_controller.cpp b/python/bindings/openravepy_controller.cpp index 4398777c8e..2af5b9adb0 100644 --- a/python/bindings/openravepy_controller.cpp +++ b/python/bindings/openravepy_controller.cpp @@ -204,10 +204,16 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(Reset_overloads, Reset, 0, 1) #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_controller(py::module& m) +ControllerBaseInitializer::ControllerBaseInitializer(py::module& m_): m(m_), + controller(m, "Controller", DOXY_CLASS(ControllerBase)) #else -void init_openravepy_controller() +ControllerBaseInitializer::ControllerBaseInitializer(): + controller("Controller", DOXY_CLASS(ControllerBase), no_init) #endif +{ +} + +void ControllerBaseInitializer::init_openravepy_controller() { { bool (PyControllerBase::*init1)(PyRobotBasePtr,const string &) = &PyControllerBase::Init; @@ -216,32 +222,28 @@ void init_openravepy_controller() bool (PyControllerBase::*setdesired2)(object,object) = &PyControllerBase::SetDesired; #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a - class_, PyInterfaceBase>(m, "Controller", DOXY_CLASS(ControllerBase)) -#else - class_, bases >("Controller", DOXY_CLASS(ControllerBase), no_init) #endif - .def("Init",init1, DOXY_FN(ControllerBase,Init)) - .def("Init",init2, PY_ARGS("robot","dofindices","controltransform") DOXY_FN(ControllerBase,Init)) - .def("GetControlDOFIndices",&PyControllerBase::GetControlDOFIndices,DOXY_FN(ControllerBase,GetControlDOFIndices)) - .def("IsControlTransformation",&PyControllerBase::IsControlTransformation, DOXY_FN(ControllerBase,IsControlTransformation)) - .def("GetRobot",&PyControllerBase::GetRobot, DOXY_FN(ControllerBase,GetRobot)) + controller.def("Init",init1, DOXY_FN(ControllerBase,Init)); + controller.def("Init",init2, PY_ARGS("robot","dofindices","controltransform") DOXY_FN(ControllerBase,Init)); + controller.def("GetControlDOFIndices",&PyControllerBase::GetControlDOFIndices,DOXY_FN(ControllerBase,GetControlDOFIndices)); + controller.def("IsControlTransformation",&PyControllerBase::IsControlTransformation, DOXY_FN(ControllerBase,IsControlTransformation)); + controller.def("GetRobot",&PyControllerBase::GetRobot, DOXY_FN(ControllerBase,GetRobot)); #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("Reset", &PyControllerBase::Reset, - "options"_a = 0, - DOXY_FN(ControllerBase, Reset) - ) + controller.def("Reset", &PyControllerBase::Reset, + "options"_a = 0, + DOXY_FN(ControllerBase, Reset) + ); #else - .def("Reset",&PyControllerBase::Reset, Reset_overloads(PY_ARGS("options") DOXY_FN(ControllerBase,Reset))) + controller.def("Reset",&PyControllerBase::Reset, Reset_overloads(PY_ARGS("options") DOXY_FN(ControllerBase,Reset))); #endif - .def("SetDesired",setdesired1, PY_ARGS("values") DOXY_FN(ControllerBase,SetDesired)) - .def("SetDesired",setdesired2, PY_ARGS("values","transform") DOXY_FN(ControllerBase,SetDesired)) - .def("SetPath",&PyControllerBase::SetPath, DOXY_FN(ControllerBase,SetPath)) - .def("SimulationStep",&PyControllerBase::SimulationStep, DOXY_FN(ControllerBase,SimulationStep "dReal")) - .def("IsDone",&PyControllerBase::IsDone, DOXY_FN(ControllerBase,IsDone)) - .def("GetTime",&PyControllerBase::GetTime, DOXY_FN(ControllerBase,GetTime)) - .def("GetVelocity",&PyControllerBase::GetVelocity, DOXY_FN(ControllerBase,GetVelocity)) - .def("GetTorque",&PyControllerBase::GetTorque, DOXY_FN(ControllerBase,GetTorque)) - ; + controller.def("SetDesired",setdesired1, PY_ARGS("values") DOXY_FN(ControllerBase,SetDesired)); + controller.def("SetDesired",setdesired2, PY_ARGS("values","transform") DOXY_FN(ControllerBase,SetDesired)); + controller.def("SetPath",&PyControllerBase::SetPath, DOXY_FN(ControllerBase,SetPath)); + controller.def("SimulationStep",&PyControllerBase::SimulationStep, DOXY_FN(ControllerBase,SimulationStep "dReal")); + controller.def("IsDone",&PyControllerBase::IsDone, DOXY_FN(ControllerBase,IsDone)); + controller.def("GetTime",&PyControllerBase::GetTime, DOXY_FN(ControllerBase,GetTime)); + controller.def("GetVelocity",&PyControllerBase::GetVelocity, DOXY_FN(ControllerBase,GetVelocity)); + controller.def("GetTorque",&PyControllerBase::GetTorque, DOXY_FN(ControllerBase,GetTorque)); } { diff --git a/python/bindings/openravepy_ikparameterization.cpp b/python/bindings/openravepy_ikparameterization.cpp index a3a2b2af0e..b8730df8c3 100644 --- a/python/bindings/openravepy_ikparameterization.cpp +++ b/python/bindings/openravepy_ikparameterization.cpp @@ -406,11 +406,15 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyIkParameterization_DeserializeJSON_over #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_ikparameterization(py::module& m) +IkParameterizationInitializer::IkParameterizationInitializer(py::module& m_): m(m_) #else -void init_openravepy_ikparameterization() +IkParameterizationInitializer::IkParameterizationInitializer() #endif { +} + +void IkParameterizationInitializer::init_openravepy_ikparameterization() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a object iktype = enum_(m, "IkParameterizationType", py::arithmetic() DOXY_ENUM(IkParameterizationType)) diff --git a/python/bindings/openravepy_iksolver.cpp b/python/bindings/openravepy_iksolver.cpp index e7b76b1fd4..3cabfb125f 100644 --- a/python/bindings/openravepy_iksolver.cpp +++ b/python/bindings/openravepy_iksolver.cpp @@ -291,11 +291,15 @@ PyIkSolverBasePtr RaveCreateIkSolver(PyEnvironmentBasePtr pyenv, const std::stri } #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_iksolver(py::module& m) +IkSolverBaseInitializer::IkSolverBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_iksolver() +IkSolverBaseInitializer::IkSolverBaseInitializer() #endif { +} + +void IkSolverBaseInitializer::init_openravepy_iksolver() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a enum_(m, "IkFilterOptions", py::arithmetic() DOXY_ENUM(IkFilterOptions)) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 191fd165a0..d01e58203a 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -31,6 +31,11 @@ #include #include #include +#include +#include +#include +#include +#include #define OPENRAVE_EXCEPTION_CLASS_NAME "_OpenRAVEException" @@ -3098,12 +3103,22 @@ py::object GetCodeStringOpenRAVEException(OpenRAVEException* p) struct OpenRAVEPyInitializer: public CollisionCheckerBaseInitializer, - public CollisionReportInitializer + public CollisionReportInitializer, + public ControllerBaseInitializer, + public IkParameterizationInitializer, + public IkSolverBaseInitializer, + public KinBodyInitializer, + public ModuleBaseInitializer, + public PhysicsEngineBaseInitializer, + public PlannerBaseInitializer, + public RobotBaseInitializer, + public SensorBaseInitializer, + public TrajectoryBaseInitializer { #ifdef USE_PYBIND11_PYTHON_BINDINGS - OpenRAVEPyInitializer(py::module& m): CollisionCheckerBaseInitializer(m), CollisionReportInitializer(m) + OpenRAVEPyInitializer(py::module& m_): CollisionCheckerBaseInitializer(m_), CollisionReportInitializer(m_), ControllerBaseInitializer(m_), IkParameterizationInitializer(m_), IkSolverBaseInitializer(m_), KinBodyInitializer(m_), ModuleBaseInitializer(m_), PhysicsEngineBaseInitializer(m_), PlannerBaseInitializer(m_), RobotBaseInitializer(m_), SensorBaseInitializer(m_), TrajectoryBaseInitializer(m_) #else - OpenRAVEPyInitializer(): CollisionCheckerBaseInitializer(), CollisionReportInitializer() + OpenRAVEPyInitializer(): CollisionCheckerBaseInitializer(), CollisionReportInitializer(), ControllerBaseInitializer(), IkParameterizationInitializer(), IkSolverBaseInitializer(), KinBodyInitializer(), ModuleBaseInitializer(), PhysicsEngineBaseInitializer(), PlannerBaseInitializer(), RobotBaseInitializer(), SensorBaseInitializer(), TrajectoryBaseInitializer() #endif { } @@ -3314,29 +3329,24 @@ Because race conditions can pop up when trying to lock the openrave environment #ifdef USE_PYBIND11_PYTHON_BINDINGS OpenRAVEPyInitializer initializer(m); - // PyCollisionCheckerBaseBinderPtr pCollisionChecker = openravepy::init_openravepy_collisioncheckerclass(m); openravepy::init_openravepy_global(m); - openravepy::init_openravepy_controller(m); - openravepy::init_openravepy_ikparameterization(m); - openravepy::init_openravepy_iksolver(m); - openravepy::init_openravepy_kinbody(m); - // openravepy::init_openravepy_collisionchecker(m, *pCollisionChecker); - initializer.init_openravepy_collisionchecker(m); - initializer.init_openravepy_collisionreport(m); - openravepy::init_openravepy_sensor(m); - openravepy::init_openravepy_sensorsystem(m); - openravepy::init_openravepy_robot(m); - - openravepy::init_openravepy_module(m); - openravepy::init_openravepy_physicsengine(m); - openravepy::init_openravepy_trajectory(m); - openravepy::init_openravepy_planner(m); - + initializer.init_openravepy_ikparameterization(); + initializer.init_openravepy_iksolver(); + initializer.init_openravepy_kinbody(); + initializer.init_openravepy_collisionchecker(); + initializer.init_openravepy_collisionreport(); + initializer.init_openravepy_sensor(); + initializer.init_openravepy_robot(); + initializer.init_openravepy_module(); + initializer.init_openravepy_physicsengine(); + initializer.init_openravepy_trajectory(); + initializer.init_openravepy_planner(); + initializer.init_openravepy_controller(); + openravepy::InitPlanningUtils(m); + openravepy::init_openravepy_sensorsystem(m); openravepy::init_openravepy_spacesampler(m); openravepy::init_openravepy_viewer(m); - - openravepy::InitPlanningUtils(m); openravepy::init_openravepy_global_functions(m); #else openravepy::init_openravepy_global(); diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 2fb72f5928..d64e50abb1 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -4655,11 +4655,15 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ComputeLocalAABBForGeometryGroup_overload #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_kinbody(py::module& m) +KinBodyInitializer::KinBodyInitializer(py::module& m_): m(m_) #else -void init_openravepy_kinbody() +KinBodyInitializer::KinBodyInitializer() #endif { +} + +void KinBodyInitializer::init_openravepy_kinbody() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a class_(m, "StateRestoreContext") diff --git a/python/bindings/openravepy_module.cpp b/python/bindings/openravepy_module.cpp index ef7326b819..b3e0367c0d 100644 --- a/python/bindings/openravepy_module.cpp +++ b/python/bindings/openravepy_module.cpp @@ -75,11 +75,15 @@ PyModuleBasePtr RaveCreateModule(PyEnvironmentBasePtr pyenv, const std::string& } #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_module(py::module& m) +ModuleBaseInitializer::ModuleBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_module() +ModuleBaseInitializer::ModuleBaseInitializer() #endif { +} + +void ModuleBaseInitializer::init_openravepy_module() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a class_, PyInterfaceBase>(m, "Module", DOXY_CLASS(ModuleBase)) diff --git a/python/bindings/openravepy_physicsengine.cpp b/python/bindings/openravepy_physicsengine.cpp index 01d224e426..45a9d98eff 100644 --- a/python/bindings/openravepy_physicsengine.cpp +++ b/python/bindings/openravepy_physicsengine.cpp @@ -210,11 +210,15 @@ PyPhysicsEngineBasePtr RaveCreatePhysicsEngine(PyEnvironmentBasePtr pyenv, const } #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_physicsengine(py::module& m) +PhysicsEngineBaseInitializer::PhysicsEngineBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_physicsengine() +PhysicsEngineBaseInitializer::PhysicsEngineBaseInitializer() #endif { +} + +void PhysicsEngineBaseInitializer::init_openravepy_physicsengine() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a class_, PyInterfaceBase>(m, "PhysicsEngine", DOXY_CLASS(PhysicsEngineBase)) diff --git a/python/bindings/openravepy_planner.cpp b/python/bindings/openravepy_planner.cpp index a3522680e8..69891bc975 100644 --- a/python/bindings/openravepy_planner.cpp +++ b/python/bindings/openravepy_planner.cpp @@ -434,11 +434,15 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SetStateValues_overloads, SetStateValues, #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_planner(py::module& m) +PlannerBaseInitializer::PlannerBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_planner() +PlannerBaseInitializer::PlannerBaseInitializer() #endif { +} + +void PlannerBaseInitializer::init_openravepy_planner() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; object plannerstatuscode = enum_(m, "PlannerStatusCode", py::arithmetic() DOXY_ENUM(PlannerStatusCode)) diff --git a/python/bindings/openravepy_robot.cpp b/python/bindings/openravepy_robot.cpp index aac96a7377..e870c5ccca 100644 --- a/python/bindings/openravepy_robot.cpp +++ b/python/bindings/openravepy_robot.cpp @@ -2346,11 +2346,15 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyRobotBaseInfo_DeserializeJSON_overloads #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_robot(py::module& m) +RobotBaseInitializer::RobotBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_robot() +RobotBaseInitializer::RobotBaseInitializer() #endif { +} + +void RobotBaseInitializer::init_openravepy_robot() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a object dofaffine = enum_(m, "DOFAffine", py::arithmetic() DOXY_ENUM(DOFAffine)) diff --git a/python/bindings/openravepy_sensor.cpp b/python/bindings/openravepy_sensor.cpp index 4e6d2e0262..bfdd5c6404 100644 --- a/python/bindings/openravepy_sensor.cpp +++ b/python/bindings/openravepy_sensor.cpp @@ -725,11 +725,17 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyForce6DGeomData_DeserializeJSON_overloa #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_sensor(py::module& m) +SensorBaseInitializer::SensorBaseInitializer(py::module& m_): m(m_), + sensor(m, "Sensor", DOXY_CLASS(SensorBase)) #else -void init_openravepy_sensor() +SensorBaseInitializer::SensorBaseInitializer(): + sensor("Sensor", DOXY_CLASS(SensorBase), no_init) #endif { +} + +void SensorBaseInitializer::init_openravepy_sensor() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a #endif @@ -737,11 +743,6 @@ void init_openravepy_sensor() { OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData1)() = &PySensorBase::GetSensorData; OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData2)(SensorBase::SensorType) = &PySensorBase::GetSensorData; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_, PyInterfaceBase> sensor(m, "Sensor", DOXY_CLASS(SensorBase)); -#else - class_, bases > sensor("Sensor", DOXY_CLASS(SensorBase), no_init); -#endif #ifdef USE_PYBIND11_PYTHON_BINDINGS // SensorData is inside SensorBase diff --git a/python/bindings/openravepy_trajectory.cpp b/python/bindings/openravepy_trajectory.cpp index 34ec0b7f07..07edcf8340 100644 --- a/python/bindings/openravepy_trajectory.cpp +++ b/python/bindings/openravepy_trajectory.cpp @@ -621,11 +621,15 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SaveToFile_overloads, SaveToFile, 1, 2) #endif // #ifdef USE_PYBIND11_PYTHON_BINDINGS -void init_openravepy_trajectory(py::module& m) +TrajectoryBaseInitializer::TrajectoryBaseInitializer(py::module& m_): m(m_) #else -void init_openravepy_trajectory() +TrajectoryBaseInitializer::TrajectoryBaseInitializer() #endif { +} + +void TrajectoryBaseInitializer::init_openravepy_trajectory() +{ #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a #endif From 42b9f11d421e90261f5139be32fadf0488b45b96 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 19:09:44 +0900 Subject: [PATCH 11/13] progress --- .../include/openravepy/openravepy_kinbody.h | 2 + .../openravepy/openravepy_plannerbase.h | 2 + .../include/openravepy/openravepy_robotbase.h | 2 + python/bindings/openravepy_int.cpp | 28 +- python/bindings/openravepy_kinbody.cpp | 835 +++++++++--------- python/bindings/openravepy_planner.cpp | 12 +- python/bindings/openravepy_robot.cpp | 12 +- 7 files changed, 446 insertions(+), 447 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_kinbody.h b/python/bindings/include/openravepy/openravepy_kinbody.h index b9b38eed51..b2b0b0c0cd 100644 --- a/python/bindings/include/openravepy/openravepy_kinbody.h +++ b/python/bindings/include/openravepy/openravepy_kinbody.h @@ -368,9 +368,11 @@ struct KinBodyInitializer KinBodyInitializer(py::module& m_); void init_openravepy_kinbody(); py::module& m; + py::class_, PyInterfaceBase> kinbody; #else KinBodyInitializer(); void init_openravepy_kinbody(); + py::class_, bases > kinbody; #endif }; diff --git a/python/bindings/include/openravepy/openravepy_plannerbase.h b/python/bindings/include/openravepy/openravepy_plannerbase.h index 9cc3a95cf3..dc73429a1c 100644 --- a/python/bindings/include/openravepy/openravepy_plannerbase.h +++ b/python/bindings/include/openravepy/openravepy_plannerbase.h @@ -152,9 +152,11 @@ struct PlannerBaseInitializer PlannerBaseInitializer(py::module& m_); void init_openravepy_planner(); py::module& m; + py::class_, PyInterfaceBase> planner; #else PlannerBaseInitializer(); void init_openravepy_planner(); + py::class_, bases > planner; #endif }; diff --git a/python/bindings/include/openravepy/openravepy_robotbase.h b/python/bindings/include/openravepy/openravepy_robotbase.h index 45cec36f57..029421a915 100644 --- a/python/bindings/include/openravepy/openravepy_robotbase.h +++ b/python/bindings/include/openravepy/openravepy_robotbase.h @@ -456,9 +456,11 @@ struct RobotBaseInitializer RobotBaseInitializer(py::module& m_); void init_openravepy_robot(); py::module& m; + py::class_, PyKinBody> robot; #else RobotBaseInitializer(); void init_openravepy_robot(); + py::class_, bases > robot; #endif }; diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index d01e58203a..cbd1fa6398 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3349,23 +3349,27 @@ Because race conditions can pop up when trying to lock the openrave environment openravepy::init_openravepy_viewer(m); openravepy::init_openravepy_global_functions(m); #else + OpenRAVEPyInitializer initializer(); openravepy::init_openravepy_global(); - openravepy::InitPlanningUtils(); + initializer.init_openravepy_ikparameterization(); + initializer.init_openravepy_iksolver(); + initializer.init_openravepy_kinbody(); + initializer.init_openravepy_collisionchecker(); + initializer.init_openravepy_collisionreport(); + initializer.init_openravepy_sensor(); + initializer.init_openravepy_robot(); + initializer.init_openravepy_module(); + initializer.init_openravepy_physicsengine(); + initializer.init_openravepy_trajectory(); + initializer.init_openravepy_planner(); + initializer.init_openravepy_controller(); + initializer.init_openravepy_global(); - openravepy::init_openravepy_collisionchecker(); - openravepy::init_openravepy_controller(); - openravepy::init_openravepy_ikparameterization(); - openravepy::init_openravepy_iksolver(); - openravepy::init_openravepy_kinbody(); - openravepy::init_openravepy_robot(); - openravepy::init_openravepy_module(); - openravepy::init_openravepy_physicsengine(); - openravepy::init_openravepy_planner(); - openravepy::init_openravepy_trajectory(); - openravepy::init_openravepy_sensor(); + openravepy::InitPlanningUtils(); openravepy::init_openravepy_sensorsystem(); openravepy::init_openravepy_spacesampler(); openravepy::init_openravepy_viewer(); + openravepy::init_openravepy_global_functions(); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index d64e50abb1..5b3b3445ac 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -4655,9 +4655,11 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(ComputeLocalAABBForGeometryGroup_overload #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -KinBodyInitializer::KinBodyInitializer(py::module& m_): m(m_) +KinBodyInitializer::KinBodyInitializer(py::module& m_): m(m_), + kinbody(m, "KinBody", py::dynamic_attr(), DOXY_CLASS(KinBody)) #else -KinBodyInitializer::KinBodyInitializer() +KinBodyInitializer::KinBodyInitializer(), + kinbody("KinBody", DOXY_CLASS(KinBody), no_init) #endif { } @@ -5342,440 +5344,435 @@ void KinBodyInitializer::init_openravepy_kinbody() std::string sInitFromBoxesDoc = std::string(DOXY_FN(KinBody,InitFromBoxes "const std::vector< AABB; bool")) + std::string("\nboxes is a Nx6 array, first 3 columsn are position, last 3 are extents"); std::string sGetChainDoc = std::string(DOXY_FN(KinBody,GetChain)) + std::string("If returnjoints is false will return a list of links, otherwise will return a list of links (default is true)"); std::string sComputeInverseDynamicsDoc = std::string(":param returncomponents: If True will return three N-element arrays that represents the torque contributions to M, C, and G.\n\n:param externalforcetorque: A dictionary of link indices and a 6-element array of forces/torques in that order.\n\n") + std::string(DOXY_FN(KinBody, ComputeInverseDynamics)); -#ifdef USE_PYBIND11_PYTHON_BINDINGS - scope_ kinbody = class_, PyInterfaceBase>(m, "KinBody", py::dynamic_attr(), DOXY_CLASS(KinBody)) -#else - scope_ kinbody = class_, bases >("KinBody", DOXY_CLASS(KinBody), no_init) -#endif - .def("Destroy",&PyKinBody::Destroy, DOXY_FN(KinBody,Destroy)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromKinBodyInfo", &PyKinBody::InitFromKinBodyInfo, - "info"_a, - DOXY_FN(KinBody, InitFromKinBodyInfo)) -#else - .def("InitFromKinBodyInfo",&PyKinBody::InitFromKinBodyInfo, DOXY_FN(KinBody, InitFromKinBodyInfo)) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromBoxes", &PyKinBody::InitFromBoxes, - "boxes"_a, - "draw"_a = true, - "uri"_a = "", - sInitFromBoxesDoc.c_str() - ) -#else - .def("InitFromBoxes",&PyKinBody::InitFromBoxes,InitFromBoxes_overloads(PY_ARGS("boxes","draw","uri") sInitFromBoxesDoc.c_str())) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromSpheres", &PyKinBody::InitFromSpheres, - "spherex"_a, - "draw"_a = true, - "uri"_a = "", - DOXY_FN(KinBody, InitFromSpheres) - ) -#else - .def("InitFromSpheres",&PyKinBody::InitFromSpheres,InitFromSpheres_overloads(PY_ARGS("spherex","draw","uri") DOXY_FN(KinBody,InitFromSpheres))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromTrimesh", &PyKinBody::InitFromTrimesh, - "trimesh"_a, - "draw"_a = true, - "uri"_a = "", - DOXY_FN(KinBody, InitFromTrimesh) - ) -#else - .def("InitFromTrimesh",&PyKinBody::InitFromTrimesh,InitFromTrimesh_overloads(PY_ARGS("trimesh","draw","uri") DOXY_FN(KinBody,InitFromTrimesh))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromGeometries", &PyKinBody::InitFromGeometries, - "geometries"_a, - "uri"_a = "", - DOXY_FN(KinBody, InitFromGeometries) - ) -#else - .def("InitFromGeometries",&PyKinBody::InitFromGeometries,InitFromGeometries_overloads(PY_ARGS("geometries", "uri") DOXY_FN(KinBody,InitFromGeometries))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("InitFromLinkInfos", &PyKinBody::InitFromLinkInfos, - "linkInfos"_a, - "uri"_a = "", - DOXY_FN(KinBody, InitFromLinkInfos) - ) -#else - .def("InitFromLinkInfos",&PyKinBody::InitFromLinkInfos,InitFromLinkInfos_overloads(PY_ARGS("linkInfos", "uri") DOXY_FN(KinBody,InitFromLinkInfos))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("Init", &PyKinBody::Init, - "linkinfos"_a, - "jointinfos"_a, - "uri"_a = "", - DOXY_FN(KinBody, Init) - ) -#else - .def("Init",&PyKinBody::Init,Init_overloads(PY_ARGS("linkinfos","jointinfos","uri") DOXY_FN(KinBody,Init))) -#endif - .def("SetLinkGeometriesFromGroup",&PyKinBody::SetLinkGeometriesFromGroup, PY_ARGS("name") DOXY_FN(KinBody,SetLinkGeometriesFromGroup)) - .def("SetLinkGroupGeometries", &PyKinBody::SetLinkGroupGeometries, PY_ARGS("name", "linkgeometries") DOXY_FN(KinBody, SetLinkGroupGeometries)) - .def("SetName", &PyKinBody::SetName,PY_ARGS("name") DOXY_FN(KinBody,SetName)) - .def("GetName",&PyKinBody::GetName,DOXY_FN(KinBody,GetName)) - .def("SetId", &PyKinBody::SetId,PY_ARGS("id") DOXY_FN(KinBody,SetId)) - .def("GetId",&PyKinBody::GetId,DOXY_FN(KinBody,GetId)) - .def("GetDOF",&PyKinBody::GetDOF,DOXY_FN(KinBody,GetDOF)) - .def("GetDOFValues",getdofvalues1,DOXY_FN(KinBody,GetDOFValues)) - .def("GetDOFValues",getdofvalues2,PY_ARGS("indices") DOXY_FN(KinBody,GetDOFValues)) - .def("GetDOFVelocities",getdofvelocities1, DOXY_FN(KinBody,GetDOFVelocities)) - .def("GetDOFVelocities",getdofvelocities2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFVelocities)) - .def("GetDOFLimits",getdoflimits1, DOXY_FN(KinBody,GetDOFLimits)) - .def("GetDOFLimits",getdoflimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFLimits)) - .def("GetDOFVelocityLimits",getdofvelocitylimits1, DOXY_FN(KinBody,GetDOFVelocityLimits)) - .def("GetDOFVelocityLimits",getdofvelocitylimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFVelocityLimits)) - .def("GetDOFAccelerationLimits",getdofaccelerationlimits1, DOXY_FN(KinBody,GetDOFAccelerationLimits)) - .def("GetDOFAccelerationLimits",getdofaccelerationlimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFAccelerationLimits)) - .def("GetDOFJerkLimits",getdofjerklimits1, DOXY_FN(KinBody,GetDOFJerkLimits1)) - .def("GetDOFJerkLimits",getdofjerklimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFJerkLimits2)) - .def("GetDOFHardVelocityLimits",getdofhardvelocitylimits1, DOXY_FN(KinBody,GetDOFHardVelocityLimits1)) - .def("GetDOFHardVelocityLimits",getdofhardvelocitylimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardVelocityLimits2)) - .def("GetDOFHardAccelerationLimits",getdofhardaccelerationlimits1, DOXY_FN(KinBody,GetDOFHardAccelerationLimits1)) - .def("GetDOFHardAccelerationLimits",getdofhardaccelerationlimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardAccelerationLimits2)) - .def("GetDOFHardJerkLimits",getdofhardjerklimits1, DOXY_FN(KinBody,GetDOFHardJerkLimits1)) - .def("GetDOFHardJerkLimits",getdofhardjerklimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardJerkLimits2)) - .def("GetDOFTorqueLimits",getdoftorquelimits1, DOXY_FN(KinBody,GetDOFTorqueLimits)) - .def("GetDOFTorqueLimits",getdoftorquelimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFTorqueLimits)) - .def("GetDOFMaxVel",&PyKinBody::GetDOFMaxVel, DOXY_FN(KinBody,GetDOFMaxVel)) - .def("GetDOFMaxTorque",&PyKinBody::GetDOFMaxTorque, DOXY_FN(KinBody,GetDOFMaxTorque)) - .def("GetDOFMaxAccel",&PyKinBody::GetDOFMaxAccel, DOXY_FN(KinBody,GetDOFMaxAccel)) - .def("GetDOFWeights",getdofweights1, DOXY_FN(KinBody,GetDOFWeights)) - .def("GetDOFWeights",getdofweights2, DOXY_FN(KinBody,GetDOFWeights)) - .def("SetDOFWeights",&PyKinBody::SetDOFWeights, PY_ARGS("weights") DOXY_FN(KinBody,SetDOFWeights)) - .def("SetDOFResolutions",&PyKinBody::SetDOFResolutions, PY_ARGS("resolutions") DOXY_FN(KinBody,SetDOFResolutions)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetDOFLimits", &PyKinBody::SetDOFLimits, - "lower"_a, - "upper"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody, SetDOFLimits) - ) -#else - .def("SetDOFLimits",&PyKinBody::SetDOFLimits, SetDOFLimits_overloads(PY_ARGS("lower","upper","indices") DOXY_FN(KinBody,SetDOFLimits))) -#endif - .def("SetDOFVelocityLimits",&PyKinBody::SetDOFVelocityLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFVelocityLimits)) - .def("SetDOFAccelerationLimits",&PyKinBody::SetDOFAccelerationLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFAccelerationLimits)) - .def("SetDOFJerkLimits",&PyKinBody::SetDOFJerkLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFJerkLimits)) - .def("SetDOFHardVelocityLimits",&PyKinBody::SetDOFHardVelocityLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardVelocityLimits)) - .def("SetDOFHardAccelerationLimits",&PyKinBody::SetDOFHardAccelerationLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardAccelerationLimits)) - .def("SetDOFHardJerkLimits",&PyKinBody::SetDOFHardJerkLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardJerkLimits)) - .def("SetDOFTorqueLimits",&PyKinBody::SetDOFTorqueLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFTorqueLimits)) - .def("GetDOFResolutions",getdofresolutions1, DOXY_FN(KinBody,GetDOFResolutions)) - .def("GetDOFResolutions",getdofresolutions2, DOXY_FN(KinBody,GetDOFResolutions)) - .def("GetLinks",getlinks1, DOXY_FN(KinBody,GetLinks)) - .def("GetLinks",getlinks2, PY_ARGS("indices") DOXY_FN(KinBody,GetLinks)) - .def("GetLink",&PyKinBody::GetLink,PY_ARGS("name") DOXY_FN(KinBody,GetLink)) - .def("GetJoints",getjoints1, DOXY_FN(KinBody,GetJoints)) - .def("GetJoints",getjoints2, PY_ARGS("indices") DOXY_FN(KinBody,GetJoints)) - .def("GetPassiveJoints",&PyKinBody::GetPassiveJoints, DOXY_FN(KinBody,GetPassiveJoints)) - .def("GetDependencyOrderedJoints",&PyKinBody::GetDependencyOrderedJoints, DOXY_FN(KinBody,GetDependencyOrderedJoints)) - .def("GetClosedLoops",&PyKinBody::GetClosedLoops,DOXY_FN(KinBody,GetClosedLoops)) - .def("GetRigidlyAttachedLinks",&PyKinBody::GetRigidlyAttachedLinks,PY_ARGS("linkindex") DOXY_FN(KinBody,GetRigidlyAttachedLinks)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetChain", &PyKinBody::GetChain, - "linkindex1"_a, - "linkindex2"_a, - "returnjoints"_a = true, - sGetChainDoc.c_str() - ) -#else - .def("GetChain",&PyKinBody::GetChain,GetChain_overloads(PY_ARGS("linkindex1","linkindex2","returnjoints") sGetChainDoc.c_str())) -#endif - .def("IsDOFInChain",&PyKinBody::IsDOFInChain,PY_ARGS("linkindex1","linkindex2","dofindex") DOXY_FN(KinBody,IsDOFInChain)) - .def("GetJointIndex",&PyKinBody::GetJointIndex,PY_ARGS("name") DOXY_FN(KinBody,GetJointIndex)) - .def("GetJoint",&PyKinBody::GetJoint,PY_ARGS("name") DOXY_FN(KinBody,GetJoint)) - .def("GetJointFromDOFIndex",&PyKinBody::GetJointFromDOFIndex,PY_ARGS("dofindex") DOXY_FN(KinBody,GetJointFromDOFIndex)) - .def("GetTransform",&PyKinBody::GetTransform, DOXY_FN(KinBody,GetTransform)) - .def("GetTransformPose",&PyKinBody::GetTransformPose, DOXY_FN(KinBody,GetTransform)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetLinkTransformations", &PyKinBody::GetLinkTransformations, - "returndoflastvlaues"_a = false, - DOXY_FN(KinBody,GetLinkTransformations) - ) -#else - .def("GetLinkTransformations",&PyKinBody::GetLinkTransformations, GetLinkTransformations_overloads(PY_ARGS("returndoflastvlaues") DOXY_FN(KinBody,GetLinkTransformations))) -#endif - .def("GetBodyTransformations",&PyKinBody::GetLinkTransformations, DOXY_FN(KinBody,GetLinkTransformations)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetLinkTransformations",&PyKinBody::SetLinkTransformations, - "transforms"_a, - "doflastsetvalues"_a = py::none_(), - DOXY_FN(KinBody,SetLinkTransformations) - ) -#else - .def("SetLinkTransformations",&PyKinBody::SetLinkTransformations,SetLinkTransformations_overloads(PY_ARGS("transforms","doflastsetvalues") DOXY_FN(KinBody,SetLinkTransformations))) -#endif - .def("SetBodyTransformations", &PyKinBody::SetLinkTransformations, PY_ARGS("transforms", "doflastsetvalues") DOXY_FN(KinBody,SetLinkTransformations)) - .def("SetLinkVelocities",&PyKinBody::SetLinkVelocities,PY_ARGS("velocities") DOXY_FN(KinBody,SetLinkVelocities)) - .def("SetVelocity",&PyKinBody::SetVelocity, PY_ARGS("linear","angular") DOXY_FN(KinBody,SetVelocity "const Vector; const Vector")) - .def("SetDOFVelocities",setdofvelocities1, PY_ARGS("dofvelocities") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t")) - .def("SetDOFVelocities",setdofvelocities2, PY_ARGS("dofvelocities","linear","angular") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; const Vector; const Vector; uint32_t")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetDOFVelocities", setdofvelocities3, - "dofvelocities"_a, - "checklimits"_a = (int) KinBody::CLA_CheckLimits, - "indices"_a = py::none_(), - DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t; const std::vector") - ) -#else - .def("SetDOFVelocities",setdofvelocities3, PY_ARGS("dofvelocities","checklimits","indices") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t; const std::vector")) -#endif - .def("SetDOFVelocities",setdofvelocities4, PY_ARGS("dofvelocities","linear","angular","checklimits") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; const Vector; const Vector; uint32_t")) - .def("GetLinkVelocities",&PyKinBody::GetLinkVelocities, DOXY_FN(KinBody,GetLinkVelocities)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetLinkAccelerations", &PyKinBody::GetLinkAccelerations, - "dofaccelerations"_a, - "externalaccelerations"_a = py::none_(), - DOXY_FN(KinBody,GetLinkAccelerations) - ) -#else - .def("GetLinkAccelerations",&PyKinBody::GetLinkAccelerations, GetLinkAccelerations_overloads(PY_ARGS("dofaccelerations", "externalaccelerations") DOXY_FN(KinBody,GetLinkAccelerations))) -#endif - .def("GetLinkEnableStates",&PyKinBody::GetLinkEnableStates, DOXY_FN(KinBody,GetLinkEnableStates)) - .def("SetLinkEnableStates",&PyKinBody::SetLinkEnableStates, DOXY_FN(KinBody,SetLinkEnableStates)) - .def("GetLinkEnableStatesMasks",&PyKinBody::GetLinkEnableStatesMasks, DOXY_FN(KinBody,GetLinkEnableStatesMasks)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeAABB", &PyKinBody::ComputeAABB, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody, ComputeAABB) - ) -#else - .def("ComputeAABB",&PyKinBody::ComputeAABB, ComputeAABB_overloads(PY_ARGS("enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABB))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeAABBFromTransform", &PyKinBody::ComputeAABBFromTransform, - "transform"_a, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody,ComputeAABBFromTransform) - ) -#else - .def("ComputeAABBFromTransform",&PyKinBody::ComputeAABBFromTransform, ComputeAABBFromTransform_overloads(PY_ARGS("transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBFromTransform))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeOBBOnAxes", &PyKinBody::ComputeOBBOnAxes, - "transform"_a, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody,ComputeOBBOnAxes) - ) -#else - .def("ComputeOBBOnAxes",&PyKinBody::ComputeOBBOnAxes, ComputeOBBOnAxes_overloads(PY_ARGS("transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeOBBOnAxes))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeLocalAABB", &PyKinBody::ComputeLocalAABB, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody,ComputeLocalAABB) - ) -#else - .def("ComputeLocalAABB",&PyKinBody::ComputeLocalAABB, ComputeLocalAABB_overloads(PY_ARGS("enabledOnlyLinks") DOXY_FN(KinBody,ComputeLocalAABB))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeAABBForGeometryGroup", &PyKinBody::ComputeAABBForGeometryGroup, - "geomgroupname"_a, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody, ComputeAABBForGeometryGroup) - ) -#else - .def("ComputeAABBForGeometryGroup",&PyKinBody::ComputeAABBForGeometryGroup, ComputeAABBForGeometryGroup_overloads(PY_ARGS("geomgroupname", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBForGeometryGroup_overloads))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeAABBForGeometryGroupFromTransform", &PyKinBody::ComputeAABBForGeometryGroupFromTransform, - "geomgroupname"_a, - "transform"_a, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody, ComputeAABBForGeometryGroupFromTransform) - ) -#else - .def("ComputeAABBForGeometryGroupFromTransform",&PyKinBody::ComputeAABBForGeometryGroupFromTransform, ComputeAABBForGeometryGroupFromTransform_overloads(PY_ARGS("geomgroupname", "transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBForGeometryGroupFromTransform_overloads))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeLocalAABBForGeometryGroup", &PyKinBody::ComputeLocalAABBForGeometryGroup, - "geomgroupname"_a, - "enabledOnlyLinks"_a = false, - DOXY_FN(KinBody, ComputeLocalAABBForGeometryGroup) - ) -#else - .def("ComputeLocalAABBForGeometryGroup",&PyKinBody::ComputeLocalAABBForGeometryGroup, ComputeLocalAABBForGeometryGroup_overloads(PY_ARGS("geomgroupname", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeLocalAABBForGeometryGroup_overloads))) -#endif - .def("GetCenterOfMass", &PyKinBody::GetCenterOfMass, DOXY_FN(KinBody,GetCenterOfMass)) - .def("Enable",&PyKinBody::Enable,PY_ARGS("enable") DOXY_FN(KinBody,Enable)) - .def("IsEnabled",&PyKinBody::IsEnabled, DOXY_FN(KinBody,IsEnabled)) - .def("SetVisible",&PyKinBody::SetVisible,PY_ARGS("visible") DOXY_FN(KinBody,SetVisible)) - .def("IsVisible",&PyKinBody::IsVisible, DOXY_FN(KinBody,IsVisible)) - .def("IsDOFRevolute",&PyKinBody::IsDOFRevolute, PY_ARGS("dofindex") DOXY_FN(KinBody,IsDOFRevolute)) - .def("IsDOFPrismatic",&PyKinBody::IsDOFPrismatic, PY_ARGS("dofindex") DOXY_FN(KinBody,IsDOFPrismatic)) - .def("SetTransform",&PyKinBody::SetTransform,PY_ARGS("transform") DOXY_FN(KinBody,SetTransform)) - .def("SetJointValues",psetdofvalues1,PY_ARGS("values") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")) - .def("SetJointValues",psetdofvalues2,PY_ARGS("values","dofindices") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")) - .def("SetDOFValues",psetdofvalues1,PY_ARGS("values") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")) - .def("SetDOFValues",psetdofvalues2,PY_ARGS("values","dofindices") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")) - .def("SetDOFValues",psetdofvalues3,PY_ARGS("values","dofindices","checklimits") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SubtractDOFValues", &PyKinBody::SubtractDOFValues, - "values0"_a, - "values1"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody,SubtractDOFValues) - ) -#else - .def("SubtractDOFValues",&PyKinBody::SubtractDOFValues,SubtractDOFValues_overloads(PY_ARGS("values0","values1") DOXY_FN(KinBody,SubtractDOFValues))) -#endif - .def("SetDOFTorques",&PyKinBody::SetDOFTorques,PY_ARGS("torques","add") DOXY_FN(KinBody,SetDOFTorques)) - .def("SetJointTorques",&PyKinBody::SetDOFTorques,PY_ARGS("torques","add") DOXY_FN(KinBody,SetDOFTorques)) - .def("SetTransformWithJointValues",&PyKinBody::SetTransformWithDOFValues,PY_ARGS("transform","values") DOXY_FN(KinBody,SetDOFValues "const std::vector; const Transform; uint32_t")) - .def("SetTransformWithDOFValues",&PyKinBody::SetTransformWithDOFValues,PY_ARGS("transform","values") DOXY_FN(KinBody,SetDOFValues "const std::vector; const Transform; uint32_t")) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeJacobianTranslation", &PyKinBody::ComputeJacobianTranslation, - "linkindex"_a, - "position"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody,ComputeJacobianTranslation) - ) -#else - .def("ComputeJacobianTranslation",&PyKinBody::ComputeJacobianTranslation,ComputeJacobianTranslation_overloads(PY_ARGS("linkindex","position","indices") DOXY_FN(KinBody,ComputeJacobianTranslation))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeJacobianAxisAngle", &PyKinBody::ComputeJacobianAxisAngle, - "linkindex"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody,ComputeJacobianAxisAngle) - ) -#else - .def("ComputeJacobianAxisAngle",&PyKinBody::ComputeJacobianAxisAngle,ComputeJacobianAxisAngle_overloads(PY_ARGS("linkindex","indices") DOXY_FN(KinBody,ComputeJacobianAxisAngle))) -#endif - .def("CalculateJacobian",&PyKinBody::CalculateJacobian,PY_ARGS("linkindex","position") DOXY_FN(KinBody,CalculateJacobian "int; const Vector; std::vector")) - .def("CalculateRotationJacobian",&PyKinBody::CalculateRotationJacobian,PY_ARGS("linkindex","quat") DOXY_FN(KinBody,CalculateRotationJacobian "int; const Vector; std::vector")) - .def("CalculateAngularVelocityJacobian",&PyKinBody::CalculateAngularVelocityJacobian,PY_ARGS("linkindex") DOXY_FN(KinBody,CalculateAngularVelocityJacobian "int; std::vector")) - .def("Grab",pgrab2,PY_ARGS("body","grablink") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr")) - .def("Grab",pgrab4,PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(KinBody,Grab "KinBodyPtr; LinkPtr; const std::set; rapidjson::Document")) - .def("Release",&PyKinBody::Release,PY_ARGS("body") DOXY_FN(KinBody,Release)) - .def("ReleaseAllGrabbed",&PyKinBody::ReleaseAllGrabbed, DOXY_FN(KinBody,ReleaseAllGrabbed)) - .def("ReleaseAllGrabbedWithLink",&PyKinBody::ReleaseAllGrabbedWithLink, PY_ARGS("grablink") DOXY_FN(KinBody,ReleaseAllGrabbedWithLink)) - .def("RegrabAll",&PyKinBody::RegrabAll, DOXY_FN(KinBody,RegrabAll)) - .def("IsGrabbing",&PyKinBody::IsGrabbing,PY_ARGS("body") DOXY_FN(KinBody,IsGrabbing)) - .def("CheckGrabbedInfo",checkgrabbedinfo2,PY_ARGS("body","grablink") DOXY_FN(KinBody,CheckGrabbedInfo "const KinBody; const Link")) - .def("CheckGrabbedInfo",checkgrabbedinfo3,PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(KinBody,CheckGrabbedInfo "const KinBody; const Link; const std::set; const rapidjson::Document")) - .def("GetNumGrabbed", &PyKinBody::GetNumGrabbed, DOXY_FN(KinBody,GetNumGrabbed)) - .def("GetGrabbed",&PyKinBody::GetGrabbed, DOXY_FN(KinBody,GetGrabbed)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetGrabbedInfo", &PyKinBody::GetGrabbedInfo, - "grabbedname"_a = py::none_(), - DOXY_FN(KinBody,GetGrabbedInfo) - ) -#else - .def("GetGrabbedInfo",&PyKinBody::GetGrabbedInfo, GetGrabbedInfo_overloads(PY_ARGS("grabbedname") DOXY_FN(KinBody,GetGrabbedInfo))) -#endif - .def("ResetGrabbed",&PyKinBody::ResetGrabbed, PY_ARGS("grabbedinfos") DOXY_FN(KinBody,ResetGrabbed)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeHessianTranslation", &PyKinBody::ComputeHessianTranslation, - "linkindex"_a, - "position"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody,ComputeHessianTranslation) - ) -#else - .def("ComputeHessianTranslation",&PyKinBody::ComputeHessianTranslation,ComputeHessianTranslation_overloads(PY_ARGS("linkindex","position","indices") DOXY_FN(KinBody,ComputeHessianTranslation))) -#endif -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeHessianAxisAngle", &PyKinBody::ComputeHessianAxisAngle, - "linkindex"_a, - "indices"_a = py::none_(), - DOXY_FN(KinBody,ComputeHessianAxisAngle) - ) + + kinbody.def("Destroy",&PyKinBody::Destroy, DOXY_FN(KinBody,Destroy)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromKinBodyInfo", &PyKinBody::InitFromKinBodyInfo, + "info"_a, + DOXY_FN(KinBody, InitFromKinBodyInfo)); +#else + kinbody.def("InitFromKinBodyInfo",&PyKinBody::InitFromKinBodyInfo, DOXY_FN(KinBody, InitFromKinBodyInfo)); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromBoxes", &PyKinBody::InitFromBoxes, + "boxes"_a, + "draw"_a = true, + "uri"_a = "", + sInitFromBoxesDoc.c_str() + ); +#else + kinbody.def("InitFromBoxes",&PyKinBody::InitFromBoxes,InitFromBoxes_overloads(PY_ARGS("boxes","draw","uri") sInitFromBoxesDoc.c_str())); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromSpheres", &PyKinBody::InitFromSpheres, + "spherex"_a, + "draw"_a = true, + "uri"_a = "", + DOXY_FN(KinBody, InitFromSpheres) + ); +#else + kinbody.def("InitFromSpheres",&PyKinBody::InitFromSpheres,InitFromSpheres_overloads(PY_ARGS("spherex","draw","uri") DOXY_FN(KinBody,InitFromSpheres))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromTrimesh", &PyKinBody::InitFromTrimesh, + "trimesh"_a, + "draw"_a = true, + "uri"_a = "", + DOXY_FN(KinBody, InitFromTrimesh) + ); +#else + kinbody.def("InitFromTrimesh",&PyKinBody::InitFromTrimesh,InitFromTrimesh_overloads(PY_ARGS("trimesh","draw","uri") DOXY_FN(KinBody,InitFromTrimesh))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromGeometries", &PyKinBody::InitFromGeometries, + "geometries"_a, + "uri"_a = "", + DOXY_FN(KinBody, InitFromGeometries) + ); +#else + kinbody.def("InitFromGeometries",&PyKinBody::InitFromGeometries,InitFromGeometries_overloads(PY_ARGS("geometries", "uri") DOXY_FN(KinBody,InitFromGeometries))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("InitFromLinkInfos", &PyKinBody::InitFromLinkInfos, + "linkInfos"_a, + "uri"_a = "", + DOXY_FN(KinBody, InitFromLinkInfos) + ); +#else + kinbody.def("InitFromLinkInfos",&PyKinBody::InitFromLinkInfos,InitFromLinkInfos_overloads(PY_ARGS("linkInfos", "uri") DOXY_FN(KinBody,InitFromLinkInfos))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("Init", &PyKinBody::Init, + "linkinfos"_a, + "jointinfos"_a, + "uri"_a = "", + DOXY_FN(KinBody, Init) + ); +#else + kinbody.def("Init",&PyKinBody::Init,Init_overloads(PY_ARGS("linkinfos","jointinfos","uri") DOXY_FN(KinBody,Init))); +#endif + kinbody.def("SetLinkGeometriesFromGroup",&PyKinBody::SetLinkGeometriesFromGroup, PY_ARGS("name") DOXY_FN(KinBody,SetLinkGeometriesFromGroup)); + kinbody.def("SetLinkGroupGeometries", &PyKinBody::SetLinkGroupGeometries, PY_ARGS("name", "linkgeometries") DOXY_FN(KinBody, SetLinkGroupGeometries)); + kinbody.def("SetName", &PyKinBody::SetName,PY_ARGS("name") DOXY_FN(KinBody,SetName)); + kinbody.def("GetName",&PyKinBody::GetName,DOXY_FN(KinBody,GetName)); + kinbody.def("SetId", &PyKinBody::SetId,PY_ARGS("id") DOXY_FN(KinBody,SetId)); + kinbody.def("GetId",&PyKinBody::GetId,DOXY_FN(KinBody,GetId)); + kinbody.def("GetDOF",&PyKinBody::GetDOF,DOXY_FN(KinBody,GetDOF)); + kinbody.def("GetDOFValues",getdofvalues1,DOXY_FN(KinBody,GetDOFValues)); + kinbody.def("GetDOFValues",getdofvalues2,PY_ARGS("indices") DOXY_FN(KinBody,GetDOFValues)); + kinbody.def("GetDOFVelocities",getdofvelocities1, DOXY_FN(KinBody,GetDOFVelocities)); + kinbody.def("GetDOFVelocities",getdofvelocities2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFVelocities)); + kinbody.def("GetDOFLimits",getdoflimits1, DOXY_FN(KinBody,GetDOFLimits)); + kinbody.def("GetDOFLimits",getdoflimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFLimits)); + kinbody.def("GetDOFVelocityLimits",getdofvelocitylimits1, DOXY_FN(KinBody,GetDOFVelocityLimits)); + kinbody.def("GetDOFVelocityLimits",getdofvelocitylimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFVelocityLimits)); + kinbody.def("GetDOFAccelerationLimits",getdofaccelerationlimits1, DOXY_FN(KinBody,GetDOFAccelerationLimits)); + kinbody.def("GetDOFAccelerationLimits",getdofaccelerationlimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFAccelerationLimits)); + kinbody.def("GetDOFJerkLimits",getdofjerklimits1, DOXY_FN(KinBody,GetDOFJerkLimits1)); + kinbody.def("GetDOFJerkLimits",getdofjerklimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFJerkLimits2)); + kinbody.def("GetDOFHardVelocityLimits",getdofhardvelocitylimits1, DOXY_FN(KinBody,GetDOFHardVelocityLimits1)); + kinbody.def("GetDOFHardVelocityLimits",getdofhardvelocitylimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardVelocityLimits2)); + kinbody.def("GetDOFHardAccelerationLimits",getdofhardaccelerationlimits1, DOXY_FN(KinBody,GetDOFHardAccelerationLimits1)); + kinbody.def("GetDOFHardAccelerationLimits",getdofhardaccelerationlimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardAccelerationLimits2)); + kinbody.def("GetDOFHardJerkLimits",getdofhardjerklimits1, DOXY_FN(KinBody,GetDOFHardJerkLimits1)); + kinbody.def("GetDOFHardJerkLimits",getdofhardjerklimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFHardJerkLimits2)); + kinbody.def("GetDOFTorqueLimits",getdoftorquelimits1, DOXY_FN(KinBody,GetDOFTorqueLimits)); + kinbody.def("GetDOFTorqueLimits",getdoftorquelimits2, PY_ARGS("indices") DOXY_FN(KinBody,GetDOFTorqueLimits)); + kinbody.def("GetDOFMaxVel",&PyKinBody::GetDOFMaxVel, DOXY_FN(KinBody,GetDOFMaxVel)); + kinbody.def("GetDOFMaxTorque",&PyKinBody::GetDOFMaxTorque, DOXY_FN(KinBody,GetDOFMaxTorque)); + kinbody.def("GetDOFMaxAccel",&PyKinBody::GetDOFMaxAccel, DOXY_FN(KinBody,GetDOFMaxAccel)); + kinbody.def("GetDOFWeights",getdofweights1, DOXY_FN(KinBody,GetDOFWeights)); + kinbody.def("GetDOFWeights",getdofweights2, DOXY_FN(KinBody,GetDOFWeights)); + kinbody.def("SetDOFWeights",&PyKinBody::SetDOFWeights, PY_ARGS("weights") DOXY_FN(KinBody,SetDOFWeights)); + kinbody.def("SetDOFResolutions",&PyKinBody::SetDOFResolutions, PY_ARGS("resolutions") DOXY_FN(KinBody,SetDOFResolutions)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("SetDOFLimits", &PyKinBody::SetDOFLimits, + "lower"_a, + "upper"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody, SetDOFLimits) + ); +#else + kinbody.def("SetDOFLimits",&PyKinBody::SetDOFLimits, SetDOFLimits_overloads(PY_ARGS("lower","upper","indices") DOXY_FN(KinBody,SetDOFLimits))); +#endif + kinbody.def("SetDOFVelocityLimits",&PyKinBody::SetDOFVelocityLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFVelocityLimits)); + kinbody.def("SetDOFAccelerationLimits",&PyKinBody::SetDOFAccelerationLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFAccelerationLimits)); + kinbody.def("SetDOFJerkLimits",&PyKinBody::SetDOFJerkLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFJerkLimits)); + kinbody.def("SetDOFHardVelocityLimits",&PyKinBody::SetDOFHardVelocityLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardVelocityLimits)); + kinbody.def("SetDOFHardAccelerationLimits",&PyKinBody::SetDOFHardAccelerationLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardAccelerationLimits)); + kinbody.def("SetDOFHardJerkLimits",&PyKinBody::SetDOFHardJerkLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFHardJerkLimits)); + kinbody.def("SetDOFTorqueLimits",&PyKinBody::SetDOFTorqueLimits, PY_ARGS("limits") DOXY_FN(KinBody,SetDOFTorqueLimits)); + kinbody.def("GetDOFResolutions",getdofresolutions1, DOXY_FN(KinBody,GetDOFResolutions)); + kinbody.def("GetDOFResolutions",getdofresolutions2, DOXY_FN(KinBody,GetDOFResolutions)); + kinbody.def("GetLinks",getlinks1, DOXY_FN(KinBody,GetLinks)); + kinbody.def("GetLinks",getlinks2, PY_ARGS("indices") DOXY_FN(KinBody,GetLinks)); + kinbody.def("GetLink",&PyKinBody::GetLink,PY_ARGS("name") DOXY_FN(KinBody,GetLink)); + kinbody.def("GetJoints",getjoints1, DOXY_FN(KinBody,GetJoints)); + kinbody.def("GetJoints",getjoints2, PY_ARGS("indices") DOXY_FN(KinBody,GetJoints)); + kinbody.def("GetPassiveJoints",&PyKinBody::GetPassiveJoints, DOXY_FN(KinBody,GetPassiveJoints)); + kinbody.def("GetDependencyOrderedJoints",&PyKinBody::GetDependencyOrderedJoints, DOXY_FN(KinBody,GetDependencyOrderedJoints)); + kinbody.def("GetClosedLoops",&PyKinBody::GetClosedLoops,DOXY_FN(KinBody,GetClosedLoops)); + kinbody.def("GetRigidlyAttachedLinks",&PyKinBody::GetRigidlyAttachedLinks,PY_ARGS("linkindex") DOXY_FN(KinBody,GetRigidlyAttachedLinks)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetChain", &PyKinBody::GetChain, + "linkindex1"_a, + "linkindex2"_a, + "returnjoints"_a = true, + sGetChainDoc.c_str() + ); +#else + kinbody.def("GetChain",&PyKinBody::GetChain,GetChain_overloads(PY_ARGS("linkindex1","linkindex2","returnjoints") sGetChainDoc.c_str()))l +#endif + kinbody.def("IsDOFInChain",&PyKinBody::IsDOFInChain,PY_ARGS("linkindex1","linkindex2","dofindex") DOXY_FN(KinBody,IsDOFInChain)); + kinbody.def("GetJointIndex",&PyKinBody::GetJointIndex,PY_ARGS("name") DOXY_FN(KinBody,GetJointIndex)); + kinbody.def("GetJoint",&PyKinBody::GetJoint,PY_ARGS("name") DOXY_FN(KinBody,GetJoint)); + kinbody.def("GetJointFromDOFIndex",&PyKinBody::GetJointFromDOFIndex,PY_ARGS("dofindex") DOXY_FN(KinBody,GetJointFromDOFIndex)); + kinbody.def("GetTransform",&PyKinBody::GetTransform, DOXY_FN(KinBody,GetTransform)); + kinbody.def("GetTransformPose",&PyKinBody::GetTransformPose, DOXY_FN(KinBody,GetTransform)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetLinkTransformations", &PyKinBody::GetLinkTransformations, + "returndoflastvlaues"_a = false, + DOXY_FN(KinBody,GetLinkTransformations) + ); +#else + kinbody.def("GetLinkTransformations",&PyKinBody::GetLinkTransformations, GetLinkTransformations_overloads(PY_ARGS("returndoflastvlaues") DOXY_FN(KinBody,GetLinkTransformations))); +#endif + kinbody.def("GetBodyTransformations",&PyKinBody::GetLinkTransformations, DOXY_FN(KinBody,GetLinkTransformations)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("SetLinkTransformations",&PyKinBody::SetLinkTransformations, + "transforms"_a, + "doflastsetvalues"_a = py::none_(), + DOXY_FN(KinBody,SetLinkTransformations) + ); +#else + kinbody.def("SetLinkTransformations",&PyKinBody::SetLinkTransformations,SetLinkTransformations_overloads(PY_ARGS("transforms","doflastsetvalues") DOXY_FN(KinBody,SetLinkTransformations))); +#endif + kinbody.def("SetBodyTransformations", &PyKinBody::SetLinkTransformations, PY_ARGS("transforms", "doflastsetvalues") DOXY_FN(KinBody,SetLinkTransformations)); + kinbody.def("SetLinkVelocities",&PyKinBody::SetLinkVelocities,PY_ARGS("velocities") DOXY_FN(KinBody,SetLinkVelocities)); + kinbody.def("SetVelocity",&PyKinBody::SetVelocity, PY_ARGS("linear","angular") DOXY_FN(KinBody,SetVelocity "const Vector; const Vector")); + kinbody.def("SetDOFVelocities",setdofvelocities1, PY_ARGS("dofvelocities") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t")); + kinbody.def("SetDOFVelocities",setdofvelocities2, PY_ARGS("dofvelocities","linear","angular") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; const Vector; const Vector; uint32_t")); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("SetDOFVelocities", setdofvelocities3, + "dofvelocities"_a, + "checklimits"_a = (int) KinBody::CLA_CheckLimits, + "indices"_a = py::none_(), + DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t; const std::vector") + ); +#else + kinbody.def("SetDOFVelocities",setdofvelocities3, PY_ARGS("dofvelocities","checklimits","indices") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; uint32_t; const std::vector")); +#endif + kinbody.def("SetDOFVelocities",setdofvelocities4, PY_ARGS("dofvelocities","linear","angular","checklimits") DOXY_FN(KinBody,SetDOFVelocities "const std::vector; const Vector; const Vector; uint32_t")); + kinbody.def("GetLinkVelocities",&PyKinBody::GetLinkVelocities, DOXY_FN(KinBody,GetLinkVelocities)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetLinkAccelerations", &PyKinBody::GetLinkAccelerations, + "dofaccelerations"_a, + "externalaccelerations"_a = py::none_(), + DOXY_FN(KinBody,GetLinkAccelerations) + ); +#else + kinbody.def("GetLinkAccelerations",&PyKinBody::GetLinkAccelerations, GetLinkAccelerations_overloads(PY_ARGS("dofaccelerations", "externalaccelerations") DOXY_FN(KinBody,GetLinkAccelerations))); +#endif + kinbody.def("GetLinkEnableStates",&PyKinBody::GetLinkEnableStates, DOXY_FN(KinBody,GetLinkEnableStates)); + kinbody.def("SetLinkEnableStates",&PyKinBody::SetLinkEnableStates, DOXY_FN(KinBody,SetLinkEnableStates)); + kinbody.def("GetLinkEnableStatesMasks",&PyKinBody::GetLinkEnableStatesMasks, DOXY_FN(KinBody,GetLinkEnableStatesMasks)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeAABB", &PyKinBody::ComputeAABB, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody, ComputeAABB) + ); +#else + kinbody.def("ComputeAABB",&PyKinBody::ComputeAABB, ComputeAABB_overloads(PY_ARGS("enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABB))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeAABBFromTransform", &PyKinBody::ComputeAABBFromTransform, + "transform"_a, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody,ComputeAABBFromTransform) + ); +#else + kinbody.def("ComputeAABBFromTransform",&PyKinBody::ComputeAABBFromTransform, ComputeAABBFromTransform_overloads(PY_ARGS("transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBFromTransform))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeOBBOnAxes", &PyKinBody::ComputeOBBOnAxes, + "transform"_a, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody,ComputeOBBOnAxes) + ); +#else + kinbody.def("ComputeOBBOnAxes",&PyKinBody::ComputeOBBOnAxes, ComputeOBBOnAxes_overloads(PY_ARGS("transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeOBBOnAxes))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeLocalAABB", &PyKinBody::ComputeLocalAABB, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody,ComputeLocalAABB) + ); +#else + kinbody.def("ComputeLocalAABB",&PyKinBody::ComputeLocalAABB, ComputeLocalAABB_overloads(PY_ARGS("enabledOnlyLinks") DOXY_FN(KinBody,ComputeLocalAABB))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeAABBForGeometryGroup", &PyKinBody::ComputeAABBForGeometryGroup, + "geomgroupname"_a, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody, ComputeAABBForGeometryGroup) + ); #else - .def("ComputeHessianAxisAngle",&PyKinBody::ComputeHessianAxisAngle,ComputeHessianAxisAngle_overloads(PY_ARGS("linkindex","indices") DOXY_FN(KinBody,ComputeHessianAxisAngle))) + kinbody.def("ComputeAABBForGeometryGroup",&PyKinBody::ComputeAABBForGeometryGroup, ComputeAABBForGeometryGroup_overloads(PY_ARGS("geomgroupname", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBForGeometryGroup_overloads))); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("ComputeInverseDynamics", &PyKinBody::ComputeInverseDynamics, - "dofaccelerations"_a, - "externalforcetorque"_a = py::none_(), - "returncomponents"_a = false, - sComputeInverseDynamicsDoc.c_str() - ) + kinbody.def("ComputeAABBForGeometryGroupFromTransform", &PyKinBody::ComputeAABBForGeometryGroupFromTransform, + "geomgroupname"_a, + "transform"_a, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody, ComputeAABBForGeometryGroupFromTransform) + ); #else - .def("ComputeInverseDynamics",&PyKinBody::ComputeInverseDynamics, ComputeInverseDynamics_overloads(PY_ARGS("dofaccelerations","externalforcetorque","returncomponents") sComputeInverseDynamicsDoc.c_str())) + kinbody.def("ComputeAABBForGeometryGroupFromTransform",&PyKinBody::ComputeAABBForGeometryGroupFromTransform, ComputeAABBForGeometryGroupFromTransform_overloads(PY_ARGS("geomgroupname", "transform", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeAABBForGeometryGroupFromTransform_overloads))); #endif - .def("GetDOFDynamicAccelerationJerkLimits",&PyKinBody::GetDOFDynamicAccelerationJerkLimits, PY_ARGS("dofPositions","dofVelocities") DOXY_FN(KinBody,ComputeDynamicLimits)) - .def("SetSelfCollisionChecker",&PyKinBody::SetSelfCollisionChecker,PY_ARGS("collisionchecker") DOXY_FN(KinBody,SetSelfCollisionChecker)) - .def("GetSelfCollisionChecker", &PyKinBody::GetSelfCollisionChecker, /*PY_ARGS("collisionchecker")*/ DOXY_FN(KinBody,GetSelfCollisionChecker)) #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("CheckSelfCollision", &PyKinBody::CheckSelfCollision, - "report"_a = py::none_(), // PyCollisionReportPtr(), - "collisionchecker"_a = py::none_(), // PyCollisionCheckerBasePtr(), - DOXY_FN(KinBody,CheckSelfCollision) - ) + kinbody.def("ComputeLocalAABBForGeometryGroup", &PyKinBody::ComputeLocalAABBForGeometryGroup, + "geomgroupname"_a, + "enabledOnlyLinks"_a = false, + DOXY_FN(KinBody, ComputeLocalAABBForGeometryGroup) + ); #else - .def("CheckSelfCollision",&PyKinBody::CheckSelfCollision, CheckSelfCollision_overloads(PY_ARGS("report","collisionchecker") DOXY_FN(KinBody,CheckSelfCollision))) + kinbody.def("ComputeLocalAABBForGeometryGroup",&PyKinBody::ComputeLocalAABBForGeometryGroup, ComputeLocalAABBForGeometryGroup_overloads(PY_ARGS("geomgroupname", "enabledOnlyLinks") DOXY_FN(KinBody,ComputeLocalAABBForGeometryGroup_overloads))); #endif - .def("IsAttached",&PyKinBody::IsAttached,PY_ARGS("body") DOXY_FN(KinBody,IsAttached)) - .def("HasAttached",&PyKinBody::HasAttached, DOXY_FN(KinBody,HasAttached)) - .def("GetAttached",&PyKinBody::GetAttached, DOXY_FN(KinBody,GetAttached)) - .def("GetAttachedEnvironmentBodyIndices",&PyKinBody::GetAttachedEnvironmentBodyIndices, DOXY_FN(KinBody,GetAttachedEnvironmentBodyIndices)) - .def("SetZeroConfiguration",&PyKinBody::SetZeroConfiguration, DOXY_FN(KinBody,SetZeroConfiguration)) - .def("SetNonCollidingConfiguration",&PyKinBody::SetNonCollidingConfiguration, DOXY_FN(KinBody,SetNonCollidingConfiguration)) + kinbody.def("GetCenterOfMass", &PyKinBody::GetCenterOfMass, DOXY_FN(KinBody,GetCenterOfMass)); + kinbody.def("Enable",&PyKinBody::Enable,PY_ARGS("enable") DOXY_FN(KinBody,Enable)); + kinbody.def("IsEnabled",&PyKinBody::IsEnabled, DOXY_FN(KinBody,IsEnabled)); + kinbody.def("SetVisible",&PyKinBody::SetVisible,PY_ARGS("visible") DOXY_FN(KinBody,SetVisible)); + kinbody.def("IsVisible",&PyKinBody::IsVisible, DOXY_FN(KinBody,IsVisible)); + kinbody.def("IsDOFRevolute",&PyKinBody::IsDOFRevolute, PY_ARGS("dofindex") DOXY_FN(KinBody,IsDOFRevolute)); + kinbody.def("IsDOFPrismatic",&PyKinBody::IsDOFPrismatic, PY_ARGS("dofindex") DOXY_FN(KinBody,IsDOFPrismatic)); + kinbody.def("SetTransform",&PyKinBody::SetTransform,PY_ARGS("transform") DOXY_FN(KinBody,SetTransform)); + kinbody.def("SetJointValues",psetdofvalues1,PY_ARGS("values") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")); + kinbody.def("SetJointValues",psetdofvalues2,PY_ARGS("values","dofindices") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")); + kinbody.def("SetDOFValues",psetdofvalues1,PY_ARGS("values") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")); + kinbody.def("SetDOFValues",psetdofvalues2,PY_ARGS("values","dofindices") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")); + kinbody.def("SetDOFValues",psetdofvalues3,PY_ARGS("values","dofindices","checklimits") DOXY_FN(KinBody,SetDOFValues "const std::vector; uint32_t; const std::vector")); #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetConfigurationSpecification", &PyKinBody::GetConfigurationSpecification, - "interpolation"_a = "", - DOXY_FN(KinBody,GetConfigurationSpecification) - ) + kinbody.def("SubtractDOFValues", &PyKinBody::SubtractDOFValues, + "values0"_a, + "values1"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody,SubtractDOFValues) + ); #else - .def("GetConfigurationSpecification",&PyKinBody::GetConfigurationSpecification, GetConfigurationSpecification_overloads(PY_ARGS("interpolation") DOXY_FN(KinBody,GetConfigurationSpecification))) + kinbody.def("SubtractDOFValues",&PyKinBody::SubtractDOFValues,SubtractDOFValues_overloads(PY_ARGS("values0","values1") DOXY_FN(KinBody,SubtractDOFValues))); #endif + kinbody.def("SetDOFTorques",&PyKinBody::SetDOFTorques,PY_ARGS("torques","add") DOXY_FN(KinBody,SetDOFTorques)); + kinbody.def("SetJointTorques",&PyKinBody::SetDOFTorques,PY_ARGS("torques","add") DOXY_FN(KinBody,SetDOFTorques)); + kinbody.def("SetTransformWithJointValues",&PyKinBody::SetTransformWithDOFValues,PY_ARGS("transform","values") DOXY_FN(KinBody,SetDOFValues "const std::vector; const Transform; uint32_t")); + kinbody.def("SetTransformWithDOFValues",&PyKinBody::SetTransformWithDOFValues,PY_ARGS("transform","values") DOXY_FN(KinBody,SetDOFValues "const std::vector; const Transform; uint32_t")); #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("GetConfigurationSpecificationIndices", &PyKinBody::GetConfigurationSpecificationIndices, - "indices"_a, - "interpolation"_a = "", - DOXY_FN(KinBody,GetConfigurationSpecificationIndices) - ) + kinbody.def("ComputeJacobianTranslation", &PyKinBody::ComputeJacobianTranslation, + "linkindex"_a, + "position"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody,ComputeJacobianTranslation) + ); #else - .def("GetConfigurationSpecificationIndices",&PyKinBody::GetConfigurationSpecificationIndices, GetConfigurationSpecificationIndices_overloads(PY_ARGS("indices","interpolation") DOXY_FN(KinBody,GetConfigurationSpecificationIndices))) + kinbody.def("ComputeJacobianTranslation",&PyKinBody::ComputeJacobianTranslation,ComputeJacobianTranslation_overloads(PY_ARGS("linkindex","position","indices") DOXY_FN(KinBody,ComputeJacobianTranslation))); #endif #ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("SetConfigurationValues", &PyKinBody::SetConfigurationValues, - "values"_a, - "checklimits"_a = (int) KinBody::CLA_CheckLimits, - DOXY_FN(KinBody,SetConfigurationValues) - ) -#else - .def("SetConfigurationValues",&PyKinBody::SetConfigurationValues, SetConfigurationValues_overloads(PY_ARGS("values","checklimits") DOXY_FN(KinBody,SetConfigurationValues))) -#endif - .def("GetConfigurationValues",&PyKinBody::GetConfigurationValues, DOXY_FN(KinBody,GetConfigurationValues)) - .def("IsRobot",&PyKinBody::IsRobot, DOXY_FN(KinBody,IsRobot)) - .def("GetEnvironmentId",&PyKinBody::GetEnvironmentId, DOXY_FN(KinBody,GetEnvironmentId)) - .def("GetEnvironmentBodyIndex",&PyKinBody::GetEnvironmentBodyIndex, DOXY_FN(KinBody,GetEnvironmentBodyIndex)) - .def("DoesAffect",&PyKinBody::DoesAffect,PY_ARGS("jointindex","linkindex") DOXY_FN(KinBody,DoesAffect)) - .def("DoesDOFAffectLink",&PyKinBody::DoesDOFAffectLink,PY_ARGS("dofindex","linkindex") DOXY_FN(KinBody,DoesDOFAffectLink)) - .def("GetURI",&PyKinBody::GetURI, DOXY_FN(InterfaceBase,GetURI)) - .def("GetXMLFilename",&PyKinBody::GetURI, DOXY_FN(InterfaceBase,GetURI)) - .def("GetNonAdjacentLinks",GetNonAdjacentLinks1, DOXY_FN(KinBody,GetNonAdjacentLinks)) - .def("GetNonAdjacentLinks",GetNonAdjacentLinks2, PY_ARGS("adjacentoptions") DOXY_FN(KinBody,GetNonAdjacentLinks)) - .def("SetAdjacentLinks",&PyKinBody::SetAdjacentLinks, PY_ARGS("linkindex0", "linkindex1") DOXY_FN(KinBody,SetAdjacentLinks)) - .def("SetAdjacentLinksCombinations",&PyKinBody::SetAdjacentLinksCombinations, PY_ARGS("linkIndices") DOXY_FN(KinBody,SetAdjacentLinksCombinations)) - .def("GetAdjacentLinks",&PyKinBody::GetAdjacentLinks, DOXY_FN(KinBody,GetAdjacentLinks)) - .def("GetManageData",&PyKinBody::GetManageData, DOXY_FN(KinBody,GetManageData)) - .def("GetUpdateStamp",&PyKinBody::GetUpdateStamp, DOXY_FN(KinBody,GetUpdateStamp)) - .def("serialize",&PyKinBody::serialize,PY_ARGS("options") DOXY_FN(KinBody,serialize)) - .def("UpdateFromKinBodyInfo",&PyKinBody::UpdateFromKinBodyInfo,PY_ARGS("info") DOXY_FN(KinBody,UpdateFromKinBodyInfo)) - .def("GetKinematicsGeometryHash",&PyKinBody::GetKinematicsGeometryHash, DOXY_FN(KinBody,GetKinematicsGeometryHash)) - .def("GetAssociatedFileEntries",&PyKinBody::GetAssociatedFileEntries, DOXY_FN(KinBody,GetAssociatedFileEntries)) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .def("CreateKinBodyStateSaver", &PyKinBody::CreateKinBodyStateSaver, - "options"_a = py::none_(), - "Creates an object that can be entered using 'with' and returns a KinBodyStateSaver" - ) + kinbody.def("ComputeJacobianAxisAngle", &PyKinBody::ComputeJacobianAxisAngle, + "linkindex"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody,ComputeJacobianAxisAngle) + ); #else - .def("CreateKinBodyStateSaver",&PyKinBody::CreateKinBodyStateSaver, CreateKinBodyStateSaver_overloads(PY_ARGS("options") "Creates an object that can be entered using 'with' and returns a KinBodyStateSaver")[return_value_policy()]) + kinbody.def("ComputeJacobianAxisAngle",&PyKinBody::ComputeJacobianAxisAngle,ComputeJacobianAxisAngle_overloads(PY_ARGS("linkindex","indices") DOXY_FN(KinBody,ComputeJacobianAxisAngle))); #endif - .def("ExtractInfo", &PyKinBody::ExtractInfo, DOXY_FN(KinBody, ExtractInfo)) - .def("__enter__",&PyKinBody::__enter__) - .def("__exit__",&PyKinBody::__exit__) - .def("__repr__",&PyKinBody::__repr__) - .def("__str__",&PyKinBody::__str__) - .def("__unicode__",&PyKinBody::__unicode__) - ; + kinbody.def("CalculateJacobian",&PyKinBody::CalculateJacobian,PY_ARGS("linkindex","position") DOXY_FN(KinBody,CalculateJacobian "int; const Vector; std::vector")); + kinbody.def("CalculateRotationJacobian",&PyKinBody::CalculateRotationJacobian,PY_ARGS("linkindex","quat") DOXY_FN(KinBody,CalculateRotationJacobian "int; const Vector; std::vector")); + kinbody.def("CalculateAngularVelocityJacobian",&PyKinBody::CalculateAngularVelocityJacobian,PY_ARGS("linkindex") DOXY_FN(KinBody,CalculateAngularVelocityJacobian "int; std::vector")); + kinbody.def("Grab",pgrab2,PY_ARGS("body","grablink") DOXY_FN(RobotBase,Grab "KinBodyPtr; LinkPtr")); + kinbody.def("Grab",pgrab4,PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(KinBody,Grab "KinBodyPtr; LinkPtr; const std::set; rapidjson::Document")); + kinbody.def("Release",&PyKinBody::Release,PY_ARGS("body") DOXY_FN(KinBody,Release)); + kinbody.def("ReleaseAllGrabbed",&PyKinBody::ReleaseAllGrabbed, DOXY_FN(KinBody,ReleaseAllGrabbed)); + kinbody.def("ReleaseAllGrabbedWithLink",&PyKinBody::ReleaseAllGrabbedWithLink, PY_ARGS("grablink") DOXY_FN(KinBody,ReleaseAllGrabbedWithLink)); + kinbody.def("RegrabAll",&PyKinBody::RegrabAll, DOXY_FN(KinBody,RegrabAll)); + kinbody.def("IsGrabbing",&PyKinBody::IsGrabbing,PY_ARGS("body") DOXY_FN(KinBody,IsGrabbing)); + kinbody.def("CheckGrabbedInfo",checkgrabbedinfo2,PY_ARGS("body","grablink") DOXY_FN(KinBody,CheckGrabbedInfo "const KinBody; const Link")); + kinbody.def("CheckGrabbedInfo",checkgrabbedinfo3,PY_ARGS("body","grablink","linkstoignore","grabbedUserData") DOXY_FN(KinBody,CheckGrabbedInfo "const KinBody; const Link; const std::set; const rapidjson::Document")); + kinbody.def("GetNumGrabbed", &PyKinBody::GetNumGrabbed, DOXY_FN(KinBody,GetNumGrabbed)); + kinbody.def("GetGrabbed",&PyKinBody::GetGrabbed, DOXY_FN(KinBody,GetGrabbed)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetGrabbedInfo", &PyKinBody::GetGrabbedInfo, + "grabbedname"_a = py::none_(), + DOXY_FN(KinBody,GetGrabbedInfo) + ); +#else + kinbody.def("GetGrabbedInfo",&PyKinBody::GetGrabbedInfo, GetGrabbedInfo_overloads(PY_ARGS("grabbedname") DOXY_FN(KinBody,GetGrabbedInfo))); +#endif + kinbody.def("ResetGrabbed",&PyKinBody::ResetGrabbed, PY_ARGS("grabbedinfos") DOXY_FN(KinBody,ResetGrabbed)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeHessianTranslation", &PyKinBody::ComputeHessianTranslation, + "linkindex"_a, + "position"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody,ComputeHessianTranslation) + ); +#else + kinbody.def("ComputeHessianTranslation",&PyKinBody::ComputeHessianTranslation,ComputeHessianTranslation_overloads(PY_ARGS("linkindex","position","indices") DOXY_FN(KinBody,ComputeHessianTranslation))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeHessianAxisAngle", &PyKinBody::ComputeHessianAxisAngle, + "linkindex"_a, + "indices"_a = py::none_(), + DOXY_FN(KinBody,ComputeHessianAxisAngle) + ); +#else + kinbody.def("ComputeHessianAxisAngle",&PyKinBody::ComputeHessianAxisAngle,ComputeHessianAxisAngle_overloads(PY_ARGS("linkindex","indices") DOXY_FN(KinBody,ComputeHessianAxisAngle))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("ComputeInverseDynamics", &PyKinBody::ComputeInverseDynamics, + "dofaccelerations"_a, + "externalforcetorque"_a = py::none_(), + "returncomponents"_a = false, + sComputeInverseDynamicsDoc.c_str() + ); +#else + kinbody.def("ComputeInverseDynamics",&PyKinBody::ComputeInverseDynamics, ComputeInverseDynamics_overloads(PY_ARGS("dofaccelerations","externalforcetorque","returncomponents") sComputeInverseDynamicsDoc.c_str())); +#endif + kinbody.def("GetDOFDynamicAccelerationJerkLimits",&PyKinBody::GetDOFDynamicAccelerationJerkLimits, PY_ARGS("dofPositions","dofVelocities") DOXY_FN(KinBody,ComputeDynamicLimits)); + kinbody.def("SetSelfCollisionChecker",&PyKinBody::SetSelfCollisionChecker,PY_ARGS("collisionchecker") DOXY_FN(KinBody,SetSelfCollisionChecker)); + kinbody.def("GetSelfCollisionChecker", &PyKinBody::GetSelfCollisionChecker, /*PY_ARGS("collisionchecker")*/ DOXY_FN(KinBody,GetSelfCollisionChecker)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("CheckSelfCollision", &PyKinBody::CheckSelfCollision, + "report"_a = py::none_(), // PyCollisionReportPtr(), + "collisionchecker"_a = py::none_(), // PyCollisionCheckerBasePtr(), + DOXY_FN(KinBody,CheckSelfCollision) + ); +#else + kinbody.def("CheckSelfCollision",&PyKinBody::CheckSelfCollision, CheckSelfCollision_overloads(PY_ARGS("report","collisionchecker") DOXY_FN(KinBody,CheckSelfCollision))); +#endif + kinbody.def("IsAttached",&PyKinBody::IsAttached,PY_ARGS("body") DOXY_FN(KinBody,IsAttached)); + kinbody.def("HasAttached",&PyKinBody::HasAttached, DOXY_FN(KinBody,HasAttached)); + kinbody.def("GetAttached",&PyKinBody::GetAttached, DOXY_FN(KinBody,GetAttached)); + kinbody.def("GetAttachedEnvironmentBodyIndices",&PyKinBody::GetAttachedEnvironmentBodyIndices, DOXY_FN(KinBody,GetAttachedEnvironmentBodyIndices)); + kinbody.def("SetZeroConfiguration",&PyKinBody::SetZeroConfiguration, DOXY_FN(KinBody,SetZeroConfiguration)); + kinbody.def("SetNonCollidingConfiguration",&PyKinBody::SetNonCollidingConfiguration, DOXY_FN(KinBody,SetNonCollidingConfiguration)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetConfigurationSpecification", &PyKinBody::GetConfigurationSpecification, + "interpolation"_a = "", + DOXY_FN(KinBody,GetConfigurationSpecification) + ); +#else + kinbody.def("GetConfigurationSpecification",&PyKinBody::GetConfigurationSpecification, GetConfigurationSpecification_overloads(PY_ARGS("interpolation") DOXY_FN(KinBody,GetConfigurationSpecification))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("GetConfigurationSpecificationIndices", &PyKinBody::GetConfigurationSpecificationIndices, + "indices"_a, + "interpolation"_a = "", + DOXY_FN(KinBody,GetConfigurationSpecificationIndices) + ); +#else + kinbody.def("GetConfigurationSpecificationIndices",&PyKinBody::GetConfigurationSpecificationIndices, GetConfigurationSpecificationIndices_overloads(PY_ARGS("indices","interpolation") DOXY_FN(KinBody,GetConfigurationSpecificationIndices))); +#endif +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("SetConfigurationValues", &PyKinBody::SetConfigurationValues, + "values"_a, + "checklimits"_a = (int) KinBody::CLA_CheckLimits, + DOXY_FN(KinBody,SetConfigurationValues) + ); +#else + kinbody.def("SetConfigurationValues",&PyKinBody::SetConfigurationValues, SetConfigurationValues_overloads(PY_ARGS("values","checklimits") DOXY_FN(KinBody,SetConfigurationValues))); +#endif + kinbody.def("GetConfigurationValues",&PyKinBody::GetConfigurationValues, DOXY_FN(KinBody,GetConfigurationValues)); + kinbody.def("IsRobot",&PyKinBody::IsRobot, DOXY_FN(KinBody,IsRobot)); + kinbody.def("GetEnvironmentId",&PyKinBody::GetEnvironmentId, DOXY_FN(KinBody,GetEnvironmentId)); + kinbody.def("GetEnvironmentBodyIndex",&PyKinBody::GetEnvironmentBodyIndex, DOXY_FN(KinBody,GetEnvironmentBodyIndex)); + kinbody.def("DoesAffect",&PyKinBody::DoesAffect,PY_ARGS("jointindex","linkindex") DOXY_FN(KinBody,DoesAffect)); + kinbody.def("DoesDOFAffectLink",&PyKinBody::DoesDOFAffectLink,PY_ARGS("dofindex","linkindex") DOXY_FN(KinBody,DoesDOFAffectLink)); + kinbody.def("GetURI",&PyKinBody::GetURI, DOXY_FN(InterfaceBase,GetURI)); + kinbody.def("GetXMLFilename",&PyKinBody::GetURI, DOXY_FN(InterfaceBase,GetURI)); + kinbody.def("GetNonAdjacentLinks",GetNonAdjacentLinks1, DOXY_FN(KinBody,GetNonAdjacentLinks)); + kinbody.def("GetNonAdjacentLinks",GetNonAdjacentLinks2, PY_ARGS("adjacentoptions") DOXY_FN(KinBody,GetNonAdjacentLinks)); + kinbody.def("SetAdjacentLinks",&PyKinBody::SetAdjacentLinks, PY_ARGS("linkindex0", "linkindex1") DOXY_FN(KinBody,SetAdjacentLinks)); + kinbody.def("SetAdjacentLinksCombinations",&PyKinBody::SetAdjacentLinksCombinations, PY_ARGS("linkIndices") DOXY_FN(KinBody,SetAdjacentLinksCombinations)); + kinbody.def("GetAdjacentLinks",&PyKinBody::GetAdjacentLinks, DOXY_FN(KinBody,GetAdjacentLinks)); + kinbody.def("GetManageData",&PyKinBody::GetManageData, DOXY_FN(KinBody,GetManageData)); + kinbody.def("GetUpdateStamp",&PyKinBody::GetUpdateStamp, DOXY_FN(KinBody,GetUpdateStamp)); + kinbody.def("serialize",&PyKinBody::serialize,PY_ARGS("options") DOXY_FN(KinBody,serialize)); + kinbody.def("UpdateFromKinBodyInfo",&PyKinBody::UpdateFromKinBodyInfo,PY_ARGS("info") DOXY_FN(KinBody,UpdateFromKinBodyInfo)); + kinbody.def("GetKinematicsGeometryHash",&PyKinBody::GetKinematicsGeometryHash, DOXY_FN(KinBody,GetKinematicsGeometryHash)); + kinbody.def("GetAssociatedFileEntries",&PyKinBody::GetAssociatedFileEntries, DOXY_FN(KinBody,GetAssociatedFileEntries)); +#ifdef USE_PYBIND11_PYTHON_BINDINGS + kinbody.def("CreateKinBodyStateSaver", &PyKinBody::CreateKinBodyStateSaver, + "options"_a = py::none_(), + "Creates an object that can be entered using 'with' and returns a KinBodyStateSaver" + ); +#else + kinbody.def("CreateKinBodyStateSaver",&PyKinBody::CreateKinBodyStateSaver, CreateKinBodyStateSaver_overloads(PY_ARGS("options") "Creates an object that can be entered using 'with' and returns a KinBodyStateSaver")[return_value_policy()]); +#endif + kinbody.def("ExtractInfo", &PyKinBody::ExtractInfo, DOXY_FN(KinBody, ExtractInfo)); + kinbody.def("__enter__",&PyKinBody::__enter__); + kinbody.def("__exit__",&PyKinBody::__exit__); + kinbody.def("__repr__",&PyKinBody::__repr__); + kinbody.def("__str__",&PyKinBody::__str__); + kinbody.def("__unicode__",&PyKinBody::__unicode__); #ifdef USE_PYBIND11_PYTHON_BINDINGS // SaveParameters belongs to KinBody, not openravepy._openravepy_.openravepy_int diff --git a/python/bindings/openravepy_planner.cpp b/python/bindings/openravepy_planner.cpp index 69891bc975..08d036a926 100644 --- a/python/bindings/openravepy_planner.cpp +++ b/python/bindings/openravepy_planner.cpp @@ -434,9 +434,11 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SetStateValues_overloads, SetStateValues, #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -PlannerBaseInitializer::PlannerBaseInitializer(py::module& m_): m(m_) +PlannerBaseInitializer::PlannerBaseInitializer(py::module& m_): m(m_), + planner(m, "Planner", DOXY_CLASS(PlannerBase)) #else -PlannerBaseInitializer::PlannerBaseInitializer() +PlannerBaseInitializer::PlannerBaseInitializer(): + planner("Planner", DOXY_CLASS(PlannerBase), no_init) #endif { } @@ -485,12 +487,6 @@ void PlannerBaseInitializer::init_openravepy_planner() ; { -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_, PyInterfaceBase> planner(m, "Planner", DOXY_CLASS(PlannerBase)); -#else - class_, bases > planner("Planner", DOXY_CLASS(PlannerBase), no_init); -#endif - #ifdef USE_PYBIND11_PYTHON_BINDINGS // PlannerParameters belongs to Planner class_(planner, "PlannerParameters", DOXY_CLASS(PlannerBase::PlannerParameters)) diff --git a/python/bindings/openravepy_robot.cpp b/python/bindings/openravepy_robot.cpp index e870c5ccca..642812223c 100644 --- a/python/bindings/openravepy_robot.cpp +++ b/python/bindings/openravepy_robot.cpp @@ -2346,9 +2346,11 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(PyRobotBaseInfo_DeserializeJSON_overloads #endif // USE_PYBIND11_PYTHON_BINDINGS #ifdef USE_PYBIND11_PYTHON_BINDINGS -RobotBaseInitializer::RobotBaseInitializer(py::module& m_): m(m_) +RobotBaseInitializer::RobotBaseInitializer(py::module& m_): m(m_), + robot(m, "Robot", py::dynamic_attr(), DOXY_CLASS(RobotBase)) #else -RobotBaseInitializer::RobotBaseInitializer() +RobotBaseInitializer::RobotBaseInitializer(): + robot("Robot", DOXY_CLASS(RobotBase), no_init) #endif { } @@ -2540,12 +2542,6 @@ void RobotBaseInitializer::init_openravepy_robot() ; { -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_, PyKinBody> robot(m, "Robot", py::dynamic_attr(), DOXY_CLASS(RobotBase)); -#else - class_, bases > robot("Robot", DOXY_CLASS(RobotBase), no_init); -#endif - object (PyRobotBase::PyManipulator::*pmanipik)(object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolution; object (PyRobotBase::PyManipulator::*pmanipikf)(object, object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolution; object (PyRobotBase::PyManipulator::*pmanipiks)(object, int, bool, bool) const = &PyRobotBase::PyManipulator::FindIKSolutions; From c3ab1beca4cdcc18eba3bf60dad74c8d77897ae4 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 19:27:22 +0900 Subject: [PATCH 12/13] progress --- .../include/openravepy/openravepy_int.h | 2 + python/bindings/openravepy_global.cpp | 69 ++++++++++-------- python/bindings/openravepy_int.cpp | 2 + python/bindings/openravepy_sensor.cpp | 71 +++++++++---------- 4 files changed, 80 insertions(+), 64 deletions(-) diff --git a/python/bindings/include/openravepy/openravepy_int.h b/python/bindings/include/openravepy/openravepy_int.h index 51c7c8f233..6431518f9a 100644 --- a/python/bindings/include/openravepy/openravepy_int.h +++ b/python/bindings/include/openravepy/openravepy_int.h @@ -825,9 +825,11 @@ OPENRAVEPY_API PyConnectedBodyInfoPtr toPyConnectedBodyInfo(const RobotBase::Con OPENRAVEPY_API PyInterfaceBasePtr RaveCreateInterface(PyEnvironmentBasePtr pyenv, InterfaceType type, const std::string& name); #ifdef USE_PYBIND11_PYTHON_BINDINGS +void init_openravepy_global_basic(py::module& m); void init_openravepy_global(py::module& m); void init_openravepy_global_functions(py::module& m); #else +void init_openravepy_global_basic(); void init_openravepy_global(); void init_openravepy_global_functions(); #endif diff --git a/python/bindings/openravepy_global.cpp b/python/bindings/openravepy_global.cpp index 972461b7eb..279de4f77a 100644 --- a/python/bindings/openravepy_global.cpp +++ b/python/bindings/openravepy_global.cpp @@ -1381,6 +1381,47 @@ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(SerializeJSON_overloads, SerializeJSON, 0 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(DeserializeJSON_overloads, DeserializeJSON, 1, 2) #endif // USE_PYBIND11_PYTHON_BINDINGS +#ifdef USE_PYBIND11_PYTHON_BINDINGS +void init_openravepy_global_basic(py::module& m) +#else +void init_openravepy_global_basic() +#endif +{ +#ifdef USE_PYBIND11_PYTHON_BINDINGS + using namespace py::literals; // "..."_a +#endif + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + enum_(m, "InterfaceType", py::arithmetic() DOXY_ENUM(InterfaceType)) +#else + enum_("InterfaceType" DOXY_ENUM(InterfaceType)) +#endif + .value(RaveGetInterfaceName(PT_Planner).c_str(),PT_Planner) + .value(RaveGetInterfaceName(PT_Robot).c_str(),PT_Robot) + .value(RaveGetInterfaceName(PT_SensorSystem).c_str(),PT_SensorSystem) + .value(RaveGetInterfaceName(PT_Controller).c_str(),PT_Controller) + .value("probleminstance",PT_Module) + .value(RaveGetInterfaceName(PT_Module).c_str(),PT_Module) + .value(RaveGetInterfaceName(PT_IkSolver).c_str(),PT_IkSolver) + .value(RaveGetInterfaceName(PT_KinBody).c_str(),PT_KinBody) + .value(RaveGetInterfaceName(PT_PhysicsEngine).c_str(),PT_PhysicsEngine) + .value(RaveGetInterfaceName(PT_Sensor).c_str(),PT_Sensor) + .value(RaveGetInterfaceName(PT_CollisionChecker).c_str(),PT_CollisionChecker) + .value(RaveGetInterfaceName(PT_Trajectory).c_str(),PT_Trajectory) + .value(RaveGetInterfaceName(PT_Viewer).c_str(),PT_Viewer) + .value(RaveGetInterfaceName(PT_SpaceSampler).c_str(),PT_SpaceSampler) + ; + +#ifdef USE_PYBIND11_PYTHON_BINDINGS + class_ >(m, "UserData", DOXY_CLASS(UserData)) +#else + class_ >("UserData", DOXY_CLASS(UserData), no_init) +#endif + .def("close",&PyUserData::Close,"deprecated") + .def("Close",&PyUserData::Close,"force releasing the user handle point.") + ; +} + #ifdef USE_PYBIND11_PYTHON_BINDINGS void init_openravepy_global(py::module& m) #else @@ -1445,26 +1486,6 @@ void init_openravepy_global() ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - enum_(m, "InterfaceType", py::arithmetic() DOXY_ENUM(InterfaceType)) -#else - enum_("InterfaceType" DOXY_ENUM(InterfaceType)) -#endif - .value(RaveGetInterfaceName(PT_Planner).c_str(),PT_Planner) - .value(RaveGetInterfaceName(PT_Robot).c_str(),PT_Robot) - .value(RaveGetInterfaceName(PT_SensorSystem).c_str(),PT_SensorSystem) - .value(RaveGetInterfaceName(PT_Controller).c_str(),PT_Controller) - .value("probleminstance",PT_Module) - .value(RaveGetInterfaceName(PT_Module).c_str(),PT_Module) - .value(RaveGetInterfaceName(PT_IkSolver).c_str(),PT_IkSolver) - .value(RaveGetInterfaceName(PT_KinBody).c_str(),PT_KinBody) - .value(RaveGetInterfaceName(PT_PhysicsEngine).c_str(),PT_PhysicsEngine) - .value(RaveGetInterfaceName(PT_Sensor).c_str(),PT_Sensor) - .value(RaveGetInterfaceName(PT_CollisionChecker).c_str(),PT_CollisionChecker) - .value(RaveGetInterfaceName(PT_Trajectory).c_str(),PT_Trajectory) - .value(RaveGetInterfaceName(PT_Viewer).c_str(),PT_Viewer) - .value(RaveGetInterfaceName(PT_SpaceSampler).c_str(),PT_SpaceSampler) - ; #ifdef USE_PYBIND11_PYTHON_BINDINGS enum_(m, "CloningOptions", py::arithmetic() DOXY_ENUM(CloningOptions)) #else @@ -1582,14 +1603,6 @@ void init_openravepy_global() .def("Close",&PyGraphHandle::Close,DOXY_FN(GraphHandle,Close)) ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - class_ >(m, "UserData", DOXY_CLASS(UserData)) -#else - class_ >("UserData", DOXY_CLASS(UserData), no_init) -#endif - .def("close",&PyUserData::Close,"deprecated") - .def("Close",&PyUserData::Close,"force releasing the user handle point.") - ; #ifdef USE_PYBIND11_PYTHON_BINDINGS class_, PyUserData >(m, "SerializableData", DOXY_CLASS(SerializableData)) .def(init<>()) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index cbd1fa6398..713ad82eec 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3141,6 +3141,7 @@ OPENRAVE_PYTHON_MODULE(openravepy_int) #ifdef USE_PYBIND11_PYTHON_BINDINGS using namespace py::literals; // "..."_a init_python_bindings(m); + init_openravepy_global_basic(m); #else // USE_PYBIND11_PYTHON_BINDINGS #if BOOST_VERSION >= 103500 docstring_options doc_options; @@ -3154,6 +3155,7 @@ OPENRAVE_PYTHON_MODULE(openravepy_int) float_from_number(); float_from_number(); init_python_bindings(); + init_openravepy_global_basic(); typedef return_value_policy< copy_const_reference > return_copy_const_ref; #endif // USE_PYBIND11_PYTHON_BINDINGS diff --git a/python/bindings/openravepy_sensor.cpp b/python/bindings/openravepy_sensor.cpp index bfdd5c6404..37ea98b75c 100644 --- a/python/bindings/openravepy_sensor.cpp +++ b/python/bindings/openravepy_sensor.cpp @@ -744,6 +744,26 @@ void SensorBaseInitializer::init_openravepy_sensor() OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData1)() = &PySensorBase::GetSensorData; OPENRAVE_SHARED_PTR (PySensorBase::*GetSensorData2)(SensorBase::SensorType) = &PySensorBase::GetSensorData; +#ifdef USE_PYBIND11_PYTHON_BINDINGS + // SensorType belongs to Sensor + enum_(sensor, "Type", py::arithmetic() DOXY_ENUM(SensorType)) +#else + enum_("Type" DOXY_ENUM(SensorType)) +#endif + .value("Invalid",SensorBase::ST_Invalid) + .value("Laser",SensorBase::ST_Laser) + .value("Camera",SensorBase::ST_Camera) + .value("JointEncoder",SensorBase::ST_JointEncoder) + .value("Force6D",SensorBase::ST_Force6D) + .value("IMU",SensorBase::ST_IMU) + .value("Odometry",SensorBase::ST_Odometry) + .value("Tactile",SensorBase::ST_Tactile) + .value("Actuator",SensorBase::ST_Actuator) +#ifdef USE_PYBIND11_PYTHON_BINDINGS + .export_values() +#endif + ; + #ifdef USE_PYBIND11_PYTHON_BINDINGS // SensorData is inside SensorBase class_ >(sensor, "SensorData", DOXY_CLASS(SensorBase::SensorData)) @@ -773,26 +793,6 @@ void SensorBaseInitializer::init_openravepy_sensor() #endif ; -#ifdef USE_PYBIND11_PYTHON_BINDINGS - // SensorType belongs to Sensor - enum_(sensor, "Type", py::arithmetic() DOXY_ENUM(SensorType)) -#else - enum_("Type" DOXY_ENUM(SensorType)) -#endif - .value("Invalid",SensorBase::ST_Invalid) - .value("Laser",SensorBase::ST_Laser) - .value("Camera",SensorBase::ST_Camera) - .value("JointEncoder",SensorBase::ST_JointEncoder) - .value("Force6D",SensorBase::ST_Force6D) - .value("IMU",SensorBase::ST_IMU) - .value("Odometry",SensorBase::ST_Odometry) - .value("Tactile",SensorBase::ST_Tactile) - .value("Actuator",SensorBase::ST_Actuator) -#ifdef USE_PYBIND11_PYTHON_BINDINGS - .export_values() -#endif - ; - #ifdef USE_PYBIND11_PYTHON_BINDINGS class_>(m, "SensorGeometry", DOXY_CLASS(PySensorGeometry)) // how to handle pure virtual @@ -926,25 +926,11 @@ void SensorBaseInitializer::init_openravepy_sensor() { #ifdef USE_PYBIND11_PYTHON_BINDINGS - scope_ actuatorsensordata = // ActuatorSensorData is inside SensorBase - class_, PySensorBase::PySensorData>(sensor, "ActuatorSensorData", DOXY_CLASS(SensorBase::ActuatorSensorData)) + class_, PySensorBase::PySensorData> actuatorsensordata(sensor, "ActuatorSensorData", DOXY_CLASS(SensorBase::ActuatorSensorData)); #else - class_, bases >("ActuatorSensorData", DOXY_CLASS(SensorBase::ActuatorSensorData),no_init) + class_, bases > actuatorsensordata("ActuatorSensorData", DOXY_CLASS(SensorBase::ActuatorSensorData),no_init); #endif - .def_readonly("state",&PySensorBase::PyActuatorSensorData::state) - .def_readonly("measuredcurrent",&PySensorBase::PyActuatorSensorData::measuredcurrent) - .def_readonly("measuredtemperature",&PySensorBase::PyActuatorSensorData::measuredtemperature) - .def_readonly("appliedcurrent",&PySensorBase::PyActuatorSensorData::appliedcurrent) - .def_readonly("maxtorque",&PySensorBase::PyActuatorSensorData::maxtorque) - .def_readonly("maxcurrent",&PySensorBase::PyActuatorSensorData::maxcurrent) - .def_readonly("nominalcurrent",&PySensorBase::PyActuatorSensorData::nominalcurrent) - .def_readonly("maxvelocity",&PySensorBase::PyActuatorSensorData::maxvelocity) - .def_readonly("maxacceleration",&PySensorBase::PyActuatorSensorData::maxacceleration) - .def_readonly("maxjerk",&PySensorBase::PyActuatorSensorData::maxjerk) - .def_readonly("staticfriction",&PySensorBase::PyActuatorSensorData::staticfriction) - .def_readonly("viscousfriction",&PySensorBase::PyActuatorSensorData::viscousfriction) - ; #ifdef USE_PYBIND11_PYTHON_BINDINGS // ActuatorState belongs to ActuatorSensorData @@ -961,6 +947,19 @@ void SensorBaseInitializer::init_openravepy_sensor() .export_values() #endif ; + + actuatorsensordata.def_readonly("state",&PySensorBase::PyActuatorSensorData::state); + actuatorsensordata.def_readonly("measuredcurrent",&PySensorBase::PyActuatorSensorData::measuredcurrent); + actuatorsensordata.def_readonly("measuredtemperature",&PySensorBase::PyActuatorSensorData::measuredtemperature); + actuatorsensordata.def_readonly("appliedcurrent",&PySensorBase::PyActuatorSensorData::appliedcurrent); + actuatorsensordata.def_readonly("maxtorque",&PySensorBase::PyActuatorSensorData::maxtorque); + actuatorsensordata.def_readonly("maxcurrent",&PySensorBase::PyActuatorSensorData::maxcurrent); + actuatorsensordata.def_readonly("nominalcurrent",&PySensorBase::PyActuatorSensorData::nominalcurrent); + actuatorsensordata.def_readonly("maxvelocity",&PySensorBase::PyActuatorSensorData::maxvelocity); + actuatorsensordata.def_readonly("maxacceleration",&PySensorBase::PyActuatorSensorData::maxacceleration); + actuatorsensordata.def_readonly("maxjerk",&PySensorBase::PyActuatorSensorData::maxjerk); + actuatorsensordata.def_readonly("staticfriction",&PySensorBase::PyActuatorSensorData::staticfriction); + actuatorsensordata.def_readonly("viscousfriction",&PySensorBase::PyActuatorSensorData::viscousfriction); } } From 4ee8d05c431dd72dde1fa32bed184f6bb3a2c0a7 Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Tue, 13 Jun 2023 19:34:25 +0900 Subject: [PATCH 13/13] fix --- python/bindings/openravepy_int.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp index 713ad82eec..409afd52d8 100644 --- a/python/bindings/openravepy_int.cpp +++ b/python/bindings/openravepy_int.cpp @@ -3365,7 +3365,6 @@ Because race conditions can pop up when trying to lock the openrave environment initializer.init_openravepy_trajectory(); initializer.init_openravepy_planner(); initializer.init_openravepy_controller(); - initializer.init_openravepy_global(); openravepy::InitPlanningUtils(); openravepy::init_openravepy_sensorsystem();