diff --git a/core/include/moveit/task_constructor/stages/simple_grasp.h b/core/include/moveit/task_constructor/stages/simple_grasp.h index df81d2fe..9823f3a8 100644 --- a/core/include/moveit/task_constructor/stages/simple_grasp.h +++ b/core/include/moveit/task_constructor/stages/simple_grasp.h @@ -80,13 +80,12 @@ public: /// set properties of IK solver void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); } void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link); - template - void setIKFrame(const T& t, const std::string& link) { - Eigen::Isometry3d transform; - transform = t; - setIKFrame(transform, link); - } void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); } + /// allow setting IK frame from any type T that converts to Eigen::Isometry3d + template + void setIKFrame(const T& transform, const std::string& link) { + setIKFrame(Eigen::Isometry3d(transform), link); + } void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); } }; diff --git a/core/python/bindings/src/core.cpp b/core/python/bindings/src/core.cpp index 3d6fdb64..f8bf00b7 100644 --- a/core/python/bindings/src/core.cpp +++ b/core/python/bindings/src/core.cpp @@ -141,7 +141,7 @@ void export_core(pybind11::module& m) { // expose name as writeable property .def_property("name", &Stage::name, &Stage::setName) // read-only access to properties + solutions - .def_property_readonly("properties", &Stage::properties, py::return_value_policy::reference_internal) + .def_property_readonly("properties", py::overload_cast<>(&Stage::properties), py::return_value_policy::reference_internal) .def_property_readonly("solutions", &Stage::solutions, py::return_value_policy::reference_internal) .def_property_readonly("failures", &Stage::failures, py::return_value_policy::reference_internal) .def("reset", &Stage::reset) @@ -189,7 +189,7 @@ void export_core(pybind11::module& m) { py::class_(m, "ContainerBase") .def("add", &ContainerBase::add) .def("insert", &ContainerBase::insert, py::arg("stage"), py::arg("before") = -1) - .def("remove", static_cast(&ContainerBase::remove)) + .def("remove", py::overload_cast(&ContainerBase::remove), "Remove child stage by index") .def("clear", &ContainerBase::clear) .def("__len__", &ContainerBase::numChildren) .def("__getitem__", [](const ContainerBase &c, const std::string &name) -> Stage* { @@ -225,7 +225,7 @@ void export_core(pybind11::module& m) { .def(py::init(), py::arg("ns") = std::string(), py::arg("introspection") = true, py::arg("container")) // read-only access to properties + solutions - .def_property_readonly("properties", &Task::properties, py::return_value_policy::reference_internal) + .def_property_readonly("properties", py::overload_cast<>(&Task::properties), py::return_value_policy::reference_internal) .def_property_readonly("solutions", &Task::solutions, py::return_value_policy::reference_internal) .def_property_readonly("failures", &Task::failures, py::return_value_policy::reference_internal) .def_property("name", &Task::name, &Task::setName) diff --git a/core/python/bindings/src/solvers.cpp b/core/python/bindings/src/solvers.cpp index 0156bd46..7901ffe1 100644 --- a/core/python/bindings/src/solvers.cpp +++ b/core/python/bindings/src/solvers.cpp @@ -46,13 +46,10 @@ namespace moveit { namespace python { void export_solvers(py::module& m) { - // resolve &PlannerInterface::properties to non-const version - PropertyMap& (PlannerInterface::*PlannerInterface_getPropertyMap)() = &PlannerInterface::properties; - properties::class_(m, "PlannerInterface") .property("max_velocity_scaling_factor") .property("max_acceleration_scaling_factor") - .def_property_readonly("properties", PlannerInterface_getPropertyMap, + .def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties), py::return_value_policy::reference_internal); properties::class_(m, "PipelinePlanner") diff --git a/core/python/bindings/src/stages.cpp b/core/python/bindings/src/stages.cpp index d54a6950..ea0a08eb 100644 --- a/core/python/bindings/src/stages.cpp +++ b/core/python/bindings/src/stages.cpp @@ -112,11 +112,11 @@ void export_stages(pybind11::module& m) { .property("ik_frame") .property("path_constraints") .def(py::init()) - .def("setGoal", &MoveTo::setGoal) - .def("setGoal", &MoveTo::setGoal) - .def("setGoal", &MoveTo::setGoal) - .def&)>("setGoal", &MoveTo::setGoal) - .def("setGoal", &MoveTo::setGoal); + .def("setGoal", py::overload_cast(&MoveTo::setGoal)) + .def("setGoal", py::overload_cast(&MoveTo::setGoal)) + .def("setGoal", py::overload_cast(&MoveTo::setGoal)) + .def("setGoal", py::overload_cast&>(&MoveTo::setGoal)) + .def("setGoal", py::overload_cast(&MoveTo::setGoal)); properties::class_>(m, "MoveRelative") .property("group") @@ -125,9 +125,9 @@ void export_stages(pybind11::module& m) { .property("max_distance") .property("path_constraints") .def(py::init()) - .def("setDirection", &MoveRelative::setDirection) - .def("setDirection", &MoveRelative::setDirection) - .def&)>("setDirection", &MoveRelative::setDirection); + .def("setDirection", py::overload_cast(&MoveRelative::setDirection)) + .def("setDirection", py::overload_cast(&MoveRelative::setDirection)) + .def("setDirection", py::overload_cast&>(&MoveRelative::setDirection)); py::enum_(m, "MergeMode") .value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL) @@ -164,8 +164,9 @@ void export_stages(pybind11::module& m) { py::arg("name") = std::string("pick")) .def("setApproachMotion", &Pick::setApproachMotion) - .def("setLiftMotion", &Pick::setLiftMotion) - .def&)>("setLiftMotion", &Pick::setLiftMotion); + .def("setLiftMotion", + py::overload_cast(&Pick::setLiftMotion)) + .def("setLiftMotion", py::overload_cast&>(&Pick::setLiftMotion)); properties::class_(m, "Place") .property("object")