diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 304babdf19..dcb22993ff 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -64,10 +64,12 @@ 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/move_group_interface/move_group_interface.cpp src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp ) target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp + moveit_ros_planning_interface::moveit_move_group_interface moveit_ros_planning::moveit_planning_scene_monitor moveit_ros_planning::moveit_trajectory_execution_manager moveit_core::moveit_utils diff --git a/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.cpp b/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.cpp new file mode 100644 index 0000000000..4f45c84fc1 --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.cpp @@ -0,0 +1,107 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Matthew Elwin + * 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: Matthew Elwin*/ + +#include "move_group_interface.h" +#include +#include + +namespace moveit_py +{ +namespace bind_move_group_interface +{ + +void initMoveGroupInterface(py::module& m) +{ + using moveit::planning_interface::MoveGroupInterface; + auto move_group_iface = + py::class_>( + m, "MoveGroupInterface", + R"(The MoveGroupInterface class is a wrapper around the moveit::planning_interface::MoveGroupInterface C++ class. + + One difference from the C++ class is that the MoveGroupInterface creates it's own internal node + with the given name (because a python node cannot easily be converted into a C++ node)") + .def(py::init([](const std::string node_name, const std::string& group_name) { + if (!rclcpp::ok()) + { + // TODO: add ability to pass arguments to the ros initialization. + rclcpp::init(0, nullptr); + } + return std::make_shared( + std::make_shared( + node_name, rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)), + group_name); + }), + R"(Args: + node_name - The name of the node that will be used to interface with an existing move_group node. + group_name - The name of the move_group.)") + .def("setPoseTarget", + py::overload_cast( + &MoveGroupInterface::setPoseTarget), + "Set the goal pose of the end-effector link. see C++ documentation for details.", py::arg("target"), + py::arg("end_effector_link") = "") + .def( + "plan", + [](MoveGroupInterface& mgi) { + MoveGroupInterface::Plan p; + // cast the moveit::core::MoveItErrorCode type into the message type so that it can be transferred to python + const auto result = static_cast(mgi.plan(p)); + return std::make_pair(result, p); + }, + R"(Compute a motion plan from the current state to specified target. + + Unlike the C++ counterpart, this method returns the plan directly rather than taking it as an output parameter. + + Returns: + (MoveItErrorCode, Plan) containing the planning result status and the actual plan)") + .def( + "execute", + [](MoveGroupInterface& mgi, const MoveGroupInterface::Plan& plan, + const std::vector& controllers) { + return static_cast(mgi.execute(plan, controllers)); + }, + "Given a plan, execute it while waiting for completion.", py::arg("plan"), + py::arg("controllers") = std::vector()); + + py::class_(move_group_iface, "Plan") + .def(py::init<>()) + .def_readwrite("start_state", &MoveGroupInterface::Plan::start_state, + R"(The full starting state used for planning)") + .def_readwrite("trajectory", &MoveGroupInterface::Plan::trajectory, R"(The trajectory of the robot.)") + .def_readwrite("planning_time", &MoveGroupInterface::Plan::planning_time, + R"(The amount of time it took to generate the plan)"); +} +} // namespace bind_move_group_interface +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.h b/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.h new file mode 100644 index 0000000000..6e574642de --- /dev/null +++ b/moveit_py/src/moveit/moveit_ros/move_group_interface/move_group_interface.h @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Matthew Elwin + * 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: Matthew Elwin*/ + +#pragma once + +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_move_group_interface +{ +void initMoveGroupInterface(py::module& m); +} // namespace bind_move_group_interface +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 2cdfeb0356..c67f616df9 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -35,6 +35,7 @@ /* Author: Matthijs van der Burgh */ #include "trajectory_execution_manager.h" +#include namespace moveit_py { diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index 5e99f70815..de63e35d12 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -36,6 +36,7 @@ #include "moveit_ros/moveit_cpp/moveit_cpp.h" #include "moveit_ros/moveit_cpp/planning_component.h" +#include "moveit_ros/move_group_interface/move_group_interface.h" #include "moveit_ros/planning_scene_monitor/planning_scene_monitor.h" #include "moveit_ros/trajectory_execution_manager/trajectory_execution_manager.h" @@ -48,6 +49,7 @@ PYBIND11_MODULE(planning, m) options.disable_function_signatures(); // Construct module classes + moveit_py::bind_move_group_interface::initMoveGroupInterface(m); moveit_py::bind_planning_component::initPlanRequestParameters(m); moveit_py::bind_planning_component::initMultiPlanRequestParameters(m); moveit_py::bind_planning_component::initPlanningComponent(m);