diff --git a/.gitignore b/.gitignore index c67054706a..42af8a8c96 100644 --- a/.gitignore +++ b/.gitignore @@ -61,3 +61,6 @@ qtcreator-* _build _autosummary + +# VSCode Devcontainer +.devcontainer diff --git a/moveit_core/collision_detection/src/pycollision_detection.cpp b/moveit_core/collision_detection/src/pycollision_detection.cpp deleted file mode 100644 index 3d78268b21..0000000000 --- a/moveit_core/collision_detection/src/pycollision_detection.cpp +++ /dev/null @@ -1,104 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Peter Mitrano - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Peter Mitrano */ - -#include -#include -#include -#include -#include -#include -#include - -namespace py = pybind11; - -using namespace collision_detection; - -void def_collision_detection_bindings(py::module& m) -{ - m.doc() = "contains collision detection, the world, and allowed collision matrices"; - py::enum_(m, "BodyType") - .value("ROBOT_ATTACHED", BodyType::ROBOT_ATTACHED) - .value("ROBOT_LINK", BodyType::ROBOT_LINK) - .value("WORLD_OBJECT", BodyType::WORLD_OBJECT) - .export_values(); - py::class_(m, "Contact") - .def(py::init<>()) - .def_readwrite("body_name_1", &Contact::body_name_1) - .def_readwrite("body_name_2", &Contact::body_name_2) - .def_readwrite("body_type_1", &Contact::body_type_1) - .def_readwrite("body_type_2", &Contact::body_type_2) - .def_readwrite("depth", &Contact::depth) - .def_property_readonly("nearest_points", - [](const Contact& contact) { - std::vector v{ contact.nearest_points[0], contact.nearest_points[1] }; - return v; - }) - .def_readwrite("normal", &Contact::normal) - .def_readwrite("percent_interpolation", &Contact::percent_interpolation) - .def_readwrite("pos", &Contact::pos) - // - ; - py::class_(m, "CollisionRequest") - .def(py::init<>()) - .def_readwrite("contacts", &CollisionRequest::contacts) - .def_readwrite("cost", &CollisionRequest::cost) - .def_readwrite("distance", &CollisionRequest::distance) - .def_readwrite("group_name", &CollisionRequest::group_name) - .def_readwrite("is_done", &CollisionRequest::is_done) - .def_readwrite("max_contacts", &CollisionRequest::max_contacts) - .def_readwrite("max_contacts_per_pair", &CollisionRequest::max_contacts_per_pair) - .def_readwrite("max_cost_sources", &CollisionRequest::max_cost_sources) - .def_readwrite("verbose", &CollisionRequest::verbose) - // - ; - py::class_(m, "CollisionResult") - .def(py::init<>()) - .def_readwrite("collision", &CollisionResult::collision) - .def_readwrite("contact_count", &CollisionResult::contact_count) - .def_readwrite("contacts", &CollisionResult::contacts) - .def_readwrite("cost_sources", &CollisionResult::cost_sources) - .def_readwrite("distance", &CollisionResult::distance) - .def("clear", &CollisionResult::clear) - // - ; - py::class_(m, "AllowedCollisionMatrix") - .def(py::init<>()) - .def("setEntry", - py::overload_cast(&AllowedCollisionMatrix::setEntry)) - // - ; - py::class_(m, "World").def(py::init<>()); -} diff --git a/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp deleted file mode 100644 index ef3e20710e..0000000000 --- a/moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Peter Mitrano - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Peter Mitrano */ - -#include -#include -#include -#include -#include - -#include -#include - -namespace py = pybind11; - -using namespace kinematic_constraints; - -void def_kinematic_constraints_bindings(py::module& m) -{ - m.doc() = "Class for joint, position, visibility, and other constraints"; - - py::class_>(m, "ConstraintEvaluationResult") - .def(py::init<>()) - .def_readwrite("satisfied", &ConstraintEvaluationResult::satisfied) - .def_readwrite("distance", &ConstraintEvaluationResult::distance) - // - ; - - py::class_(m, "KinematicConstraintSet") - .def(py::init(), py::arg("robot_model")) - .def("add", py::overload_cast( - &KinematicConstraintSet::add)) - .def("decide", - py::overload_cast(&KinematicConstraintSet::decide, py::const_), - py::arg("state"), py::arg("verbose") = false) - // - ; - - m.def("constructGoalConstraints", - py::overload_cast( - &constructGoalConstraints), - py::arg("link_name"), py::arg("pose"), py::arg("tolerance_pos") = 1e-3, py::arg("tolerance_angle") = 1e-2) - // - ; -} diff --git a/moveit_core/planning_scene/src/pyplanning_scene.cpp b/moveit_core/planning_scene/src/pyplanning_scene.cpp deleted file mode 100644 index 11d9e4c38b..0000000000 --- a/moveit_core/planning_scene/src/pyplanning_scene.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Peter Mitrano - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Peter Mitrano */ - -#include -#include -#include -#include -#include - -namespace py = pybind11; -using namespace planning_scene; - -void def_planning_scene_bindings(py::module& m) -{ - m.doc() = "The planning scene represents the state of the world and the robot, " - "and can be used for collision checking"; - - py::class_(m, "PlanningScene") - .def(py::init(), - py::arg("robot_model"), py::arg("world") = std::make_shared()) - .def("checkSelfCollision", - py::overload_cast( - &PlanningScene::checkSelfCollision, py::const_)) - .def("checkSelfCollision", - py::overload_cast( - &PlanningScene::checkSelfCollision, py::const_)) - .def("checkCollision", - py::overload_cast( - &PlanningScene::checkCollision, py::const_)) - .def("checkCollision", - py::overload_cast( - &PlanningScene::checkCollision, py::const_)) - .def("getCurrentStateNonConst", &PlanningScene::getCurrentStateNonConst) - .def("getCurrentState", &PlanningScene::getCurrentState) - .def("getAllowedCollisionMatrix", &PlanningScene::getAllowedCollisionMatrix) - .def("isStateConstrained", - py::overload_cast( - &PlanningScene::isStateConstrained, py::const_), - py::arg("state"), py::arg("constr"), py::arg("verbose") = false) - .def( - "isStateConstrained", - py::overload_cast( - &PlanningScene::isStateConstrained, py::const_), - py::arg("state"), py::arg("constr"), py::arg("verbose") = false) - .def("isStateConstrained", - py::overload_cast( - &PlanningScene::isStateConstrained, py::const_), - py::arg("state"), py::arg("constr"), py::arg("verbose") = false) - .def("isStateConstrained", - py::overload_cast( - &PlanningScene::isStateConstrained, py::const_), - py::arg("state"), py::arg("constr"), py::arg("verbose") = false) - .def("getTransforms", py::overload_cast<>(&PlanningScene::getTransforms, py::const_), - py::return_value_policy::reference) - // .def("setStateFeasibilityPredicate", &PlanningScene::setStateFeasibilityPredicate) - .def("isStateValid", - py::overload_cast(&PlanningScene::isStateValid, - py::const_), - py::arg("state"), py::arg("group"), py::arg("verbose") = false) - .def("isStateValid", - py::overload_cast(&PlanningScene::isStateValid, - py::const_), - py::arg("state"), py::arg("group"), py::arg("verbose") = false) - .def("isStateValid", - py::overload_cast( - &PlanningScene::isStateValid, py::const_), - py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false) - .def("isStateValid", - py::overload_cast( - &PlanningScene::isStateValid, py::const_), - py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false) - .def("isStateValid", - py::overload_cast(&PlanningScene::isStateValid, py::const_), - py::arg("state"), py::arg("constr"), py::arg("group"), py::arg("verbose") = false) - .def("setCurrentState", py::overload_cast(&PlanningScene::setCurrentState)) - .def("setCurrentState", py::overload_cast(&PlanningScene::setCurrentState)) - // - ; -} diff --git a/moveit_core/python/CMakeLists.txt b/moveit_core/python/CMakeLists.txt deleted file mode 100644 index f7c6b56eb7..0000000000 --- a/moveit_core/python/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -add_subdirectory(tools) diff --git a/moveit_core/python/pymoveit_core.cpp b/moveit_core/python/pymoveit_core.cpp deleted file mode 100644 index 3179656d7e..0000000000 --- a/moveit_core/python/pymoveit_core.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Peter Mitrano - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Peter Mitrano */ - -#include -#include -#include -#include - -namespace py = pybind11; - -// Each of these functions defines the bindings for a namespace/subfolder within moveit_core -void def_collision_detection_bindings(py::module& contact); - -void def_robot_model_bindings(py::module& m); - -void def_robot_state_bindings(py::module& m); - -void def_transforms_bindings(py::module& m); - -void def_planning_scene_bindings(py::module& m); - -void def_kinematic_constraints_bindings(py::module& m); - -auto load_robot_model(const std::string& urdf_path, const std::string& srdf_path) -{ - auto urdf_model = urdf::parseURDFFile(urdf_path); - auto srdf_model = std::make_shared(); - srdf_model->initFile(*urdf_model, srdf_path); - auto robot_model = std::make_shared(urdf_model, srdf_model); - return robot_model; -} - -PYBIND11_MODULE(pymoveit_core, m) -{ - auto collision_detection_m = m.def_submodule("collision_detection"); - auto planning_scene_m = m.def_submodule("planning_scene"); - auto robot_model_m = m.def_submodule("robot_model"); - auto robot_state_m = m.def_submodule("robot_state"); - auto kinematic_constraints_m = m.def_submodule("kinematic_constraints"); - auto transforms_m = m.def_submodule("transforms"); - - def_collision_detection_bindings(collision_detection_m); - def_robot_model_bindings(robot_model_m); - def_robot_state_bindings(robot_state_m); - def_planning_scene_bindings(planning_scene_m); - def_transforms_bindings(transforms_m); - def_kinematic_constraints_bindings(kinematic_constraints_m); - - // convenience function - m.def("load_robot_model", &load_robot_model); -} diff --git a/moveit_core/python/src/moveit/__init__.py b/moveit_core/python/src/moveit/__init__.py deleted file mode 100644 index 1e2408706e..0000000000 --- a/moveit_core/python/src/moveit/__init__.py +++ /dev/null @@ -1,2 +0,0 @@ -# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages -__path__ = __import__("pkgutil").extend_path(__path__, __name__) diff --git a/moveit_core/python/src/moveit/core/__init__.py b/moveit_core/python/src/moveit/core/__init__.py deleted file mode 100644 index 3903855dbd..0000000000 --- a/moveit_core/python/src/moveit/core/__init__.py +++ /dev/null @@ -1,12 +0,0 @@ -# load symbols from C++ extension lib -from pymoveit_core import load_robot_model - -# and augment with symbols from python modules (order is important to allow overriding!) -from . import ( - collision_detection, - kinematic_constraints, - planning_scene, - robot_model, - robot_state, - transforms, -) diff --git a/moveit_core/python/src/moveit/core/collision_detection/__init__.py b/moveit_core/python/src/moveit/core/collision_detection/__init__.py deleted file mode 100644 index 7c4f406f41..0000000000 --- a/moveit_core/python/src/moveit/core/collision_detection/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.collision_detection import * diff --git a/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py b/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py deleted file mode 100644 index 739336ede6..0000000000 --- a/moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.kinematic_constraints import * diff --git a/moveit_core/python/src/moveit/core/planning_scene/__init__.py b/moveit_core/python/src/moveit/core/planning_scene/__init__.py deleted file mode 100644 index e233204063..0000000000 --- a/moveit_core/python/src/moveit/core/planning_scene/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.planning_scene import * diff --git a/moveit_core/python/src/moveit/core/robot_model/__init__.py b/moveit_core/python/src/moveit/core/robot_model/__init__.py deleted file mode 100644 index 0b265fd59c..0000000000 --- a/moveit_core/python/src/moveit/core/robot_model/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.robot_model import * diff --git a/moveit_core/python/src/moveit/core/robot_state/__init__.py b/moveit_core/python/src/moveit/core/robot_state/__init__.py deleted file mode 100644 index 70599f4aed..0000000000 --- a/moveit_core/python/src/moveit/core/robot_state/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.robot_state import * diff --git a/moveit_core/python/src/moveit/core/transforms/__init__.py b/moveit_core/python/src/moveit/core/transforms/__init__.py deleted file mode 100644 index 508e97d204..0000000000 --- a/moveit_core/python/src/moveit/core/transforms/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from pymoveit_core.transforms import * diff --git a/moveit_core/python/tools/CMakeLists.txt b/moveit_core/python/tools/CMakeLists.txt deleted file mode 100644 index b7781686e2..0000000000 --- a/moveit_core/python/tools/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_library(moveit_python_tools - src/pybind_rosmsg_typecasters.cpp -) -#add_dependencies(moveit_python_tools ${catkin_EXPORTED_TARGETS}) -target_link_libraries(moveit_python_tools ${catkin_LIBRARIES}) # TODO: Fix -set_target_properties(moveit_python_tools PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -install( - TARGETS - moveit_python_tools - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) - -install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h b/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h deleted file mode 100644 index 9b71586b8e..0000000000 --- a/moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Robert Haschke - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Robert Haschke may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Robert Haschke */ - -#pragma once - -#include -#include -#include - -/** Provide pybind11 type converters for ROS types */ - -namespace moveit -{ -namespace python -{ -PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); -PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name); - -} // namespace python -} // namespace moveit - -namespace pybind11 -{ -namespace detail -{ -/// Convert ros::Duration / ros::WallDuration into a float -template -struct DurationCaster -{ - // C++ -> Python - static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */) - { - return PyFloat_FromDouble(src.toSec()); - } - - // Python -> C++ - bool load(handle src, bool convert) - { - if (hasattr(src, "to_sec")) - { - value = T(src.attr("to_sec")().cast()); - } - else if (convert) - { - value = T(src.cast()); - } - else - return false; - return true; - } - - PYBIND11_TYPE_CASTER(T, _("Duration")); -}; - -template <> -struct type_caster : DurationCaster -{ -}; - -template <> -struct type_caster : DurationCaster -{ -}; - -/// Convert ROS message types (C++ <-> python) -template -struct type_caster::value>> -{ - // C++ -> Python - static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) - { - // serialize src into (python) buffer - std::size_t size = ros::serialization::serializationLength(src); - object pbuffer = reinterpret_steal(PyBytes_FromStringAndSize(nullptr, size)); - ros::serialization::OStream stream(reinterpret_cast(PyBytes_AsString(pbuffer.ptr())), size); - ros::serialization::serialize(stream, src); - // deserialize python type from buffer - object msg = moveit::python::createMessage(ros::message_traits::DataType::value()); - msg.attr("deserialize")(pbuffer); - return msg.release(); - } - - // Python -> C++ - bool load(handle src, bool /*convert*/) - { - if (!moveit::python::convertible(src, ros::message_traits::DataType::value())) - return false; - // serialize src into (python) buffer - object pstream = module::import("io").attr("BytesIO")(); - src.attr("serialize")(pstream); - object pbuffer = pstream.attr("getvalue")(); - // deserialize C++ type from buffer - char* cbuffer = nullptr; - Py_ssize_t size; - PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size); - ros::serialization::IStream cstream(const_cast(reinterpret_cast(cbuffer)), size); - ros::serialization::deserialize(cstream, value); - return true; - } - - PYBIND11_TYPE_CASTER(T, _()); -}; -} // namespace detail -} // namespace pybind11 diff --git a/moveit_core/robot_state/src/pyrobot_state.cpp b/moveit_core/robot_state/src/pyrobot_state.cpp deleted file mode 100644 index 9dea327e18..0000000000 --- a/moveit_core/robot_state/src/pyrobot_state.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2021, Peter Mitrano - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Peter Mitrano */ - -#include -#include -#include -#include - -#include -#include - -namespace py = pybind11; -using namespace robot_state; - -void def_robot_state_bindings(py::module& m) -{ - m.doc() = "Representation of a robot's state. This includes position, velocity, acceleration and effort."; - py::class_(m, "RobotState") - .def(py::init(), py::arg("robot_model")) - .def("setToRandomPositions", py::overload_cast<>(&RobotState::setToRandomPositions)) - .def("setToRandomPositions", py::overload_cast(&RobotState::setToRandomPositions)) - .def("getJointModelGroup", &RobotState::getJointModelGroup, py::return_value_policy::reference) - .def("getJointModel", &RobotState::getJointModel, py::return_value_policy::reference) - .def("getLinkModel", &RobotState::getLinkModel, py::return_value_policy::reference) - .def("getVariableNames", &RobotState::getVariableNames) - .def("getVariablePositions", py::overload_cast<>(&RobotState::getVariablePositions, py::const_), - py::return_value_policy::reference) - .def("getVariableCount", &RobotState::getVariableCount) - .def("hasVelocities", &RobotState::hasVelocities) - .def("setJointGroupPositions", - py::overload_cast&>(&RobotState::setJointGroupPositions)) - .def("setJointGroupPositions", - py::overload_cast&>(&RobotState::setJointGroupPositions)) - .def("satisfiesBounds", - py::overload_cast(&RobotState::satisfiesBounds, py::const_), - py::arg("joint_model_group"), py::arg("margin") = 0.0) - .def("update", &RobotState::update, py::arg("force") = false) - .def("printStateInfo", - [](const RobotState& s) { - std::stringstream ss; - s.printStateInfo(ss); - return ss.str(); - }) - .def("printStatePositions", - [](const RobotState& s) { - std::stringstream ss; - s.printStatePositions(ss); - return ss.str(); - }) - .def("__repr__", - [](const RobotState& s) { - std::stringstream ss; - s.printStatePositions(ss); - return ss.str(); - }) - // - ; - - m.def("jointStateToRobotState", &jointStateToRobotState); - m.def( - "robotStateToRobotStateMsg", - [](const RobotState& state, bool copy_attached_bodies) { - moveit_msgs::RobotState state_msg; - robotStateToRobotStateMsg(state, state_msg, copy_attached_bodies); - return state_msg; - }, - py::arg("state"), py::arg("copy_attached_bodies") = true); -} diff --git a/moveit_core/setup.py b/moveit_core/setup.py deleted file mode 100644 index 0da9425595..0000000000 --- a/moveit_core/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - -from catkin_pkg.python_setup import generate_distutils_setup -from setuptools import setup, find_packages - -packages = find_packages("python/src/") - -d = generate_distutils_setup(packages=packages, package_dir={"": "python/src"}) - -setup(**d) diff --git a/moveit_py/CHANGELOG.rst b/moveit_py/CHANGELOG.rst new file mode 100644 index 0000000000..d7dfa37350 --- /dev/null +++ b/moveit_py/CHANGELOG.rst @@ -0,0 +1,3 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_py +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt new file mode 100644 index 0000000000..1ad76f6282 --- /dev/null +++ b/moveit_py/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.5) +project(moveit_py) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(moveit_core REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) + +add_subdirectory(src/moveit/moveit_py_utils) + +ament_python_install_package(moveit) + +# Set the build location and install location for a CPython extension +function(configure_build_install_location _library_name) + install(TARGETS ${_library_name} + DESTINATION "${PYTHON_INSTALL_DIR}/moveit" + ) +endfunction() + +pybind11_add_module(core + src/moveit/core.cpp + src/moveit/moveit_core/collision_detection/collision_common.cpp + src/moveit/moveit_core/collision_detection/collision_matrix.cpp + src/moveit/moveit_core/collision_detection/world.cpp + src/moveit/moveit_core/controller_manager/controller_manager.cpp + src/moveit/moveit_core/kinematic_constraints/utils.cpp + src/moveit/moveit_core/planning_interface/planning_response.cpp + src/moveit/moveit_core/planning_scene/planning_scene.cpp + src/moveit/moveit_core/transforms/transforms.cpp + src/moveit/moveit_core/robot_model/joint_model.cpp + src/moveit/moveit_core/robot_model/joint_model_group.cpp + src/moveit/moveit_core/robot_model/robot_model.cpp + src/moveit/moveit_core/robot_state/robot_state.cpp + src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp + ) +target_link_libraries(core PRIVATE moveit_ros_planning::moveit_cpp + rclcpp::rclcpp + moveit_core::moveit_transforms + moveit_core::moveit_kinematic_constraints + moveit_core::moveit_planning_interface + moveit_core::moveit_planning_scene + moveit_core::moveit_utils + moveit_core::moveit_robot_model + moveit_core::moveit_robot_state + moveit_py_utils) +configure_build_install_location(core) + +pybind11_add_module(planning + src/moveit/planning.cpp + src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp + src/moveit/moveit_ros/moveit_cpp/planning_component.cpp + src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp + ) +target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp + moveit_ros_planning::moveit_planning_scene_monitor + moveit_core::moveit_utils + rclcpp::rclcpp + moveit_py_utils) +configure_build_install_location(planning) + +ament_package() diff --git a/moveit_py/README.md b/moveit_py/README.md new file mode 100644 index 0000000000..ecc5018d39 --- /dev/null +++ b/moveit_py/README.md @@ -0,0 +1,24 @@ +# MoveIt 2 Python Library + + +`moveit` is a Python library for interfacing with the core functionalities of MoveIt 2. +The goal of this library is to provide a simplified interface for MoveIt 2 Python users. + +This Python library depends on [pybind11](https://pybind11.readthedocs.io/en/stable/index.html) for generating Python bindings. +The project is split into the following folders: + + ├── docs # Sphinx documentation files + ├── moveit # Python library stubs; Python functionalities built on top of bindings + ├── src/moveit # pybind11 binding code + ├── test # Unit and integration testing + +## Tutorials +We are continuing to add tutorials for the MoveIt 2 Python library. Of particular note is the fact that one can interact with MoveIt interactively since Python is an interpreted language, our tutorials demonstrate this through leveraging Jupyter notebooks. For further details please consult the [MoveIt 2 tutorials site](https://moveit.picknik.ai/main/index.html). + +## Contribution Guidelines +Community contributions are welcome. + +For detailed contribution guidelines please consult the official [MoveIt contribution guidelines](https://moveit.ros.org/documentation/contributing/). + +## Acknowledgements +Thank you to the [Google Summer of Code program](https://summerofcode.withgoogle.com/) for sponsoring the development of this Python library. Thank you to the MoveIt maintainers Henning Kayser (@henningkayser) and Michael Gorner (@v4hn) for their guidance as supervisors of my GSoC project. Finally thank you to the [ML Collective](https://mlcollective.org/) for providing compute support for this project. diff --git a/moveit_py/banner.png b/moveit_py/banner.png new file mode 100644 index 0000000000..e1ea199c18 Binary files /dev/null and b/moveit_py/banner.png differ diff --git a/moveit_py/docs/source/conf.py b/moveit_py/docs/source/conf.py new file mode 100644 index 0000000000..0f5dd25f84 --- /dev/null +++ b/moveit_py/docs/source/conf.py @@ -0,0 +1,86 @@ +# Configuration file for the Sphinx documentation builder. +# +# This file only contains a selection of the most common options. For a full +# list see the documentation: +# https://www.sphinx-doc.org/en/master/usage/configuration.html + +# -- Path setup -------------------------------------------------------------- + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +# +# import os +# import sys +# sys.path.insert(0, os.path.abspath('../../../../../../build/moveit_py')) + + +# -- Project information ----------------------------------------------------- + +project = "moveit_py" +copyright = "2022, Peter David Fagan" +author = "Peter David Fagan" + + +# -- General configuration --------------------------------------------------- + +# Add any Sphinx extension module names here, as strings. They can be +# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom +# ones. +extensions = [ + "sphinx.ext.napoleon", + "sphinx.ext.autodoc", + "sphinx.ext.autosummary", + "sphinx_rtd_theme", + "sphinx.ext.intersphinx", +] + +intersphinx_mapping = { + "numpy": ("https://numpy.org/doc/stable/", None), + "geometry_msgs": ("http://docs.ros.org/en/latest/api/geometry_msgs/html/", None), +} +autodoc_typehints = "signature" + +autodoc_default_options = { + "members": True, + "undoc-members": True, + "member-order": "bysource", +} + +autosummary_generate = True + +# Napoleon settings +napoleon_google_docstring = True +napoleon_numpy_docstring = False +napoleon_include_init_with_doc = False +napoleon_include_private_with_doc = False +napoleon_include_special_with_doc = False +napoleon_use_admonition_for_examples = False +napoleon_use_admonition_for_notes = False +napoleon_use_admonition_for_references = False +napoleon_use_ivar = False +napoleon_use_param = False +napoleon_use_rtype = False + +# Add any paths that contain templates here, relative to this directory. +templates_path = ["_templates"] + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +# This pattern also affects html_static_path and html_extra_path. +exclude_patterns = [] + + +# -- Options for HTML output ------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +# +html_theme = "sphinx_rtd_theme" + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ["_static"] + +master_doc = "index" diff --git a/moveit_py/docs/source/index.rst b/moveit_py/docs/source/index.rst new file mode 100644 index 0000000000..104580cc28 --- /dev/null +++ b/moveit_py/docs/source/index.rst @@ -0,0 +1,12 @@ +MoveItPy API Documentation +===================================== + +.. toctree:: + +.. autosummary:: + :toctree: _autosummary + :recursive: + + moveit_py.core + moveit_py.planning + moveit_py.servo_client diff --git a/moveit_py/moveit/README.md b/moveit_py/moveit/README.md new file mode 100644 index 0000000000..e958d4d193 --- /dev/null +++ b/moveit_py/moveit/README.md @@ -0,0 +1,18 @@ +# Stubfile Generation +What is a stub? + +``` +A stub file in Python is a file that is used to define type hinting information (.pyi file extension). +This type hint information enables users to interactively retrieve data about a method or class within their chosem IDE. +``` + +For this project we leverage (stubgen)[https://mypy.readthedocs.io/en/stable/stubgen.html] from the (mypy)[https://mypy.readthedocs.io/en/stable/index.html] +Python package to autogenerate stub files. To do so we simply build the moveit_py Python library and run the following command from an environment containing the built package: + +``` +stubgen -p moveit_py +``` + +This outputs the outgenerated stub files to an `out` directory. These are the stub files we use. + +Note: manual edits are likely to be made to the autogenerated files for official library releases.](https://mypy.readthedocs.io/en/stable/index.html) diff --git a/moveit_py/moveit/__init__.py b/moveit_py/moveit/__init__.py new file mode 100644 index 0000000000..0f44d4a088 --- /dev/null +++ b/moveit_py/moveit/__init__.py @@ -0,0 +1,3 @@ +from moveit.core import * +from moveit.planning import * +from moveit.utils import get_launch_params_filepath diff --git a/moveit_py/moveit/core/__init__.pyi b/moveit_py/moveit/core/__init__.pyi new file mode 100644 index 0000000000..0813a8800b --- /dev/null +++ b/moveit_py/moveit/core/__init__.pyi @@ -0,0 +1,43 @@ +from typing import Any + +class JointModelGroup: + def __init__(self, *args, **kwargs) -> None: ... + def satisfies_position_bounds(self, *args, **kwargs) -> Any: ... + @property + def active_joint_model_bounds(self) -> Any: ... + @property + def active_joint_model_names(self) -> Any: ... + @property + def joint_model_names(self) -> Any: ... + @property + def name(self) -> Any: ... + +class VariableBounds: + def __init__(self, *args, **kwargs) -> None: ... + @property + def acceleration_bounded(self) -> Any: ... + @property + def jerk_bounded(self) -> Any: ... + @property + def max_acceleration(self) -> Any: ... + @property + def max_jerk(self) -> Any: ... + @property + def max_position(self) -> Any: ... + @property + def max_velocity(self) -> Any: ... + @property + def min_acceleration(self) -> Any: ... + @property + def min_jerk(self) -> Any: ... + @property + def min_position(self) -> Any: ... + @property + def min_velocity(self) -> Any: ... + @property + def position_bounded(self) -> Any: ... + @property + def velocity_bounded(self) -> Any: ... + +class World: + def __init__(self, *args, **kwargs) -> None: ... diff --git a/moveit_py/moveit/core/collision_detection.pyi b/moveit_py/moveit/core/collision_detection.pyi new file mode 100644 index 0000000000..756716035b --- /dev/null +++ b/moveit_py/moveit/core/collision_detection.pyi @@ -0,0 +1,26 @@ +from typing import Any + +class AllowedCollisionMatrix: + def __init__(self, *args, **kwargs) -> None: ... + def get_entry(self, *args, **kwargs) -> Any: ... + def remove_entry(self, *args, **kwargs) -> Any: ... + def set_entry(self, *args, **kwargs) -> Any: ... + +class CollisionRequest: + contacts: Any + cost: Any + distance: Any + joint_model_group_name: Any + max_contacts: Any + max_contacts_per_pair: Any + max_cost_sources: Any + verbose: Any + def __init__(self, *args, **kwargs) -> None: ... + +class CollisionResult: + collision: Any + contact_count: Any + contacts: Any + cost_sources: Any + distance: Any + def __init__(self, *args, **kwargs) -> None: ... diff --git a/moveit_py/moveit/core/controller_manager.pyi b/moveit_py/moveit/core/controller_manager.pyi new file mode 100644 index 0000000000..d3ef91f880 --- /dev/null +++ b/moveit_py/moveit/core/controller_manager.pyi @@ -0,0 +1,6 @@ +from typing import Any + +class ExecutionStatus: + def __init__(self, *args, **kwargs) -> None: ... + @property + def status(self) -> Any: ... diff --git a/moveit_py/moveit/core/kinematic_constraints.pyi b/moveit_py/moveit/core/kinematic_constraints.pyi new file mode 100644 index 0000000000..f425d39e62 --- /dev/null +++ b/moveit_py/moveit/core/kinematic_constraints.pyi @@ -0,0 +1,5 @@ +from typing import Any + +def construct_constraints_from_node(*args, **kwargs) -> Any: ... +def construct_joint_constraint(*args, **kwargs) -> Any: ... +def construct_link_constraint(*args, **kwargs) -> Any: ... diff --git a/moveit_py/moveit/core/planning_interface.pyi b/moveit_py/moveit/core/planning_interface.pyi new file mode 100644 index 0000000000..3bdeb186ab --- /dev/null +++ b/moveit_py/moveit/core/planning_interface.pyi @@ -0,0 +1,15 @@ +from typing import Any + +class MotionPlanResponse: + def __init__(self, *args, **kwargs) -> None: ... + def __bool__(self) -> Any: ... + @property + def error_code(self) -> Any: ... + @property + def planner_id(self) -> Any: ... + @property + def planning_time(self) -> Any: ... + @property + def start_state(self) -> Any: ... + @property + def trajectory(self) -> Any: ... diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi new file mode 100644 index 0000000000..a18ecb7b3b --- /dev/null +++ b/moveit_py/moveit/core/planning_scene.pyi @@ -0,0 +1,31 @@ +from typing import Any + +class PlanningScene: + current_state: Any + name: Any + def __init__(self, *args, **kwargs) -> None: ... + def apply_collision_object(self, *args, **kwargs) -> Any: ... + def check_collision(self, *args, **kwargs) -> Any: ... + def check_collision_unpadded(self, *args, **kwargs) -> Any: ... + def check_self_collision(self, *args, **kwargs) -> Any: ... + def get_frame_transform(self, *args, **kwargs) -> Any: ... + def is_path_valid(self, *args, **kwargs) -> Any: ... + def is_state_colliding(self, *args, **kwargs) -> Any: ... + def is_state_constrained(self, *args, **kwargs) -> Any: ... + def is_state_valid(self, *args, **kwargs) -> Any: ... + def knows_frame_transform(self, *args, **kwargs) -> Any: ... + def process_attached_collision_object(self, *args, **kwargs) -> Any: ... + def process_octomap(self, *args, **kwargs) -> Any: ... + def process_planning_scene_world(self, *args, **kwargs) -> Any: ... + def remove_all_collision_objects(self, *args, **kwargs) -> Any: ... + def set_object_color(self, *args, **kwargs) -> Any: ... + def __copy__(self) -> Any: ... + def __deepcopy__(self) -> Any: ... + @property + def planning_frame(self) -> Any: ... + @property + def planning_scene_message(self) -> Any: ... + @property + def robot_model(self) -> Any: ... + @property + def transforms(self) -> Any: ... diff --git a/moveit_py/moveit/core/robot_model.pyi b/moveit_py/moveit/core/robot_model.pyi new file mode 100644 index 0000000000..8f28ff0f0e --- /dev/null +++ b/moveit_py/moveit/core/robot_model.pyi @@ -0,0 +1,19 @@ +from typing import Any + +class RobotModel: + def __init__(self, *args, **kwargs) -> None: ... + def get_joint_model_group(self, *args, **kwargs) -> Any: ... + def get_model_info(self, *args, **kwargs) -> Any: ... + def has_joint_model_group(self, *args, **kwargs) -> Any: ... + @property + def end_effectors(self) -> Any: ... + @property + def joint_model_group_names(self) -> Any: ... + @property + def joint_model_groups(self) -> Any: ... + @property + def model_frame(self) -> Any: ... + @property + def name(self) -> Any: ... + @property + def root_joint_name(self) -> Any: ... diff --git a/moveit_py/moveit/core/robot_state.pyi b/moveit_py/moveit/core/robot_state.pyi new file mode 100644 index 0000000000..8ba90828b3 --- /dev/null +++ b/moveit_py/moveit/core/robot_state.pyi @@ -0,0 +1,35 @@ +from typing import Any, ClassVar + +class RobotState: + state_info: ClassVar[Any] = ... # read-only + joint_accelerations: Any + joint_efforts: Any + joint_positions: Any + joint_velocities: Any + def __init__(self, *args, **kwargs) -> None: ... + def clear_attached_bodies(self, *args, **kwargs) -> Any: ... + def get_frame_transform(self, *args, **kwargs) -> Any: ... + def get_global_link_transform(self, *args, **kwargs) -> Any: ... + def get_jacobian(self, *args, **kwargs) -> Any: ... + def get_joint_group_accelerations(self, *args, **kwargs) -> Any: ... + def get_joint_group_positions(self, *args, **kwargs) -> Any: ... + def get_joint_group_velocities(self, *args, **kwargs) -> Any: ... + def get_pose(self, *args, **kwargs) -> Any: ... + def set_from_ik(self, *args, **kwargs) -> Any: ... + def set_joint_group_accelerations(self, *args, **kwargs) -> Any: ... + def set_joint_group_active_positions(self, *args, **kwargs) -> Any: ... + def set_joint_group_positions(self, *args, **kwargs) -> Any: ... + def set_joint_group_velocities(self, *args, **kwargs) -> Any: ... + def set_to_default_values(self, *args, **kwargs) -> Any: ... + def set_to_random_positions(self, *args, **kwargs) -> Any: ... + def update(self, *args, **kwargs) -> Any: ... + def __copy__(self) -> Any: ... + def __deepcopy__(self) -> Any: ... + @property + def dirty(self) -> Any: ... + @property + def robot_model(self) -> Any: ... + @property + def state_tree(self) -> Any: ... + +def robotStateToRobotStateMsg(*args, **kwargs) -> Any: ... diff --git a/moveit_py/moveit/core/robot_trajectory.pyi b/moveit_py/moveit/core/robot_trajectory.pyi new file mode 100644 index 0000000000..6007a39827 --- /dev/null +++ b/moveit_py/moveit/core/robot_trajectory.pyi @@ -0,0 +1,18 @@ +from typing import Any + +class RobotTrajectory: + joint_model_group_name: Any + def __init__(self, *args, **kwargs) -> None: ... + def get_robot_trajectory_msg(self, *args, **kwargs) -> Any: ... + def get_waypoint_durations(self, *args, **kwargs) -> Any: ... + def unwind(self, *args, **kwargs) -> Any: ... + def __getitem__(self, index) -> Any: ... + def __iter__(self) -> Any: ... + def __len__(self) -> Any: ... + def __reverse__(self, *args, **kwargs) -> Any: ... + @property + def average_segment_duration(self) -> Any: ... + @property + def duration(self) -> Any: ... + @property + def robot_model(self) -> Any: ... diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi new file mode 100644 index 0000000000..a8415f76c7 --- /dev/null +++ b/moveit_py/moveit/planning.pyi @@ -0,0 +1,63 @@ +from typing import Any + +class LockedPlanningSceneContextManagerRO: + def __init__(self, *args, **kwargs) -> None: ... + def __enter__(self) -> Any: ... + def __exit__(self, type, value, traceback) -> Any: ... + +class LockedPlanningSceneContextManagerRW: + def __init__(self, *args, **kwargs) -> None: ... + def __enter__(self) -> Any: ... + def __exit__(self, type, value, traceback) -> Any: ... + +class MoveItPy: + def __init__(self, *args, **kwargs) -> None: ... + def execute(self, *args, **kwargs) -> Any: ... + def get_planning_component(self, *args, **kwargs) -> Any: ... + def get_planning_scene_monitor(self, *args, **kwargs) -> Any: ... + def get_robot_model(self, *args, **kwargs) -> Any: ... + def shutdown(self, *args, **kwargs) -> Any: ... + +class MultiPipelinePlanRequestParameters: + def __init__(self, *args, **kwargs) -> None: ... + @property + def multi_plan_request_parameters(self) -> Any: ... + +class PlanRequestParameters: + max_acceleration_scaling_factor: Any + max_velocity_scaling_factor: Any + planner_id: Any + planning_attempts: Any + planning_pipeline: Any + planning_time: Any + def __init__(self, *args, **kwargs) -> None: ... + +class PlanningComponent: + def __init__(self, *args, **kwargs) -> None: ... + def get_start_state(self, *args, **kwargs) -> Any: ... + def plan(self, *args, **kwargs) -> Any: ... + def set_goal_state(self, *args, **kwargs) -> Any: ... + def set_path_constraints(self, *args, **kwargs) -> Any: ... + def set_start_state(self, *args, **kwargs) -> Any: ... + def set_start_state_to_current_state(self, *args, **kwargs) -> Any: ... + def set_workspace(self, *args, **kwargs) -> Any: ... + def unset_workspace(self, *args, **kwargs) -> Any: ... + @property + def named_target_state_values(self) -> Any: ... + @property + def named_target_states(self) -> Any: ... + @property + def planning_group_name(self) -> Any: ... + +class PlanningSceneMonitor: + def __init__(self, *args, **kwargs) -> None: ... + def clear_octomap(self, *args, **kwargs) -> Any: ... + def read_only(self, *args, **kwargs) -> Any: ... + def read_write(self, *args, **kwargs) -> Any: ... + def start_scene_monitor(self, *args, **kwargs) -> Any: ... + def start_state_monitor(self, *args, **kwargs) -> Any: ... + def stop_scene_monitor(self, *args, **kwargs) -> Any: ... + def stop_state_monitor(self, *args, **kwargs) -> Any: ... + def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ... + @property + def name(self) -> Any: ... diff --git a/moveit_py/moveit/py.typed b/moveit_py/moveit/py.typed new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/moveit/servo_client/README.md b/moveit_py/moveit/servo_client/README.md new file mode 100644 index 0000000000..57d21a427b --- /dev/null +++ b/moveit_py/moveit/servo_client/README.md @@ -0,0 +1,6 @@ +# Servo Module +This module provides a client for interacting with MoveIt Servo. + +# Supported Devices +* PS4 DualShock Controller +* Oculus Quest 2 (plans to support this device) diff --git a/moveit_py/moveit/servo_client/__init__.py b/moveit_py/moveit/servo_client/__init__.py new file mode 100644 index 0000000000..d0c06776e1 --- /dev/null +++ b/moveit_py/moveit/servo_client/__init__.py @@ -0,0 +1,4 @@ +"""A client providing access to the MoveIt Servo node. + +This module can be used to configure new devices for teleoperation. It is also possible to use this module for sending position or velocity commands to your robot which can be useful for learning based approaches. +""" diff --git a/moveit_py/moveit/servo_client/__init__.pyi b/moveit_py/moveit/servo_client/__init__.pyi new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/moveit/servo_client/devices/__init__.py b/moveit_py/moveit/servo_client/devices/__init__.py new file mode 100644 index 0000000000..6acd66691f --- /dev/null +++ b/moveit_py/moveit/servo_client/devices/__init__.py @@ -0,0 +1 @@ +""" Teleoperation device implementations. """ diff --git a/moveit_py/moveit/servo_client/devices/__init__.pyi b/moveit_py/moveit/servo_client/devices/__init__.pyi new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/moveit/servo_client/devices/ps4_dualshock.py b/moveit_py/moveit/servo_client/devices/ps4_dualshock.py new file mode 100644 index 0000000000..29d39faace --- /dev/null +++ b/moveit_py/moveit/servo_client/devices/ps4_dualshock.py @@ -0,0 +1,144 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2022, Peter David Fagan +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Peter David Fagan +""" PS4 dualshock teleop device implementation. """ + +import rclpy +from multiprocessing import Process + +from geometry_msgs.msg import TwistStamped +from sensor_msgs.msg import Joy +from std_srvs.srv import Trigger + +from dataclasses import dataclass +from moveit.servo_client.teleop import TeleopDevice + + +############################################################################### +# DualShock4 Device Mappings +############################################################################### + + +@dataclass +class DualShockAxes: + LEFT_STICK_X: int = 0 + LEFT_STICK_Y: int = 1 + LEFT_TRIGGER: int = 2 + RIGHT_STICK_X: int = 3 + RIGHT_STICK_Y: int = 4 + RIGHT_TRIGGER: int = 5 + D_PAD_X: int = 6 + D_PAD_Y: int = 7 + + +@dataclass +class DualShockButtons: + X: int = 0 + O: int = 1 + TRIANGLE: int = 2 + SQUARE: int = 3 + L1: int = 4 + R1: int = 5 + L2: int = 6 + R2: int = 7 + SHARE: int = 8 + OPTIONS: int = 9 + HOME: int = 10 + LEFT_STICK_TRIGGER: int = 11 + RIGHT_STICK_TRIGGER: int = 12 + + +@dataclass +class PS4DualShock: + """ + A dataclass to store device config. This class follows the conventions of Joy message (http://docs.ros.org/en/noetic/api/sensor_msgs/html/msg/Joy.html) + """ + + Axes: DualShockAxes = DualShockAxes() + + Buttons: DualShockButtons = DualShockButtons() + + +############################################################################### +# DualShock4 Device Teleop +############################################################################### + + +class PS4DualShockTeleop(TeleopDevice): + "A class which encapsulates teleoperation functionalities for ps4 dualshock device." + + def __init__( + self, + ee_frame_name: str, + node_name: str = "ps4_dualshock_teleop", + device_name: str = "ps4_dualshock", + device_config: PS4DualShock = PS4DualShock(), + ): + super().__init__( + node_name=node_name, + device_name=device_name, + device_config=device_config, + ee_frame_name=ee_frame_name, + ) + self.logger = rclpy.logging.get_logger("ps4_dualshock_teleop") + + def publish_command(self, data): + """ + Publishes the teleop command. + """ + try: + # convert joy data to twist command + twist = TwistStamped() + twist.twist.linear.z = data.axes[self.device_config.Axes.RIGHT_STICK_Y] + twist.twist.linear.y = data.axes[self.device_config.Axes.RIGHT_STICK_X] + + lin_x_right = -0.5 * (data.axes[self.device_config.Axes.RIGHT_TRIGGER]) + lin_x_left = 0.5 * (data.axes[self.device_config.Axes.LEFT_TRIGGER]) + twist.twist.linear.x = lin_x_right + lin_x_left + + twist.twist.angular.y = data.axes[self.device_config.Axes.LEFT_STICK_Y] + twist.twist.angular.x = data.axes[self.device_config.Axes.LEFT_STICK_X] + + roll_positive = data.buttons[self.device_config.Buttons.R1] + roll_negative = -1 * data.buttons[self.device_config.Buttons.L1] + twist.twist.angular.z = float(roll_positive + roll_negative) + + twist.header.frame_id = self.ee_frame_name + twist.header.stamp = self.get_clock().now().to_msg() + self.twist_publisher.publish(twist) + except Exception as e: + self.logger.info.error(e) + print(e) + + def record(): + pass diff --git a/moveit_py/moveit/servo_client/devices/ps4_dualshock.pyi b/moveit_py/moveit/servo_client/devices/ps4_dualshock.pyi new file mode 100644 index 0000000000..fd845bc079 --- /dev/null +++ b/moveit_py/moveit/servo_client/devices/ps4_dualshock.pyi @@ -0,0 +1,73 @@ +from rclpy.impl.rcutils_logger import RCUtilsLogger +from moveit.servo_client.teleop import TeleopDevice as TeleopDevice +from multiprocessing import Process as Process +from sensor_msgs.msg import Joy as Joy +from std_srvs.srv import Trigger as Trigger + +class DualShockAxes: + LEFT_STICK_X: int + LEFT_STICK_Y: int + LEFT_TRIGGER: int + RIGHT_STICK_X: int + RIGHT_STICK_Y: int + RIGHT_TRIGGER: int + D_PAD_X: int + D_PAD_Y: int + def __init__( + self, + LEFT_STICK_X, + LEFT_STICK_Y, + LEFT_TRIGGER, + RIGHT_STICK_X, + RIGHT_STICK_Y, + RIGHT_TRIGGER, + D_PAD_X, + D_PAD_Y, + ) -> None: ... + +class DualShockButtons: + X: int + O: int + TRIANGLE: int + SQUARE: int + L1: int + R1: int + L2: int + R2: int + SHARE: int + OPTIONS: int + HOME: int + LEFT_STICK_TRIGGER: int + RIGHT_STICK_TRIGGER: int + def __init__( + self, + X, + O, + TRIANGLE, + SQUARE, + L1, + R1, + L2, + R2, + SHARE, + OPTIONS, + HOME, + LEFT_STICK_TRIGGER, + RIGHT_STICK_TRIGGER, + ) -> None: ... + +class PS4DualShock: + Axes: DualShockAxes + Buttons: DualShockButtons + def __init__(self, Axes, Buttons) -> None: ... + +class PS4DualShockTeleop(TeleopDevice): + logger: RCUtilsLogger + def __init__( + self, + node_name: str = ..., + device_name: str = ..., + device_config: PS4DualShock = ..., + ) -> None: ... + def publish_command(self, data) -> None: ... + def record() -> None: ... diff --git a/moveit_py/moveit/servo_client/teleop.py b/moveit_py/moveit/servo_client/teleop.py new file mode 100644 index 0000000000..7791f69384 --- /dev/null +++ b/moveit_py/moveit/servo_client/teleop.py @@ -0,0 +1,118 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2022, Peter David Fagan +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Peter David Fagan +""" Definition of an abstract base class for device teleoperation. """ + +import rclpy + +from abc import ABC, abstractmethod +import threading +from rclpy.node import Node + +# messages/services +from geometry_msgs.msg import TwistStamped +from sensor_msgs.msg import Joy +from std_srvs.srv import Trigger + + +class TeleopDevice(ABC, Node): + """ + An abstract base class for a teleoperation device. + + This class expects both the `publish_command` and `record` methods to be overridden by inheriting classes. + """ + + def __init__(self, node_name, device_name, device_config, ee_frame_name): + super().__init__(node_name=node_name) + self.device_name = device_name + self.device_config = device_config + self.ee_frame_name = ee_frame_name + + # default subscriber and publisher setup + self.joy_subscriber = self.create_subscription( + Joy, "/joy", self.publish_command, 10 + ) + self.twist_publisher = self.create_publisher( + TwistStamped, "/servo_node/delta_twist_cmds", 10 + ) + self.servo_node_start_client = self.create_client( + Trigger, "/servo_node/start_servo" + ) + self.servo_node_stop_client = self.create_client( + Trigger, "/servo_node/stop_servo" + ) + + self.teleop_thread = None # gets created when teleop starts + + def start_teleop(self): + """ + Starts servo client and teleop process. + """ + try: + # start servo client + self.servo_node_start_client.wait_for_service(10.0) + self.servo_node_start_client.call_async(Trigger.Request()) + + # spin the teleop node + # use multithreaded because Servo has concurrent processes for moving the robot and avoiding collisions + ex = rclpy.executors.MultiThreadedExecutor() + ex.add_node(self) + self.teleop_thread = threading.Thread(target=ex.spin, daemon=True) + self.teleop_thread.start() + except Exception as e: + print(e) + + def stop_teleop(self): + """ + Stops servo client and teleop process. + """ + try: + self.servo_node_stop_client.wait_for_service(10.0) + self.servo_node_stop_client.call_async(Trigger.Request()) + self.teleop_thread.join() + except Exception as e: + print(e) + + @abstractmethod + def publish_command(self): + """ + Publishes the teleop command. + """ + pass + + @abstractmethod + def record(self): + """ + Records trajectory data. + """ + pass diff --git a/moveit_py/moveit/servo_client/teleop.pyi b/moveit_py/moveit/servo_client/teleop.pyi new file mode 100644 index 0000000000..fc19ded10c --- /dev/null +++ b/moveit_py/moveit/servo_client/teleop.pyi @@ -0,0 +1,20 @@ +import abc +from abc import ABC, abstractmethod +from rclpy.node import Node +from typing import Any + +class TeleopDevice(ABC, Node, metaclass=abc.ABCMeta): + device_name: Any + device_config: Any + joy_subscriber: Any + twist_publisher: Any + servo_node_start_client: Any + servo_node_stop_client: Any + teleop_process: Any + def __init__(self, node_name, device_name, device_config) -> None: ... + def start_teleop(self) -> None: ... + def stop_teleop(self) -> None: ... + @abstractmethod + def publish_command(self): ... + @abstractmethod + def record(self): ... diff --git a/moveit_py/moveit/utils.py b/moveit_py/moveit/utils.py new file mode 100644 index 0000000000..a40cd12284 --- /dev/null +++ b/moveit_py/moveit/utils.py @@ -0,0 +1,58 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2022, Peter David Fagan +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: Peter David Fagan + +import sys +import traceback +import yaml + +from tempfile import NamedTemporaryFile + + +def create_params_file_from_dict(params, node_name): + with NamedTemporaryFile(mode="w", prefix="launch_params_", delete=False) as h: + param_file_path = h.name + param_dict = {node_name: {"ros__parameters": params}} + yaml.dump(param_dict, h, default_flow_style=False) + return param_file_path + + +def get_launch_params_filepath(): + """ + A utility that returns the path value after argument --params-file. + """ + try: + cli_args = sys.argv + return sys.argv[sys.argv.index("--params-file") + 1] + except ValueError: + return "Failed to parse params file path from command line arguments. Check that --params-file command line argument is specified." diff --git a/moveit_py/moveit/utils.pyi b/moveit_py/moveit/utils.pyi new file mode 100644 index 0000000000..1b43712276 --- /dev/null +++ b/moveit_py/moveit/utils.pyi @@ -0,0 +1,2 @@ +def create_params_file_from_dict(params, node_name): ... +def get_launch_params_filepath(): ... diff --git a/moveit_py/package.xml b/moveit_py/package.xml new file mode 100644 index 0000000000..77d6610845 --- /dev/null +++ b/moveit_py/package.xml @@ -0,0 +1,30 @@ + + + + moveit_py + 0.0.0 + Python binding for MoveIt 2 + + Peter David Fagan + + BSD + + Peter David Fagan + + ament_cmake + + pybind11_vendor + + ament_index_python + + rclcpp + rclpy + geometry_msgs + octomap_msgs + moveit_ros_planning_interface + moveit_ros_planning + moveit_core + + ament_cmake + + diff --git a/moveit_py/src/moveit/README.md b/moveit_py/src/moveit/README.md new file mode 100644 index 0000000000..c19da0b7ec --- /dev/null +++ b/moveit_py/src/moveit/README.md @@ -0,0 +1,7 @@ +# MoveIt 2 Pybind11 Bindings + +This directory contains the [Pybind11](https://pybind11.readthedocs.io/en/stable/) binding code used by the moveit_py package. +The root of this directory contains the actual module definitions (e.g. `core.cpp` defines the `moveit_py.core` module). + +The structure of subfolders in this directory reflects that of the MoveIt 2 C++ codebase. Within each subfolder you will find the actual binding code that each module leverages. +For instance, to see how the `PlanningScene` object is being binded you can refer to the source files in `./moveit_core/planning_scene/`. diff --git a/moveit_py/src/moveit/core.cpp b/moveit_py/src/moveit/core.cpp new file mode 100644 index 0000000000..4f4f6e58b4 --- /dev/null +++ b/moveit_py/src/moveit/core.cpp @@ -0,0 +1,81 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "moveit_core/collision_detection/collision_common.h" +#include "moveit_core/collision_detection/collision_matrix.h" +#include "moveit_core/collision_detection/world.h" +#include "moveit_core/controller_manager/controller_manager.h" +#include "moveit_core/kinematic_constraints/utils.h" +#include "moveit_core/planning_interface/planning_response.h" +#include "moveit_core/planning_scene/planning_scene.h" +#include "moveit_core/robot_model/joint_model.h" +#include "moveit_core/robot_model/joint_model_group.h" +#include "moveit_core/robot_model/robot_model.h" +#include "moveit_core/robot_state/robot_state.h" +#include "moveit_core/robot_trajectory/robot_trajectory.h" + +PYBIND11_MODULE(core, m) +{ + m.doc() = R"( + Python bindings for moveit_core functionalities. + )"; + + // Provide custom function signatures + py::options options; + options.disable_function_signatures(); + + // Construct module classes + moveit_py::bind_collision_detection::init_collision_request(m); + moveit_py::bind_collision_detection::init_collision_result(m); + moveit_py::bind_collision_detection::init_world(m); + moveit_py::bind_collision_detection::init_acm(m); + moveit_py::bind_controller_manager::init_execution_status(m); + moveit_py::bind_kinematic_constraints::init_kinematic_constraints(m); + moveit_py::bind_planning_scene::init_planning_scene(m); + moveit_py::bind_planning_interface::init_motion_plan_response(m); + moveit_py::bind_robot_model::init_joint_model(m); + moveit_py::bind_robot_model::init_joint_model_group(m); + moveit_py::bind_robot_model::init_robot_model(m); + moveit_py::bind_robot_state::init_robot_state(m); + moveit_py::bind_robot_trajectory::init_robot_trajectory(m); + // TODO (peterdavidfagan): complete LinkModel bindings + // LinkModel + // py::class_(m, "LinkModel"); + + // TODO (peterdavidfagan): complete JointModel bindings + // JointModel (this is an abstract base class) + // py::class_(m, "JointModel"); +} diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp new file mode 100644 index 0000000000..af2b9a6f7b --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp @@ -0,0 +1,133 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "collision_common.h" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void init_collision_request(py::module& m) +{ + py::module collision_detection = m.def_submodule("collision_detection"); + + py::class_(collision_detection, "CollisionRequest", R"( + Representation of a collision checking request. + )") + + .def(py::init<>()) + + .def_readwrite("joint_model_group_name", &collision_detection::CollisionRequest::group_name, + R"( + str: The group name to check collisions for (optional; if empty, assume the complete robot) + )") + + .def_readwrite("distance", &collision_detection::CollisionRequest::distance, + R"( + bool: If true, compute proximity distance. + )") + + .def_readwrite("cost", &collision_detection::CollisionRequest::cost, + R"( + bool: If true, a collision cost is computed. + )") + + .def_readwrite("contacts", &collision_detection::CollisionRequest::contacts, + R"( + bool: If true, compute contacts. + )") + + .def_readwrite("max_contacts", &collision_detection::CollisionRequest::max_contacts, + R"( + int: Overall maximum number of contacts to compute. + )") + + .def_readwrite("max_contacts_per_pair", &collision_detection::CollisionRequest::max_contacts_per_pair, + R"( + int: Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at different configurations). + )") + + .def_readwrite("max_cost_sources", &collision_detection::CollisionRequest::max_cost_sources, + R"( + int: When costs are computed, this value defines how many of the top cost sources should be returned. + )") + + // TODO (peterdavidfagan): define is_done as function call. + //.def_readwrite("is_done", &collision_detection::CollisionRequest::is_done, + // R"( + // )") + + .def_readwrite("verbose", &collision_detection::CollisionRequest::verbose, + R"( + bool: Flag indicating whether information about detected collisions should be reported. + )"); +} + +void init_collision_result(py::module& m) +{ + py::module collision_detection = m.def_submodule("collision_detection"); + + py::class_(collision_detection, "CollisionResult", R"( + Representation of a collision checking result. + )") + .def_readwrite("collision", &collision_detection::CollisionResult::collision, + R"( + bool: True if collision was found, false otherwise. + )") + + .def_readwrite("distance", &collision_detection::CollisionResult::distance, + R"( + float: Closest distance between two bodies. + )") + + .def_readwrite("contact_count", &collision_detection::CollisionResult::contact_count, + R"( + int: Number of contacts returned. + )") + + // TODO (peterdavidfagan): define binding and test for ContactMap. + .def_readwrite("contacts", &collision_detection::CollisionResult::contacts, + R"( + dict: A dict returning the pairs of ids of the bodies in contact, plus information about the contacts themselves. + )") + + .def_readwrite("cost_sources", &collision_detection::CollisionResult::cost_sources, + R"( + dict: The individual cost sources from computed costs. + )"); +} +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h new file mode 100644 index 0000000000..f3f8866cb0 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void init_collision_request(py::module& m); +void init_collision_result(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp new file mode 100644 index 0000000000..371c33a206 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -0,0 +1,111 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "collision_matrix.h" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +std::pair +get_entry(const std::shared_ptr& acm, const std::string& name1, + const std::string& name2) +{ + // check acm for collision + collision_detection::AllowedCollision::Type type; + bool collision_allowed = acm->getEntry(name1, name2, type); + + // should return a tuple true/false and the allowed collision type + std::pair result = std::make_pair(collision_allowed, type); + return result; +} + +void init_acm(py::module& m) +{ + py::module collision_detection = m.def_submodule("collision_detection"); + + py::class_>( + collision_detection, "AllowedCollisionMatrix", + R"( + Definition of a structure for the allowed collision matrix. All elements in the collision world are referred to by their names. This class represents which collisions are allowed to happen and which are not. + )") + .def(py::init&, bool>(), + R"( + Initialize the allowed collision matrix using a list of names of collision objects. + Args: + names (list of str): A list of names of the objects in the collision world (corresponding to object IDs in the collision world). + allowed (bool): If false, indicates that collisions between all elements must be checked for and no collisions will be ignored. + )", + py::arg("names"), py::arg("default_entry") = false) + + .def("get_entry", &moveit_py::bind_collision_detection::get_entry, + R"( + Get the allowed collision entry for a pair of objects. + Args: + name1 (str): The name of the first object. + name2 (str): The name of the second object. + Returns: + (bool, str): Whether the collision is allowed and the type of allowed collision. + )", + py::arg("name1"), py::arg("name2")) + + .def("set_entry", + py::overload_cast( + &collision_detection::AllowedCollisionMatrix::setEntry), + py::arg("name1"), py::arg("name2"), py::arg("allowed"), + R"(Set the allowed collision state between two objects. + Args: + name1 (str): The name of the first object. + name2 (str): The name of the second object. + allowed (bool): If true, indicates that the collision between the two objects is allowed. If false, indicates that the collision between the two objects is not allowed. + )") + + .def("remove_entry", + py::overload_cast( + &collision_detection::AllowedCollisionMatrix::removeEntry), + py::arg("name1"), py::arg("name2"), + R"(Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in the collision matrix. + Args: + name1 (str): The name of the first object. + name2 (str): The name of the second object. + ")) + + .def("clear", &collision_detection::AllowedCollisionMatrix::clear, R"(Clear the allowed collision matrix.)"); +} +// getEntry + +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h new file mode 100644 index 0000000000..653096679d --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.h @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void init_acm(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp new file mode 100644 index 0000000000..c98489363f --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Jafar Uruç + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Uruç */ + +#include "world.h" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void init_world(py::module& m) +{ + py::module collision_detection = m.def_submodule("collision_detection"); + + py::class_(m, "World").def(py::init<>()); +} +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.h b/moveit_py/src/moveit/moveit_core/collision_detection/world.h new file mode 100644 index 0000000000..6cec992791 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.h @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Jafar Uruç + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Uruç */ + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void init_world(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp new file mode 100644 index 0000000000..0fe70b43eb --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "controller_manager.h" + +namespace moveit_py +{ +namespace bind_controller_manager +{ +void init_execution_status(py::module& m) +{ + py::module controller_manager = m.def_submodule("controller_manager"); + + py::class_>( + controller_manager, "ExecutionStatus", R"( Execution status of planned robot trajectory. )") + .def_property_readonly("status", &moveit_controller_manager::ExecutionStatus::asString); +} +} // namespace bind_controller_manager +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h new file mode 100644 index 0000000000..f1df3681a7 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.h @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_controller_manager +{ +void init_execution_status(py::module& m); +} +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp new file mode 100644 index 0000000000..b227128341 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -0,0 +1,159 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "utils.h" +#include +#include +#include + +namespace moveit_py +{ +namespace bind_kinematic_constraints +{ +moveit_msgs::msg::Constraints construct_link_constraint(const std::string& link_name, const std::string& source_frame, + std::optional> cartesian_position, + std::optional cartesian_position_tolerance, + std::optional> orientation, + std::optional orientation_tolerance) +{ + // check that link cartesian and/or orientation constraints are specified + if (!cartesian_position && !orientation) + { + throw std::invalid_argument("No link cartesian or orientation constraints specified"); + } + + moveit_msgs::msg::Constraints constraints_cpp; + + // merge constraints if necessary + if (cartesian_position && orientation) + { + // define point stamped message + geometry_msgs::msg::PointStamped point; + point.header.frame_id = source_frame; + point.point.x = cartesian_position.value()[0]; + point.point.y = cartesian_position.value()[1]; + point.point.z = cartesian_position.value()[2]; + + moveit_msgs::msg::Constraints position_constraints = + kinematic_constraints::constructGoalConstraints(link_name, point, cartesian_position_tolerance.value()); + + // define quaternion message + geometry_msgs::msg::QuaternionStamped quaternion; + quaternion.header.frame_id = source_frame; + quaternion.quaternion.x = orientation.value()[0]; + quaternion.quaternion.y = orientation.value()[1]; + quaternion.quaternion.z = orientation.value()[2]; + quaternion.quaternion.w = orientation.value()[3]; + + moveit_msgs::msg::Constraints orientation_constraints = + kinematic_constraints::constructGoalConstraints(link_name, quaternion, orientation_tolerance.value()); + + constraints_cpp = kinematic_constraints::mergeConstraints(position_constraints, orientation_constraints); + } + + // generate cartesian constraint + else if (cartesian_position) + { + // define point stamped message + geometry_msgs::msg::PointStamped point; + point.header.frame_id = source_frame; + point.point.x = cartesian_position.value()[0]; + point.point.y = cartesian_position.value()[1]; + point.point.z = cartesian_position.value()[2]; + + // instantiate logger + auto logger = rclcpp::get_logger("moveit_py"); + // check point with logger + RCLCPP_DEBUG(rclcpp::get_logger("moveit_py"), "Point: %f, %f, %f", point.point.x, point.point.y, point.point.z); + + constraints_cpp = kinematic_constraints::constructGoalConstraints(link_name, point, *cartesian_position_tolerance); + } + + // generate orientation constraint + else + { + // define quaternion message + geometry_msgs::msg::QuaternionStamped quaternion; + quaternion.header.frame_id = source_frame; + quaternion.quaternion.x = orientation.value()[0]; + quaternion.quaternion.y = orientation.value()[1]; + quaternion.quaternion.z = orientation.value()[2]; + quaternion.quaternion.w = orientation.value()[3]; + constraints_cpp = + kinematic_constraints::constructGoalConstraints(link_name, quaternion, orientation_tolerance.value()); + } + + return constraints_cpp; +} + +moveit_msgs::msg::Constraints construct_joint_constraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance) +{ + // generate joint constraint message + moveit_msgs::msg::Constraints joint_constraints = + kinematic_constraints::constructGoalConstraints(robot_state, joint_model_group, tolerance); + + return joint_constraints; +} + +moveit_msgs::msg::Constraints construct_constraints_from_node(const std::shared_ptr& node_name, + const std::string& ns) +{ + // construct constraint message + moveit_msgs::msg::Constraints constraints_cpp; + kinematic_constraints::constructConstraints(node_name, ns, constraints_cpp); + + return constraints_cpp; +} + +void init_kinematic_constraints(py::module& m) +{ + py::module kinematic_constraints = m.def_submodule("kinematic_constraints"); + + kinematic_constraints.def("construct_link_constraint", &construct_link_constraint, py::arg("link_name"), + py::arg("source_frame"), py::arg("cartesian_position") = nullptr, + py::arg("cartesian_position_tolerance") = nullptr, py::arg("orientation") = nullptr, + py::arg("orientation_tolerance") = nullptr, "Construct a link constraint message"); + kinematic_constraints.def("construct_joint_constraint", &construct_joint_constraint, py::arg("robot_state"), + py::arg("joint_model_group"), py::arg("tolerance") = 0.01, + "Construct a joint constraint message"); + kinematic_constraints.def("construct_constraints_from_node", &construct_constraints_from_node, py::arg("node_name"), + py::arg("ns"), "Construct a constraint message from a node"); +} + +} // namespace bind_kinematic_constraints +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h new file mode 100644 index 0000000000..d27238d4a4 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.h @@ -0,0 +1,66 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_kinematic_constraints +{ +moveit_msgs::msg::Constraints construct_link_constraint(const std::string& link_name, const std::string& source_frame, + std::optional> cartesian_position, + std::optional cartesian_position_tolerance, + std::optional> orientation, + std::optional orientation_tolerance); + +moveit_msgs::msg::Constraints construct_joint_constraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance); + +moveit_msgs::msg::Constraints construct_constraints_from_node(const std::shared_ptr& node_name, + const std::string& ns); + +void init_kinematic_constraints(py::module& m); + +} // namespace bind_kinematic_constraints +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp new file mode 100644 index 0000000000..481f8a4eab --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -0,0 +1,103 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "planning_response.h" + +namespace moveit_py +{ +namespace bind_planning_interface +{ +std::shared_ptr +get_motion_plan_response_trajectory(std::shared_ptr& response) +{ + return response->trajectory; +} + +moveit_msgs::msg::RobotState +get_motion_plan_response_start_state(std::shared_ptr& response) +{ + moveit_msgs::msg::RobotState robot_state_msg = response->start_state; + return robot_state_msg; +} + +moveit_msgs::msg::MoveItErrorCodes +get_motion_plan_response_error_code(std::shared_ptr& response) +{ + moveit_msgs::msg::MoveItErrorCodes error_code_msg = + static_cast(response->error_code); + return error_code_msg; +} + +double get_motion_plan_response_planning_time(std::shared_ptr& response) +{ + return response->planning_time; +} + +std::string get_motion_plan_response_planner_id(std::shared_ptr& response) +{ + return response->planner_id; +} + +void init_motion_plan_response(py::module& m) +{ + py::module planning_interface = m.def_submodule("planning_interface"); + + py::class_>( + planning_interface, "MotionPlanResponse", R"()") + + //.def(py::init<>(), R"()") + + .def_property("trajectory", &moveit_py::bind_planning_interface::get_motion_plan_response_trajectory, nullptr, + py::return_value_policy::copy, R"()") + + .def_readonly("planning_time", &planning_interface::MotionPlanResponse::planning_time, + py::return_value_policy::copy, R"()") + + .def_property("error_code", &moveit_py::bind_planning_interface::get_motion_plan_response_error_code, nullptr, + py::return_value_policy::copy, R"()") + + .def_property("start_state", &moveit_py::bind_planning_interface::get_motion_plan_response_start_state, nullptr, + py::return_value_policy::copy, R"()") + + .def_readonly("planner_id", &planning_interface::MotionPlanResponse::planner_id, py::return_value_policy::copy, + R"()") + + .def("__bool__", [](std::shared_ptr& response) { + return response->error_code.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + }); +} +} // namespace bind_planning_interface +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h new file mode 100644 index 0000000000..ebb408ab70 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.h @@ -0,0 +1,68 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_planning_interface +{ +std::shared_ptr +get_motion_plan_response_trajectory(std::shared_ptr& response); + +moveit_msgs::msg::RobotState +get_motion_plan_response_start_state(std::shared_ptr& response); + +moveit_msgs::msg::MoveItErrorCodes +get_motion_plan_response_error_code(std::shared_ptr& response); + +double get_motion_plan_response_planning_time(std::shared_ptr& response); + +std::string get_motion_plan_response_planner_id(std::shared_ptr& response); + +void init_motion_plan_response(py::module& m); +} // namespace bind_planning_interface +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp new file mode 100644 index 0000000000..f483938c64 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -0,0 +1,404 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "planning_scene.h" +#include +#include + +namespace moveit_py +{ +namespace bind_planning_scene +{ +void apply_collision_object(std::shared_ptr& planning_scene, + moveit_msgs::msg::CollisionObject& collision_object_msg, + std::optional color_msg) +{ + // apply collision object + planning_scene->processCollisionObjectMsg(collision_object_msg); + + // check if color message is provided + if (color_msg.has_value()) + { + // set object color + planning_scene->setObjectColor(color_msg.value().id, color_msg.value().color); + } +} + +Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene, + const std::string& id) +{ + auto transformation = planning_scene->getFrameTransform(id); + return transformation.matrix(); +} + +moveit_msgs::msg::PlanningScene get_planning_scene_msg(std::shared_ptr& planning_scene) +{ + moveit_msgs::msg::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + return planning_scene_msg; +} + +void init_planning_scene(py::module& m) +{ + py::module planning_scene = m.def_submodule("planning_scene"); + + py::class_>(planning_scene, + "PlanningScene", + R"( + Representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained. + )") + + .def(py::init(), + py::arg("robot_model"), py::arg("world") = std::make_shared()) + // properties + .def_property("name", &planning_scene::PlanningScene::getName, &planning_scene::PlanningScene::setName, + R"( + str: The name of the planning scene. + )") + + .def_property("robot_model", &planning_scene::PlanningScene::getRobotModel, nullptr, + py::return_value_policy::move, + R"( + :py:class:`moveit_py.core.RobotModel`: The robot model associated to this planning scene. + )") + + .def_property("planning_frame", &planning_scene::PlanningScene::getPlanningFrame, nullptr, + py::return_value_policy::move, + R"( + str: The frame in which planning is performed. + )") + + .def_property("current_state", &planning_scene::PlanningScene::getCurrentState, + py::overload_cast(&planning_scene::PlanningScene::setCurrentState), + py::return_value_policy::reference_internal, + R"( + :py:class:`moveit_py.core.RobotState`: The current state of the robot. + )") + + .def_property("planning_scene_message", &moveit_py::bind_planning_scene::get_planning_scene_msg, nullptr, + py::return_value_policy::move) + + .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr) + + // methods + .def("__copy__", + [](const planning_scene::PlanningScene* self) { + return planning_scene::PlanningScene::clone(self->shared_from_this()); + }) + .def("__deepcopy__", + [](const planning_scene::PlanningScene* self, py::dict /* memo */) { + return planning_scene::PlanningScene::clone(self->shared_from_this()); + }) + .def("knows_frame_transform", + py::overload_cast( + &planning_scene::PlanningScene::knowsFrameTransform, py::const_), + py::arg("robot_state"), py::arg("frame_id"), + R"( + Check if a transform to the frame id is known. + This will be known if id is a link name, an attached body id or a collision object. + Args: + robot_state (:py:class:`moveit_py.core.RobotState`): The robot state to check. + frame_id (str): The frame id to check. + Returns: + bool: True if the transform is known, false otherwise. + )") + + .def("knows_frame_transform", + py::overload_cast(&planning_scene::PlanningScene::knowsFrameTransform, py::const_), + py::arg("frame_id"), + R"( + Check if a transform to the frame id is known. + This will be known if id is a link name, an attached body id or a collision object. + Args: + frame_id (str): The frame id to check. + Returns: + bool: True if the transform is known, false otherwise. + )") + + .def("get_frame_transform", &moveit_py::bind_planning_scene::get_frame_transform, py::arg("frame_id"), + R"( + Get the transform corresponding to the frame id. + This will be known if id is a link name, an attached body id or a collision object. Return identity when no transform is available. + Args: + frame_id (str): The frame id to get the transform for. + Returns: + :py:class:`numpy.ndarray`: The transform corresponding to the frame id. + )") + + // writing to the planning scene + .def("process_planning_scene_world", &planning_scene::PlanningScene::processPlanningSceneWorldMsg, py::arg("msg"), + R"( + Process a planning scene world message. + Args: + msg (:py:class:`moveit_msgs.msg.PlanningSceneWorld`): The planning scene world message. + )") + + .def("apply_collision_object", &moveit_py::bind_planning_scene::apply_collision_object, + py::arg("collision_object_msg"), py::arg("color_msg") = nullptr, + R"( + Apply a collision object to the planning scene. + Args: + object (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. + color (moveit_msgs.msg.ObjectColor, optional): The color of the collision object. Defaults to None if not specified. + )") + + .def("set_object_color", &planning_scene::PlanningScene::setObjectColor, py::arg("object_id"), + py::arg("color_msg"), R"( + Set the color of a collision object. + Args: + object_id (str): The id of the collision object to set the color of. + color (std_msgs.msg.ObjectColor): The color of the collision object. + )") + + .def("process_attached_collision_object", &planning_scene::PlanningScene::processAttachedCollisionObjectMsg, + py::arg("object"), + R"( + Apply an attached collision object to the planning scene. + Args: + object (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene. + )") + + .def("process_octomap", + py::overload_cast(&planning_scene::PlanningScene::processOctomapMsg), + py::arg("msg"), + R"( + Apply an octomap to the planning scene. + Args: + octomap (moveit_msgs.msg.Octomap): The octomap to apply to the planning scene. + )") + + .def("remove_all_collision_objects", &planning_scene::PlanningScene::removeAllCollisionObjects, + R"( + Removes collision objects from the planning scene. + This method will remove all collision object from the scene except for attached collision objects. + )") + + // checking state validity + .def("is_state_valid", + py::overload_cast( + &planning_scene::PlanningScene::isStateValid, py::const_), + py::arg("robot_state"), py::arg("joint_model_group_name"), py::arg("verbose") = false) + .def("is_state_colliding", + py::overload_cast(&planning_scene::PlanningScene::isStateColliding), + py::arg("joint_model_group_name"), py::arg("verbose") = false, + R"( + Check if the robot state is in collision. + Args: + joint_model_group_name (str): The name of the group to check collision for. + verbose (bool): If true, print the link names of the links in collision. + Returns: + bool: True if the robot state is in collision, false otherwise. + )") + + .def("is_state_colliding", + py::overload_cast( + &planning_scene::PlanningScene::isStateColliding, py::const_), + py::arg("robot_state"), py::arg("joint_model_group_name"), py::arg("verbose") = false, + R"( + Check if the robot state is in collision. + Args: + robot_state (:py:class:`moveit_py.core.RobotState`): The robot state to check collision for. + joint_model_group_name (str): The name of the group to check collision for. + verbose (bool): If true, print the link names of the links in collision. + Returns: + bool: True if the robot state is in collision, false otherwise. + )") + + .def("is_state_constrained", + py::overload_cast( + &planning_scene::PlanningScene::isStateConstrained, py::const_), + py::arg("state"), py::arg("constraints"), py::arg("verbose") = false, + R"( + Check if the robot state fulfills the passed constraints + Args: + state (moveit_py.core.RobotState): The robot state to check constraints for. + constraints (moveit_msgs.msg.Constraints): The constraints to check. + verbose (bool): + Returns: + bool: true if state is constrained otherwise false. + )") + + .def("is_path_valid", + py::overload_cast*>(&planning_scene::PlanningScene::isPathValid, py::const_), + py::arg("trajectory"), py::arg("joint_model_group_name"), py::arg("verbose") = false, + py::arg("invalid_index") = nullptr, + R"( + Check if a given path is valid. + Each state is checked for validity (collision avoidance and feasibility) + Args: + trajectory (:py:class:`moveit_py.core.RobotTrajectory`): The trajectory to check. + joint_model_group_name (str): The joint model group to check the path against. + verbose (bool): + invalid_index (list): + Returns: + bool: true if the path is valid otherwise false. + )") + + // TODO (peterdavidfagan): remove collision result from input parameters and write separate binding code. + // TODO (peterdavidfagan): consider merging check_collision and check_collision_unpadded into one function with unpadded_param + .def("check_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkCollision), + py::arg("collision_request"), py::arg("collision_result"), + R"( + Check whether the current state is in collision, and if needed, updates the collision transforms of the current state before the computation. + Args: + collision_request (:py:class:`moveit_py.core.CollisionRequest`): The collision request to use. + collision_result (:py:class:`moveit_py.core.CollisionResult`): The collision result to update + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_collision", + py::overload_cast(&planning_scene::PlanningScene::checkCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + state (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + state (): + acm (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_collision_unpadded", + py::overload_cast( + &planning_scene::PlanningScene::checkCollisionUnpadded), + py::arg("req"), py::arg("result"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_collision_unpadded", + py::overload_cast(&planning_scene::PlanningScene::checkCollisionUnpadded, + py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + state (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_collision_unpadded", + py::overload_cast( + &planning_scene::PlanningScene::checkCollisionUnpadded, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + state (): + acm (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_self_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkSelfCollision), + py::arg("collision_request"), py::arg("collision_result"), + R"( + Check if the robot state is in collision. + Args: + collision_request (): + collision_result (): + Returns: + bool: true if state is in collision otherwise false. + )") + + .def("check_self_collision", + py::overload_cast(&planning_scene::PlanningScene::checkSelfCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), + R"( + Check if the robot state is in collision. + Args: + collision request (): + collision_result (): + state (): + Returns: + bool: true if state is in self collision otherwise false. + )") + + .def("check_self_collision", + py::overload_cast( + &planning_scene::PlanningScene::checkSelfCollision, py::const_), + py::arg("collision_request"), py::arg("collision_result"), py::arg("state"), py::arg("acm"), + R"( + Check if the robot state is in collision. + Args: + collision request (): + collision_result (): + state (): + acm(): + Returns: + bool: true if state is in self collision otherwise false. + )"); +} +} // namespace bind_planning_scene +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h new file mode 100644 index 0000000000..b9ec922278 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.h @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_planning_scene +{ +void apply_collision_object(std::shared_ptr& planning_scene, + moveit_msgs::msg::CollisionObject& collision_object_msg, + std::optional color_msg); + +Eigen::MatrixXd get_frame_transform(std::shared_ptr& planning_scene, + const std::string& id); + +moveit_msgs::msg::PlanningScene get_planning_scene_msg(std::shared_ptr& planning_scene); + +void init_planning_scene(py::module& m); +} // namespace bind_planning_scene +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp new file mode 100644 index 0000000000..49d85bd79b --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp @@ -0,0 +1,62 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Jafar Uruç + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Uruç */ + +#include "joint_model.h" +#include + +namespace moveit_py +{ +namespace bind_robot_model +{ + +void init_joint_model(py::module& m) +{ + py::class_>(m, "VariableBounds") + .def_readonly("min_position", &moveit::core::VariableBounds::min_position_) + .def_readonly("max_position", &moveit::core::VariableBounds::max_position_) + .def_readonly("position_bounded", &moveit::core::VariableBounds::position_bounded_) + .def_readonly("min_velocity", &moveit::core::VariableBounds::min_velocity_) + .def_readonly("max_velocity", &moveit::core::VariableBounds::max_velocity_) + .def_readonly("velocity_bounded", &moveit::core::VariableBounds::velocity_bounded_) + .def_readonly("min_acceleration", &moveit::core::VariableBounds::min_acceleration_) + .def_readonly("max_acceleration", &moveit::core::VariableBounds::max_acceleration_) + .def_readonly("acceleration_bounded", &moveit::core::VariableBounds::acceleration_bounded_) + .def_readonly("min_jerk", &moveit::core::VariableBounds::min_jerk_) + .def_readonly("max_jerk", &moveit::core::VariableBounds::max_jerk_) + .def_readonly("jerk_bounded", &moveit::core::VariableBounds::jerk_bounded_); +} +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h new file mode 100644 index 0000000000..a2a70ef6d5 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.h @@ -0,0 +1,52 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Jafar Uruç + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Jafar Uruç */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_robot_model +{ +void init_joint_model(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp new file mode 100644 index 0000000000..45c3c0608f --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -0,0 +1,73 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "joint_model_group.h" + +namespace moveit_py +{ +namespace bind_robot_model +{ +bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, + const double margin) +{ + assert(joint_positions.size() == jmg->getActiveVariableCount()); + return jmg->satisfiesPositionBounds(joint_positions.data(), margin); +} + +void init_joint_model_group(py::module& m) +{ + py::class_(m, "JointModelGroup", + R"( + Representation of a group of joints that are part of a robot model. + )") + + .def_property("name", &moveit::core::JointModelGroup::getName, nullptr, + R"( + str: The name of the joint model group. + )") + + .def_property("joint_model_names", &moveit::core::JointModelGroup::getJointModelNames, nullptr, + R"( + list[str]: The names of the joint models in the group. + )") + .def_property("active_joint_model_names", &moveit::core::JointModelGroup::getActiveJointModelNames, nullptr) + .def_property("active_joint_model_bounds", &moveit::core::JointModelGroup::getActiveJointModelsBounds, nullptr, + py::return_value_policy::reference_internal) + .def("satisfies_position_bounds", &moveit_py::bind_robot_model::satisfies_position_bounds, py::arg("values"), + py::arg("margin") = 0.0); +} +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_core/robot_model/src/pyrobot_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h similarity index 59% rename from moveit_core/robot_model/src/pyrobot_model.cpp rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h index 73d8d23f21..a86358802b 100644 --- a/moveit_core/robot_model/src/pyrobot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2021, Peter Mitrano + * Copyright (c) 2022, Peter David Fagan * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,9 +14,9 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -32,32 +32,23 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter Mitrano */ +/* Author: Peter David Fagan */ + +#pragma once #include #include +#include #include -#include -#include namespace py = pybind11; -using namespace robot_model; -void def_robot_model_bindings(py::module& m) +namespace moveit_py { - m.doc() = "Definition of a kinematic model. Not thread safe, however multiple instances can be created."; - py::class_(m, "RobotModel") - .def(py::init(), py::arg("urdf_model"), - py::arg("srdf_model")) - .def("getName", &RobotModel::getName) - .def("getLinkModelNames", &RobotModel::getLinkModelNames) - .def("getJointModelNames", &RobotModel::getJointModelNames) - // - ; - - py::class_(m, "JointModelGroup") - .def("getLinkModelNames", &JointModelGroup::getLinkModelNames) - .def("getJointModelNames", &JointModelGroup::getJointModelNames) - // - ; -} +namespace bind_robot_model +{ +bool satisfies_position_bounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, + const double margin); +void init_joint_model_group(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp new file mode 100644 index 0000000000..e689041b13 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -0,0 +1,160 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "robot_model.h" +#include +#include +#include +#include +#include + +namespace moveit_py +{ +namespace bind_robot_model +{ +void init_robot_model(py::module& m) +{ + py::module robot_model = m.def_submodule("robot_model"); + + py::class_>(robot_model, "RobotModel", + R"( + Representation of a kinematic model. + )") + // TODO (peterdavidfagan): rewrite with RobotModelLoader. + .def(py::init([](std::string& urdf_xml_path, std::string& srdf_xml_path) { + // Read in URDF + std::string xml_string; + std::fstream xml_file(urdf_xml_path.c_str(), std::fstream::in); + while (xml_file.good()) + { + std::string line; + std::getline(xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + + urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(xml_string); + + // Read in SRDF + srdf::Model srdf_model; + srdf_model.initFile(*urdf_model, srdf_xml_path); + + // Instantiate robot model + return std::make_shared( + urdf_model, std::make_shared(std::move(srdf_model))); + }), + py::arg("urdf_xml_path"), py::arg("srdf_xml_path"), + R"( + Initializes a kinematic model for a robot. + + Args: + urdf_xml_path (str): The filepath to the urdf file describing the robot. + srdf_xml_path (str): The filepath to the srdf file describing the robot semantics. + Returns: + moveit_py.core.RobotModel: The kinematic model for the robot. + )") + + .def_property("name", &moveit::core::RobotModel::getName, nullptr, + R"( + str: The name of the robot model. + )") + + .def_property("model_frame", &moveit::core::RobotModel::getModelFrame, nullptr, + R"( + str: Get the frame in which the transforms for this model are computed (when using a :py:class:`moveit_py.core.RobotState`). + This frame depends on the root joint. As such, the frame is either extracted from SRDF, or it is assumed to be the name of the root link. + )") + + // TODO (peterdavidfagan): make available when JointModel is binded + //.def_property("root_joint", &moveit::core::RobotModel::getRootJoint, nullptr, "The root joint of the robot model.") + + .def_property("root_joint_name", &moveit::core::RobotModel::getRootJointName, nullptr, + R"( + str: The name of the root joint. + )") + + .def( + "get_model_info", + [](std::shared_ptr& s) { + std::stringstream ss; + s->printModelInfo(ss); + return ss.str(); + }, + py::return_value_policy::move, + R"( + Gets a formatted string containing a summary of relevant information from the robot model. + Returns: + str: Formatted string containing generic robot model information. + )") + + // Interacting with joint model groups + .def_property("joint_model_group_names", &moveit::core::RobotModel::getJointModelGroupNames, nullptr, + R"( + list of str: The names of the joint model groups in the robot model. + )") + + .def_property("joint_model_groups", py::overload_cast<>(&moveit::core::RobotModel::getJointModelGroups), nullptr, + py::return_value_policy::reference_internal, + R"( + list of moveit_py.core.JointModelGroup: The joint model groups available in the robot model. + )") + + .def_property("end_effectors", &moveit::core::RobotModel::getEndEffectors, nullptr, + py::return_value_policy::reference_internal, + R"( + TODO + )") + + .def("has_joint_model_group", &moveit::core::RobotModel::hasJointModelGroup, py::arg("joint_model_group_name"), + R"( + Checks if a joint model group with the given name exists in the robot model. + Returns: + bool: true if joint model group exists. + )") + + .def("get_joint_model_group", + py::overload_cast(&moveit::core::RobotModel::getJointModelGroup), + py::arg("joint_model_group_name"), py::return_value_policy::reference_internal, + R"( + Gets a joint model group instance by name. + Args: + joint_model_group_name (str): The name of the joint model group to return. + Returns: + :py:class:`moveit_py.core.JointModelGroup`: joint model group instance that corresponds with joint_model_group_name parameter. + )"); +} +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h new file mode 100644 index 0000000000..dd79de0608 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.h @@ -0,0 +1,51 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_robot_model +{ +void init_robot_model(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp new file mode 100644 index 0000000000..1b14f0b394 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -0,0 +1,503 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "robot_state.h" +#include +#include +#include +#include + +namespace moveit_py +{ +namespace bind_robot_state +{ +void update(moveit::core::RobotState* self, bool force, std::string& category) +{ + if (category == "all") + { + self->update(force); + } + else if (category == "links_only") + { + self->updateLinkTransforms(); + } + else if (category == "collisions_only") + { + self->updateCollisionBodyTransforms(); + } + else + { + throw std::invalid_argument("Invalid category"); + } +} + +Eigen::MatrixXd get_frame_transform(const moveit::core::RobotState* self, std::string& frame_id) +{ + bool frame_found; + auto transformation = self->getFrameTransform(frame_id, &frame_found); + return transformation.matrix(); +} + +Eigen::MatrixXd get_global_link_transform(const moveit::core::RobotState* self, std::string& link_name) +{ + auto transformation = self->getGlobalLinkTransform(link_name); + return transformation.matrix(); +} + +geometry_msgs::msg::Pose get_pose(const moveit::core::RobotState* self, const std::string& link_name) +{ + Eigen::Isometry3d pose = self->getGlobalLinkTransform(link_name); + return tf2::toMsg(pose); +} + +std::map get_joint_positions(const moveit::core::RobotState* self) +{ + std::map joint_positions; + const std::vector& variable_name = self->getVariableNames(); + for (auto& name : variable_name) + { + joint_positions[name.c_str()] = self->getVariablePosition(name); + } + return joint_positions; +} + +void set_joint_positions(moveit::core::RobotState* self, std::map& joint_positions) +{ + for (const auto& item : joint_positions) + { + self->setVariablePosition(item.first, item.second); + } +} + +std::map get_joint_velocities(const moveit::core::RobotState* self) +{ + std::map joint_velocity; + const std::vector& variable_name = self->getVariableNames(); + for (auto& name : variable_name) + { + joint_velocity[name.c_str()] = self->getVariableVelocity(name); + } + return joint_velocity; +} + +void set_joint_velocities(moveit::core::RobotState* self, std::map& joint_velocities) +{ + for (const auto& item : joint_velocities) + { + self->setVariableVelocity(item.first, item.second); + } +} + +std::map get_joint_accelerations(const moveit::core::RobotState* self) +{ + std::map joint_acceleration; + const std::vector& variable_name = self->getVariableNames(); + for (auto& name : variable_name) + { + joint_acceleration[name.c_str()] = self->getVariableAcceleration(name); + } + return joint_acceleration; +} + +void set_joint_accelerations(moveit::core::RobotState* self, std::map& joint_accelerations) +{ + for (const auto& item : joint_accelerations) + { + self->setVariableAcceleration(item.first, item.second); + } +} + +std::map get_joint_efforts(const moveit::core::RobotState* self) +{ + std::map joint_effort; + const std::vector& variable_name = self->getVariableNames(); + for (auto& name : variable_name) + { + joint_effort[name.c_str()] = self->getVariableEffort(name); + } + return joint_effort; +} + +void set_joint_efforts(moveit::core::RobotState* self, std::map& joint_efforts) +{ + for (const auto& item : joint_efforts) + { + self->setVariableEffort(item.first, item.second); + } +} + +Eigen::VectorXd copy_joint_group_positions(const moveit::core::RobotState* self, + const std::string& joint_model_group_name) +{ + Eigen::VectorXd values; + self->copyJointGroupPositions(joint_model_group_name, values); + return values; +} + +Eigen::VectorXd copy_joint_group_velocities(const moveit::core::RobotState* self, + const std::string& joint_model_group_name) +{ + Eigen::VectorXd values; + self->copyJointGroupVelocities(joint_model_group_name, values); + return values; +} + +Eigen::VectorXd copy_joint_group_accelerations(const moveit::core::RobotState* self, + const std::string& joint_model_group_name) +{ + Eigen::VectorXd values; + self->copyJointGroupAccelerations(joint_model_group_name, values); + return values; +} + +Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const Eigen::Vector3d& reference_point_position) +{ + const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); + return self->getJacobian(joint_model_group, reference_point_position); +} + +Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, + bool use_quaternion_representation) +{ + Eigen::MatrixXd jacobian; + const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); + const moveit::core::LinkModel* link_model = self->getLinkModel(link_model_name); + self->getJacobian(joint_model_group, link_model, reference_point_position, jacobian, use_quaternion_representation); + return jacobian; +} + +bool set_to_default_values(moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& state_name) + +{ + const moveit::core::JointModelGroup* joint_model_group = self->getJointModelGroup(joint_model_group_name); + return self->setToDefaultValues(joint_model_group, state_name); +} + +void init_robot_state(py::module& m) +{ + py::module robot_state = m.def_submodule("robot_state"); + + robot_state.def( + "robotStateToRobotStateMsg", + [](const moveit::core::RobotState& state, bool copy_attached_bodies) { + moveit_msgs::msg::RobotState state_msg; + moveit::core::robotStateToRobotStateMsg(state, state_msg, copy_attached_bodies); + return state_msg; + }, + py::arg("state"), py::arg("copy_attached_bodies") = true); + + py::class_>(robot_state, "RobotState", + R"( + Representation of a robot's state. + At the lowest level, a state is a collection of variables. Each variable has a name and can have position, velocity, acceleration and effort associated to it. Effort and acceleration share the memory area for efficiency reasons (one should not set both acceleration and effort in the same state and expect things to work). Often variables correspond to joint names as well (joints with one degree of freedom have one variable), but joints with multiple degrees of freedom have more variables. Operations are allowed at variable level, joint level (see JointModel) and joint group level (see JointModelGroup). + For efficiency reasons a state computes forward kinematics in a lazy fashion. This can sometimes lead to problems if the update() function was not called on the state. + )") + + .def(py::init&>(), + R"( + Initializes robot state from a robot model. + + Args: + :py:class:`moveit_py.core.RobotModel`: The robot model associated to the instantiated robot state. + + )") + .def("__copy__", [](const moveit::core::RobotState* self) { return moveit::core::RobotState{ *self }; }) + .def("__deepcopy__", + [](const moveit::core::RobotState* self, py::dict /* memo */) { return moveit::core::RobotState{ *self }; }) + + // Get underlying robot model, frame transformations and jacobian + .def_property("robot_model", &moveit::core::RobotState::getRobotModel, nullptr, + py::return_value_policy::reference, + R"( + :py:class:`moveit_py.core.RobotModel`: The robot model instance associated to this robot state. + )") + + .def_property("dirty", &moveit::core::RobotState::dirty, nullptr, + R"( + bool: True if the robot state is dirty. + )") + + .def("get_frame_transform", &moveit_py::bind_robot_state::get_frame_transform, py::arg("frame_id"), + py::return_value_policy::move, + R"( + Get the transformation matrix from the model frame (root of model) to the frame identified by frame_id. + If frame_id was not found, frame_found is set to false and an identity transform is returned. + This method is restricted to frames defined within the robot state and doesn't include collision object present in the collision world. Please use the PlanningScene.get_frame_transform method for collision world objects. + Args: + frame_id (str): The id of the frame to get the transform for. + Returns: + :py:class:`numpy.ndarray`: The transformation matrix from the model frame to the frame identified by frame_id. + )") + + .def("get_pose", &moveit_py::bind_robot_state::get_pose, py::arg("link_name"), + R"( + Get the pose of a link that is defined in the robot model. + Args: + link_name (str): The name of the link to get the pose for. + Returns: + :py:class: `geometry_msgs.msg.Pose`: A ROS geometry message containing the pose of the link. + )") + + .def("get_jacobian", + py::overload_cast( + &moveit_py::bind_robot_state::get_jacobian), + py::arg("joint_model_group_name"), py::arg("reference_point_position"), py::return_value_policy::move, + R"( + Compute the Jacobian with reference to the last link of a specified group. + Args: + joint_model_group_name (str): The name of the joint model group to compute the Jacobian for. + reference_point_position (:py:class:`numpy.ndarray`): The position of the reference point in the link frame. + Returns: + :py:class:`numpy.ndarray`: The Jacobian of the specified group with respect to the reference point. + Raises: + Exception: If the group is not a chain. + )") + + .def("get_jacobian", + py::overload_cast(&moveit_py::bind_robot_state::get_jacobian), + py::arg("joint_model_group_name"), py::arg("link_name"), py::arg("reference_point_position"), + py::arg("use_quaternion_representation") = false, py::return_value_policy::move, + R"( + Compute the Jacobian with reference to a particular point on a given link, for a specified group. + Args: + joint_model_group_name (str): The name of the joint model group to compute the Jacobian for. + link_name (str): The name of the link model to compute the Jacobian for. + reference_point_position (:py:class:`numpy.ndarray`): The position of the reference point in the link frame. + use_quaternion_representation (bool): If true, the Jacobian will be represented using a quaternion representation, if false it defaults to euler angle representation. + Returns: + :py:class:`numpy.ndarray`: The Jacobian of the specified group with respect to the reference point. + )") + + // Get state information + .def_property("state_tree", py::overload_cast<>(&moveit::core::RobotState::getStateTreeString, py::const_), + nullptr, py::return_value_policy::move, + R"( + str: represents the state tree of the robot state. + )") + + .def_property_readonly_static( + "state_info", + [](const moveit::core::RobotState& s) { + std::stringstream ss; + s.printStateInfo(ss); + return ss.str(); + }, + py::return_value_policy::move, + R"( + str: the state information of the robot state. + )") + + // Getting and setting joint model group positions, velocities, accelerations + .def_property("joint_positions", &moveit_py::bind_robot_state::get_joint_positions, + &moveit_py::bind_robot_state::set_joint_positions, py::return_value_policy::copy) + + .def_property("joint_velocities", &moveit_py::bind_robot_state::get_joint_velocities, + &moveit_py::bind_robot_state::set_joint_velocities, py::return_value_policy::copy) + + .def_property("joint_accelerations", &moveit_py::bind_robot_state::get_joint_accelerations, + &moveit_py::bind_robot_state::set_joint_accelerations, py::return_value_policy::copy) + + .def_property("joint_efforts", &moveit_py::bind_robot_state::get_joint_efforts, + &moveit_py::bind_robot_state::set_joint_efforts, py::return_value_policy::copy) + + .def("set_joint_group_positions", + py::overload_cast( + &moveit::core::RobotState::setJointGroupPositions), + py::arg("joint_model_group_name"), py::arg("position_values"), + R"( + Sets the positions of the joints in the specified joint model group. + Args: + joint_model_group_name (str): + position_values (:py:class:`numpy.ndarray`): The positions of the joints in the joint model group. + )") + + // peterdavidfagan: I am not sure if additional function names are better than having function parameters for joint setting. + .def("set_joint_group_active_positions", + py::overload_cast( + &moveit::core::RobotState::setJointGroupActivePositions), + py::arg("joint_model_group_name"), py::arg("position_values"), + R"( + Sets the active positions of joints in the specified joint model group. + + Args: + joint_model_group_name (str): The name of the joint model group to set the active positions for. + position_values (:py:class:`numpy.ndarray`): The positions of the joints in the joint model group. + )") + + .def("get_joint_group_positions", &moveit_py::bind_robot_state::copy_joint_group_positions, + py::arg("joint_model_group_name"), + R"( + For a given group, get the position values of the variables that make up the group. + Args: + joint_model_group_name (str): The name of the joint model group to copy the positions for. + Returns: + :py:class:`numpy.ndarray`: The positions of the joints in the joint model group. + )") + + .def("set_joint_group_velocities", + py::overload_cast( + &moveit::core::RobotState::setJointGroupVelocities), + py::arg("joint_model_group_name"), py::arg("velocity_values"), + R"( + Sets the velocities of the joints in the specified joint model group. + Args: + joint_model_group_name (str): The name of the joint model group to set the velocities for. + velocity_values (:py:class:`numpy.ndarray`): The velocities of the joints in the joint model group. + )") + + .def("get_joint_group_velocities", &moveit_py::bind_robot_state::copy_joint_group_velocities, + py::arg("joint_model_group_name"), + R"( + For a given group, get the velocity values of the variables that make up the group. + + Args: + joint_model_group_name (str): The name of the joint model group to copy the velocities for. + Returns: + :py:class:`numpy.ndarray`: The velocities of the joints in the joint model group. + )") + + .def("set_joint_group_accelerations", + py::overload_cast( + &moveit::core::RobotState::setJointGroupAccelerations), + py::arg("joint_model_group_name"), py::arg("acceleration_values"), + R"( + Sets the accelerations of the joints in the specified joint model group. + + Args: + joint_model_group_name (str): The name of the joint model group to set the accelerations for. + acceleration_values (:py:class:`numpy.ndarray`): The accelerations of the joints in the joint model group. + )") + + .def("get_joint_group_accelerations", &moveit_py::bind_robot_state::copy_joint_group_accelerations, + py::arg("joint_model_group_name"), + R"( + For a given group, get the acceleration values of the variables that make up the group. + Args: + joint_model_group_name (str): The name of the joint model group to copy the accelerations for. + Returns: + :py:class:`numpy.ndarray`: The accelerations of the joints in the joint model group. + )") + + // Forward kinematics + .def("get_global_link_transform", &moveit_py::bind_robot_state::get_global_link_transform, py::arg("link_name"), + R"( + Returns the transform of the specified link in the global frame. + Args: + link_name (str): The name of the link to get the transform for. + Returns: + :py:class:`numpy.ndarray`: The transform of the specified link in the global frame. + )") + + // Setting state from inverse kinematics + .def( + "set_from_ik", + [](moveit::core::RobotState* self, const std::string& group, const geometry_msgs::msg::Pose& pose, + const std::string& tip, + double timeout) { return self->setFromIK(self->getJointModelGroup(group), pose, tip, timeout); }, + py::arg("joint_model_group_name"), py::arg("geometry_pose"), py::arg("tip_name"), py::arg("timeout") = 0.0, + R"( + Sets the state of the robot to the one that results from solving the inverse kinematics for the specified group. + Args: + joint_model_group_name (str): The name of the joint model group to set the state for. + geometry_pose (:py:class: `geometry_msgs.msg.Pose`): The pose of the end-effector to solve the inverse kinematics for. + tip_name (str): The name of the link that is the tip of the end-effector. + timeout (float): The amount of time to wait for the IK solution to be found. + )") + + // Setting entire state values + .def("set_to_default_values", py::overload_cast<>(&moveit::core::RobotState::setToDefaultValues), + R"( + Set all joints to their default positions. + The default position is 0, or if that is not within bounds then half way between min and max bound. + )") + + .def("set_to_default_values", + py::overload_cast( + &moveit::core::RobotState::setToDefaultValues), + py::arg("joint_model_group"), py::arg("name"), + R"( + Set the joints in group to the position name defined in the SRDF. + Args: + joint_model_group (:py:class:`moveit_py.core.JointModelGroup`): The joint model group to set the default values for. + name (str): The name of a predefined state which is defined in the robot model SRDF. + )") + + .def("set_to_default_values", + py::overload_cast( + &moveit_py::bind_robot_state::set_to_default_values), + py::arg("joint_model_group_name"), py::arg("name"), + R"( + Set the joints in group to the position name defined in the SRDF. + Args: + joint_model_group_name (str): The name of the joint model group to set the default values for. + name (str): The name of a predefined state which is defined in the robot model SRDF. + )") + + .def("set_to_random_positions", py::overload_cast<>(&moveit::core::RobotState::setToRandomPositions), + R"( + Set all joints to random positions within the default bounds. + )") + + .def("set_to_random_positions", + py::overload_cast(&moveit::core::RobotState::setToRandomPositions), + py::arg("joint_model_group"), + R"( + Set all joints in the joint model group to random positions within the default bounds. + Args: + joint_model_group (:py:class:`moveit_py.core.JointModelGroup`): The joint model group to set the random values for. + )") + + .def("clear_attached_bodies", py::overload_cast<>(&moveit::core::RobotState::clearAttachedBodies), + R"( + Clear all attached bodies. + + We only allow for attaching of objects via the PlanningScene instance. This method allows any attached objects that are associated to this RobotState instance to be removed. + )") + + .def("update", &moveit_py::bind_robot_state::update, py::arg("force") = false, py::arg("type") = "all", + R"( + Update state transforms. + + Args: + force (bool): when true forces the update of the transforms from scratch. + category (str): specifies the category to update. All indicates updating all transforms while "links_only" and "collisions_only" ensure that only links or collision transforms are updated. )"); +} +} // namespace bind_robot_state +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h new file mode 100644 index 0000000000..01819d6918 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h @@ -0,0 +1,79 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_robot_state +{ +void update(moveit::core::RobotState* self, bool force, std::string& category); + +Eigen::MatrixXd get_frame_transform(const moveit::core::RobotState* self, std::string& frame_id); + +Eigen::MatrixXd get_global_link_transform(const moveit::core::RobotState* self, std::string& link_name); + +geometry_msgs::msg::Pose get_pose(const moveit::core::RobotState* self, const std::string& link_name); + +Eigen::VectorXd copy_joint_group_positions(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); +Eigen::VectorXd copy_joint_group_velocities(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); +Eigen::VectorXd copy_joint_group_accelerations(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); + +Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& link_model_name, const Eigen::Vector3d& reference_point_position, + bool use_quaternion_representation); + +Eigen::MatrixXd get_jacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const Eigen::Vector3d& reference_point_position); + +bool set_to_default_values(moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& state_name); + +void init_robot_state(py::module& m); +} // namespace bind_robot_state +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp new file mode 100644 index 0000000000..57d00fa68b --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -0,0 +1,131 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "robot_trajectory.h" +#include + +namespace moveit_py +{ +namespace bind_robot_trajectory +{ +moveit_msgs::msg::RobotTrajectory +get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter) +{ + moveit_msgs::msg::RobotTrajectory msg; + robot_trajectory->getRobotTrajectoryMsg(msg, joint_filter); + return msg; +} + +void init_robot_trajectory(py::module& m) +{ + py::module robot_trajectory = m.def_submodule("robot_trajectory"); + + py::class_>(robot_trajectory, + "RobotTrajectory", + R"( + Maintains a sequence of waypoints and the durations between these waypoints. + )") + + .def("__getitem__", &robot_trajectory::RobotTrajectory::getWayPoint, py::arg("idx"), + R"( + Get the waypoint at the specified index in the trajectory. + + Returns: + :py:class:`moveit_py.core.RobotState`: The robot state corresponding to a waypoint at the specified index in the trajectory. + )") + .def( + "__iter__", + [](robot_trajectory::RobotTrajectory& self) { return py::make_iterator(self.begin(), self.end()); }, + py::keep_alive<0, 1>() /* Essential: keep object alive while iterator exists */, + R"( + Iterate over the waypoints in the trajectory. + )") + + .def("__len__", &robot_trajectory::RobotTrajectory::getWayPointCount, + R"( + Returns: + int: The number of waypoints in the trajectory. + )") + + .def("__reverse__", &robot_trajectory::RobotTrajectory::reverse, + R"( + Reverse the trajectory. + )") + + .def_property("joint_model_group_name", &robot_trajectory::RobotTrajectory::getGroupName, + &robot_trajectory::RobotTrajectory::setGroupName, + R"( + str: The name of the joint model group that this trajectory is for. + )") + + .def_property("robot_model", &robot_trajectory::RobotTrajectory::getRobotModel, nullptr, + R"( + :py:class:`moveit_py.core.RobotModel`: The robot model that this trajectory is for. + )") + + .def_property("duration", &robot_trajectory::RobotTrajectory::getDuration, nullptr, + R"( + float: The duration of the trajectory. + )") + + .def_property("average_segment_duration", &robot_trajectory::RobotTrajectory::getAverageSegmentDuration, nullptr, + R"( + float: The average duration of the segments in the trajectory. + )") + + .def("unwind", py::overload_cast<>(&robot_trajectory::RobotTrajectory::unwind), + R"( + Unwind the trajectory. + )") + + .def("get_waypoint_durations", &robot_trajectory::RobotTrajectory::getWayPointDurations, + R"( + Get the durations from the previous waypoint in the trajectory. + Returns: + list of float: The duration from previous of each waypoint in the trajectory. + )") + .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::get_robot_trajectory_msg, + py::arg("joint_filter") = std::vector(), + R"( + Get the trajectory as a `moveit_msgs.msg.RobotTrajectory` message. + Returns: + moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. + )"); + // TODO (peterdavidfagan): support other methods such as appending trajectories +} +} // namespace bind_robot_trajectory +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h new file mode 100644 index 0000000000..a8bdb91eb0 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_robot_trajectory +{ +moveit_msgs::msg::RobotTrajectory +get_robot_trajectory_msg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter); + +void init_robot_trajectory(py::module& m); +} // namespace bind_robot_trajectory +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp new file mode 100644 index 0000000000..b2ab5408b3 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "transforms.h" + +namespace moveit_py +{ +namespace bind_transforms +{ +Eigen::MatrixXd get_transform(std::shared_ptr& transforms, std::string& from_frame) +{ + auto transform = transforms->getTransform(from_frame); + return transform.matrix(); +} + +std::map get_all_transforms(std::shared_ptr& transforms) +{ + std::map transforms_map; + for (auto& transform : transforms->getAllTransforms()) + { + transforms_map[transform.first] = transform.second.matrix(); + } + return transforms_map; +} + +void init_transforms(py::module& m) +{ + py::module transforms = m.def_submodule("transforms"); + + py::class_>(transforms, "Transforms", + R"(A snapshot of a transform tree.)") + + .def(py::init(), R"(Create a new Transforms object.)", py::arg("target_frame")) + + .def("get_target_frame", &moveit::core::Transforms::getTargetFrame, R"(Get the target frame.)") + + .def("get_transform", &moveit_py::bind_transforms::get_transform, py::arg("from_frame"), + R"(Get the transform for from_frame with respect to the target frame.)") + + .def("get_all_transforms", &moveit_py::bind_transforms::get_all_transforms, + R"(Get all transforms with respect to the target frame.)"); + + // TODO(peterdavidfagan): Add methods for applying transforms. +} + +} // namespace bind_transforms +} // namespace moveit_py diff --git a/moveit_core/transforms/src/pytransforms.cpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.h similarity index 50% rename from moveit_core/transforms/src/pytransforms.cpp rename to moveit_py/src/moveit/moveit_core/transforms/transforms.h index 3699147720..f286b7e1da 100644 --- a/moveit_core/transforms/src/pytransforms.cpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2021, Peter Mitrano + * Copyright (c) 2022, Peter David Fagan * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,9 +14,9 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * The name of Peter Mitrano may not be used to endorse or promote - * products derived from this software without specific prior - * written permission. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -32,37 +32,26 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter Mitrano */ +/* Author: Peter David Fagan */ + +#pragma once #include #include -#include - +#include #include namespace py = pybind11; -using namespace moveit::core; -void def_transforms_bindings(py::module& m) +namespace moveit_py +{ +namespace bind_transforms { - m.doc() = "Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming " - "different quantities. Transforms are maintained as a list of transforms to a particular frame. All stored " - "transforms are considered fixed."; - py::class_(m, "Transforms") - .def(py::init()) - .def("canTransform", &Transforms::canTransform) - .def("getTargetFrame", &Transforms::getTargetFrame) - .def("getTransform", &Transforms::getTransform) - .def("isFixedFrame", &Transforms::isFixedFrame) - .def("getAllTransforms", &Transforms::getAllTransforms) - .def("setTransform", py::overload_cast(&Transforms::setTransform)) - .def("setTransform", py::overload_cast(&Transforms::setTransform)) - .def("setTransforms", &Transforms::setTransforms) - .def("setAllTransforms", &Transforms::setAllTransforms) - .def("transformVector3", &Transforms::transformVector3) - .def("transformQuaternion", &Transforms::transformQuaternion) - .def("transformRotationMatrix", &Transforms::transformRotationMatrix) - .def("transformPose", &Transforms::transformPose) - // - ; -} +Eigen::MatrixXd get_transform(std::shared_ptr& transforms, std::string& from_frame); + +std::map get_all_transforms(std::shared_ptr& transforms); + +void init_transforms(py::module& m); + +} // namespace bind_transforms +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt new file mode 100644 index 0000000000..daa7d74f2e --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt @@ -0,0 +1,16 @@ +add_library(moveit_py_utils SHARED src/copy_ros_msg.cpp + src/ros_msg_typecasters.cpp) +target_include_directories(moveit_py_utils PUBLIC include) +set_target_properties(moveit_py_utils PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") + +ament_target_dependencies(moveit_py_utils rclcpp moveit_msgs geometry_msgs + pybind11) + +install( + TARGETS moveit_py_utils + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h new file mode 100644 index 0000000000..cf36206f76 --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h @@ -0,0 +1,82 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace moveit_py_utils +{ +geometry_msgs::msg::PoseStamped PoseStampedToCpp(const py::object& pose_stamped); + +// TODO(peterdavidfagan): consider creating typecaster +geometry_msgs::msg::Pose PoseToCpp(const py::object& pose); +py::object PoseToPy(geometry_msgs::msg::Pose pose); + +geometry_msgs::msg::Point PointToCpp(const py::object& point); + +geometry_msgs::msg::Vector3 Vector3ToCpp(const py::object& vector3); + +geometry_msgs::msg::Quaternion QuaternionToCpp(const py::object& quaternion); + +shape_msgs::msg::SolidPrimitive SolidPrimitiveToCpp(const py::object& primitive); + +shape_msgs::msg::MeshTriangle MeshTriangleToCpp(const py::object& mesh_triangle); + +shape_msgs::msg::Mesh MeshToCpp(const py::object& mesh); + +moveit_msgs::msg::BoundingVolume BoundingVolumeToCpp(const py::object& bounding_volume); + +moveit_msgs::msg::JointConstraint JointConstraintToCpp(const py::object& joint_constraint); + +moveit_msgs::msg::PositionConstraint PositionConstraintToCpp(const py::object& position_constraint); + +moveit_msgs::msg::OrientationConstraint OrientationConstraintToCpp(const py::object& orientation_constraint); + +moveit_msgs::msg::VisibilityConstraint VisibilityConstraintToCpp(const py::object& visibility_constraint); + +moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collision_object); + +moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints); +} // namespace moveit_py_utils +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h new file mode 100644 index 0000000000..da589020f8 --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h @@ -0,0 +1,136 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan, Robert Haschke + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * The name of Robert Haschke may not be use to endorse or promote + * products derived from this software without specific prior + * written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan, Robert Haschke */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace moveit_py_utils +{ +PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name); +PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const std::string& ros_msg_name); +} // namespace moveit_py_utils +} // namespace moveit_py + +namespace pybind11 +{ +namespace detail +{ +// Base class for type conversion (C++ <-> python) of ROS message types +template +struct RosMsgTypeCaster +{ + // C++ -> Python + // TODO: add error handling + static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */) + { + // serialize src + rclcpp::Serialization serializer; + rclcpp::SerializedMessage serialized_msg; + serializer.serialize_message(&src, &serialized_msg); + py::bytes bytes = py::bytes(reinterpret_cast(serialized_msg.get_rcl_serialized_message().buffer), + serialized_msg.get_rcl_serialized_message().buffer_length); + + // get Python object type + const std::string ros_msg_name = rosidl_generator_traits::name(); + + // find delimiting '/' in ros_msg_name + std::size_t pos1 = ros_msg_name.find('/'); + std::size_t pos2 = ros_msg_name.find('/', pos1 + 1); + py::module m = py::module::import((ros_msg_name.substr(0, pos1) + ".msg").c_str()); + + // retrieve type instance + py::object cls = m.attr(ros_msg_name.substr(pos2 + 1).c_str()); + + // deserialize into python object + py::module rclpy = py::module::import("rclpy.serialization"); + py::object msg = rclpy.attr("deserialize_message")(bytes, cls); + + return msg.release(); + } + + // Python -> C++ + bool load(handle src, bool /*convert*/) + { + // check datatype of src + if (!moveit_py::moveit_py_utils::convertible(src, rosidl_generator_traits::name())) + return false; + + // serialize src into python buffer + py::module rclpy = py::module::import("rclpy.serialization"); + py::bytes bytes = rclpy.attr("serialize_message")(src); + + // deserialize into C++ object + rcl_serialized_message_t rcl_serialized_msg = rmw_get_zero_initialized_serialized_message(); + char* serialized_buffer; + Py_ssize_t length; + if (PYBIND11_BYTES_AS_STRING_AND_SIZE(bytes.ptr(), &serialized_buffer, &length)) + { + throw py::error_already_set(); + } + if (length < 0) + { + throw py::error_already_set(); + } + rcl_serialized_msg.buffer_capacity = length; + rcl_serialized_msg.buffer_length = length; + rcl_serialized_msg.buffer = reinterpret_cast(serialized_buffer); + rmw_ret_t rmw_ret = + rmw_deserialize(&rcl_serialized_msg, rosidl_typesupport_cpp::get_message_type_support_handle(), &value); + if (RMW_RET_OK != rmw_ret) + { + throw std::runtime_error("failed to deserialize ROS message"); + } + return true; + } + + PYBIND11_TYPE_CASTER(T, _()); +}; + +template +struct type_caster::value>> : RosMsgTypeCaster +{ +}; + +} // namespace detail +} // namespace pybind11 diff --git a/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp new file mode 100644 index 0000000000..20f222e769 --- /dev/null +++ b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp @@ -0,0 +1,347 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include +#include + +namespace moveit_py +{ +namespace moveit_py_utils +{ +// Ros Message Copy Definitions (Note: copying faster than serialize/deserialize) + +geometry_msgs::msg::PoseStamped PoseStampedToCpp(const py::object& pose_stamped) +{ + // recreate instance in C++ using python object data + geometry_msgs::msg::PoseStamped pose_stamped_cpp; + pose_stamped_cpp.header.frame_id = pose_stamped.attr("header").attr("frame_id").cast(); + pose_stamped_cpp.pose = PoseToCpp(pose_stamped.attr("pose")); + return pose_stamped_cpp; +} + +geometry_msgs::msg::Pose PoseToCpp(const py::object& pose) +{ + // recreate instance in C++ using python object data + geometry_msgs::msg::Pose pose_cpp; + pose_cpp.orientation.w = pose.attr("orientation").attr("w").cast(); + pose_cpp.orientation.x = pose.attr("orientation").attr("x").cast(); + pose_cpp.orientation.y = pose.attr("orientation").attr("y").cast(); + pose_cpp.orientation.z = pose.attr("orientation").attr("z").cast(); + pose_cpp.position.x = pose.attr("position").attr("x").cast(); + pose_cpp.position.y = pose.attr("position").attr("y").cast(); + pose_cpp.position.z = pose.attr("position").attr("z").cast(); + + return pose_cpp; +} + +py::object PoseToPy(geometry_msgs::msg::Pose pose) +{ + // recreate instance in Python using C++ object data + py::object pose_py = py::module_::import("geometry_msgs.msg").attr("Pose")(); + + pose_py.attr("orientation").attr("w") = pose.orientation.w; + pose_py.attr("orientation").attr("x") = pose.orientation.x; + pose_py.attr("orientation").attr("y") = pose.orientation.y; + pose_py.attr("orientation").attr("z") = pose.orientation.z; + pose_py.attr("position").attr("x") = pose.position.x; + pose_py.attr("position").attr("y") = pose.position.y; + pose_py.attr("position").attr("z") = pose.position.z; + + return pose_py; +} + +geometry_msgs::msg::Point PointToCpp(const py::object& point) +{ + // recreate instance in C++ using python object data + geometry_msgs::msg::Point point_cpp; + point_cpp.x = point.attr("x").cast(); + point_cpp.y = point.attr("y").cast(); + point_cpp.z = point.attr("z").cast(); + + return point_cpp; +} + +geometry_msgs::msg::Vector3 Vector3ToCpp(const py::object& vector3) +{ + // recreate instance in C++ using python object data + geometry_msgs::msg::Vector3 vector3_cpp; + vector3_cpp.x = vector3.attr("x").cast(); + vector3_cpp.y = vector3.attr("y").cast(); + vector3_cpp.z = vector3.attr("z").cast(); + + return vector3_cpp; +} + +geometry_msgs::msg::Quaternion QuaternionToCpp(const py::object& quaternion) +{ + // recreate instance in C++ using python object data + geometry_msgs::msg::Quaternion quaternion_cpp; + quaternion_cpp.w = quaternion.attr("w").cast(); + quaternion_cpp.x = quaternion.attr("x").cast(); + quaternion_cpp.y = quaternion.attr("y").cast(); + quaternion_cpp.z = quaternion.attr("z").cast(); + + return quaternion_cpp; +} + +shape_msgs::msg::SolidPrimitive SolidPrimitiveToCpp(const py::object& primitive) +{ + // recreate instance in C++ using python object data + shape_msgs::msg::SolidPrimitive primitive_cpp; + primitive_cpp.type = primitive.attr("type").cast(); + for (auto& dimension : primitive.attr("dimensions")) + { + primitive_cpp.dimensions.push_back(py::reinterpret_borrow(dimension).cast()); + } + + return primitive_cpp; +} + +shape_msgs::msg::MeshTriangle MeshTriangleToCpp(const py::object& mesh_triangle) +{ + // recreate instance in C++ using python object data + shape_msgs::msg::MeshTriangle mesh_triangle_cpp; + mesh_triangle_cpp.vertex_indices[0] = mesh_triangle.attr("vertex_indices").attr("__getitem__")(0).cast(); + mesh_triangle_cpp.vertex_indices[1] = mesh_triangle.attr("vertex_indices").attr("__getitem__")(1).cast(); + mesh_triangle_cpp.vertex_indices[2] = mesh_triangle.attr("vertex_indices").attr("__getitem__")(2).cast(); + + return mesh_triangle_cpp; +} + +shape_msgs::msg::Mesh MeshToCpp(const py::object& mesh) +{ + // recreate instance in C++ using python object data + shape_msgs::msg::Mesh mesh_cpp; + mesh_cpp.vertices.resize(mesh.attr("vertices").attr("__len__")().cast()); + for (const auto& vertex : mesh.attr("vertices")) + { + mesh_cpp.vertices.push_back(PointToCpp(py::reinterpret_borrow(vertex))); + } + mesh_cpp.triangles.resize(mesh.attr("triangles").attr("__len__")().cast()); + for (const auto& triangle : mesh.attr("triangles")) + { + mesh_cpp.triangles.push_back(MeshTriangleToCpp(py::reinterpret_borrow(triangle))); + } + + return mesh_cpp; +} + +moveit_msgs::msg::BoundingVolume BoundingVolumeToCpp(const py::object& bounding_volume) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::BoundingVolume bounding_volume_cpp; + + // primitives + for (const auto& primitive : bounding_volume.attr("primitives")) + { + bounding_volume_cpp.primitives.push_back(SolidPrimitiveToCpp(py::reinterpret_borrow(primitive))); + } + + // primitive poses + for (const auto& primitive_pose : bounding_volume.attr("primitive_poses")) + { + bounding_volume_cpp.primitive_poses.push_back(PoseToCpp(py::reinterpret_borrow(primitive_pose))); + } + + // meshes + for (const auto& mesh : bounding_volume.attr("meshes")) + { + bounding_volume_cpp.meshes.push_back(MeshToCpp(py::reinterpret_borrow(mesh))); + } + + // mesh poses + for (const auto& mesh_poses : bounding_volume.attr("mesh_poses")) + { + bounding_volume_cpp.mesh_poses.push_back(PoseToCpp(py::reinterpret_borrow(mesh_poses))); + } + + return bounding_volume_cpp; +} + +moveit_msgs::msg::JointConstraint JointConstraintToCpp(const py::object& joint_constraint) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::JointConstraint joint_constraint_cpp; + joint_constraint_cpp.joint_name = joint_constraint.attr("joint_name").cast(); + joint_constraint_cpp.position = joint_constraint.attr("position").cast(); + joint_constraint_cpp.tolerance_above = joint_constraint.attr("tolerance_above").cast(); + joint_constraint_cpp.tolerance_below = joint_constraint.attr("tolerance_below").cast(); + joint_constraint_cpp.weight = joint_constraint.attr("weight").cast(); + + return joint_constraint_cpp; +} + +moveit_msgs::msg::PositionConstraint PositionConstraintToCpp(const py::object& position_constraint) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::PositionConstraint position_constraint_cpp; + position_constraint_cpp.header.frame_id = position_constraint.attr("header").attr("frame_id").cast(); + position_constraint_cpp.link_name = position_constraint.attr("link_name").cast(); + position_constraint_cpp.target_point_offset = Vector3ToCpp(position_constraint.attr("target_point_offset")); + position_constraint_cpp.constraint_region = BoundingVolumeToCpp(position_constraint.attr("constraint_region")); + position_constraint_cpp.weight = position_constraint.attr("weight").cast(); + + return position_constraint_cpp; +} + +moveit_msgs::msg::OrientationConstraint OrientationConstraintToCpp(const py::object& orientation_constraint) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::OrientationConstraint orientation_constraint_cpp; + orientation_constraint_cpp.header.frame_id = + orientation_constraint.attr("header").attr("frame_id").cast(); + orientation_constraint_cpp.link_name = orientation_constraint.attr("link_name").cast(); + orientation_constraint_cpp.orientation = QuaternionToCpp(orientation_constraint.attr("target_quaternion")); + orientation_constraint_cpp.absolute_x_axis_tolerance = + orientation_constraint.attr("absolute_x_axis_tolerance").cast(); + orientation_constraint_cpp.absolute_y_axis_tolerance = + orientation_constraint.attr("absolute_y_axis_tolerance").cast(); + orientation_constraint_cpp.absolute_z_axis_tolerance = + orientation_constraint.attr("absolute_z_axis_tolerance").cast(); + orientation_constraint_cpp.parameterization = orientation_constraint.attr("parameterization").cast(); + orientation_constraint_cpp.weight = orientation_constraint.attr("weight").cast(); + + return orientation_constraint_cpp; +} + +moveit_msgs::msg::VisibilityConstraint VisibilityConstraintToCpp(const py::object& visibility_constraint) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::VisibilityConstraint visibility_constraint_cpp; + visibility_constraint_cpp.target_radius = visibility_constraint.attr("target_radius").cast(); + visibility_constraint_cpp.target_pose = PoseStampedToCpp(visibility_constraint.attr("target_pose")); + visibility_constraint_cpp.cone_sides = visibility_constraint.attr("cone_sides").cast(); + visibility_constraint_cpp.sensor_pose = PoseStampedToCpp(visibility_constraint.attr("sensor_pose")); + visibility_constraint_cpp.max_view_angle = visibility_constraint.attr("max_view_angle").cast(); + visibility_constraint_cpp.max_range_angle = visibility_constraint.attr("max_range_angle").cast(); + visibility_constraint_cpp.sensor_view_direction = visibility_constraint.attr("sensor_view_direction").cast(); + visibility_constraint_cpp.weight = visibility_constraint.attr("weight").cast(); + + return visibility_constraint_cpp; +} + +moveit_msgs::msg::CollisionObject CollisionObjectToCpp(const py::object& collision_object) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::CollisionObject collision_object_cpp; + + // header + collision_object_cpp.header.frame_id = collision_object.attr("header").attr("frame_id").cast(); + + // object pose + collision_object_cpp.pose = PoseToCpp(collision_object.attr("pose")); + + // object id + collision_object_cpp.id = collision_object.attr("id").cast(); + + // object type + collision_object_cpp.type.key = collision_object.attr("type").attr("key").cast(); + collision_object_cpp.type.db = collision_object.attr("type").attr("db").cast(); + + // iterate through python list creating C++ vector of primitives + for (const auto& primitive : collision_object.attr("primitives")) + { + auto primitive_cpp = SolidPrimitiveToCpp(py::reinterpret_borrow(primitive)); + collision_object_cpp.primitives.push_back(primitive_cpp); + } + + // iterate through python list creating C++ vector of primitive poses + for (const auto& primitive_pose : collision_object.attr("primitive_poses")) + { + auto primitive_pose_cpp = PoseToCpp(py::reinterpret_borrow(primitive_pose)); + collision_object_cpp.primitive_poses.push_back(primitive_pose_cpp); + } + + // iterate through python list creating C++ vector of meshes + for (const auto& mesh : collision_object.attr("meshes")) + { + // TODO (peterdavidfagan): implement mesh conversion + auto mesh_cpp = MeshToCpp(py::reinterpret_borrow(mesh)); + collision_object_cpp.meshes.push_back(mesh_cpp); + } + + // iterate through python list creating C++ vector of mesh poses + for (const auto& mesh_pose : collision_object.attr("mesh_poses")) + { + auto mesh_pose_cpp = PoseToCpp(py::reinterpret_borrow(mesh_pose)); + collision_object_cpp.mesh_poses.push_back(mesh_pose_cpp); + } + + // operation + collision_object_cpp.operation = collision_object.attr("operation").cast(); + + return collision_object_cpp; +} + +moveit_msgs::msg::Constraints ConstraintsToCpp(const py::object& constraints) +{ + // recreate instance in C++ using python object data + moveit_msgs::msg::Constraints constraints_cpp; + + // iterate through python list creating C++ vector of joint constraints + for (const auto& joint_constraint : constraints.attr("joint_constraints")) + { + auto joint_constraint_cpp = JointConstraintToCpp(py::reinterpret_borrow(joint_constraint)); + constraints_cpp.joint_constraints.push_back(joint_constraint_cpp); + } + + // iterate through python list creating C++ vector of position constraints + for (const auto& position_constraint : constraints.attr("position_constraints")) + { + auto position_constraint_cpp = PositionConstraintToCpp(py::reinterpret_borrow(position_constraint)); + constraints_cpp.position_constraints.push_back(position_constraint_cpp); + } + + // iterate through python list creating C++ vector of orientation constraints + for (const auto& orientation_constraint : constraints.attr("orientation_constraints")) + { + auto orientation_constraint_cpp = + OrientationConstraintToCpp(py::reinterpret_borrow(orientation_constraint)); + constraints_cpp.orientation_constraints.push_back(orientation_constraint_cpp); + } + + // iterate through python list creating C++ vector of visibility constraints + for (const auto& visibility_constraint : constraints.attr("visibility_constraints")) + { + auto visibility_constraint_cpp = + VisibilityConstraintToCpp(py::reinterpret_borrow(visibility_constraint)); + constraints_cpp.visibility_constraints.push_back(visibility_constraint_cpp); + } + + return constraints_cpp; +} +} // namespace moveit_py_utils +} // namespace moveit_py diff --git a/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp b/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp similarity index 76% rename from moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp rename to moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp index c94c57f85b..138d4f600a 100644 --- a/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp +++ b/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2021, Robert Haschke + * Copyright (c) 2022, Peter David Fagan, Robert Haschke * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. - * * The name of Robert Haschke may not be used to endorse or promote + * * The name of Robert Haschke may not be use to endorse or promote * products derived from this software without specific prior * written permission. * @@ -32,14 +32,14 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* Author: Peter David Fagan, Robert Haschke */ -#include +#include namespace py = pybind11; -namespace moveit +namespace moveit_py { -namespace python +namespace moveit_py_utils { py::object createMessage(const std::string& ros_msg_name) { @@ -53,17 +53,13 @@ py::object createMessage(const std::string& ros_msg_name) return cls(); } -bool convertible(const pybind11::handle& h, const char* ros_msg_name) +bool convertible(const pybind11::handle& h, const std::string& ros_msg_name) { - try - { - PyObject* o = h.attr("_type").ptr(); - return py::cast(o) == ros_msg_name; - } - catch (const std::exception& e) - { - return false; - } + PyObject* o = h.attr("__class__").attr("__name__").ptr(); + std::size_t pos = ros_msg_name.find_last_of('/'); + std::string class_name = ros_msg_name.substr(pos + 1); + return py::cast(o) == class_name; } -} // namespace python -} // namespace moveit + +} // namespace moveit_py_utils +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp new file mode 100644 index 0000000000..420f102653 --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -0,0 +1,165 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "moveit_cpp.h" + +namespace moveit_py +{ +namespace bind_moveit_cpp +{ +std::shared_ptr +get_planning_component(std::shared_ptr& moveit_cpp_ptr, const std::string& planning_component) +{ + return std::make_shared(planning_component, moveit_cpp_ptr); +} + +void init_moveit_py(py::module& m) +{ + auto utils = py::module::import("moveit.utils"); + + py::class_>(m, "MoveItPy", R"( + The MoveItPy class is the main interface to the MoveIt Python API. It is a wrapper around the MoveIt C++ API. + )") + + .def(py::init([](const std::string& node_name, const std::string& launch_params_filepath, + const py::object& config_dict, bool provide_planning_service) { + static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_cpp_initializer"); + + if (!rclcpp::ok()) + { + RCLCPP_INFO(LOGGER, "Initialize rclcpp"); + rclcpp::init(0, nullptr); + } + + RCLCPP_INFO(LOGGER, "Initialize node parameters"); + rclcpp::NodeOptions node_options; + + // This section is used to load the appropriate node parameters before spinning a moveit_cpp instance + // Priority is given to parameters supplied directly via a config_dict, followed by launch parameters + // and finally no supplied parameters. + if (!config_dict.is(py::none())) + { + auto utils = py::module::import("moveit.utils"); + // TODO (peterdavidfagan): replace python method with C++ method + std::string params_filepath = + utils.attr("create_params_file_from_dict")(config_dict, "moveit_py").cast(); + RCLCPP_INFO(LOGGER, "params_filepath: %s", params_filepath.c_str()); + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true) + .arguments({ "--ros-args", "--params-file", params_filepath }); + } + else if (!launch_params_filepath.empty()) + { + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true) + .arguments({ "--ros-args", "--params-file", launch_params_filepath }); + } + else + { + // TODO (peterdavidfagan): consider failing initialization if no params file is provided + node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true); + } + RCLCPP_INFO(LOGGER, "Initialize node and executor"); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, "", node_options); + std::shared_ptr executor = + std::make_shared(); + + RCLCPP_INFO(LOGGER, "Spin separate thread"); + auto spin_node = [node, executor]() { + executor->add_node(node); + executor->spin(); + }; + std::thread execution_thread(spin_node); + execution_thread.detach(); + + auto custom_deleter = [executor](moveit_cpp::MoveItCpp* moveit_cpp) { + executor->cancel(); + rclcpp::shutdown(); + delete moveit_cpp; + }; + + std::shared_ptr moveit_cpp_ptr(new moveit_cpp::MoveItCpp(node), custom_deleter); + + if (provide_planning_service) + { + moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService(); + }; + + return moveit_cpp_ptr; + }), + py::arg("node_name") = "moveit_py", + py::arg("launch_params_filepath") = utils.attr("get_launch_params_filepath")().cast(), + py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true, + py::return_value_policy::take_ownership, + R"( + Initialize moveit_cpp node and the planning scene service. + )") + .def("execute", + py::overload_cast&>( + &moveit_cpp::MoveItCpp::execute), + py::arg("robot_trajectory"), py::arg("blocking") = true, py::arg("controllers"), + R"( + Execute a trajectory (planning group is inferred from robot trajectory object). + )") + .def("get_planning_component", &moveit_py::bind_moveit_cpp::get_planning_component, + py::arg("planning_component_name"), py::return_value_policy::take_ownership, + R"( + Creates a planning component instance. + Args: + planning_component_name (str): The name of the planning component. + Returns: + :py:class:`moveit_py.planning.PlanningComponent`: A planning component instance corresponding to the provided plan component name. + )") + + .def( + "shutdown", [](std::shared_ptr& /*moveit_cpp*/) { rclcpp::shutdown(); }, + R"( + Shutdown the moveit_cpp node. + )") + + .def("get_planning_scene_monitor", &moveit_cpp::MoveItCpp::getPlanningSceneMonitor, + py::return_value_policy::reference, + R"( + Returns the planning scene monitor. + )") + + .def("get_robot_model", &moveit_cpp::MoveItCpp::getRobotModel, py::return_value_policy::reference, + R"( + Returns robot model. + )"); +} +} // namespace bind_moveit_cpp +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h new file mode 100644 index 0000000000..4e95652267 --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.h @@ -0,0 +1,60 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "planning_component.h" + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_moveit_cpp +{ +void init_moveit_py(py::module& m); +} // namespace bind_moveit_cpp +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp new file mode 100644 index 0000000000..de43e609f2 --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -0,0 +1,360 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "planning_component.h" +#include + +namespace moveit_py +{ +namespace bind_planning_component +{ +planning_interface::MotionPlanResponse +plan(std::shared_ptr& planning_component, + std::shared_ptr& single_plan_parameters, + std::shared_ptr& multi_plan_parameters, + std::optional solution_selection_callback, + std::optional stopping_criterion_callback) +{ + // parameter argument checking + if (single_plan_parameters && multi_plan_parameters) + { + throw std::invalid_argument("Cannot specify both single and multi plan parameters"); + } + + // check whether single or multi pipeline + if (single_plan_parameters) + { + // cast parameters + std::shared_ptr const_single_plan_parameters = + std::const_pointer_cast(single_plan_parameters); + + return planning_component->plan(*const_single_plan_parameters); + } + else if (multi_plan_parameters) + { + // cast parameters + std::shared_ptr const_multi_plan_parameters = + std::const_pointer_cast( + multi_plan_parameters); + + if (solution_selection_callback && stopping_criterion_callback) + { + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback), + *stopping_criterion_callback); + } + else if (solution_selection_callback) + { + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_callback)); + } + else if (stopping_criterion_callback) + { + return planning_component->plan(*const_multi_plan_parameters, moveit_cpp::getShortestSolution, + *stopping_criterion_callback); + } + else + { + return planning_component->plan(*const_multi_plan_parameters); + } + } + else + { + return planning_component->plan(); + } +} + +bool set_goal(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state, + std::optional pose_stamped_msg, std::optional pose_link, + std::optional> motion_plan_constraints) +{ + // check that no more than one argument is specified + if (configuration_name && robot_state) + { + throw std::invalid_argument("Cannot specify both configuration name and robot state"); + } + else if (configuration_name && pose_stamped_msg) + { + throw std::invalid_argument("Cannot specify both configuration name and pose msg"); + } + else if (configuration_name && motion_plan_constraints) + { + throw std::invalid_argument("Cannot specify both configuration name and motion plan constraints"); + } + else if (robot_state && pose_stamped_msg) + { + throw std::invalid_argument("Cannot specify both robot state and pose msg"); + } + else if (robot_state && motion_plan_constraints) + { + throw std::invalid_argument("Cannot specify both robot state and motion plan constraints"); + } + else if (pose_stamped_msg && motion_plan_constraints) + { + throw std::invalid_argument("Cannot specify both pose goal and motion plan constraints"); + } + else if ((pose_stamped_msg && !pose_link) || (!pose_stamped_msg && pose_link)) + { + throw std::invalid_argument("Must specify both message and corresponding link"); + } + + // check that at least one argument is specified + if (!configuration_name && !robot_state && !pose_stamped_msg && !pose_link && !motion_plan_constraints) + { + throw std::invalid_argument("Must specify at least one argument"); + } + + // 1. set goal from configuration name + if (configuration_name) + { + return planning_component->setGoal(*configuration_name); + } + // 2. set goal from robot_state + else if (robot_state) + { + return planning_component->setGoal(*robot_state); + } + // 3. set goal from pose_goal + else if (pose_stamped_msg && pose_link) + { + return planning_component->setGoal(*pose_stamped_msg, *pose_link); + } + // 4. set goal from motion_plan_constraints + else + { + return planning_component->setGoal(*motion_plan_constraints); + } +} + +bool set_start_state(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state) +{ + // check that no more than one argument is specified + if (configuration_name && robot_state) + { + throw std::invalid_argument("Cannot specify both configuration name and robot state"); + } + + // check that at least one argument is specified + if (!configuration_name && !robot_state) + { + throw std::invalid_argument("Must specify at least one argument"); + } + + // 1. set start state from configuration name + if (configuration_name) + { + return planning_component->setStartState(*configuration_name); + } + // 2. set start state from robot_state + else + { + return planning_component->setStartState(*robot_state); + } +} + +void init_plan_request_parameters(py::module& m) +{ + py::class_>(m, "PlanRequestParameters", + R"( + Planner parameters provided with a MotionPlanRequest. + )") + .def(py::init([](std::shared_ptr& moveit_cpp, const std::string& ns) { + const rclcpp::Node::SharedPtr& node = moveit_cpp->getNode(); + moveit_cpp::PlanningComponent::PlanRequestParameters params; + params.load(node, ns); + return params; + })) + .def_readwrite("planner_id", &moveit_cpp::PlanningComponent::PlanRequestParameters::planner_id, + R"( + str: The planner id to use. + )") + .def_readwrite("planning_pipeline", &moveit_cpp::PlanningComponent::PlanRequestParameters::planning_pipeline, + R"( + str: The planning pipeline to use. + )") + .def_readwrite("planning_attempts", &moveit_cpp::PlanningComponent::PlanRequestParameters::planning_attempts, + R"( + int: The number of planning attempts to make. + )") + .def_readwrite("planning_time", &moveit_cpp::PlanningComponent::PlanRequestParameters::planning_time, + R"( + float: The amount of time to spend planning. + )") + .def_readwrite("max_velocity_scaling_factor", + &moveit_cpp::PlanningComponent::PlanRequestParameters::max_velocity_scaling_factor, + R"( + float: The maximum velocity scaling factor that can be used. + )") + .def_readwrite("max_acceleration_scaling_factor", + &moveit_cpp::PlanningComponent::PlanRequestParameters::max_acceleration_scaling_factor, + R"( + float: The maximum scaling factor that can be used. + )"); +} + +void init_multi_plan_request_parameters(py::module& m) +{ + py::class_>( + m, "MultiPipelinePlanRequestParameters", + R"( + Planner parameters provided with a MotionPlanRequest. + )") + .def(py::init([](std::shared_ptr& moveit_cpp, + const std::vector& planning_pipeline_names) { + const rclcpp::Node::SharedPtr& node = moveit_cpp->getNode(); + moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters params{ node, planning_pipeline_names }; + return params; + })) + .def_readonly("multi_plan_request_parameters", + &moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters::multi_plan_request_parameters); +} +void init_planning_component(py::module& m) +{ + py::class_>(m, "PlanningComponent", + R"( + Represents a joint model group and motion plans corresponding to this joint model group. + )") + + .def(py::init&>(), + py::arg("joint_model_group_name"), py::arg("moveit_py_instance"), + R"( + Constructs a PlanningComponent instance. + Args: + joint_model_group_name (str): The name of the joint model group to plan for. + moveit_py_instance (:py:class:`moveit_py.core.MoveItPy`): The MoveItPy instance to use. + )") + + .def_property("planning_group_name", &moveit_cpp::PlanningComponent::getPlanningGroupName, nullptr, + R"( + str: The name of the planning group to plan for. + )") + + .def_property("named_target_states", &moveit_cpp::PlanningComponent::getNamedTargetStates, nullptr, + R"( + list of str: The names of the named robot states available as targets. + )") + + // TODO (peterdavidfagan): write test case for this method. + .def_property("named_target_state_values", &moveit_cpp::PlanningComponent::getNamedTargetStateValues, nullptr, + py::return_value_policy::move, + R"( + dict: The joint values for targets specified by name. + )") + + // start state methods + .def("set_start_state_to_current_state", &moveit_cpp::PlanningComponent::setStartStateToCurrentState, + R"( + Set the start state of the plan to the current state of the robot. + )") + + .def("set_start_state", &moveit_py::bind_planning_component::set_start_state, + py::arg("configuration_name") = nullptr, py::arg("robot_state") = nullptr, + R"( + Set the start state of the plan to the given robot state. + Args: + configuration_name (str): The name of the configuration to use as the start state. + robot_state (:py:class:`moveit_msgs.msg.RobotState`): The robot state to use as the start state. + )") + + .def("get_start_state", &moveit_cpp::PlanningComponent::getStartState, + py::return_value_policy::reference_internal, + R"( + Returns the current start state for the planning component. + )") + + // goal state methods + .def("set_goal_state", + py::overload_cast&, std::optional, + std::optional, std::optional, + std::optional, std::optional>>( + &moveit_py::bind_planning_component::set_goal), + py::arg("configuration_name") = nullptr, py::arg("robot_state") = nullptr, + py::arg("pose_stamped_msg") = nullptr, py::arg("pose_link") = nullptr, + py::arg("motion_plan_constraints") = nullptr, + R"( + Set the goal state for the planning component. + Args: + configuration_name (str): The name of the configuration to set the goal to. + robot_state (moveit_py.core.RobotState): The state to set the goal to. + pose_stamped_msg (geometry_msgs.msg.PoseStamped): A PoseStamped ros message. + pose_link (str): The name of the link for which the pose constraint is specified. + motion_plan_constraints (list): The motion plan constraints to set the goal to. + )") + + // plan/execution methods + + // TODO (peterdavidfagan): improve the plan API + .def("plan", &moveit_py::bind_planning_component::plan, py::arg("single_plan_parameters") = nullptr, + py::arg("multi_plan_parameters") = nullptr, py::arg("solution_selection_callback") = nullptr, + py::arg("stopping_criterion_callback") = nullptr, py::return_value_policy::move, + R"( + Plan a motion plan using the current start and goal states. + Args: + plan_parameters (moveit_py.core.PlanParameters): The parameters to use for planning. + )") + + .def("set_path_constraints", &moveit_cpp::PlanningComponent::setPathConstraints, py::arg("path_constraints"), + py::return_value_policy::move, + R"( + Set the path constraints generated from a moveit msg Constraints. + Args: + path_constraints (moveit_msgs.msg.Constraints): The path constraints. + )") + + // Interacting with workspace + .def("set_workspace", &moveit_cpp::PlanningComponent::setWorkspace, py::arg("min_x"), py::arg("min_y"), + py::arg("min_z"), py::arg("max_x"), py::arg("max_y"), py::arg("max_z"), + R"( + Specify the workspace bounding box. + The box is specified in the planning frame (i.e. relative to the robot root link start position). The workspace applies only to the root joint of a mobile robot (driving base, quadrotor) and does not limit the workspace of a robot arm. + Args: + min_x (float): The minimum x value of the workspace. + min_y (float): The minimum y value of the workspace. + min_z (float): The minimum z value of the workspace. + max_x (float): The maximum x value of the workspace. + max_y (float): The maximum y value of the workspace. + max_z (float): The maximum z value of the workspace. + )") + + .def("unset_workspace", &moveit_cpp::PlanningComponent::unsetWorkspace, + R"( + Remove the workspace bounding box from planning. + )"); +} +} // namespace bind_planning_component +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h new file mode 100644 index 0000000000..44b23a73dc --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.h @@ -0,0 +1,84 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "moveit_cpp.h" +#include "../planning_scene_monitor/planning_scene_monitor.h" +#include "../../moveit_core/planning_interface/planning_response.h" + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_planning_component +{ +planning_interface::MotionPlanResponse +plan(std::shared_ptr& planning_component, + std::shared_ptr& single_plan_parameters, + std::shared_ptr& multi_plan_parameters, + std::optional solution_selection_callback, + std::optional stopping_criterion_callback); + +bool set_goal(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state, + std::optional pose_stamped_msg, std::optional pose_link, + std::optional> motion_plan_constraints); + +bool set_start_state(std::shared_ptr& planning_component, + std::optional configuration_name, + std::optional robot_state); + +void init_plan_request_parameters(py::module& m); + +void init_multi_plan_request_parameters(py::module& m); + +void init_planning_component(py::module& m); +} // namespace bind_planning_component +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp new file mode 100644 index 0000000000..c18e57732a --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -0,0 +1,191 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "planning_scene_monitor.h" + +namespace moveit_py +{ +namespace bind_planning_scene_monitor +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_py.bind_planning_scene_monitor"); + +LockedPlanningSceneContextManagerRO +read_only(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +{ + return LockedPlanningSceneContextManagerRO(planning_scene_monitor); +}; + +LockedPlanningSceneContextManagerRW +read_write(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +{ + return LockedPlanningSceneContextManagerRW(planning_scene_monitor); +}; + +const planning_scene::PlanningSceneConstPtr& LockedPlanningSceneContextManagerRO::lockedPlanningSceneRoEnter() const +{ + return static_cast(planning_scene_monitor_.get()) + ->getPlanningScene(); +} + +const planning_scene::PlanningScenePtr& LockedPlanningSceneContextManagerRW::lockedPlanningSceneRwEnter() +{ + return planning_scene_monitor_->getPlanningScene(); +} + +void LockedPlanningSceneContextManagerRO::lockedPlanningSceneRoExit(const py::object& /*type*/, + const py::object& /*value*/, + const py::object& /*traceback*/) +{ + ls_ro_.reset(); +} + +void LockedPlanningSceneContextManagerRW::lockedPlanningSceneRwExit(const py::object& /*type*/, + const py::object& /*value*/, + const py::object& /*traceback*/) +{ + ls_rw_.reset(); +} + +// TODO: simplify with typecaster +void apply_planning_scene(std::shared_ptr& planning_scene_monitor, + const moveit_msgs::msg::PlanningScene& planning_scene) +{ + // lock planning scene + { + planning_scene_monitor::LockedPlanningSceneRW scene(planning_scene_monitor); + scene->usePlanningSceneMsg(planning_scene); + } +} + +void init_planning_scene_monitor(py::module& m) +{ + py::class_( + m, "PlanningSceneMonitor", R"( + Maintains the internal state of the planning scene. + )") + + .def_property("name", &planning_scene_monitor::PlanningSceneMonitor::getName, nullptr, + R"( + str: The name of this planning scene monitor. + )") + + .def("start_scene_monitor", &planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor, + R"( + Starts the scene monitor. + )") + + .def("stop_scene_monitor", &planning_scene_monitor::PlanningSceneMonitor::stopSceneMonitor, + R"( + Stops the scene monitor. + )") + + .def("start_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::startStateMonitor, + R"( + Starts the state monitor. + )") + + .def("stop_state_monitor", &planning_scene_monitor::PlanningSceneMonitor::stopStateMonitor, + R"( + Stops the state monitor. + )") + + .def("wait_for_current_robot_state", &planning_scene_monitor::PlanningSceneMonitor::waitForCurrentRobotState, + R"( + Waits for the current robot state to be received. + )") + + .def("clear_octomap", &planning_scene_monitor::PlanningSceneMonitor::clearOctomap, + R"( + Clears the octomap. + )") + + .def("read_only", &moveit_py::bind_planning_scene_monitor::read_only, + R"( + Returns a read-only context manager for the planning scene. + )") + + .def("read_write", &moveit_py::bind_planning_scene_monitor::read_write, + R"( + Returns a read-write context manager for the planning scene. + )"); +} + +void init_context_managers(py::module& m) +{ + // In Python we lock the planning scene using a with statement as this allows us to have control over resources. + // To this end each of the below manager classes binds special methods __enter__ and __exit__. + // LockedPlanningSceneContextManagerRO + py::class_( + m, "LockedPlanningSceneContextManagerRO", R"( + A context manager that locks the planning scene for reading. + )") + + .def("__enter__", + &moveit_py::bind_planning_scene_monitor::LockedPlanningSceneContextManagerRO::lockedPlanningSceneRoEnter, + R"( + Special method that is used with the with statement, provides access to a locked plannning scene instance. + Returns: + :py:class:`moveit_py.core.PlanningScene`: The locked planning scene. + )") + .def("__exit__", + &moveit_py::bind_planning_scene_monitor::LockedPlanningSceneContextManagerRO::lockedPlanningSceneRoExit, + R"( + Special method that is used with the with statement, releases the lock on the planning scene. + )"); + + // LockedPlanningSceneContextManagerRW + py::class_( + m, "LockedPlanningSceneContextManagerRW", R"( + A context manager that locks the planning scene for reading and writing. + )") + + .def("__enter__", + &moveit_py::bind_planning_scene_monitor::LockedPlanningSceneContextManagerRW::lockedPlanningSceneRwEnter, + py::return_value_policy::take_ownership, + R"( + Special method that is used with the with statement, provides access to a locked plannning scene instance. + Returns: + :py:class:`moveit_py.core.PlanningScene`: The locked planning scene. + )") + + .def("__exit__", + &moveit_py::bind_planning_scene_monitor::LockedPlanningSceneContextManagerRW::lockedPlanningSceneRwExit, + R"( + Special method that is used with the with statement, releases the lock on the planning scene. + )"); +} +} // namespace bind_planning_scene_monitor +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h new file mode 100644 index 0000000000..15f6a29d1b --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.h @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_planning_scene_monitor +{ +class LockedPlanningSceneContextManagerRW +{ +public: + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + std::unique_ptr ls_rw_; + + LockedPlanningSceneContextManagerRW(const planning_scene_monitor::PlanningSceneMonitorPtr& psm) + : planning_scene_monitor_(psm) + { + ls_rw_ = std::make_unique(planning_scene_monitor_); + } + + const planning_scene::PlanningScenePtr& lockedPlanningSceneRwEnter(); + + void lockedPlanningSceneRwExit(const py::object& type, const py::object& value, const py::object& traceback); +}; + +class LockedPlanningSceneContextManagerRO +{ +public: + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + std::unique_ptr ls_ro_; + + LockedPlanningSceneContextManagerRO(const planning_scene_monitor::PlanningSceneMonitorPtr& psm) + : planning_scene_monitor_(psm) + { + ls_ro_ = std::make_unique(planning_scene_monitor_); + } + + const planning_scene::PlanningSceneConstPtr& lockedPlanningSceneRoEnter() const; + + void lockedPlanningSceneRoExit(const py::object& type, const py::object& value, const py::object& traceback); +}; + +LockedPlanningSceneContextManagerRW +read_write(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + +LockedPlanningSceneContextManagerRO +read_only(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + +// const planning_scene::PlanningScenePtr& locked_planning_scene_enter_(LockedPlanningSceneContextManager& context_manager); + +// void locked_planning_scene_exit_(LockedPlanningSceneContextManager& context_manager, const py::object& type, +// const py::object& value, const py::object& traceback); + +void apply_planning_scene(std::shared_ptr& planning_scene_monitor, + const moveit_msgs::msg::PlanningScene& planning_scene); + +void init_planning_scene_monitor(py::module& m); + +void init_context_managers(py::module& m); +} // namespace bind_planning_scene_monitor +} // namespace moveit_py diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp new file mode 100644 index 0000000000..93e21b1624 --- /dev/null +++ b/moveit_py/src/moveit/planning.cpp @@ -0,0 +1,56 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Peter David Fagan */ + +#include "moveit_ros/moveit_cpp/moveit_cpp.h" +#include "moveit_ros/moveit_cpp/planning_component.h" +#include "moveit_ros/planning_scene_monitor/planning_scene_monitor.h" + +PYBIND11_MODULE(planning, m) +{ + m.doc() = "Python bindings for moveit_cpp functionalities."; + + // Provide custom function signatures + py::options options; + options.disable_function_signatures(); + + // Construct module classes + moveit_py::bind_planning_component::init_plan_request_parameters(m); + moveit_py::bind_planning_component::init_multi_plan_request_parameters(m); + moveit_py::bind_planning_component::init_planning_component(m); + moveit_py::bind_planning_scene_monitor::init_planning_scene_monitor(m); + moveit_py::bind_planning_scene_monitor::init_context_managers(m); + moveit_py::bind_moveit_cpp::init_moveit_py(m); +} diff --git a/moveit_py/test/__init__.py b/moveit_py/test/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/test/integration/__init__.py b/moveit_py/test/integration/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/test/unit/__init__.py b/moveit_py/test/unit/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/test/unit/fixtures/panda.srdf b/moveit_py/test/unit/fixtures/panda.srdf new file mode 100644 index 0000000000..6433749dde --- /dev/null +++ b/moveit_py/test/unit/fixtures/panda.srdf @@ -0,0 +1,120 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_py/test/unit/fixtures/panda.urdf b/moveit_py/test/unit/fixtures/panda.urdf new file mode 100644 index 0000000000..09c5c7d48c --- /dev/null +++ b/moveit_py/test/unit/fixtures/panda.urdf @@ -0,0 +1,224 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_py/test/unit/test_robot_model.py b/moveit_py/test/unit/test_robot_model.py new file mode 100644 index 0000000000..9cad84dc94 --- /dev/null +++ b/moveit_py/test/unit/test_robot_model.py @@ -0,0 +1,87 @@ +import unittest + +from moveit_py.core import JointModelGroup, RobotModel + +# TODO (peterdavidfagan): depend on moveit_resources package directly. +# (https://github.com/peterdavidfagan/moveit2/blob/moveit_py/moveit_core/utils/src/robot_model_test_utils.cpp) +class TestRobotModel(unittest.TestCase): + def test_initialization(self): + """ + Test that the RobotModel can be initialized with xml filepaths. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertIsInstance(robot, RobotModel) + + def test_name_property(self): + """ + Test that the RobotModel name property returns the correct name. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertEqual(robot.name, "panda") + + def test_model_frame_property(self): + """ + Test that the RobotModel model_frame property returns the correct name. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertEqual(robot.model_frame, "world") + + def test_root_joint_name_property(self): + """ + Test that the RobotModel root_link property returns the correct name. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertEqual(robot.root_joint_name, "virtual_joint") + + def test_joint_model_group_names_property(self): + """ + Test that the RobotModel joint_model_group_names property returns the correct names. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertCountEqual( + robot.joint_model_group_names, ["panda_arm", "hand", "panda_arm_hand"] + ) + + def test_joint_model_groups_property(self): + """ + Test that the RobotModel joint_model_groups returns a list of JointModelGroups. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertIsInstance(robot.joint_model_groups[0], JointModelGroup) + + def test_has_joint_model_group(self): + """ + Test that the RobotModel has_joint_model_group returns True for existing groups. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + self.assertTrue(robot.has_joint_model_group("panda_arm")) + self.assertFalse(robot.has_joint_model_group("The answer is 42.")) + + def test_get_joint_model_group(self): + """ + Test that the RobotModel get_joint_model_group returns the correct group. + """ + robot = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + jmg = robot.get_joint_model_group("panda_arm") + self.assertIsInstance(jmg, JointModelGroup) + self.assertEqual(jmg.name, "panda_arm") + + +if __name__ == "__main__": + unittest.main() diff --git a/moveit_py/test/unit/test_robot_state.py b/moveit_py/test/unit/test_robot_state.py new file mode 100644 index 0000000000..d4ccd2fe02 --- /dev/null +++ b/moveit_py/test/unit/test_robot_state.py @@ -0,0 +1,176 @@ +import unittest +import numpy as np + +from geometry_msgs.msg import Pose + +from moveit_py.core import RobotState, RobotModel + + +class TestRobotState(unittest.TestCase): + def test_initialization(self): + """ + Test that RobotState can be initialized with a RobotModel + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + self.assertIsInstance(robot_state, RobotState) + + def test_robot_model_property(self): + """ + Test that the robot_model property returns the correct RobotModel + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + self.assertEqual(robot_state.robot_model, robot_model) + + def test_get_frame_transform(self): + """ + Test that the frame transform is correct + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + frame_transform = robot_state.get_frame_transform("panda_link0") + + self.assertIsInstance(frame_transform, np.ndarray) + # TODO(peterdavidfagan): add assertion for particular values + + def test_get_pose(self): + """ + Test that the pose is correct + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + pose = robot_state.get_pose(link_name="panda_link8") + + self.assertIsInstance(pose, Pose) + # TODO(peterdavidfagan): add assertion for particular values + + def test_get_jacobian_1(self): + """ + Test that the jacobian is correct + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + jacobian = robot_state.get_jacobian( + joint_model_group_name="panda_arm", + reference_point_position=np.array([0.0, 0.0, 0.0]), + ) + + self.assertIsInstance(jacobian, np.ndarray) + # TODO(peterdavidfagan): add assertion for particular values + + def test_get_jacobian_2(self): + """ + Test that the jacobian is correct + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + jacobian = robot_state.get_jacobian( + joint_model_group_name="panda_arm", + link_name="panda_link6", + reference_point_position=np.array([0.0, 0.0, 0.0]), + ) + + self.assertIsInstance(jacobian, np.ndarray) + # TODO(peterdavidfagan): add assertion for particular values + + def test_set_joint_group_positions(self): + """ + Test that the joint group positions can be set + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + joint_group_positions = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + robot_state.set_joint_group_positions( + joint_model_group_name="panda_arm", position_values=joint_group_positions + ) + + self.assertEqual( + joint_group_positions.tolist(), + robot_state.copy_joint_group_positions("panda_arm").tolist(), + ) + + def test_set_joint_group_velocities(self): + """ + Test that the joint group velocities can be set + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + joint_group_velocities = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + robot_state.set_joint_group_velocities( + joint_model_group_name="panda_arm", velocity_values=joint_group_velocities + ) + + self.assertEqual( + joint_group_velocities.tolist(), + robot_state.copy_joint_group_velocities("panda_arm").tolist(), + ) + + def test_set_joint_group_accelerations(self): + """ + Test that the joint group accelerations can be set + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + joint_group_accelerations = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]) + robot_state.set_joint_group_accelerations( + joint_model_group_name="panda_arm", + acceleration_values=joint_group_accelerations, + ) + + self.assertEqual( + joint_group_accelerations.tolist(), + robot_state.copy_joint_group_accelerations("panda_arm").tolist(), + ) + + # TODO (peterdavidfagan): requires kinematics solver to be loaded + def test_set_from_ik(self): + """ + Test that the robot state can be set from an IK solution + """ + robot_model = RobotModel( + urdf_xml_path="./fixtures/panda.urdf", srdf_xml_path="./fixtures/panda.srdf" + ) + robot_state = RobotState(robot_model) + + pose = Pose() + pose.position.x = 0.5 + pose.position.y = 0.5 + pose.position.z = 0.5 + pose.orientation.w = 1.0 + + robot_state.set_from_ik( + joint_model_group_name="panda_arm", + geometry_pose=pose, + tip_name="panda_link8", + ) + + self.assertEqual(robot_state.get_pose("panda_link8"), pose) + + +if __name__ == "__main__": + unittest.main() diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 7a7d0f8045..3cbcd55a59 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -308,6 +308,7 @@ bool PlanningComponent::setStartState(const std::string& start_state_name) return false; } moveit::core::RobotState start_state(moveit_cpp_->getRobotModel()); + start_state.setToDefaultValues(); // required to ensure all joints are initialized start_state.setToDefaultValues(joint_model_group_, start_state_name); return setStartState(start_state); }