From cceb1558932757765700296a39ca14b25e3a2e2a Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Tue, 20 May 2025 11:22:25 +0200 Subject: [PATCH 01/28] Add initial set of Python bindings --- .../moveit/planning_scene/planning_scene.h | 3 + .../planning_scene/src/planning_scene.cpp | 5 + moveit_py/CMakeLists.txt | 108 ++++ moveit_py/README.md | 11 + moveit_py/moveit/__init__.py | 0 .../collision_detection/collision_common.cpp | 100 ++++ .../collision_detection/collision_common.hpp | 17 + .../collision_detection/collision_matrix.cpp | 100 ++++ .../collision_detection/collision_matrix.hpp | 16 + .../moveit_core/collision_detection/world.cpp | 14 + .../moveit_core/collision_detection/world.hpp | 16 + .../controller_manager/controller_manager.cpp | 56 ++ .../controller_manager/controller_manager.hpp | 52 ++ .../kinematic_constraints/utils.cpp | 123 +++++ .../kinematic_constraints/utils.hpp | 32 ++ .../planning_interface/planning_response.cpp | 103 ++++ .../planning_interface/planning_response.hpp | 68 +++ .../planning_scene/planning_scene.cpp | 493 +++++++++++++++++ .../planning_scene/planning_scene.hpp | 62 +++ .../moveit_core/robot_model/joint_model.cpp | 64 +++ .../moveit_core/robot_model/joint_model.hpp | 52 ++ .../robot_model/joint_model_group.cpp | 86 +++ .../robot_model/joint_model_group.hpp | 54 ++ .../moveit_core/robot_model/robot_model.cpp | 160 ++++++ .../moveit_core/robot_model/robot_model.hpp | 51 ++ .../moveit_core/robot_state/robot_state.cpp | 515 ++++++++++++++++++ .../moveit_core/robot_state/robot_state.hpp | 83 +++ .../robot_trajectory/robot_trajectory.cpp | 189 +++++++ .../robot_trajectory/robot_trajectory.hpp | 61 +++ .../moveit_core/transforms/transforms.cpp | 80 +++ .../moveit_core/transforms/transforms.hpp | 57 ++ .../moveit/moveit_py_utils/CMakeLists.txt | 19 + .../moveit_py/moveit_py_utils/copy_ros_msg.h | 51 ++ .../moveit_py_utils/copy_ros_msg.hpp | 82 +++ .../moveit_py_utils/rclpy_time_typecaster.hpp | 89 +++ .../moveit_py_utils/ros_msg_typecasters.h | 51 ++ .../moveit_py_utils/ros_msg_typecasters.hpp | 136 +++++ .../moveit_py_utils/src/copy_ros_msg.cpp | 347 ++++++++++++ .../src/ros_msg_typecasters.cpp | 65 +++ .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 201 +++++++ .../moveit_ros/moveit_cpp/moveit_cpp.hpp | 60 ++ .../moveit_cpp/planning_component.cpp | 373 +++++++++++++ .../moveit_cpp/planning_component.hpp | 83 +++ .../planning_scene_monitor.cpp | 243 +++++++++ .../planning_scene_monitor.hpp | 99 ++++ .../trajectory_execution_manager.cpp | 285 ++++++++++ .../trajectory_execution_manager.hpp | 57 ++ moveit_py/package.xml | 34 ++ 48 files changed, 5106 insertions(+) create mode 100644 moveit_py/CMakeLists.txt create mode 100644 moveit_py/README.md create mode 100644 moveit_py/moveit/__init__.py create mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp create mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp create mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp create mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp create mode 100644 moveit_py/moveit/moveit_core/collision_detection/world.cpp create mode 100644 moveit_py/moveit/moveit_core/collision_detection/world.hpp create mode 100644 moveit_py/moveit/moveit_core/controller_manager/controller_manager.cpp create mode 100644 moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp create mode 100644 moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp create mode 100644 moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp create mode 100644 moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp create mode 100644 moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp create mode 100644 moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp create mode 100644 moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/joint_model.cpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/joint_model.hpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/robot_model.cpp create mode 100644 moveit_py/moveit/moveit_core/robot_model/robot_model.hpp create mode 100644 moveit_py/moveit/moveit_core/robot_state/robot_state.cpp create mode 100644 moveit_py/moveit/moveit_core/robot_state/robot_state.hpp create mode 100644 moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp create mode 100644 moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp create mode 100644 moveit_py/moveit/moveit_core/transforms/transforms.cpp create mode 100644 moveit_py/moveit/moveit_core/transforms/transforms.hpp create mode 100644 moveit_py/moveit/moveit_py_utils/CMakeLists.txt create mode 100644 moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h create mode 100644 moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp create mode 100644 moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp create mode 100644 moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h create mode 100644 moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp create mode 100644 moveit_py/moveit/moveit_py_utils/src/copy_ros_msg.cpp create mode 100644 moveit_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp create mode 100644 moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp create mode 100644 moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp create mode 100644 moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp create mode 100644 moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp create mode 100644 moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp create mode 100644 moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp create mode 100644 moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp create mode 100644 moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp create mode 100644 moveit_py/package.xml diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index de3981af86..9b56df8736 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -294,6 +294,9 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the allowed collision matrix */ collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst(); + /** \brief Set the allowed collision matrix */ + void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm); + /**@}*/ /** diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 02831c9f8f..157b667b18 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -518,6 +518,11 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM return *acm_; } +void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm) +{ + getAllowedCollisionMatrixNonConst() = acm; +} + const moveit::core::Transforms& PlanningScene::getTransforms() { // Trigger an update of the robot transforms diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt new file mode 100644 index 0000000000..02db8709a9 --- /dev/null +++ b/moveit_py/CMakeLists.txt @@ -0,0 +1,108 @@ +cmake_minimum_required(VERSION 3.22) +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) +find_package(Eigen3 REQUIRED) + +# enables using the Python extensions from the build space for testing +file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_moveit/__init__.py" "") + +add_subdirectory(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 into test_moveit folder in build space for unit tests to import + set_target_properties( + ${library_name} + PROPERTIES # Use generator expression to avoid prepending a build type + # specific directory on Windows + LIBRARY_OUTPUT_DIRECTORY + $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit> + RUNTIME_OUTPUT_DIRECTORY + $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>) + + install(TARGETS ${library_name} DESTINATION "${PYTHON_INSTALL_DIR}/moveit") +endfunction() + +pybind11_add_module( + core + # src/moveit/core.cpp + moveit/moveit_core/collision_detection/collision_common.cpp + moveit/moveit_core/collision_detection/collision_matrix.cpp + moveit/moveit_core/collision_detection/world.cpp + moveit/moveit_core/controller_manager/controller_manager.cpp + moveit/moveit_core/kinematic_constraints/utils.cpp + moveit/moveit_core/planning_interface/planning_response.cpp + moveit/moveit_core/planning_scene/planning_scene.cpp + moveit/moveit_core/transforms/transforms.cpp + moveit/moveit_core/robot_model/joint_model.cpp + moveit/moveit_core/robot_model/joint_model_group.cpp + moveit/moveit_core/robot_model/robot_model.cpp + moveit/moveit_core/robot_state/robot_state.cpp + 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 + # moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp + # moveit/moveit_ros/moveit_cpp/planning_component.cpp + 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::moveit_planning_scene_monitor + # moveit_ros_planning::moveit_trajectory_execution_manager + moveit_core::moveit_utils + rclcpp::rclcpp + moveit_py_utils) +configure_build_install_location(planning) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + # set(_pytest_tests test/unit/test_robot_model.py test/unit/test_robot_state.py) + # foreach(test_path ${_pytest_tests}) + # get_filename_component(_test_name ${test_path} NAME_WE) + # ament_add_pytest_test( + # ${_test_name} + # ${test_path} + # APPEND_ENV + # AMENT_PREFIX_INDEX=${ament_index_build_path} + # PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + # TIMEOUT + # 60 + # WORKING_DIRECTORY + # "${CMAKE_SOURCE_DIR}") + # endforeach() +endif() + +# ament_export_targets(moveit_py_utilsTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(moveit_ros_planning_interface) +ament_package() \ No newline at end of file diff --git a/moveit_py/README.md b/moveit_py/README.md new file mode 100644 index 0000000000..8961c430e4 --- /dev/null +++ b/moveit_py/README.md @@ -0,0 +1,11 @@ +## Notes on Python Bindings Modifications + +1. `planning_interface::MotionPlanResponse` does not have the `start_state` and `planner_id` attributes. I commented out the related property definitions with their setters and getters. + +2. In `planning_scene.cpp`, the method `PlanningScene::setAllowedCollisionMatrix` was not implemented in `moveit_core::planning_scene`. I copied the implementation, but since full compilation would be required, I commented out the setter method and replaced it with a read-only version. + +3. In `trajectory_tools`, newer versions include `applyTOTGTimeParameterization`, which is not available here. It applies the Time-Optimal Trajectory Generation; currently commented out, but we could copy the implementation if needed. + +4. In `planning_scene_monitor.cpp` (part of `moveit_ros`, now in `moveit_py`), `process_attached_collision_object` and `process_collision_object` were defined but are not actual methods of `PlanningSceneMonitor`—only of `PlanningScene`. Consider implementing them in `PlanningSceneMonitor` as well. + +5. In `moveit.cpp`, `moveit/utils/logger.hpp` was used, but it no longer exists. It seems only `rclcpp`'s logger is now used. I followed the logging pattern from other modules. diff --git a/moveit_py/moveit/__init__.py b/moveit_py/moveit/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp b/moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp new file mode 100644 index 0000000000..4dedd6e238 --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp @@ -0,0 +1,100 @@ +#include "collision_common.hpp" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initCollisionRequest(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 initCollisionResult(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(py::init<>()) + + .def_readwrite("dsdsfdsgds", &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/moveit/moveit_core/collision_detection/collision_common.hpp b/moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp new file mode 100644 index 0000000000..66a0b4a1e6 --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initCollisionRequest(py::module& m); +void initCollisionResult(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp new file mode 100644 index 0000000000..e2f70d1026 --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -0,0 +1,100 @@ +#include "collision_matrix.hpp" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +// TODO: Create a custom typecaster/revise the current implementation to return std::pair +std::pair getEntry(const collision_detection::AllowedCollisionMatrix& 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); + std::string type_str; + if (type == collision_detection::AllowedCollision::Type::NEVER) + { + type_str = "NEVER"; + } + else if (type == collision_detection::AllowedCollision::Type::ALWAYS) + { + type_str = "ALWAYS"; + } + else if (type == collision_detection::AllowedCollision::Type::CONDITIONAL) + { + type_str = "CONDITIONAL"; + } + else + { + type_str = "UNKNOWN"; + } + + // should return a tuple true/false and the allowed collision type + std::pair result = std::make_pair(collision_allowed, type_str); + return result; +} + +void initAcm(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::getEntry, + 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 \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp new file mode 100644 index 0000000000..87473423a8 --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initAcm(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/collision_detection/world.cpp b/moveit_py/moveit/moveit_core/collision_detection/world.cpp new file mode 100644 index 0000000000..f15b21519c --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/world.cpp @@ -0,0 +1,14 @@ +#include "world.hpp" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initWorld(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 \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/collision_detection/world.hpp b/moveit_py/moveit/moveit_core/collision_detection/world.hpp new file mode 100644 index 0000000000..85ce9e77e8 --- /dev/null +++ b/moveit_py/moveit/moveit_core/collision_detection/world.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initWorld(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/moveit/moveit_core/controller_manager/controller_manager.cpp new file mode 100644 index 0000000000..dffcb5550b --- /dev/null +++ b/moveit_py/moveit/moveit_core/controller_manager/controller_manager.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 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: Peter David Fagan */ + +#include "controller_manager.hpp" + +namespace moveit_py +{ +namespace bind_controller_manager +{ +void initExecutionStatus(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) + + .def("__bool__", [](std::shared_ptr& status) { + return static_cast(*status); + }); +} +} // namespace bind_controller_manager +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp b/moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp new file mode 100644 index 0000000000..2fa5390292 --- /dev/null +++ b/moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp @@ -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 initExecutionStatus(py::module& m); +} +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp new file mode 100644 index 0000000000..904a96853a --- /dev/null +++ b/moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -0,0 +1,123 @@ +#include "utils.hpp" +#include +#include +#include + +namespace moveit_py +{ +namespace bind_kinematic_constraints +{ +moveit_msgs::msg::Constraints constructLinkConstraint(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 constructJointConstraint(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 constructConstraintsFromNode(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 initKinematicConstraints(py::module& m) +{ + py::module kinematic_constraints = m.def_submodule("kinematic_constraints"); + + kinematic_constraints.def("construct_link_constraint", &constructLinkConstraint, 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", &constructJointConstraint, 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", &constructConstraintsFromNode, py::arg("node_name"), + py::arg("ns"), "Construct a constraint message from a node"); +} + +} // namespace bind_kinematic_constraints +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp new file mode 100644 index 0000000000..29ecf11d42 --- /dev/null +++ b/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_kinematic_constraints +{ +moveit_msgs::msg::Constraints constructLinkConstraint(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 constructJointConstraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance); + +moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr& node_name, + const std::string& ns); + +void initKinematicConstraints(py::module& m); + +} // namespace bind_kinematic_constraints +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp b/moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp new file mode 100644 index 0000000000..4e395d8224 --- /dev/null +++ b/moveit_py/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.hpp" + +namespace moveit_py +{ +namespace bind_planning_interface +{ +std::shared_ptr +getMotionPlanResponseTrajectory(std::shared_ptr& response) +{ + return response->trajectory_; +} + +// moveit_msgs::msg::RobotState +// getMotionPlanResponseStartState(std::shared_ptr& response) +// { +// moveit_msgs::msg::RobotState robot_state_msg = response->start_state; +// return robot_state_msg; +// } + +moveit_msgs::msg::MoveItErrorCodes +getMotionPlanResponseErrorCode(std::shared_ptr& response) +{ + moveit_msgs::msg::MoveItErrorCodes error_code_msg = + static_cast(response->error_code_); + return error_code_msg; +} + +double getMotionPlanResponsePlanningTime(std::shared_ptr& response) +{ + return response->planning_time_; +} + +// std::string getMotionPlanResponsePlannerId(std::shared_ptr& response) +// { +// return response->planner_id; +// } + +void initMotionPlanResponse(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::getMotionPlanResponseTrajectory, 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::getMotionPlanResponseErrorCode, nullptr, + py::return_value_policy::copy, R"()") + + // .def_property("start_state", &moveit_py::bind_planning_interface::getMotionPlanResponseStartState, 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/moveit/moveit_core/planning_interface/planning_response.hpp b/moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp new file mode 100644 index 0000000000..5173d51393 --- /dev/null +++ b/moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp @@ -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 +getMotionPlanResponseTrajectory(std::shared_ptr& response); + +// moveit_msgs::msg::RobotState +// getMotionPlanResponseStartState(std::shared_ptr& response); + +moveit_msgs::msg::MoveItErrorCodes +getMotionPlanResponseErrorCode(std::shared_ptr& response); + +double getMotionPlanResponsePlanningTime(std::shared_ptr& response); + +// std::string getMotionPlanResponsePlannerId(std::shared_ptr& response); + +void initMotionPlanResponse(py::module& m); +} // namespace bind_planning_interface +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp new file mode 100644 index 0000000000..67a184206f --- /dev/null +++ b/moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -0,0 +1,493 @@ +/********************************************************************* + * 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.hpp" +#include +#include + +#include + +namespace +{ +bool saveGeometryToFile(std::shared_ptr& planning_scene, + const std::string& file_path_and_name) +{ + std::ofstream file(file_path_and_name); + if (!file.is_open()) + { + return false; + } + planning_scene->saveGeometryToStream(file); + file.close(); + return true; +} + +bool loadGeometryFromFile(std::shared_ptr& planning_scene, + const std::string& file_path_and_name) +{ + std::ifstream file(file_path_and_name); + planning_scene->loadGeometryFromStream(file); + file.close(); + return true; +} +} // namespace + +namespace moveit_py +{ +namespace bind_planning_scene +{ +void applyCollisionObject(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 getFrameTransform(std::shared_ptr& planning_scene, const std::string& id) +{ + auto transformation = planning_scene->getFrameTransform(id); + return transformation.matrix(); +} + +moveit_msgs::msg::PlanningScene getPlanningSceneMsg(std::shared_ptr& planning_scene) +{ + moveit_msgs::msg::PlanningScene planning_scene_msg; + planning_scene->getPlanningSceneMsg(planning_scene_msg); + return planning_scene_msg; +} + +void initPlanningScene(py::module& m) +{ + py::module planning_scene = m.def_submodule("planning_scene"); + +// Remove once checkCollisionUnpadded is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + 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::getPlanningSceneMsg, nullptr, + py::return_value_policy::move) + + .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr) + .def_property_readonly("allowed_collision_matrix", + &planning_scene::PlanningScene::getAllowedCollisionMatrix, + py::return_value_policy::reference_internal) + + // .def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, + // &planning_scene::PlanningScene::setAllowedCollisionMatrix, + // py::return_value_policy::reference_internal) + // 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 */) { // NOLINT + 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::getFrameTransform, 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::applyCollisionObject, + 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. + )") + // DEPRECATED! Use check_collision instead + .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. + )") + // DEPRECATED! Use check_collision instead + .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. + )") + // DEPRECATED! Use check_collision instead + .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. + )") + + .def("save_geometry_to_file", &saveGeometryToFile, py::arg("file_path_and_name"), + R"( + Save the CollisionObjects in the PlanningScene to a file + + Args: + file_path_and_name (str): The file to save the CollisionObjects to. + + Returns: + bool: true if save to file was successful otherwise false. + )") + + .def("load_geometry_from_file", &loadGeometryFromFile, py::arg("file_path_and_name"), + R"( + Load the CollisionObjects from a file to the PlanningScene + + Args: + file_path_and_name (str): The file to load the CollisionObjects from. + + Returns: + bool: true if load from file was successful otherwise false. + )"); +#pragma GCC diagnostic pop // TODO remove once checkCollisionUnpadded is removed +} +} // namespace bind_planning_scene +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp b/moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp new file mode 100644 index 0000000000..8f18b905d8 --- /dev/null +++ b/moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp @@ -0,0 +1,62 @@ +/********************************************************************* + * 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 applyCollisionObject(std::shared_ptr& planning_scene, + moveit_msgs::msg::CollisionObject& collision_object_msg, + std::optional color_msg); + +Eigen::MatrixXd getFrameTransform(std::shared_ptr& planning_scene, const std::string& id); + +moveit_msgs::msg::PlanningScene getPlanningSceneMsg(std::shared_ptr& planning_scene); + +void initPlanningScene(py::module& m); +} // namespace bind_planning_scene +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/moveit/moveit_core/robot_model/joint_model.cpp new file mode 100644 index 0000000000..dda099e189 --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_model/joint_model.cpp @@ -0,0 +1,64 @@ +/********************************************************************* + * 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.hpp" +#include + +namespace moveit_py +{ +namespace bind_robot_model +{ + +void initJointModel(py::module& m) +{ + py::module robot_model = m.def_submodule("robot_model"); + + py::class_>(robot_model, "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/moveit/moveit_core/robot_model/joint_model.hpp b/moveit_py/moveit/moveit_core/robot_model/joint_model.hpp new file mode 100644 index 0000000000..bcd72d0171 --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_model/joint_model.hpp @@ -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 initJointModel(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp new file mode 100644 index 0000000000..5325ee4159 --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp @@ -0,0 +1,86 @@ +/********************************************************************* + * 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.hpp" +#include + +namespace moveit_py +{ +namespace bind_robot_model +{ +bool satisfiesPositionBounds(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 initJointModelGroup(py::module& m) +{ + py::module robot_model = m.def_submodule("robot_model"); + + py::class_(robot_model, "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_readonly("link_model_names", &moveit::core::JointModelGroup::getLinkModelNames) + .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_property_readonly("eef_name", + [](const moveit::core::JointModelGroup* self) { + const auto eef = self->getOnlyOneEndEffectorTip(); + if (!eef) + { + throw std::runtime_error("Error getting the end effector name - see log for details"); + } + return eef->getName(); + }) + .def("satisfies_position_bounds", &moveit_py::bind_robot_model::satisfiesPositionBounds, py::arg("values"), + py::arg("margin") = 0.0); +} +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp b/moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp new file mode 100644 index 0000000000..be54b09e6a --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * 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_robot_model +{ +bool satisfiesPositionBounds(const moveit::core::JointModelGroup* jmg, const Eigen::VectorXd& joint_positions, + const double margin); +void initJointModelGroup(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_model/robot_model.cpp b/moveit_py/moveit/moveit_core/robot_model/robot_model.cpp new file mode 100644 index 0000000000..f9cb943c6a --- /dev/null +++ b/moveit_py/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.hpp" +#include +#include +#include +#include +#include + +namespace moveit_py +{ +namespace bind_robot_model +{ +void initRobotModel(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/moveit/moveit_core/robot_model/robot_model.hpp b/moveit_py/moveit/moveit_core/robot_model/robot_model.hpp new file mode 100644 index 0000000000..9b5e8589f6 --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_model/robot_model.hpp @@ -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 initRobotModel(py::module& m); +} // namespace bind_robot_model +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/moveit/moveit_core/robot_state/robot_state.cpp new file mode 100644 index 0000000000..1a67df70f4 --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_state/robot_state.cpp @@ -0,0 +1,515 @@ +/********************************************************************* + * 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.hpp" +#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 getFrameTransform(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 getGlobalLinkTransform(const moveit::core::RobotState* self, std::string& link_name) +{ + auto transformation = self->getGlobalLinkTransform(link_name); + return transformation.matrix(); +} + +geometry_msgs::msg::Pose getPose(const moveit::core::RobotState* self, const std::string& link_name) +{ + return tf2::toMsg(self->getGlobalLinkTransform(link_name)); +} + +std::map getJointPositions(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 setJointPositions(moveit::core::RobotState* self, std::map& joint_positions) +{ + for (const auto& item : joint_positions) + { + self->setVariablePosition(item.first, item.second); + } +} + +std::map getJointVelocities(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 setJointVelocities(moveit::core::RobotState* self, std::map& joint_velocities) +{ + for (const auto& item : joint_velocities) + { + self->setVariableVelocity(item.first, item.second); + } +} + +std::map getJointAccelerations(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 setJointAccelerations(moveit::core::RobotState* self, std::map& joint_accelerations) +{ + for (const auto& item : joint_accelerations) + { + self->setVariableAcceleration(item.first, item.second); + } +} + +std::map getJointEfforts(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 setJointEfforts(moveit::core::RobotState* self, std::map& joint_efforts) +{ + for (const auto& item : joint_efforts) + { + self->setVariableEffort(item.first, item.second); + } +} + +Eigen::VectorXd copyJointGroupPositions(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 copyJointGroupVelocities(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 copyJointGroupAccelerations(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 getJacobian(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 getJacobian(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 setToDefaultValues(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 initRobotState(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 }; }) // NOLINT + + // 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::getFrameTransform, 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.getFrameTransform 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::getPose, 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: + 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::getJacobian), + 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::getJacobian), + 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::getJointPositions, + &moveit_py::bind_robot_state::setJointPositions, py::return_value_policy::copy) + + .def_property("joint_velocities", &moveit_py::bind_robot_state::getJointVelocities, + &moveit_py::bind_robot_state::setJointVelocities, py::return_value_policy::copy) + + .def_property("joint_accelerations", &moveit_py::bind_robot_state::getJointAccelerations, + &moveit_py::bind_robot_state::setJointAccelerations, py::return_value_policy::copy) + + .def_property("joint_efforts", &moveit_py::bind_robot_state::getJointEfforts, + &moveit_py::bind_robot_state::setJointEfforts, 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::copyJointGroupPositions, + 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::copyJointGroupVelocities, + 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::copyJointGroupAccelerations, + 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::getGlobalLinkTransform, 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 (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::setToDefaultValues), + 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/moveit/moveit_core/robot_state/robot_state.hpp b/moveit_py/moveit/moveit_core/robot_state/robot_state.hpp new file mode 100644 index 0000000000..7eb2d9b4fa --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_state/robot_state.hpp @@ -0,0 +1,83 @@ +/********************************************************************* + * 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 +#pragma GCC diagnostic push +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif +#include +#pragma GCC diagnostic pop +#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 getFrameTransform(const moveit::core::RobotState* self, std::string& frame_id); + +Eigen::MatrixXd getGlobalLinkTransform(const moveit::core::RobotState* self, std::string& link_name); + +geometry_msgs::msg::Pose getPose(const moveit::core::RobotState* self, const std::string& link_name); + +Eigen::VectorXd copyJointGroupPositions(const moveit::core::RobotState* self, const std::string& joint_model_group_name); +Eigen::VectorXd copyJointGroupVelocities(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); +Eigen::VectorXd copyJointGroupAccelerations(const moveit::core::RobotState* self, + const std::string& joint_model_group_name); + +Eigen::MatrixXd getJacobian(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 getJacobian(const moveit::core::RobotState* self, const std::string& joint_model_group_name, + const Eigen::Vector3d& reference_point_position); + +bool setToDefaultValues(moveit::core::RobotState* self, const std::string& joint_model_group_name, + const std::string& state_name); + +void initRobotState(py::module& m); +} // namespace bind_robot_state +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp new file mode 100644 index 0000000000..a61f53213e --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -0,0 +1,189 @@ +/********************************************************************* + * 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.hpp" +#include +#include + +namespace moveit_py +{ +namespace bind_robot_trajectory +{ +moveit_msgs::msg::RobotTrajectory +getRobotTrajectoryMsg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter) +{ + moveit_msgs::msg::RobotTrajectory msg; + robot_trajectory->getRobotTrajectoryMsg(msg, joint_filter); + return msg; +} + +robot_trajectory::RobotTrajectory +setRobotTrajectoryMsg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg) +{ + return robot_trajectory->setRobotTrajectoryMsg(robot_state, msg); +} + +void initRobotTrajectory(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(py::init&>(), + R"( + Initializes an empty robot trajectory from a robot model. + + Args: + :py:class:`moveit_py.core.RobotModel`: The robot model associated to the instantiated robot state. + + )") + .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("apply_totg_time_parameterization", &trajectory_processing::applyTOTGTimeParameterization, + // py::arg("velocity_scaling_factor"), py::arg("acceleration_scaling_factor"), py::kw_only(), + // py::arg("path_tolerance") = 0.1, py::arg("resample_dt") = 0.1, py::arg("min_angle_change") = 0.001, + // R"( + // Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm. + + // Args: + // velocity_scaling_factor (float): The velocity scaling factor. + // acceleration_scaling_factor (float): The acceleration scaling factor. + // path_tolerance (float): The path tolerance to use for time parameterization (default: 0.1). + // resample_dt (float): The time step to use for time parameterization (default: 0.1). + // min_angle_change (float): The minimum angle change to use for time parameterization (default: 0.001). + // Returns: + // bool: True if the trajectory was successfully retimed, false otherwise. + // )") + // .def("apply_ruckig_smoothing", &trajectory_processing::applyRuckigSmoothing, py::arg("velocity_scaling_factor"), + // py::arg("acceleration_scaling_factor"), py::kw_only(), py::arg("mitigate_overshoot") = false, + // py::arg("overshoot_threshold") = 0.01, + // R"( + // Applies Ruckig smoothing to the trajectory. + + // Args: + // velocity_scaling_factor (float): The velocity scaling factor. + // acceleration_scaling_factor (float): The acceleration scaling factor. + // mitigate_overshoot (bool): Whether to mitigate overshoot during smoothing (default: false). + // overshoot_threshold (float): The maximum allowed overshoot during smoothing (default: 0.01). + // Returns: + // bool: True if the trajectory was successfully retimed, false otherwise. + // )") + .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::getRobotTrajectoryMsg, + py::arg("joint_filter") = std::vector(), + R"( + Get the trajectory as a moveit_msgs.msg.RobotTrajectory message. + + Args: + joint_filter (list[string]): List of joints to consider in creating the message. If empty, uses all joints. + Returns: + moveit_msgs.msg.RobotTrajectory: A ROS robot trajectory message. + )") + .def("set_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::setRobotTrajectoryMsg, py::arg("robot_state"), + py::arg("msg"), + R"( + Set the trajectory from a moveit_msgs.msg.RobotTrajectory message. + + Args: + robot_state (:py:class:`moveit_py.core.RobotState`): The reference robot starting state. + msg (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/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp b/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp new file mode 100644 index 0000000000..7e1cca5ace --- /dev/null +++ b/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp @@ -0,0 +1,61 @@ +/********************************************************************* + * 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_trajectory +{ +moveit_msgs::msg::RobotTrajectory +getRobotTrajectoryMsg(const robot_trajectory::RobotTrajectoryConstPtr& robot_trajectory, + const std::vector& joint_filter); +robot_trajectory::RobotTrajectory +setRobotTrajectoryMsg(const std::shared_ptr& robot_trajectory, + const moveit::core::RobotState& robot_state, const moveit_msgs::msg::RobotTrajectory& msg); + +void initRobotTrajectory(py::module& m); +} // namespace bind_robot_trajectory +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/transforms/transforms.cpp b/moveit_py/moveit/moveit_core/transforms/transforms.cpp new file mode 100644 index 0000000000..5ea7a5ce00 --- /dev/null +++ b/moveit_py/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.hpp" + +namespace moveit_py +{ +namespace bind_transforms +{ +Eigen::MatrixXd getTransform(std::shared_ptr& transforms, std::string& from_frame) +{ + auto transform = transforms->getTransform(from_frame); + return transform.matrix(); +} + +std::map getAllTransforms(std::shared_ptr& transforms) +{ + std::map transforms_map; + for (auto& transform : transforms->getAllTransforms()) + { + transforms_map[transform.first] = transform.second.matrix(); + } + return transforms_map; +} + +void initTransforms(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::getTransform, 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::getAllTransforms, + 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_py/moveit/moveit_core/transforms/transforms.hpp b/moveit_py/moveit/moveit_core/transforms/transforms.hpp new file mode 100644 index 0000000000..c8a72ccdb9 --- /dev/null +++ b/moveit_py/moveit/moveit_core/transforms/transforms.hpp @@ -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 + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_transforms +{ +Eigen::MatrixXd getTransform(std::shared_ptr& transforms, std::string& from_frame); + +std::map getAllTransforms(std::shared_ptr& transforms); + +void initTransforms(py::module& m); + +} // namespace bind_transforms +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_py_utils/CMakeLists.txt b/moveit_py/moveit/moveit_py_utils/CMakeLists.txt new file mode 100644 index 0000000000..132da7425c --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/CMakeLists.txt @@ -0,0 +1,19 @@ +add_library(moveit_py_utils SHARED src/copy_ros_msg.cpp + src/ros_msg_typecasters.cpp) +target_include_directories( + moveit_py_utils PUBLIC $ + $) +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 + EXPORT moveit_py_utilsTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + +install(DIRECTORY include/ DESTINATION include/moveit_py) diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h new file mode 100644 index 0000000000..6554326a71 --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * 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 +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp new file mode 100644 index 0000000000..48c5540fe1 --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp @@ -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/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp new file mode 100644 index 0000000000..bffb9412ab --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp @@ -0,0 +1,89 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Shobin Vinod + * 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: Shobin Vinod */ + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace pybind11 +{ +namespace detail +{ +template <> +struct type_caster +{ + PYBIND11_TYPE_CASTER(rclcpp::Time, _("rclcpp::Time")); + + // convert from rclpy::Time to rclcpp::Time + bool load(py::handle src, bool) + { + if (src.is_none()) + return false; + + // check to validate if the object is a rclcpp::Time object + if (!py::hasattr(src, "nanoseconds") || !py::hasattr(src, "clock_type")) + { + return false; + } + + // Extract the value for constructing the rclcpp::Time object + int64_t nanoseconds = src.attr("nanoseconds").cast(); + int clock_type = src.attr("clock_type").cast(); + + // Construct the rclcpp::Time object + value = rclcpp::Time(nanoseconds, static_cast(clock_type)); + return true; + } + + // convert from rclcpp::Time to rclpy::Time + static py::handle cast(const rclcpp::Time& src, return_value_policy /* policy */, py::handle /* parent */) + { + py::module rclpy_time = py::module::import("rclpy.time"); + py::object Time = rclpy_time.attr("Time"); + + int64_t nanoseconds = src.nanoseconds(); + int clock_type = static_cast(src.get_clock_type()); + + return Time(py::arg("nanoseconds") = nanoseconds, + py::arg("clock_type") = clock_type) + .release(); // release the ownership of the object + } +}; +} // namespace detail +} // namespace pybind11 diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h new file mode 100644 index 0000000000..d5d06c50a3 --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h @@ -0,0 +1,51 @@ + +/********************************************************************* + * All MoveIt 2 headers have been updated to use the .hpp extension. + * + * .h headers are now autogenerated via create_deprecated_headers.py, + * and will import the corresponding .hpp with a deprecation warning. + * + * imports via .h files may be removed in future releases, so please + * modify your imports to use the corresponding .hpp imports. + * + * See https://github.com/moveit/moveit2/pull/3113 for extra details. + *********************************************************************/ +/********************************************************************* + * 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 +#pragma message(".h header is obsolete. Please use the .hpp header instead.") +#include diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp new file mode 100644 index 0000000000..da589020f8 --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp @@ -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/moveit/moveit_py_utils/src/copy_ros_msg.cpp b/moveit_py/moveit/moveit_py_utils/src/copy_ros_msg.cpp new file mode 100644 index 0000000000..b596ac889c --- /dev/null +++ b/moveit_py/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_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp b/moveit_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp new file mode 100644 index 0000000000..6215ca3dda --- /dev/null +++ b/moveit_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp @@ -0,0 +1,65 @@ +/********************************************************************* + * 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 */ + +#include + +namespace py = pybind11; +namespace moveit_py +{ +namespace moveit_py_utils +{ +py::object createMessage(const std::string& ros_msg_name) +{ + // find delimiting '/' in ros msg name + std::size_t pos = ros_msg_name.find('/'); + // import module + py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str()); + // retrieve type instance + py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str()); + // create message instance + return cls(); +} + +bool convertible(const pybind11::handle& h, const std::string& ros_msg_name) +{ + 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 moveit_py_utils +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp new file mode 100644 index 0000000000..59c54ca25d --- /dev/null +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -0,0 +1,201 @@ +/********************************************************************* + * 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 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: Peter David Fagan */ + +#include "moveit_cpp.hpp" +#include +#include +// #include +#include + +namespace moveit_py +{ +namespace bind_moveit_cpp +{ +rclcpp::Logger getLogger() +{ + return rclcpp::get_logger("moveit.py.cpp_initializer"); +} + +std::shared_ptr +getPlanningComponent(std::shared_ptr& moveit_cpp_ptr, const std::string& planning_component) +{ + return std::make_shared(planning_component, moveit_cpp_ptr); +} + +void initMoveitPy(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& name_space, + const std::vector& launch_params_filepaths, const py::object& config_dict, + bool provide_planning_service, + const std::optional>& remappings) { + // 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. + std::vector launch_arguments; + 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, node_name).cast(); + launch_arguments = { "--ros-args", "--params-file", params_filepath }; + } + else if (!launch_params_filepaths.empty()) + { + launch_arguments = { "--ros-args" }; + for (const auto& launch_params_filepath : launch_params_filepaths) + { + launch_arguments.push_back("--params-file"); + launch_arguments.push_back(launch_params_filepath); + } + } + + if (remappings.has_value()) + { + for (const auto& [key, value] : *remappings) + { + std::string argument = key; + argument.append(":=").append(value); + launch_arguments.push_back("--remap"); + launch_arguments.push_back(std::move(argument)); + } + } + + // Initialize ROS, pass launch arguments with rclcpp::init() + if (!rclcpp::ok()) + { + std::vector chars; + chars.reserve(launch_arguments.size()); + for (const auto& arg : launch_arguments) + { + chars.push_back(arg.c_str()); + } + + rclcpp::init(launch_arguments.size(), chars.data()); + RCLCPP_INFO(getLogger(), "Initialize rclcpp"); + } + + // Build NodeOptions + RCLCPP_INFO(getLogger(), "Initialize node parameters"); + rclcpp::NodeOptions node_options; + node_options.allow_undeclared_parameters(true) + .automatically_declare_parameters_from_overrides(true) + .arguments(launch_arguments); + + RCLCPP_INFO(getLogger(), "Initialize node and executor"); + rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared(node_name, name_space, node_options); + std::shared_ptr executor = + std::make_shared(); + + RCLCPP_INFO(getLogger(), "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->getPlanningSceneMonitorNonConst()->providePlanningSceneService(); + }; + + return moveit_cpp_ptr; + }), + py::arg("node_name") = "moveit_py", py::arg("name_space") = "", + py::arg("launch_params_filepaths") = + utils.attr("get_launch_params_filepaths")().cast>(), + py::arg("config_dict") = py::none(), py::arg("provide_planning_service") = true, + py::arg("remappings") = py::none(), 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("controllers"), py::call_guard(), + R"( + Execute a trajectory (planning group is inferred from robot trajectory object). + )") + .def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent, + 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::getPlanningSceneMonitorNonConst, + py::return_value_policy::reference, + R"( + Returns the planning scene monitor. + )") + + .def("get_trajectory_execution_manager", &moveit_cpp::MoveItCpp::getTrajectoryExecutionManagerNonConst, + py::return_value_policy::reference, + R"( + Returns the trajectory execution manager. + )") + + .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/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp new file mode 100644 index 0000000000..e5194ddebc --- /dev/null +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp @@ -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.hpp" + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_moveit_cpp +{ +void initMoveitPy(py::module& m); +} // namespace bind_moveit_cpp +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp new file mode 100644 index 0000000000..0053e1129d --- /dev/null +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -0,0 +1,373 @@ +/********************************************************************* + * 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.hpp" +#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::shared_ptr& planning_scene, + std::optional solution_selection_function, + 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, planning_scene); + } + else if (multi_plan_parameters) + { + // cast parameters + std::shared_ptr const_multi_plan_parameters = + std::const_pointer_cast( + multi_plan_parameters); + + if (solution_selection_function && stopping_criterion_callback) + { + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), + *stopping_criterion_callback, planning_scene); + } + else if (solution_selection_function) + { + return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), nullptr, + planning_scene); + } + else if (stopping_criterion_callback) + { + return planning_component->plan(*const_multi_plan_parameters, + moveit::planning_pipeline_interfaces::getShortestSolution, + *stopping_criterion_callback, planning_scene); + } + else + { + return planning_component->plan(*const_multi_plan_parameters, + moveit::planning_pipeline_interfaces::getShortestSolution, nullptr, + planning_scene); + } + } + else + { + if (planning_scene) + { + throw std::invalid_argument("Cannot specify planning scene without specifying plan parameters"); + } + return planning_component->plan(); + } +} + +bool setGoal(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 setStartState(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 initPlanRequestParameters(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 initMultiPlanRequestParameters(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::plan_request_parameter_vector); +} +void initPlanningComponent(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("get_named_target_state_values", &moveit_cpp::PlanningComponent::getNamedTargetStateValues, py::arg("name"), + R"( + dict: The joint values for targets specified by name. + )") + + .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. + )") + + // 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::setStartState, + 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::setGoal), + 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("planning_scene") = nullptr, + py::arg("solution_selection_function") = 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/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp new file mode 100644 index 0000000000..b39e4e50de --- /dev/null +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -0,0 +1,83 @@ +/********************************************************************* + * 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 "moveit_cpp.hpp" +#include "../planning_scene_monitor/planning_scene_monitor.hpp" +#include "../../moveit_core/planning_interface/planning_response.hpp" + +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::shared_ptr& planning_scene, + std::optional solution_selection_function, + std::optional stopping_criterion_callback); + +bool setGoal(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 setStartState(std::shared_ptr& planning_component, + std::optional configuration_name, std::optional robot_state); + +void initPlanRequestParameters(py::module& m); + +void initMultiPlanRequestParameters(py::module& m); + +void initPlanningComponent(py::module& m); +} // namespace bind_planning_component +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp new file mode 100644 index 0000000000..011ff8c560 --- /dev/null +++ b/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -0,0 +1,243 @@ +/********************************************************************* + * 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.hpp" + +namespace moveit_py +{ +namespace bind_planning_scene_monitor +{ + +// bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, +// moveit_msgs::msg::CollisionObject& collision_object_msg, +// std::optional color_msg) +// { +// moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr = +// std::make_shared(collision_object_msg); +// if (color_msg) +// { +// return planning_scene_monitor->processCollisionObjectMsg(const_ptr, *std::move(color_msg)); +// } +// return planning_scene_monitor->processCollisionObjectMsg(const_ptr); +// } + +// bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, +// moveit_msgs::msg::AttachedCollisionObject& attached_collision_object_msg) +// { +// moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr const_ptr = +// std::make_shared(attached_collision_object_msg); +// return planning_scene_monitor->processAttachedCollisionObjectMsg(const_ptr); +// } + +LockedPlanningSceneContextManagerRO +readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) +{ + return LockedPlanningSceneContextManagerRO(planning_scene_monitor); +}; + +LockedPlanningSceneContextManagerRW +readWrite(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(); +} + +void initPlanningSceneMonitor(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("update_frame_transforms", &planning_scene_monitor::PlanningSceneMonitor::updateFrameTransforms, + R"( + Update the transforms for the frames that are not part of the kinematic model using tf. + + Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called + when data that uses transforms is received. + However, this function should also be called before starting a planning request, for example. + )") + + .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("request_planning_scene_state", &planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState, + py::arg("service_name"), + R"( + Request the planning scene. + + Args: + service_name (str): The name of the service to call. + )") + + .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("process_collision_object", &moveit_py::bind_planning_scene_monitor::processCollisionObject, + // py::arg("collision_object_msg"), py::arg("color_msg") = nullptr, + // R"( + // Apply a collision object to the planning scene. + + // Args: + // collision_object_msg (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. + // )") + // .def("process_attached_collision_object", + // &moveit_py::bind_planning_scene_monitor::processAttachedCollisionObjectMsg, + // py::arg("attached_collision_object_msg"), + // R"( + // Apply an attached collision object msg to the planning scene. + + // Args: + // attached_collision_object_msg (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene. + // )") + + .def("new_planning_scene_message", &planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage, + py::arg("scene"), + R"( + Called to update the planning scene with a new message. + + Args: + scene (moveit_msgs.msg.PlanningScene): The new planning scene message. + )") + + .def("read_only", &moveit_py::bind_planning_scene_monitor::readOnly, + R"( + Returns a read-only context manager for the planning scene. + )") + + .def("read_write", &moveit_py::bind_planning_scene_monitor::readWrite, + R"( + Returns a read-write context manager for the planning scene. + )"); +} + +void initContextManagers(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/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp new file mode 100644 index 0000000000..5cef0fced7 --- /dev/null +++ b/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp @@ -0,0 +1,99 @@ +/********************************************************************* + * 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 + +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 +readWrite(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + +LockedPlanningSceneContextManagerRO +readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + +void initPlanningSceneMonitor(py::module& m); + +void initContextManagers(py::module& m); +} // namespace bind_planning_scene_monitor +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp new file mode 100644 index 0000000000..e74260e6a0 --- /dev/null +++ b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -0,0 +1,285 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Matthijs van der Burgh + * 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: Matthijs van der Burgh */ + +#include "trajectory_execution_manager.hpp" + +namespace moveit_py +{ +namespace bind_trajectory_execution_manager +{ +void initTrajectoryExecutionManager(py::module& m) +{ + py::class_(m, "TrajectoryExecutionManager", R"( + Manages the trajectory execution. + )") + + .def("is_managing_controllers", &trajectory_execution_manager::TrajectoryExecutionManager::isManagingControllers, + R"( + If this function returns true, then this instance of the manager is allowed to load/unload/switch controllers. + )") + + .def("process_event", &trajectory_execution_manager::TrajectoryExecutionManager::processEvent, py::arg("event"), + R"( + Execute a named event (e.g., 'stop'). + )") + + .def("ensure_active_controllers_for_group", + &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForGroup, py::arg("group"), + R"( + Make sure the active controllers are such that trajectories that actuate joints in the specified group can be executed. + + If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not cover the joints in the group to be actuated, this function fails. + )") + + .def("ensure_active_controllers_for_joints", + &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllersForJoints, + py::arg("joints"), + R"( + Make sure the active controllers are such that trajectories that actuate joints in the specified set can be executed. + + If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not cover the joints to be actuated, this function fails. + )") + + .def("ensure_active_controller", + &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveController, py::arg("controller"), + R"( + Make sure a particular controller is active. + + If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not include the one specified as argument, this function fails. + )") + + .def("ensure_active_controllers", + &trajectory_execution_manager::TrajectoryExecutionManager::ensureActiveControllers, py::arg("controllers"), + R"( + Make sure a particular set of controllers are active. + + If the 'moveit_manage_controllers' parameter is false and the controllers that happen to be active do not include the ones specified as argument, this function fails. + )") + + .def("is_controller_active", &trajectory_execution_manager::TrajectoryExecutionManager::isControllerActive, + py::arg("controller"), + R"( + Check if a controller is active. + )") + + .def("are_controllers_active", &trajectory_execution_manager::TrajectoryExecutionManager::areControllersActive, + py::arg("controllers"), + R"( + Check if a set of controllers is active. + )") + + .def("push", + (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(const moveit_msgs::msg::RobotTrajectory&, + const std::string&)) & + trajectory_execution_manager::TrajectoryExecutionManager::push, + py::arg("trajectory"), py::arg("controller") = "", + R"( + Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. + + If no controller is specified, a default is used. + )") + + .def("push", + (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)( + const trajectory_msgs::msg::JointTrajectory&, const std::string&)) & + trajectory_execution_manager::TrajectoryExecutionManager::push, + py::arg("trajectory"), py::arg("controller") = "", + R"( + Add a trajectory for future execution. Optionally specify a controller to use for the trajectory. + + If no controller is specified, a default is used. + )") + + .def("push", + (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)(const moveit_msgs::msg::RobotTrajectory&, + const std::vector&)) & + trajectory_execution_manager::TrajectoryExecutionManager::push, + py::arg("trajectory"), py::arg("controllers"), + R"( + Add a trajectory for future execution. + + Optionally specify a set of controllers to consider using for the trajectory. + Multiple controllers can be used simultaneously to execute the different parts of the trajectory. + If multiple controllers can be used, preference is given to the already loaded ones. + If no controller is specified, a default is used. + )") + + .def("push", + (bool(trajectory_execution_manager::TrajectoryExecutionManager::*)( + const trajectory_msgs::msg::JointTrajectory&, const std::vector&)) & + trajectory_execution_manager::TrajectoryExecutionManager::push, + py::arg("trajectory"), py::arg("controllers"), + R"( + Add a trajectory for future execution. + + Optionally specify a set of controllers to consider using for the trajectory. + Multiple controllers can be used simultaneously to execute the different parts of the trajectory. + If multiple controllers can be used, preference is given to the already loaded ones. + If no controller is specified, a default is used. + )") + + // ToDo(MatthijsBurgh) + // See https://github.com/moveit/moveit2/issues/2442 + // get_trajectories + .def("execute", + py::overload_cast(&trajectory_execution_manager::TrajectoryExecutionManager::execute), + py::arg("callback"), py::arg("auto_clear") = true, + R"( + Start the execution of pushed trajectories. + + This does not wait for completion, but calls a callback when done. + )") + .def( + "execute", + py::overload_cast(&trajectory_execution_manager::TrajectoryExecutionManager::execute), + py::arg("callback"), py::arg("part_callback"), py::arg("auto_clear") = true, + R"( + Start the execution of pushed trajectories. + + This does not wait for completion, but calls a callback when done. A callback is also called for every + trajectory part that completes successfully. + )") + .def("execute_and_wait", &trajectory_execution_manager::TrajectoryExecutionManager::executeAndWait, + py::arg("auto_clear") = true, py::call_guard(), + R"( + Execute a trajectory and wait for it to finish. + )") + .def("wait_for_execution", &trajectory_execution_manager::TrajectoryExecutionManager::waitForExecution, + py::call_guard(), + R"( + Wait for the current trajectory to finish execution. + )") + // ToDo(MatthijsBurgh) + // See https://github.com/moveit/moveit2/issues/2442 + // get_current_expected_trajectory_index + .def("get_last_execution_status", + &trajectory_execution_manager::TrajectoryExecutionManager::getLastExecutionStatus, + R"( + Get the status of the last execution. + )") + .def("stop_execution", &trajectory_execution_manager::TrajectoryExecutionManager::stopExecution, + py::arg("auto_clear") = true, py::call_guard(), + R"( + Stop whatever executions are active, if any. + )") + + .def("enable_execution_duration_monitoring", + &trajectory_execution_manager::TrajectoryExecutionManager::enableExecutionDurationMonitoring, + py::arg("flag"), + R"( + Enable or disable the monitoring of trajectory execution duration. + + If a controller takes longer than expected, the trajectory is canceled. + )") + + .def("execution_duration_monitoring", + &trajectory_execution_manager::TrajectoryExecutionManager::executionDurationMonitoring, + R"( + Get the current status of the monitoring of trajectory execution duration. + )") + + .def("set_allowed_execution_duration_scaling", + &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling, + py::arg("scaling"), + R"( + When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution. + )") + + .def("allowed_execution_duration_scaling", + &trajectory_execution_manager::TrajectoryExecutionManager::allowedExecutionDurationScaling, + R"( + Get the current scaling of the duration of a trajectory to get the allowed duration of execution. + )") + + .def("set_allowed_goal_duration_margin", + &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin, py::arg("margin"), + R"( + When determining the expected duration of a trajectory, this additional margin s applied after scalign to allow more than the expected execution time before triggering trajectory cancel. + )") + + .def("allowed_goal_duration_margin", + &trajectory_execution_manager::TrajectoryExecutionManager::allowedGoalDurationMargin, + R"( + Get the current margin of the duration of a trajectory to get the allowed duration of execution. + )") + + .def("set_execution_velocity_scaling", + &trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling, py::arg("scaling"), + R"( + Before sending a trajectory to a controller, scale the velocities by the factor specified. + + By default, this is 1.0 + )") + + .def("execution_velocity_scaling", + &trajectory_execution_manager::TrajectoryExecutionManager::executionVelocityScaling, + R"( + Get the current scaling of the execution velocities. + )") + + .def("set_allowed_start_tolerance", + &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"), + R"( + Set joint-value tolerance for validating trajectory's start point against current robot state. + )") + + .def("allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::allowedStartTolerance, + R"( + Get the current joint-value for validating trajectory's start point against current robot state. + )") + + .def("set_wait_for_trajectory_completion", + &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"), + R"( + Enable or disable waiting for trajectory completion. + )") + + .def("wait_for_trajectory_completion", + &trajectory_execution_manager::TrajectoryExecutionManager::waitForTrajectoryCompletion, + R"( + Get the current state of waiting for the trajectory being completed. + )"); + + // ToDo(MatthijsBurgh) + // https://github.com/moveit/moveit2/issues/2442 + // get_controller_manager_node +} +} // namespace bind_trajectory_execution_manager +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp new file mode 100644 index 0000000000..c74caba117 --- /dev/null +++ b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -0,0 +1,57 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Matthijs van der Burgh + * 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: Matthijs van der Burgh */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_trajectory_execution_manager +{ + +void initTrajectoryExecutionManager(py::module& m); + +} // namespace bind_trajectory_execution_manager +} // namespace moveit_py diff --git a/moveit_py/package.xml b/moveit_py/package.xml new file mode 100644 index 0000000000..1db1481c35 --- /dev/null +++ b/moveit_py/package.xml @@ -0,0 +1,34 @@ + + + + moveit_py + 2.13.2 + Python binding for MoveIt 2 + + Samuele Sandrini + + BSD + + Samuele Sandrini + + ament_cmake + + pybind11_vendor + + ament_index_python + + rclcpp + rclpy + geometry_msgs + octomap_msgs + moveit_ros_planning_interface + moveit_ros_planning + moveit_core + + ament_cmake_pytest + python3-pytest + + + ament_cmake + + \ No newline at end of file From c4da4f1b687814778c4d77b3693c8ce50690b7cf Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Wed, 21 May 2025 19:00:29 +0200 Subject: [PATCH 02/28] Add remaining Python bindings --- moveit_py/CMakeLists.txt | 14 +-- moveit_py/README.md | 10 ++ .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 15 ++- .../moveit_cpp/planning_component.cpp | 103 +++++------------- .../moveit_cpp/planning_component.hpp | 10 +- .../trajectory_execution_manager.cpp | 59 +++++----- moveit_py/moveit/planning.cpp | 57 ++++++++++ 7 files changed, 142 insertions(+), 126 deletions(-) create mode 100644 moveit_py/moveit/planning.cpp diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 02db8709a9..470f3ebab0 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -69,17 +69,17 @@ configure_build_install_location(core) pybind11_add_module( planning - # src/moveit/planning.cpp - # moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp - # moveit/moveit_ros/moveit_cpp/planning_component.cpp + moveit/planning.cpp + moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp + moveit/moveit_ros/moveit_cpp/planning_component.cpp moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp - # src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp + moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp ) target_link_libraries( planning PRIVATE moveit_ros_planning::moveit_cpp - # moveit_ros_planning::moveit_planning_scene_monitor - # moveit_ros_planning::moveit_trajectory_execution_manager + moveit_ros_planning::moveit_planning_scene_monitor + moveit_ros_planning::moveit_trajectory_execution_manager moveit_core::moveit_utils rclcpp::rclcpp moveit_py_utils) @@ -103,6 +103,6 @@ if(BUILD_TESTING) # endforeach() endif() -# ament_export_targets(moveit_py_utilsTargets HAS_LIBRARY_TARGET) +ament_export_targets(moveit_py_utilsTargets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_ros_planning_interface) ament_package() \ No newline at end of file diff --git a/moveit_py/README.md b/moveit_py/README.md index 8961c430e4..cb1b5ae9f8 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -9,3 +9,13 @@ 4. In `planning_scene_monitor.cpp` (part of `moveit_ros`, now in `moveit_py`), `process_attached_collision_object` and `process_collision_object` were defined but are not actual methods of `PlanningSceneMonitor`—only of `PlanningScene`. Consider implementing them in `PlanningSceneMonitor` as well. 5. In `moveit.cpp`, `moveit/utils/logger.hpp` was used, but it no longer exists. It seems only `rclcpp`'s logger is now used. I followed the logging pattern from other modules. + +6. In trajectory_Execution_manager commentato il getter execution_duration_monitoring e i vari getters che non c'erano in trajectory_execution_manager.h/.cpp. + +7. In planning_component.hpp rimossi tutti i riferimenti a MultiPipelinePlanRequestParameters e plan con if se single_plan_request param o multi plan rimosso e lasciato parameters come .hpp di moveit2. + +8. TODOOOO: In plannin_component.hpp/.cpp plan() ritornava un planning_interface::MotionPlanResponse, però in humble ritornava un moveit_cpp::PlanningComponent::PlanSolution. Anche in humble c'era definito un planning_interface::MotionPlanResponse in planning_response.hpp ma dato che torna un planSOlution ora ritorno plan solution. Però, non mi sembra definito la classe python corrispondente, come la faccio? Dove la metto? + +9. In initPlanRequestParameters viene fatto params.load(node, ns) però ns non era usato in humble, non c'era proprio, come verranno gestiti i namespace? + +10. :py:class::`moveit_py.controller_manager.ExecutionStatus`: The status of the execution. \\ TODO(@samu): verify the module return type diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index 59c54ca25d..f955a2983e 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -158,12 +158,17 @@ void initMoveitPy(py::module& m) Initialize moveit_cpp node and the planning scene service. )") .def("execute", - py::overload_cast&>( - &moveit_cpp::MoveItCpp::execute), - py::arg("robot_trajectory"), py::arg("controllers"), py::call_guard(), + &moveit_cpp::MoveItCpp::execute, + py::arg("group_name"), py::arg("robot_trajectory"), py::arg("blocking") = true, R"( - Execute a trajectory (planning group is inferred from robot trajectory object). - )") + Execute a trajectory for a specific planning group. + Args: + group_name (str): Name of the planning group. + robot_trajectory (RobotTrajectory): The trajectory to execute. + blocking (bool): Whether to block until execution finishes. Defaults to True. + Returns: + :py:class::`moveit_py.controller_manager.ExecutionStatus`: The status of the execution. \\ TODO(@samu): verify the module return type + )") .def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent, py::arg("planning_component_name"), py::return_value_policy::take_ownership, R"( diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp index 0053e1129d..2754834478 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -41,67 +41,16 @@ namespace moveit_py { namespace bind_planning_component { -planning_interface::MotionPlanResponse +moveit_cpp::PlanningComponent::PlanSolution plan(std::shared_ptr& planning_component, - std::shared_ptr& single_plan_parameters, - std::shared_ptr& multi_plan_parameters, - std::shared_ptr& planning_scene, - std::optional solution_selection_function, - std::optional stopping_criterion_callback) + std::shared_ptr& parameters +) { - // 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, planning_scene); - } - else if (multi_plan_parameters) - { - // cast parameters - std::shared_ptr const_multi_plan_parameters = - std::const_pointer_cast( - multi_plan_parameters); + std::const_pointer_cast(parameters); - if (solution_selection_function && stopping_criterion_callback) - { - return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), - *stopping_criterion_callback, planning_scene); - } - else if (solution_selection_function) - { - return planning_component->plan(*const_multi_plan_parameters, std::ref(*solution_selection_function), nullptr, - planning_scene); - } - else if (stopping_criterion_callback) - { - return planning_component->plan(*const_multi_plan_parameters, - moveit::planning_pipeline_interfaces::getShortestSolution, - *stopping_criterion_callback, planning_scene); - } - else - { - return planning_component->plan(*const_multi_plan_parameters, - moveit::planning_pipeline_interfaces::getShortestSolution, nullptr, - planning_scene); - } - } - else - { - if (planning_scene) - { - throw std::invalid_argument("Cannot specify planning scene without specifying plan parameters"); - } - return planning_component->plan(); - } + return planning_component->plan(*const_single_plan_parameters); } bool setGoal(std::shared_ptr& planning_component, @@ -201,10 +150,10 @@ void initPlanRequestParameters(py::module& m) R"( Planner parameters provided with a MotionPlanRequest. )") - .def(py::init([](std::shared_ptr& moveit_cpp, const std::string& ns) { + .def(py::init([](std::shared_ptr& moveit_cpp) { // TODO(@samu), const std::string& ns const rclcpp::Node::SharedPtr& node = moveit_cpp->getNode(); moveit_cpp::PlanningComponent::PlanRequestParameters params; - params.load(node, ns); + params.load(node); // TODO(@samu) , ns how to manage namespace? return params; })) .def_readwrite("planner_id", &moveit_cpp::PlanningComponent::PlanRequestParameters::planner_id, @@ -235,23 +184,23 @@ void initPlanRequestParameters(py::module& m) )"); } -void initMultiPlanRequestParameters(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::plan_request_parameter_vector); -} +// void initMultiPlanRequestParameters(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::plan_request_parameter_vector); +// } void initPlanningComponent(py::module& m) { py::class_>(m, "PlanningComponent", @@ -329,9 +278,7 @@ void initPlanningComponent(py::module& m) // 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("planning_scene") = nullptr, - py::arg("solution_selection_function") = nullptr, py::arg("stopping_criterion_callback") = nullptr, + .def("plan", &moveit_py::bind_planning_component::plan, py::arg("parameters") = nullptr, py::return_value_policy::move, R"( Plan a motion plan using the current start and goal states. diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp index b39e4e50de..9cd22d202a 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -58,13 +58,9 @@ namespace moveit_py { namespace bind_planning_component { -planning_interface::MotionPlanResponse +moveit_cpp::PlanningComponent::PlanSolution plan(std::shared_ptr& planning_component, - std::shared_ptr& single_plan_parameters, - std::shared_ptr& multi_plan_parameters, - std::shared_ptr& planning_scene, - std::optional solution_selection_function, - std::optional stopping_criterion_callback); + std::shared_ptr& single_plan_parameters); bool setGoal(std::shared_ptr& planning_component, std::optional configuration_name, std::optional robot_state, @@ -76,7 +72,7 @@ bool setStartState(std::shared_ptr& planning_comp void initPlanRequestParameters(py::module& m); -void initMultiPlanRequestParameters(py::module& m); +// void initMultiPlanRequestParameters(py::module& m); void initPlanningComponent(py::module& m); } // namespace bind_planning_component diff --git a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index e74260e6a0..caabc85794 100644 --- a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -209,11 +209,11 @@ void initTrajectoryExecutionManager(py::module& m) If a controller takes longer than expected, the trajectory is canceled. )") - .def("execution_duration_monitoring", - &trajectory_execution_manager::TrajectoryExecutionManager::executionDurationMonitoring, - R"( - Get the current status of the monitoring of trajectory execution duration. - )") + // .def("execution_duration_monitoring", + // &trajectory_execution_manager::TrajectoryExecutionManager::executionDurationMonitoring, + // R"( + // Get the current status of the monitoring of trajectory execution duration. + // )") .def("set_allowed_execution_duration_scaling", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling, @@ -222,11 +222,11 @@ void initTrajectoryExecutionManager(py::module& m) When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution. )") - .def("allowed_execution_duration_scaling", - &trajectory_execution_manager::TrajectoryExecutionManager::allowedExecutionDurationScaling, - R"( - Get the current scaling of the duration of a trajectory to get the allowed duration of execution. - )") + // .def("allowed_execution_duration_scaling", + // &trajectory_execution_manager::TrajectoryExecutionManager::allowedExecutionDurationScaling, + // R"( + // Get the current scaling of the duration of a trajectory to get the allowed duration of execution. + // )") .def("set_allowed_goal_duration_margin", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin, py::arg("margin"), @@ -234,11 +234,11 @@ void initTrajectoryExecutionManager(py::module& m) When determining the expected duration of a trajectory, this additional margin s applied after scalign to allow more than the expected execution time before triggering trajectory cancel. )") - .def("allowed_goal_duration_margin", - &trajectory_execution_manager::TrajectoryExecutionManager::allowedGoalDurationMargin, - R"( - Get the current margin of the duration of a trajectory to get the allowed duration of execution. - )") + // .def("allowed_goal_duration_margin", + // &trajectory_execution_manager::TrajectoryExecutionManager::allowedGoalDurationMargin, + // R"( + // Get the current margin of the duration of a trajectory to get the allowed duration of execution. + // )") .def("set_execution_velocity_scaling", &trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling, py::arg("scaling"), @@ -248,11 +248,11 @@ void initTrajectoryExecutionManager(py::module& m) By default, this is 1.0 )") - .def("execution_velocity_scaling", - &trajectory_execution_manager::TrajectoryExecutionManager::executionVelocityScaling, - R"( - Get the current scaling of the execution velocities. - )") + // .def("execution_velocity_scaling", + // &trajectory_execution_manager::TrajectoryExecutionManager::executionVelocityScaling, + // R"( + // Get the current scaling of the execution velocities. + // )") .def("set_allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"), @@ -260,10 +260,10 @@ void initTrajectoryExecutionManager(py::module& m) Set joint-value tolerance for validating trajectory's start point against current robot state. )") - .def("allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::allowedStartTolerance, - R"( - Get the current joint-value for validating trajectory's start point against current robot state. - )") + // .def("allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::allowedStartTolerance, + // R"( + // Get the current joint-value for validating trajectory's start point against current robot state. + // )") .def("set_wait_for_trajectory_completion", &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"), @@ -271,11 +271,12 @@ void initTrajectoryExecutionManager(py::module& m) Enable or disable waiting for trajectory completion. )") - .def("wait_for_trajectory_completion", - &trajectory_execution_manager::TrajectoryExecutionManager::waitForTrajectoryCompletion, - R"( - Get the current state of waiting for the trajectory being completed. - )"); + // .def("wait_for_trajectory_completion", + // &trajectory_execution_manager::TrajectoryExecutionManager::waitForTrajectoryCompletion, + // R"( + // Get the current state of waiting for the trajectory being completed. + // )") + ; // ToDo(MatthijsBurgh) // https://github.com/moveit/moveit2/issues/2442 diff --git a/moveit_py/moveit/planning.cpp b/moveit_py/moveit/planning.cpp new file mode 100644 index 0000000000..9861cb9603 --- /dev/null +++ b/moveit_py/moveit/planning.cpp @@ -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 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: Peter David Fagan */ + +#include "moveit_ros/moveit_cpp/moveit_cpp.hpp" +#include "moveit_ros/moveit_cpp/planning_component.hpp" +#include "moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp" +#include "moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp" + +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::initPlanRequestParameters(m); + moveit_py::bind_planning_component::initPlanningComponent(m); + moveit_py::bind_planning_scene_monitor::initPlanningSceneMonitor(m); + moveit_py::bind_planning_scene_monitor::initContextManagers(m); + moveit_py::bind_trajectory_execution_manager::initTrajectoryExecutionManager(m); + moveit_py::bind_moveit_cpp::initMoveitPy(m); +} From 0c8e4559bd144491c1de1af1e56f03ec78c22411 Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Wed, 21 May 2025 19:03:29 +0200 Subject: [PATCH 03/28] Add remaining Python bindings --- moveit_py/CMakeLists.txt | 2 +- moveit_py/moveit/core.cpp | 81 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 82 insertions(+), 1 deletion(-) create mode 100644 moveit_py/moveit/core.cpp diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 470f3ebab0..81c0aaf319 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -37,7 +37,7 @@ endfunction() pybind11_add_module( core - # src/moveit/core.cpp + moveit/core.cpp moveit/moveit_core/collision_detection/collision_common.cpp moveit/moveit_core/collision_detection/collision_matrix.cpp moveit/moveit_core/collision_detection/world.cpp diff --git a/moveit_py/moveit/core.cpp b/moveit_py/moveit/core.cpp new file mode 100644 index 0000000000..e65f2b1caa --- /dev/null +++ b/moveit_py/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.hpp" +#include "moveit_core/collision_detection/collision_matrix.hpp" +#include "moveit_core/collision_detection/world.hpp" +#include "moveit_core/controller_manager/controller_manager.hpp" +#include "moveit_core/kinematic_constraints/utils.hpp" +#include "moveit_core/planning_interface/planning_response.hpp" +#include "moveit_core/planning_scene/planning_scene.hpp" +#include "moveit_core/robot_model/joint_model.hpp" +#include "moveit_core/robot_model/joint_model_group.hpp" +#include "moveit_core/robot_model/robot_model.hpp" +#include "moveit_core/robot_state/robot_state.hpp" +#include "moveit_core/robot_trajectory/robot_trajectory.hpp" + +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::initCollisionRequest(m); + moveit_py::bind_collision_detection::initCollisionResult(m); + moveit_py::bind_collision_detection::initWorld(m); + moveit_py::bind_collision_detection::initAcm(m); + moveit_py::bind_controller_manager::initExecutionStatus(m); + moveit_py::bind_kinematic_constraints::initKinematicConstraints(m); + moveit_py::bind_planning_scene::initPlanningScene(m); + moveit_py::bind_planning_interface::initMotionPlanResponse(m); + moveit_py::bind_robot_model::initJointModel(m); + moveit_py::bind_robot_model::initJointModelGroup(m); + moveit_py::bind_robot_model::initRobotModel(m); + moveit_py::bind_robot_state::initRobotState(m); + moveit_py::bind_robot_trajectory::initRobotTrajectory(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"); +} From 37350f18a0a18de41f63213c0a0874d944ee57c5 Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Thu, 22 May 2025 13:34:31 +0200 Subject: [PATCH 04/28] Add init and moveit_py utils --- moveit_py/README.md | 17 +++++++--- moveit_py/moveit/__init__.py | 3 ++ moveit_py/moveit/utils.py | 65 ++++++++++++++++++++++++++++++++++++ 3 files changed, 80 insertions(+), 5 deletions(-) create mode 100644 moveit_py/moveit/utils.py diff --git a/moveit_py/README.md b/moveit_py/README.md index cb1b5ae9f8..cdb6666637 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -10,12 +10,19 @@ 5. In `moveit.cpp`, `moveit/utils/logger.hpp` was used, but it no longer exists. It seems only `rclcpp`'s logger is now used. I followed the logging pattern from other modules. -6. In trajectory_Execution_manager commentato il getter execution_duration_monitoring e i vari getters che non c'erano in trajectory_execution_manager.h/.cpp. +6. In `trajectory_execution_manager`, commented out the `execution_duration_monitoring` getter and other missing getters that are not defined in the `.h/.cpp` files. + +7. In `planning_component.hpp`, removed all references to `MultiPipelinePlanRequestParameters` and `plan` branches based on `single_plan_request_param`; retained only `parameters` as done in MoveIt 2. + +8. In `planning_component.hpp/.cpp`, `plan()` used to return a `planning_interface::MotionPlanResponse`, but in Humble it returns a `moveit_cpp::PlanningComponent::PlanSolution`. Although `MotionPlanResponse` still exists in `planning_response.hpp`, the Python equivalent for `PlanSolution` is missing and needs to be implemented and exposed via bindings. + +9. In `initPlanRequestParameters`, the method call `params.load(node, ns)` includes `ns`, which was not used or present in Humble. Needs clarification on how namespaces are now handled for parameter loading. + +10. In `controller_manager`, `:py:class:\`moveit_py.controller_manager.ExecutionStatus\`` documents the execution status. \\ TODO(@samu): verify the actual return type from the module. + + + -7. In planning_component.hpp rimossi tutti i riferimenti a MultiPipelinePlanRequestParameters e plan con if se single_plan_request param o multi plan rimosso e lasciato parameters come .hpp di moveit2. -8. TODOOOO: In plannin_component.hpp/.cpp plan() ritornava un planning_interface::MotionPlanResponse, però in humble ritornava un moveit_cpp::PlanningComponent::PlanSolution. Anche in humble c'era definito un planning_interface::MotionPlanResponse in planning_response.hpp ma dato che torna un planSOlution ora ritorno plan solution. Però, non mi sembra definito la classe python corrispondente, come la faccio? Dove la metto? -9. In initPlanRequestParameters viene fatto params.load(node, ns) però ns non era usato in humble, non c'era proprio, come verranno gestiti i namespace? -10. :py:class::`moveit_py.controller_manager.ExecutionStatus`: The status of the execution. \\ TODO(@samu): verify the module return type diff --git a/moveit_py/moveit/__init__.py b/moveit_py/moveit/__init__.py index e69de29bb2..d7c49b2af2 100644 --- a/moveit_py/moveit/__init__.py +++ 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_filepaths diff --git a/moveit_py/moveit/utils.py b/moveit_py/moveit/utils.py new file mode 100644 index 0000000000..18bc7ae553 --- /dev/null +++ b/moveit_py/moveit/utils.py @@ -0,0 +1,65 @@ +# 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 + +from typing import List, Optional + +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_filepaths(cli_args: Optional[List[str]] = None) -> List[str]: + """ + A utility that returns the path value after the --params-file arguments. + """ + if cli_args is None: + cli_args = sys.argv + + try: + indexes = [i for i, v in enumerate(cli_args) if v == "--params-file"] + return [cli_args[i + 1] for i in indexes] + except IndexError: + return [ + "Failed to parse params file paths from command line arguments. Check that --params-file command line argument is specified." + ] From 03695bbd5de49a35112dac9e00a4602b2c1b0998 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Fri, 23 May 2025 00:02:57 +0200 Subject: [PATCH 05/28] Fix plan method and add PlanSolution binding --- .../moveit_cpp/planning_component.cpp | 20 +++++++++++++++++++ .../moveit_cpp/planning_component.hpp | 4 +++- moveit_py/moveit/planning.cpp | 1 + 3 files changed, 24 insertions(+), 1 deletion(-) diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp index 2754834478..921beed704 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -46,11 +46,15 @@ plan(std::shared_ptr& planning_component, std::shared_ptr& parameters ) { + if(parameters) + { // cast parameters std::shared_ptr const_single_plan_parameters = std::const_pointer_cast(parameters); return planning_component->plan(*const_single_plan_parameters); + } + return planning_component->plan(); } bool setGoal(std::shared_ptr& planning_component, @@ -316,5 +320,21 @@ void initPlanningComponent(py::module& m) Remove the workspace bounding box from planning. )"); } +void initPlanSolution(py::module& m) +{ +py::class_(m, "PlanSolution", R"( + Represents a planning solution, including the trajectory and any error codes. +)") + .def_readonly("start_state", &moveit_cpp::PlanningComponent::PlanSolution::start_state, + "The starting state used for planning.") + .def_readonly("trajectory", &moveit_cpp::PlanningComponent::PlanSolution::trajectory, + "The resulting planned trajectory.") + .def_readonly("error_code", &moveit_cpp::PlanningComponent::PlanSolution::error_code, + "The MoveIt error code of the planning request.") + .def("__bool__", [](const moveit_cpp::PlanningComponent::PlanSolution &self) { + return static_cast(self); + }, "Return True if planning was successful."); +} } // namespace bind_planning_component } // namespace moveit_py diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp index 9cd22d202a..d332e1a6f0 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -60,7 +60,7 @@ namespace bind_planning_component { moveit_cpp::PlanningComponent::PlanSolution plan(std::shared_ptr& planning_component, - std::shared_ptr& single_plan_parameters); + std::shared_ptr& parameters); bool setGoal(std::shared_ptr& planning_component, std::optional configuration_name, std::optional robot_state, @@ -75,5 +75,7 @@ void initPlanRequestParameters(py::module& m); // void initMultiPlanRequestParameters(py::module& m); void initPlanningComponent(py::module& m); + +void initPlanSolution(py::module& m); } // namespace bind_planning_component } // namespace moveit_py diff --git a/moveit_py/moveit/planning.cpp b/moveit_py/moveit/planning.cpp index 9861cb9603..3fc5fd1a1e 100644 --- a/moveit_py/moveit/planning.cpp +++ b/moveit_py/moveit/planning.cpp @@ -50,6 +50,7 @@ PYBIND11_MODULE(planning, m) // Construct module classes moveit_py::bind_planning_component::initPlanRequestParameters(m); moveit_py::bind_planning_component::initPlanningComponent(m); + moveit_py::bind_planning_component::initPlanSolution(m); moveit_py::bind_planning_scene_monitor::initPlanningSceneMonitor(m); moveit_py::bind_planning_scene_monitor::initContextManagers(m); moveit_py::bind_trajectory_execution_manager::initTrajectoryExecutionManager(m); From 0834aaaf457d6ea5e5f40ef3d1818ec4d59de389 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 12:02:20 +0200 Subject: [PATCH 06/28] Add initial tests for moveit_py --- moveit_py/CMakeLists.txt | 28 +-- moveit_py/test/__init__.py | 0 moveit_py/test/integration/__init__.py | 0 moveit_py/test/unit/__init__.py | 0 moveit_py/test/unit/fixtures/panda.srdf | 120 +++++++++++++ moveit_py/test/unit/fixtures/panda.urdf | 224 ++++++++++++++++++++++++ moveit_py/test/unit/test_robot_model.py | 83 +++++++++ moveit_py/test/unit/test_robot_state.py | 177 +++++++++++++++++++ 8 files changed, 618 insertions(+), 14 deletions(-) create mode 100644 moveit_py/test/__init__.py create mode 100644 moveit_py/test/integration/__init__.py create mode 100644 moveit_py/test/unit/__init__.py create mode 100644 moveit_py/test/unit/fixtures/panda.srdf create mode 100644 moveit_py/test/unit/fixtures/panda.urdf create mode 100644 moveit_py/test/unit/test_robot_model.py create mode 100644 moveit_py/test/unit/test_robot_state.py diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 81c0aaf319..ce224d1d21 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -87,20 +87,20 @@ configure_build_install_location(planning) if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) - # set(_pytest_tests test/unit/test_robot_model.py test/unit/test_robot_state.py) - # foreach(test_path ${_pytest_tests}) - # get_filename_component(_test_name ${test_path} NAME_WE) - # ament_add_pytest_test( - # ${_test_name} - # ${test_path} - # APPEND_ENV - # AMENT_PREFIX_INDEX=${ament_index_build_path} - # PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - # TIMEOUT - # 60 - # WORKING_DIRECTORY - # "${CMAKE_SOURCE_DIR}") - # endforeach() + set(_pytest_tests test/unit/test_robot_model.py test/unit/test_robot_state.py) + foreach(test_path ${_pytest_tests}) + get_filename_component(_test_name ${test_path} NAME_WE) + ament_add_pytest_test( + ${_test_name} + ${test_path} + APPEND_ENV + AMENT_PREFIX_INDEX=${ament_index_build_path} + PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT + 60 + WORKING_DIRECTORY + "${CMAKE_SOURCE_DIR}") + endforeach() endif() ament_export_targets(moveit_py_utilsTargets HAS_LIBRARY_TARGET) 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..9584ec624f --- /dev/null +++ b/moveit_py/test/unit/test_robot_model.py @@ -0,0 +1,83 @@ +import unittest +from moveit.core.robot_model 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) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) + + +class TestRobotModel(unittest.TestCase): + def test_initialization(self): + """ + Test that the RobotModel can be initialized with xml filepaths. + """ + robot = get_robot_model() + self.assertIsInstance(robot, RobotModel) + + def test_name_property(self): + """ + Test that the RobotModel name property returns the correct name. + """ + robot = get_robot_model() + self.assertEqual(robot.name, "panda") + + def test_model_frame_property(self): + """ + Test that the RobotModel model_frame property returns the correct name. + """ + robot = get_robot_model() + 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 = get_robot_model() + 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 = get_robot_model() + 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 = get_robot_model() + 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 = get_robot_model() + 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 = get_robot_model() + 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..2a1e8db516 --- /dev/null +++ b/moveit_py/test/unit/test_robot_state.py @@ -0,0 +1,177 @@ +import unittest +import numpy as np + +from geometry_msgs.msg import Pose + +from moveit.core.robot_state import RobotState +from moveit.core.robot_model import 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) + +import os + +dir_path = os.path.dirname(os.path.realpath(__file__)) +URDF_FILE = "{}/fixtures/panda.urdf".format(dir_path) +SRDF_FILE = "{}/fixtures/panda.srdf".format(dir_path) + + +def get_robot_model(): + """Helper function that returns a RobotModel instance.""" + return RobotModel(urdf_xml_path=URDF_FILE, srdf_xml_path=SRDF_FILE) + + +class TestRobotState(unittest.TestCase): + def test_initialization(self): + """ + Test that RobotState can be initialized with a RobotModel + """ + robot_model = get_robot_model() + 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 = get_robot_model() + 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 = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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 = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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 = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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 = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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 = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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.get_joint_group_positions("panda_arm").tolist(), + ) + + def test_set_joint_group_velocities(self): + """ + Test that the joint group velocities can be set + """ + robot_model = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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.get_joint_group_velocities("panda_arm").tolist(), + ) + + def test_set_joint_group_accelerations(self): + """ + Test that the joint group accelerations can be set + """ + robot_model = get_robot_model() + robot_state = RobotState(robot_model) + robot_state.update() + 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.get_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) + # robot_state.update() + # 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() From 8f948a739a829a7e8faa974c43c38de14fde45fa Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 13:45:44 +0200 Subject: [PATCH 07/28] Drop PlanningScene changes not relevant to moveit_py --- .../include/moveit/planning_scene/planning_scene.h | 3 --- moveit_core/planning_scene/src/planning_scene.cpp | 5 ----- 2 files changed, 8 deletions(-) diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 9b56df8736..de3981af86 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -294,9 +294,6 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the allowed collision matrix */ collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrixNonConst(); - /** \brief Set the allowed collision matrix */ - void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm); - /**@}*/ /** diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index 157b667b18..02831c9f8f 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -518,11 +518,6 @@ collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionM return *acm_; } -void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm) -{ - getAllowedCollisionMatrixNonConst() = acm; -} - const moveit::core::Transforms& PlanningScene::getTransforms() { // Trigger an update of the robot transforms From 5da139176b67fe58cb6c0b5cb49488799e5e5a7e Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 17:45:42 +0200 Subject: [PATCH 08/28] Update moveit_py structure for uniformity --- moveit_py/CMakeLists.txt | 40 +++++------ .../collision_detection/collision_common.hpp | 17 ----- .../collision_detection/collision_matrix.hpp | 16 ----- .../moveit_core/collision_detection/world.cpp | 14 ---- .../moveit_core/collision_detection/world.hpp | 16 ----- .../kinematic_constraints/utils.hpp | 32 --------- moveit_py/{ => src}/moveit/core.cpp | 0 .../collision_detection/collision_common.cpp | 38 +++++++++- .../collision_detection/collision_common.hpp | 54 +++++++++++++++ .../collision_detection/collision_matrix.cpp | 36 ++++++++++ .../collision_detection/collision_matrix.hpp | 53 ++++++++++++++ .../moveit_core/collision_detection/world.cpp | 50 ++++++++++++++ .../moveit_core/collision_detection/world.hpp | 53 ++++++++++++++ .../controller_manager/controller_manager.cpp | 0 .../controller_manager/controller_manager.hpp | 3 +- .../kinematic_constraints/utils.cpp | 37 ++++++++++ .../kinematic_constraints/utils.hpp | 69 +++++++++++++++++++ .../planning_interface/planning_response.cpp | 3 +- .../planning_interface/planning_response.hpp | 3 +- .../planning_scene/planning_scene.cpp | 3 +- .../planning_scene/planning_scene.hpp | 3 +- .../moveit_core/robot_model/joint_model.cpp | 0 .../moveit_core/robot_model/joint_model.hpp | 3 +- .../robot_model/joint_model_group.cpp | 0 .../robot_model/joint_model_group.hpp | 3 +- .../moveit_core/robot_model/robot_model.cpp | 3 +- .../moveit_core/robot_model/robot_model.hpp | 0 .../moveit_core/robot_state/robot_state.cpp | 5 +- .../moveit_core/robot_state/robot_state.hpp | 5 +- .../robot_trajectory/robot_trajectory.cpp | 3 +- .../robot_trajectory/robot_trajectory.hpp | 3 +- .../moveit_core/transforms/transforms.cpp | 0 .../moveit_core/transforms/transforms.hpp | 3 +- .../moveit/moveit_py_utils/CMakeLists.txt | 0 .../moveit_py/moveit_py_utils/copy_ros_msg.h | 0 .../moveit_py_utils/copy_ros_msg.hpp | 0 .../moveit_py_utils/rclpy_time_typecaster.hpp | 0 .../moveit_py_utils/ros_msg_typecasters.h | 0 .../moveit_py_utils/ros_msg_typecasters.hpp | 0 .../moveit_py_utils/src/copy_ros_msg.cpp | 0 .../src/ros_msg_typecasters.cpp | 0 .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 5 +- .../moveit_ros/moveit_cpp/moveit_cpp.hpp | 5 +- .../moveit_cpp/planning_component.cpp | 9 +-- .../moveit_cpp/planning_component.hpp | 5 +- .../planning_scene_monitor.cpp | 5 +- .../planning_scene_monitor.hpp | 5 +- .../trajectory_execution_manager.cpp | 3 +- .../trajectory_execution_manager.hpp | 3 +- moveit_py/{ => src}/moveit/planning.cpp | 3 +- 50 files changed, 463 insertions(+), 148 deletions(-) delete mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp delete mode 100644 moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp delete mode 100644 moveit_py/moveit/moveit_core/collision_detection/world.cpp delete mode 100644 moveit_py/moveit/moveit_core/collision_detection/world.hpp delete mode 100644 moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp rename moveit_py/{ => src}/moveit/core.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/collision_detection/collision_common.cpp (67%) create mode 100644 moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp rename moveit_py/{ => src}/moveit/moveit_core/collision_detection/collision_matrix.cpp (68%) create mode 100644 moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp create mode 100644 moveit_py/src/moveit/moveit_core/collision_detection/world.cpp create mode 100644 moveit_py/src/moveit/moveit_core/collision_detection/world.hpp rename moveit_py/{ => src}/moveit/moveit_core/controller_manager/controller_manager.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/controller_manager/controller_manager.hpp (95%) rename moveit_py/{ => src}/moveit/moveit_core/kinematic_constraints/utils.cpp (74%) create mode 100644 moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp rename moveit_py/{ => src}/moveit/moveit_core/planning_interface/planning_response.cpp (97%) rename moveit_py/{ => src}/moveit/moveit_core/planning_interface/planning_response.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_core/planning_scene/planning_scene.cpp (99%) rename moveit_py/{ => src}/moveit/moveit_core/planning_scene/planning_scene.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/joint_model.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/joint_model.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/joint_model_group.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/joint_model_group.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/robot_model.cpp (98%) rename moveit_py/{ => src}/moveit/moveit_core/robot_model/robot_model.hpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/robot_state/robot_state.cpp (99%) rename moveit_py/{ => src}/moveit/moveit_core/robot_state/robot_state.hpp (97%) rename moveit_py/{ => src}/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp (99%) rename moveit_py/{ => src}/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_core/transforms/transforms.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_core/transforms/transforms.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_py_utils/CMakeLists.txt (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/src/copy_ros_msg.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp (100%) rename moveit_py/{ => src}/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp (98%) rename moveit_py/{ => src}/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp (93%) rename moveit_py/{ => src}/moveit/moveit_ros/moveit_cpp/planning_component.cpp (98%) rename moveit_py/{ => src}/moveit/moveit_ros/moveit_cpp/planning_component.hpp (95%) rename moveit_py/{ => src}/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp (98%) rename moveit_py/{ => src}/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp (96%) rename moveit_py/{ => src}/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp (99%) rename moveit_py/{ => src}/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp (96%) rename moveit_py/{ => src}/moveit/planning.cpp (96%) diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index ce224d1d21..1d3c6a2523 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(Eigen3 REQUIRED) # enables using the Python extensions from the build space for testing file(WRITE "${CMAKE_CURRENT_BINARY_DIR}/test_moveit/__init__.py" "") -add_subdirectory(moveit/moveit_py_utils) +add_subdirectory(src/moveit/moveit_py_utils) ament_python_install_package(moveit) @@ -37,20 +37,20 @@ endfunction() pybind11_add_module( core - moveit/core.cpp - moveit/moveit_core/collision_detection/collision_common.cpp - moveit/moveit_core/collision_detection/collision_matrix.cpp - moveit/moveit_core/collision_detection/world.cpp - moveit/moveit_core/controller_manager/controller_manager.cpp - moveit/moveit_core/kinematic_constraints/utils.cpp - moveit/moveit_core/planning_interface/planning_response.cpp - moveit/moveit_core/planning_scene/planning_scene.cpp - moveit/moveit_core/transforms/transforms.cpp - moveit/moveit_core/robot_model/joint_model.cpp - moveit/moveit_core/robot_model/joint_model_group.cpp - moveit/moveit_core/robot_model/robot_model.cpp - moveit/moveit_core/robot_state/robot_state.cpp - moveit/moveit_core/robot_trajectory/robot_trajectory.cpp + 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 @@ -69,11 +69,11 @@ configure_build_install_location(core) pybind11_add_module( planning - moveit/planning.cpp - moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp - moveit/moveit_ros/moveit_cpp/planning_component.cpp - moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp - moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp + 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 + src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp ) target_link_libraries( planning diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp b/moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp deleted file mode 100644 index 66a0b4a1e6..0000000000 --- a/moveit_py/moveit/moveit_core/collision_detection/collision_common.hpp +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace bind_collision_detection -{ -void initCollisionRequest(py::module& m); -void initCollisionResult(py::module& m); -} // namespace bind_collision_detection -} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp deleted file mode 100644 index 87473423a8..0000000000 --- a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace bind_collision_detection -{ -void initAcm(py::module& m); -} // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/collision_detection/world.cpp b/moveit_py/moveit/moveit_core/collision_detection/world.cpp deleted file mode 100644 index f15b21519c..0000000000 --- a/moveit_py/moveit/moveit_core/collision_detection/world.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "world.hpp" - -namespace moveit_py -{ -namespace bind_collision_detection -{ -void initWorld(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 \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/collision_detection/world.hpp b/moveit_py/moveit/moveit_core/collision_detection/world.hpp deleted file mode 100644 index 85ce9e77e8..0000000000 --- a/moveit_py/moveit/moveit_core/collision_detection/world.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace bind_collision_detection -{ -void initWorld(py::module& m); -} // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp deleted file mode 100644 index 29ecf11d42..0000000000 --- a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.hpp +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace py = pybind11; - -namespace moveit_py -{ -namespace bind_kinematic_constraints -{ -moveit_msgs::msg::Constraints constructLinkConstraint(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 constructJointConstraint(moveit::core::RobotState& robot_state, - moveit::core::JointModelGroup* joint_model_group, - double tolerance); - -moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr& node_name, - const std::string& ns); - -void initKinematicConstraints(py::module& m); - -} // namespace bind_kinematic_constraints -} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/core.cpp b/moveit_py/src/moveit/core.cpp similarity index 100% rename from moveit_py/moveit/core.cpp rename to moveit_py/src/moveit/core.cpp diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp similarity index 67% rename from moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp rename to moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp index 4dedd6e238..f5744a980d 100644 --- a/moveit_py/moveit/moveit_core/collision_detection/collision_common.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * 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.hpp" namespace moveit_py @@ -70,7 +106,7 @@ void initCollisionResult(py::module& m) .def(py::init<>()) - .def_readwrite("dsdsfdsgds", &collision_detection::CollisionResult::collision, + .def_readwrite("collision", &collision_detection::CollisionResult::collision, R"( bool: True if collision was found, false otherwise. )") diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp new file mode 100644 index 0000000000..e797f654c8 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp @@ -0,0 +1,54 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini + * 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. + *********************************************************************/ + +/* Authors: Peter David Fagan, Samuele Sandrini */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initCollisionRequest(py::module& m); +void initCollisionResult(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py diff --git a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp similarity index 68% rename from moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp rename to moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp index e2f70d1026..d6d14701b6 100644 --- a/moveit_py/moveit/moveit_core/collision_detection/collision_matrix.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -1,3 +1,39 @@ +/********************************************************************* + * 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.hpp" namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp new file mode 100644 index 0000000000..6f412c8676 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini + * 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. + *********************************************************************/ + +/* Authors: Peter David Fagan, Samuele Sandrini */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initAcm(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py \ No newline at end of file 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..72f44d17f8 --- /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.hpp" + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initWorld(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 \ No newline at end of file diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp new file mode 100644 index 0000000000..45799de5fd --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp @@ -0,0 +1,53 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Jafar Uruç + * Copyright (c) 2025, Samuele Sandrini + * 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ç, Samuele Sandrini */ + +#pragma once + +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_collision_detection +{ +void initWorld(py::module& m); +} // namespace bind_collision_detection +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/controller_manager/controller_manager.cpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp similarity index 100% rename from moveit_py/moveit/moveit_core/controller_manager/controller_manager.cpp rename to moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.cpp diff --git a/moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp similarity index 95% rename from moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp rename to moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp index 2fa5390292..dd4e690e5f 100644 --- a/moveit_py/moveit/moveit_core/controller_manager/controller_manager.hpp +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp similarity index 74% rename from moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp rename to moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp index 904a96853a..fb844df7ca 100644 --- a/moveit_py/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -1,3 +1,40 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini + * 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. + *********************************************************************/ + +/* Authors: Peter David Fagan, Samuele Sandrini */ + #include "utils.hpp" #include #include diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp new file mode 100644 index 0000000000..fc95933146 --- /dev/null +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -0,0 +1,69 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini + * 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. + *********************************************************************/ + +/* Authors: Peter David Fagan, Samuele Sandrini */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +namespace moveit_py +{ +namespace bind_kinematic_constraints +{ +moveit_msgs::msg::Constraints constructLinkConstraint(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 constructJointConstraint(moveit::core::RobotState& robot_state, + moveit::core::JointModelGroup* joint_model_group, + double tolerance); + +moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr& node_name, + const std::string& ns); + +void initKinematicConstraints(py::module& m); + +} // namespace bind_kinematic_constraints +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp similarity index 97% rename from moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp rename to moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp index 4e395d8224..bc43d52981 100644 --- a/moveit_py/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "planning_response.hpp" diff --git a/moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp rename to moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp index 5173d51393..3b73eb4670 100644 --- a/moveit_py/moveit/moveit_core/planning_interface/planning_response.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp similarity index 99% rename from moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp rename to moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp index 67a184206f..39be1e03a6 100644 --- a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Author: Peter David Fagan, Samuele Sandrini */ #include "planning_scene.hpp" #include diff --git a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp rename to moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp index 8f18b905d8..931a6803f0 100644 --- a/moveit_py/moveit/moveit_core/planning_scene/planning_scene.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Author: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp similarity index 100% rename from moveit_py/moveit/moveit_core/robot_model/joint_model.cpp rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model.cpp diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/robot_model/joint_model.hpp rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp index bcd72d0171..e602da75af 100644 --- a/moveit_py/moveit/moveit_core/robot_model/joint_model.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Jafar Uruç + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Jafar Uruç */ +/* Authors: Jafar Uruç, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp similarity index 100% rename from moveit_py/moveit/moveit_core/robot_model/joint_model_group.cpp rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.cpp diff --git a/moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp rename to moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp index be54b09e6a..c3ef4d5e19 100644 --- a/moveit_py/moveit/moveit_core/robot_model/joint_model_group.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/robot_model/robot_model.cpp b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp similarity index 98% rename from moveit_py/moveit/moveit_core/robot_model/robot_model.cpp rename to moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp index f9cb943c6a..42fd63b970 100644 --- a/moveit_py/moveit/moveit_core/robot_model/robot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "robot_model.hpp" #include diff --git a/moveit_py/moveit/moveit_core/robot_model/robot_model.hpp b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.hpp similarity index 100% rename from moveit_py/moveit/moveit_core/robot_model/robot_model.hpp rename to moveit_py/src/moveit/moveit_core/robot_model/robot_model.hpp diff --git a/moveit_py/moveit/moveit_core/robot_state/robot_state.cpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp similarity index 99% rename from moveit_py/moveit/moveit_core/robot_state/robot_state.cpp rename to moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp index 1a67df70f4..017fbf03ee 100644 --- a/moveit_py/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "robot_state.hpp" #include @@ -314,7 +315,7 @@ void initRobotState(py::module& m) str: represents the state tree of the robot state. )") - .def_property_readonly_static( + .def_property_readonly( "state_info", [](const moveit::core::RobotState& s) { std::stringstream ss; diff --git a/moveit_py/moveit/moveit_core/robot_state/robot_state.hpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp similarity index 97% rename from moveit_py/moveit/moveit_core/robot_state/robot_state.hpp rename to moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp index 7eb2d9b4fa..c7a4d57a7f 100644 --- a/moveit_py/moveit/moveit_core/robot_state/robot_state.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Author: Peter David Fagan, Samuele Sandrini */ #pragma once @@ -80,4 +81,4 @@ bool setToDefaultValues(moveit::core::RobotState* self, const std::string& joint void initRobotState(py::module& m); } // namespace bind_robot_state -} // namespace moveit_py +} // namespace moveit_py \ No newline at end of file diff --git a/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp similarity index 99% rename from moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp rename to moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp index a61f53213e..390bd768d9 100644 --- a/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "robot_trajectory.hpp" #include diff --git a/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp rename to moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp index 7e1cca5ace..06e09330d9 100644 --- a/moveit_py/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_core/transforms/transforms.cpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.cpp similarity index 100% rename from moveit_py/moveit/moveit_core/transforms/transforms.cpp rename to moveit_py/src/moveit/moveit_core/transforms/transforms.cpp diff --git a/moveit_py/moveit/moveit_core/transforms/transforms.hpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp similarity index 96% rename from moveit_py/moveit/moveit_core/transforms/transforms.hpp rename to moveit_py/src/moveit/moveit_core/transforms/transforms.hpp index c8a72ccdb9..ce903167fb 100644 --- a/moveit_py/moveit/moveit_core/transforms/transforms.hpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_py_utils/CMakeLists.txt b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt similarity index 100% rename from moveit_py/moveit/moveit_py_utils/CMakeLists.txt rename to moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt diff --git a/moveit_py/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 similarity index 100% rename from moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h rename to moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.h diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp similarity index 100% rename from moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp rename to moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/copy_ros_msg.hpp diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp similarity index 100% rename from moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp rename to moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp diff --git a/moveit_py/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 similarity index 100% rename from moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h rename to moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.h diff --git a/moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp similarity index 100% rename from moveit_py/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp rename to moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/ros_msg_typecasters.hpp diff --git a/moveit_py/moveit/moveit_py_utils/src/copy_ros_msg.cpp b/moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp similarity index 100% rename from moveit_py/moveit/moveit_py_utils/src/copy_ros_msg.cpp rename to moveit_py/src/moveit/moveit_py_utils/src/copy_ros_msg.cpp diff --git a/moveit_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp b/moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp similarity index 100% rename from moveit_py/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp rename to moveit_py/src/moveit/moveit_py_utils/src/ros_msg_typecasters.cpp diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp similarity index 98% rename from moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index f955a2983e..8a281be33b 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "moveit_cpp.hpp" #include @@ -167,7 +168,7 @@ void initMoveitPy(py::module& m) robot_trajectory (RobotTrajectory): The trajectory to execute. blocking (bool): Whether to block until execution finishes. Defaults to True. Returns: - :py:class::`moveit_py.controller_manager.ExecutionStatus`: The status of the execution. \\ TODO(@samu): verify the module return type + :py:class::`moveit_py.core.ExecutionStatus`: The status of the execution. )") .def("get_planning_component", &moveit_py::bind_moveit_cpp::getPlanningComponent, py::arg("planning_component_name"), py::return_value_policy::take_ownership, diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp similarity index 93% rename from moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp index e5194ddebc..501bc12e11 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +15,7 @@ * 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 + * * 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. * @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp similarity index 98% rename from moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp index 921beed704..31c722ccbf 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +15,7 @@ * 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 + * * 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. * @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "planning_component.hpp" #include @@ -154,10 +155,10 @@ void initPlanRequestParameters(py::module& m) R"( Planner parameters provided with a MotionPlanRequest. )") - .def(py::init([](std::shared_ptr& moveit_cpp) { // TODO(@samu), const std::string& ns + .def(py::init([](std::shared_ptr& moveit_cpp) { const rclcpp::Node::SharedPtr& node = moveit_cpp->getNode(); moveit_cpp::PlanningComponent::PlanRequestParameters params; - params.load(node); // TODO(@samu) , ns how to manage namespace? + params.load(node); return params; })) .def_readwrite("planner_id", &moveit_cpp::PlanningComponent::PlanRequestParameters::planner_id, diff --git a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp similarity index 95% rename from moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp rename to moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index d332e1a6f0..e92c2eefec 100644 --- a/moveit_py/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +15,7 @@ * 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 + * * 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. * @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp similarity index 98% rename from moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp rename to moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp index 011ff8c560..24c4f3c26e 100644 --- a/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +15,7 @@ * 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 + * * 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. * @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #include "planning_scene_monitor.hpp" diff --git a/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp similarity index 96% rename from moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp rename to moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp index 5cef0fced7..ee4fe8d07f 100644 --- a/moveit_py/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +15,7 @@ * 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 + * * 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. * @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Authors: Peter David Fagan, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp similarity index 99% rename from moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp rename to moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index caabc85794..c749392bb4 100644 --- a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Matthijs van der Burgh + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Matthijs van der Burgh */ +/* Author: Matthijs van der Burgh, Samuele Sandrini */ #include "trajectory_execution_manager.hpp" diff --git a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp similarity index 96% rename from moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp rename to moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp index c74caba117..dfd031f619 100644 --- a/moveit_py/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Matthijs van der Burgh + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Matthijs van der Burgh */ +/* Authors: Matthijs van der Burgh, Samuele Sandrini */ #pragma once diff --git a/moveit_py/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp similarity index 96% rename from moveit_py/moveit/planning.cpp rename to moveit_py/src/moveit/planning.cpp index 3fc5fd1a1e..e58c8dd098 100644 --- a/moveit_py/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan + * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,7 +33,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan */ +/* Author: Peter David Fagan, Samuele Sandrini */ #include "moveit_ros/moveit_cpp/moveit_cpp.hpp" #include "moveit_ros/moveit_cpp/planning_component.hpp" From 237f394c06730700df0ddd7af29423bd2d757208 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 17:54:28 +0200 Subject: [PATCH 09/28] Remove comments about missing Humble interfaces --- .../planning_interface/planning_response.cpp | 18 --------- .../planning_interface/planning_response.hpp | 5 --- .../planning_scene/planning_scene.cpp | 3 -- .../robot_trajectory/robot_trajectory.cpp | 29 -------------- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 1 - .../moveit_cpp/planning_component.hpp | 2 - .../planning_scene_monitor.cpp | 38 ------------------- .../trajectory_execution_manager.cpp | 35 ----------------- 8 files changed, 131 deletions(-) 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 index bc43d52981..1a4a407b1f 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -47,13 +47,6 @@ getMotionPlanResponseTrajectory(std::shared_ptrtrajectory_; } -// moveit_msgs::msg::RobotState -// getMotionPlanResponseStartState(std::shared_ptr& response) -// { -// moveit_msgs::msg::RobotState robot_state_msg = response->start_state; -// return robot_state_msg; -// } - moveit_msgs::msg::MoveItErrorCodes getMotionPlanResponseErrorCode(std::shared_ptr& response) { @@ -67,11 +60,6 @@ double getMotionPlanResponsePlanningTime(std::shared_ptrplanning_time_; } -// std::string getMotionPlanResponsePlannerId(std::shared_ptr& response) -// { -// return response->planner_id; -// } - void initMotionPlanResponse(py::module& m) { py::module planning_interface = m.def_submodule("planning_interface"); @@ -90,12 +78,6 @@ void initMotionPlanResponse(py::module& m) .def_property("error_code", &moveit_py::bind_planning_interface::getMotionPlanResponseErrorCode, nullptr, py::return_value_policy::copy, R"()") - // .def_property("start_state", &moveit_py::bind_planning_interface::getMotionPlanResponseStartState, 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; }); diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp index 3b73eb4670..6d36eef127 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp @@ -54,16 +54,11 @@ namespace bind_planning_interface std::shared_ptr getMotionPlanResponseTrajectory(std::shared_ptr& response); -// moveit_msgs::msg::RobotState -// getMotionPlanResponseStartState(std::shared_ptr& response); - moveit_msgs::msg::MoveItErrorCodes getMotionPlanResponseErrorCode(std::shared_ptr& response); double getMotionPlanResponsePlanningTime(std::shared_ptr& response); -// std::string getMotionPlanResponsePlannerId(std::shared_ptr& response); - void initMotionPlanResponse(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 index 39be1e03a6..3464ab7e8e 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -146,9 +146,6 @@ void initPlanningScene(py::module& m) &planning_scene::PlanningScene::getAllowedCollisionMatrix, py::return_value_policy::reference_internal) - // .def_property("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, - // &planning_scene::PlanningScene::setAllowedCollisionMatrix, - // py::return_value_policy::reference_internal) // methods .def("__copy__", [](const planning_scene::PlanningScene* self) { 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 index 390bd768d9..1fc6d0ebff 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -136,35 +136,6 @@ void initRobotTrajectory(py::module& m) Returns: list of float: The duration from previous of each waypoint in the trajectory. )") - // .def("apply_totg_time_parameterization", &trajectory_processing::applyTOTGTimeParameterization, - // py::arg("velocity_scaling_factor"), py::arg("acceleration_scaling_factor"), py::kw_only(), - // py::arg("path_tolerance") = 0.1, py::arg("resample_dt") = 0.1, py::arg("min_angle_change") = 0.001, - // R"( - // Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm. - - // Args: - // velocity_scaling_factor (float): The velocity scaling factor. - // acceleration_scaling_factor (float): The acceleration scaling factor. - // path_tolerance (float): The path tolerance to use for time parameterization (default: 0.1). - // resample_dt (float): The time step to use for time parameterization (default: 0.1). - // min_angle_change (float): The minimum angle change to use for time parameterization (default: 0.001). - // Returns: - // bool: True if the trajectory was successfully retimed, false otherwise. - // )") - // .def("apply_ruckig_smoothing", &trajectory_processing::applyRuckigSmoothing, py::arg("velocity_scaling_factor"), - // py::arg("acceleration_scaling_factor"), py::kw_only(), py::arg("mitigate_overshoot") = false, - // py::arg("overshoot_threshold") = 0.01, - // R"( - // Applies Ruckig smoothing to the trajectory. - - // Args: - // velocity_scaling_factor (float): The velocity scaling factor. - // acceleration_scaling_factor (float): The acceleration scaling factor. - // mitigate_overshoot (bool): Whether to mitigate overshoot during smoothing (default: false). - // overshoot_threshold (float): The maximum allowed overshoot during smoothing (default: 0.01). - // Returns: - // bool: True if the trajectory was successfully retimed, false otherwise. - // )") .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::getRobotTrajectoryMsg, py::arg("joint_filter") = std::vector(), R"( 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 index 8a281be33b..b8b20df9c0 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -38,7 +38,6 @@ #include "moveit_cpp.hpp" #include #include -// #include #include namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index e92c2eefec..6c1c9d2d3f 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -73,8 +73,6 @@ bool setStartState(std::shared_ptr& planning_comp void initPlanRequestParameters(py::module& m); -// void initMultiPlanRequestParameters(py::module& m); - void initPlanningComponent(py::module& m); void initPlanSolution(py::module& m); 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 index 24c4f3c26e..ea177d8791 100644 --- 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 @@ -42,27 +42,6 @@ namespace moveit_py namespace bind_planning_scene_monitor { -// bool processCollisionObject(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, -// moveit_msgs::msg::CollisionObject& collision_object_msg, -// std::optional color_msg) -// { -// moveit_msgs::msg::CollisionObject::ConstSharedPtr const_ptr = -// std::make_shared(collision_object_msg); -// if (color_msg) -// { -// return planning_scene_monitor->processCollisionObjectMsg(const_ptr, *std::move(color_msg)); -// } -// return planning_scene_monitor->processCollisionObjectMsg(const_ptr); -// } - -// bool processAttachedCollisionObjectMsg(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, -// moveit_msgs::msg::AttachedCollisionObject& attached_collision_object_msg) -// { -// moveit_msgs::msg::AttachedCollisionObject::ConstSharedPtr const_ptr = -// std::make_shared(attached_collision_object_msg); -// return planning_scene_monitor->processAttachedCollisionObjectMsg(const_ptr); -// } - LockedPlanningSceneContextManagerRO readOnly(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) { @@ -158,23 +137,6 @@ void initPlanningSceneMonitor(py::module& m) R"( Clears the octomap. )") - // .def("process_collision_object", &moveit_py::bind_planning_scene_monitor::processCollisionObject, - // py::arg("collision_object_msg"), py::arg("color_msg") = nullptr, - // R"( - // Apply a collision object to the planning scene. - - // Args: - // collision_object_msg (moveit_msgs.msg.CollisionObject): The collision object to apply to the planning scene. - // )") - // .def("process_attached_collision_object", - // &moveit_py::bind_planning_scene_monitor::processAttachedCollisionObjectMsg, - // py::arg("attached_collision_object_msg"), - // R"( - // Apply an attached collision object msg to the planning scene. - - // Args: - // attached_collision_object_msg (moveit_msgs.msg.AttachedCollisionObject): The attached collision object to apply to the planning scene. - // )") .def("new_planning_scene_message", &planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage, py::arg("scene"), 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 c749392bb4..ffad45935f 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 @@ -210,12 +210,6 @@ void initTrajectoryExecutionManager(py::module& m) If a controller takes longer than expected, the trajectory is canceled. )") - // .def("execution_duration_monitoring", - // &trajectory_execution_manager::TrajectoryExecutionManager::executionDurationMonitoring, - // R"( - // Get the current status of the monitoring of trajectory execution duration. - // )") - .def("set_allowed_execution_duration_scaling", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedExecutionDurationScaling, py::arg("scaling"), @@ -223,24 +217,12 @@ void initTrajectoryExecutionManager(py::module& m) When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution. )") - // .def("allowed_execution_duration_scaling", - // &trajectory_execution_manager::TrajectoryExecutionManager::allowedExecutionDurationScaling, - // R"( - // Get the current scaling of the duration of a trajectory to get the allowed duration of execution. - // )") - .def("set_allowed_goal_duration_margin", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedGoalDurationMargin, py::arg("margin"), R"( When determining the expected duration of a trajectory, this additional margin s applied after scalign to allow more than the expected execution time before triggering trajectory cancel. )") - // .def("allowed_goal_duration_margin", - // &trajectory_execution_manager::TrajectoryExecutionManager::allowedGoalDurationMargin, - // R"( - // Get the current margin of the duration of a trajectory to get the allowed duration of execution. - // )") - .def("set_execution_velocity_scaling", &trajectory_execution_manager::TrajectoryExecutionManager::setExecutionVelocityScaling, py::arg("scaling"), R"( @@ -249,34 +231,17 @@ void initTrajectoryExecutionManager(py::module& m) By default, this is 1.0 )") - // .def("execution_velocity_scaling", - // &trajectory_execution_manager::TrajectoryExecutionManager::executionVelocityScaling, - // R"( - // Get the current scaling of the execution velocities. - // )") - .def("set_allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::setAllowedStartTolerance, py::arg("tolerance"), R"( Set joint-value tolerance for validating trajectory's start point against current robot state. )") - // .def("allowed_start_tolerance", &trajectory_execution_manager::TrajectoryExecutionManager::allowedStartTolerance, - // R"( - // Get the current joint-value for validating trajectory's start point against current robot state. - // )") - .def("set_wait_for_trajectory_completion", &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"), R"( Enable or disable waiting for trajectory completion. )") - - // .def("wait_for_trajectory_completion", - // &trajectory_execution_manager::TrajectoryExecutionManager::waitForTrajectoryCompletion, - // R"( - // Get the current state of waiting for the trajectory being completed. - // )") ; // ToDo(MatthijsBurgh) From db1d79e1b77f24089f3adc3d08df309d36fcf883 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 19:12:13 +0200 Subject: [PATCH 10/28] Add stub files --- moveit_py/moveit/__init__.pyi | 3 + moveit_py/moveit/core/__init__.pyi | 4 + moveit_py/moveit/core/collision_detection.pyi | 27 ++++++ moveit_py/moveit/core/controller_manager.pyi | 5 + .../moveit/core/kinematic_constraints.pyi | 3 + moveit_py/moveit/core/planning_interface.pyi | 9 ++ moveit_py/moveit/core/planning_scene.pyi | 35 +++++++ moveit_py/moveit/core/robot_model.pyi | 60 ++++++++++++ moveit_py/moveit/core/robot_state.pyi | 36 +++++++ moveit_py/moveit/core/robot_trajectory.pyi | 19 ++++ moveit_py/moveit/planning.pyi | 95 +++++++++++++++++++ moveit_py/moveit/utils.pyi | 2 + 12 files changed, 298 insertions(+) create mode 100644 moveit_py/moveit/__init__.pyi create mode 100644 moveit_py/moveit/core/__init__.pyi create mode 100644 moveit_py/moveit/core/collision_detection.pyi create mode 100644 moveit_py/moveit/core/controller_manager.pyi create mode 100644 moveit_py/moveit/core/kinematic_constraints.pyi create mode 100644 moveit_py/moveit/core/planning_interface.pyi create mode 100644 moveit_py/moveit/core/planning_scene.pyi create mode 100644 moveit_py/moveit/core/robot_model.pyi create mode 100644 moveit_py/moveit/core/robot_state.pyi create mode 100644 moveit_py/moveit/core/robot_trajectory.pyi create mode 100644 moveit_py/moveit/planning.pyi create mode 100644 moveit_py/moveit/utils.pyi diff --git a/moveit_py/moveit/__init__.pyi b/moveit_py/moveit/__init__.pyi new file mode 100644 index 0000000000..713f9aff74 --- /dev/null +++ b/moveit_py/moveit/__init__.pyi @@ -0,0 +1,3 @@ +from moveit.core import * +from moveit.planning import * +from moveit.utils import get_launch_params_filepaths as get_launch_params_filepaths diff --git a/moveit_py/moveit/core/__init__.pyi b/moveit_py/moveit/core/__init__.pyi new file mode 100644 index 0000000000..35267ed1dc --- /dev/null +++ b/moveit_py/moveit/core/__init__.pyi @@ -0,0 +1,4 @@ +from . import collision_detection as collision_detection, controller_manager as controller_manager, kinematic_constraints as kinematic_constraints, planning_interface as planning_interface, planning_scene as planning_scene, robot_model as robot_model, robot_state as robot_state, robot_trajectory as robot_trajectory + +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..f33209a10f --- /dev/null +++ b/moveit_py/moveit/core/collision_detection.pyi @@ -0,0 +1,27 @@ +from _typeshed import Incomplete + +class AllowedCollisionMatrix: + def __init__(self, *args, **kwargs) -> None: ... + def clear(self, *args, **kwargs): ... + def get_entry(self, *args, **kwargs): ... + def remove_entry(self, *args, **kwargs): ... + def set_entry(self, *args, **kwargs): ... + +class CollisionRequest: + contacts: Incomplete + cost: Incomplete + distance: Incomplete + joint_model_group_name: Incomplete + max_contacts: Incomplete + max_contacts_per_pair: Incomplete + max_cost_sources: Incomplete + verbose: Incomplete + def __init__(self, *args, **kwargs) -> None: ... + +class CollisionResult: + collision: Incomplete + contact_count: Incomplete + contacts: Incomplete + cost_sources: Incomplete + distance: Incomplete + 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..5856147eca --- /dev/null +++ b/moveit_py/moveit/core/controller_manager.pyi @@ -0,0 +1,5 @@ +class ExecutionStatus: + def __init__(self, *args, **kwargs) -> None: ... + def __bool__(self) -> bool: ... + @property + def status(self): ... diff --git a/moveit_py/moveit/core/kinematic_constraints.pyi b/moveit_py/moveit/core/kinematic_constraints.pyi new file mode 100644 index 0000000000..4755843c53 --- /dev/null +++ b/moveit_py/moveit/core/kinematic_constraints.pyi @@ -0,0 +1,3 @@ +def construct_constraints_from_node(*args, **kwargs): ... +def construct_joint_constraint(*args, **kwargs): ... +def construct_link_constraint(*args, **kwargs): ... diff --git a/moveit_py/moveit/core/planning_interface.pyi b/moveit_py/moveit/core/planning_interface.pyi new file mode 100644 index 0000000000..2b0e962c0e --- /dev/null +++ b/moveit_py/moveit/core/planning_interface.pyi @@ -0,0 +1,9 @@ +class MotionPlanResponse: + def __init__(self, *args, **kwargs) -> None: ... + def __bool__(self) -> bool: ... + @property + def error_code(self): ... + @property + def planning_time(self): ... + @property + def trajectory(self): ... diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi new file mode 100644 index 0000000000..a0aa6dda50 --- /dev/null +++ b/moveit_py/moveit/core/planning_scene.pyi @@ -0,0 +1,35 @@ +from _typeshed import Incomplete + +class PlanningScene: + current_state: Incomplete + name: Incomplete + def __init__(self, *args, **kwargs) -> None: ... + def apply_collision_object(self, *args, **kwargs): ... + def check_collision(self, *args, **kwargs): ... + def check_collision_unpadded(self, *args, **kwargs): ... + def check_self_collision(self, *args, **kwargs): ... + def get_frame_transform(self, *args, **kwargs): ... + def is_path_valid(self, *args, **kwargs): ... + def is_state_colliding(self, *args, **kwargs): ... + def is_state_constrained(self, *args, **kwargs): ... + def is_state_valid(self, *args, **kwargs): ... + def knows_frame_transform(self, *args, **kwargs): ... + def load_geometry_from_file(self, *args, **kwargs): ... + def process_attached_collision_object(self, *args, **kwargs): ... + def process_octomap(self, *args, **kwargs): ... + def process_planning_scene_world(self, *args, **kwargs): ... + def remove_all_collision_objects(self, *args, **kwargs): ... + def save_geometry_to_file(self, *args, **kwargs): ... + def set_object_color(self, *args, **kwargs): ... + def __copy__(self): ... + def __deepcopy__(self): ... + @property + def allowed_collision_matrix(self): ... + @property + def planning_frame(self): ... + @property + def planning_scene_message(self): ... + @property + def robot_model(self): ... + @property + def transforms(self): ... diff --git a/moveit_py/moveit/core/robot_model.pyi b/moveit_py/moveit/core/robot_model.pyi new file mode 100644 index 0000000000..4941562826 --- /dev/null +++ b/moveit_py/moveit/core/robot_model.pyi @@ -0,0 +1,60 @@ +class JointModelGroup: + def __init__(self, *args, **kwargs) -> None: ... + def satisfies_position_bounds(self, *args, **kwargs): ... + @property + def active_joint_model_bounds(self): ... + @property + def active_joint_model_names(self): ... + @property + def eef_name(self): ... + @property + def joint_model_names(self): ... + @property + def link_model_names(self): ... + @property + def name(self): ... + +class RobotModel: + def __init__(self, *args, **kwargs) -> None: ... + def get_joint_model_group(self, *args, **kwargs): ... + def get_model_info(self, *args, **kwargs): ... + def has_joint_model_group(self, *args, **kwargs): ... + @property + def end_effectors(self): ... + @property + def joint_model_group_names(self): ... + @property + def joint_model_groups(self): ... + @property + def model_frame(self): ... + @property + def name(self): ... + @property + def root_joint_name(self): ... + +class VariableBounds: + def __init__(self, *args, **kwargs) -> None: ... + @property + def acceleration_bounded(self): ... + @property + def jerk_bounded(self): ... + @property + def max_acceleration(self): ... + @property + def max_jerk(self): ... + @property + def max_position(self): ... + @property + def max_velocity(self): ... + @property + def min_acceleration(self): ... + @property + def min_jerk(self): ... + @property + def min_position(self): ... + @property + def min_velocity(self): ... + @property + def position_bounded(self): ... + @property + def velocity_bounded(self): ... diff --git a/moveit_py/moveit/core/robot_state.pyi b/moveit_py/moveit/core/robot_state.pyi new file mode 100644 index 0000000000..40ebd614f8 --- /dev/null +++ b/moveit_py/moveit/core/robot_state.pyi @@ -0,0 +1,36 @@ +from _typeshed import Incomplete + +class RobotState: + joint_accelerations: Incomplete + joint_efforts: Incomplete + joint_positions: Incomplete + joint_velocities: Incomplete + def __init__(self, *args, **kwargs) -> None: ... + def clear_attached_bodies(self, *args, **kwargs): ... + def get_frame_transform(self, *args, **kwargs): ... + def get_global_link_transform(self, *args, **kwargs): ... + def get_jacobian(self, *args, **kwargs): ... + def get_joint_group_accelerations(self, *args, **kwargs): ... + def get_joint_group_positions(self, *args, **kwargs): ... + def get_joint_group_velocities(self, *args, **kwargs): ... + def get_pose(self, *args, **kwargs): ... + def set_from_ik(self, *args, **kwargs): ... + def set_joint_group_accelerations(self, *args, **kwargs): ... + def set_joint_group_active_positions(self, *args, **kwargs): ... + def set_joint_group_positions(self, *args, **kwargs): ... + def set_joint_group_velocities(self, *args, **kwargs): ... + def set_to_default_values(self, *args, **kwargs): ... + def set_to_random_positions(self, *args, **kwargs): ... + def update(self, *args, **kwargs): ... + def __copy__(self): ... + def __deepcopy__(self): ... + @property + def dirty(self): ... + @property + def robot_model(self): ... + @property + def state_info(self): ... + @property + def state_tree(self): ... + +def robotStateToRobotStateMsg(*args, **kwargs): ... diff --git a/moveit_py/moveit/core/robot_trajectory.pyi b/moveit_py/moveit/core/robot_trajectory.pyi new file mode 100644 index 0000000000..48bfc9ffdc --- /dev/null +++ b/moveit_py/moveit/core/robot_trajectory.pyi @@ -0,0 +1,19 @@ +from _typeshed import Incomplete + +class RobotTrajectory: + joint_model_group_name: Incomplete + def __init__(self, *args, **kwargs) -> None: ... + def get_robot_trajectory_msg(self, *args, **kwargs): ... + def get_waypoint_durations(self, *args, **kwargs): ... + def set_robot_trajectory_msg(self, *args, **kwargs): ... + def unwind(self, *args, **kwargs): ... + def __getitem__(self, index): ... + def __iter__(self): ... + def __len__(self) -> int: ... + def __reverse__(self, *args, **kwargs): ... + @property + def average_segment_duration(self): ... + @property + def duration(self): ... + @property + def robot_model(self): ... diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi new file mode 100644 index 0000000000..a11b599bf2 --- /dev/null +++ b/moveit_py/moveit/planning.pyi @@ -0,0 +1,95 @@ +import types +from _typeshed import Incomplete + +class LockedPlanningSceneContextManagerRO: + def __init__(self, *args, **kwargs) -> None: ... + def __enter__(self): ... + def __exit__(self, type: type[BaseException] | None, value: BaseException | None, traceback: types.TracebackType | None): ... + +class LockedPlanningSceneContextManagerRW: + def __init__(self, *args, **kwargs) -> None: ... + def __enter__(self): ... + def __exit__(self, type: type[BaseException] | None, value: BaseException | None, traceback: types.TracebackType | None): ... + +class MoveItPy: + def __init__(self, *args, **kwargs) -> None: ... + def execute(self, *args, **kwargs): ... + def get_planning_component(self, *args, **kwargs): ... + def get_planning_scene_monitor(self, *args, **kwargs): ... + def get_robot_model(self, *args, **kwargs): ... + def get_trajectory_execution_manager(self, *args, **kwargs): ... + def shutdown(self, *args, **kwargs): ... + +class PlanRequestParameters: + max_acceleration_scaling_factor: Incomplete + max_velocity_scaling_factor: Incomplete + planner_id: Incomplete + planning_attempts: Incomplete + planning_pipeline: Incomplete + planning_time: Incomplete + def __init__(self, *args, **kwargs) -> None: ... + +class PlanSolution: + def __init__(self, *args, **kwargs) -> None: ... + def __bool__(self) -> bool: ... + @property + def error_code(self): ... + @property + def start_state(self): ... + @property + def trajectory(self): ... + +class PlanningComponent: + def __init__(self, *args, **kwargs) -> None: ... + def get_named_target_state_values(self, *args, **kwargs): ... + def get_start_state(self, *args, **kwargs): ... + def plan(self, *args, **kwargs): ... + def set_goal_state(self, *args, **kwargs): ... + def set_path_constraints(self, *args, **kwargs): ... + def set_start_state(self, *args, **kwargs): ... + def set_start_state_to_current_state(self, *args, **kwargs): ... + def set_workspace(self, *args, **kwargs): ... + def unset_workspace(self, *args, **kwargs): ... + @property + def named_target_states(self): ... + @property + def planning_group_name(self): ... + +class PlanningSceneMonitor: + def __init__(self, *args, **kwargs) -> None: ... + def clear_octomap(self, *args, **kwargs): ... + def new_planning_scene_message(self, *args, **kwargs): ... + def read_only(self, *args, **kwargs): ... + def read_write(self, *args, **kwargs): ... + def request_planning_scene_state(self, *args, **kwargs): ... + def start_scene_monitor(self, *args, **kwargs): ... + def start_state_monitor(self, *args, **kwargs): ... + def stop_scene_monitor(self, *args, **kwargs): ... + def stop_state_monitor(self, *args, **kwargs): ... + def update_frame_transforms(self, *args, **kwargs): ... + def wait_for_current_robot_state(self, *args, **kwargs): ... + @property + def name(self): ... + +class TrajectoryExecutionManager: + def __init__(self, *args, **kwargs) -> None: ... + def are_controllers_active(self, *args, **kwargs): ... + def enable_execution_duration_monitoring(self, *args, **kwargs): ... + def ensure_active_controller(self, *args, **kwargs): ... + def ensure_active_controllers(self, *args, **kwargs): ... + def ensure_active_controllers_for_group(self, *args, **kwargs): ... + def ensure_active_controllers_for_joints(self, *args, **kwargs): ... + def execute(self, *args, **kwargs): ... + def execute_and_wait(self, *args, **kwargs): ... + def get_last_execution_status(self, *args, **kwargs): ... + def is_controller_active(self, *args, **kwargs): ... + def is_managing_controllers(self, *args, **kwargs): ... + def process_event(self, *args, **kwargs): ... + def push(self, *args, **kwargs): ... + def set_allowed_execution_duration_scaling(self, *args, **kwargs): ... + def set_allowed_goal_duration_margin(self, *args, **kwargs): ... + def set_allowed_start_tolerance(self, *args, **kwargs): ... + def set_execution_velocity_scaling(self, *args, **kwargs): ... + def set_wait_for_trajectory_completion(self, *args, **kwargs): ... + def stop_execution(self, *args, **kwargs): ... + def wait_for_execution(self, *args, **kwargs): ... diff --git a/moveit_py/moveit/utils.pyi b/moveit_py/moveit/utils.pyi new file mode 100644 index 0000000000..ce88a9f080 --- /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_filepaths(cli_args: list[str] | None = None) -> list[str]: ... From c97ec4a7e82296a272eb1b69b060f8380c1080a6 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 19:15:08 +0200 Subject: [PATCH 11/28] Add Sphinx configuration and index --- moveit_py/docs/script/conf.py | 86 +++++++++++++++++++++++++++++++++ moveit_py/docs/script/index.rst | 13 +++++ 2 files changed, 99 insertions(+) create mode 100644 moveit_py/docs/script/conf.py create mode 100644 moveit_py/docs/script/index.rst diff --git a/moveit_py/docs/script/conf.py b/moveit_py/docs/script/conf.py new file mode 100644 index 0000000000..d1036c875e --- /dev/null +++ b/moveit_py/docs/script/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; 2025, Samuele Sandrini" +author = "Peter David Fagan, Samuele Sandrini" + + +# -- 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" \ No newline at end of file diff --git a/moveit_py/docs/script/index.rst b/moveit_py/docs/script/index.rst new file mode 100644 index 0000000000..9d598f9ae2 --- /dev/null +++ b/moveit_py/docs/script/index.rst @@ -0,0 +1,13 @@ +MoveItPy API Documentation +===================================== + +.. toctree:: + +.. autosummary:: + :toctree: _autosummary + :recursive: + + moveit_py.core + moveit_py.planning + moveit_py.policies + moveit_py.servo_client From 72217ccb8b6e6f6a12baf90bef8464023b22f7d2 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 19:20:53 +0200 Subject: [PATCH 12/28] Apply clang-format to moveit_py --- .../planning_scene/planning_scene.cpp | 5 ++-- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 5 ++-- .../moveit_cpp/planning_component.cpp | 27 +++++++++---------- .../trajectory_execution_manager.cpp | 3 +-- 4 files changed, 18 insertions(+), 22 deletions(-) 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 index 3464ab7e8e..2bd3c8232c 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -142,9 +142,8 @@ void initPlanningScene(py::module& m) py::return_value_policy::move) .def_property("transforms", py::overload_cast<>(&planning_scene::PlanningScene::getTransforms), nullptr) - .def_property_readonly("allowed_collision_matrix", - &planning_scene::PlanningScene::getAllowedCollisionMatrix, - py::return_value_policy::reference_internal) + .def_property_readonly("allowed_collision_matrix", &planning_scene::PlanningScene::getAllowedCollisionMatrix, + py::return_value_policy::reference_internal) // methods .def("__copy__", 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 index b8b20df9c0..de4ea77c64 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -157,9 +157,8 @@ void initMoveitPy(py::module& m) R"( Initialize moveit_cpp node and the planning scene service. )") - .def("execute", - &moveit_cpp::MoveItCpp::execute, - py::arg("group_name"), py::arg("robot_trajectory"), py::arg("blocking") = true, + .def("execute", &moveit_cpp::MoveItCpp::execute, py::arg("group_name"), py::arg("robot_trajectory"), + py::arg("blocking") = true, R"( Execute a trajectory for a specific planning group. Args: 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 index 31c722ccbf..563bddf20b 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -44,10 +44,9 @@ namespace bind_planning_component { moveit_cpp::PlanningComponent::PlanSolution plan(std::shared_ptr& planning_component, - std::shared_ptr& parameters -) + std::shared_ptr& parameters) { - if(parameters) + if (parameters) { // cast parameters std::shared_ptr const_single_plan_parameters = @@ -323,19 +322,19 @@ void initPlanningComponent(py::module& m) } void initPlanSolution(py::module& m) { -py::class_(m, "PlanSolution", R"( + py::class_( + m, "PlanSolution", R"( Represents a planning solution, including the trajectory and any error codes. )") - .def_readonly("start_state", &moveit_cpp::PlanningComponent::PlanSolution::start_state, - "The starting state used for planning.") - .def_readonly("trajectory", &moveit_cpp::PlanningComponent::PlanSolution::trajectory, - "The resulting planned trajectory.") - .def_readonly("error_code", &moveit_cpp::PlanningComponent::PlanSolution::error_code, - "The MoveIt error code of the planning request.") - .def("__bool__", [](const moveit_cpp::PlanningComponent::PlanSolution &self) { - return static_cast(self); - }, "Return True if planning was successful."); + .def_readonly("start_state", &moveit_cpp::PlanningComponent::PlanSolution::start_state, + "The starting state used for planning.") + .def_readonly("trajectory", &moveit_cpp::PlanningComponent::PlanSolution::trajectory, + "The resulting planned trajectory.") + .def_readonly("error_code", &moveit_cpp::PlanningComponent::PlanSolution::error_code, + "The MoveIt error code of the planning request.") + .def( + "__bool__", [](const moveit_cpp::PlanningComponent::PlanSolution& self) { return static_cast(self); }, + "Return True if planning was successful."); } } // namespace bind_planning_component } // 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 ffad45935f..5611a12c44 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 @@ -241,8 +241,7 @@ void initTrajectoryExecutionManager(py::module& m) &trajectory_execution_manager::TrajectoryExecutionManager::setWaitForTrajectoryCompletion, py::arg("flag"), R"( Enable or disable waiting for trajectory completion. - )") - ; + )"); // ToDo(MatthijsBurgh) // https://github.com/moveit/moveit2/issues/2442 From af08fc7e3b03cd428efd24e81bc35f6043c0d9c6 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 21:00:01 +0200 Subject: [PATCH 13/28] Apply pre-commit fixes --- moveit_py/CMakeLists.txt | 2 +- moveit_py/README.md | 7 ------- moveit_py/docs/script/conf.py | 2 +- moveit_py/moveit/core/__init__.pyi | 11 ++++++++++- moveit_py/moveit/planning.pyi | 14 ++++++++++++-- moveit_py/package.xml | 2 +- .../collision_detection/collision_matrix.cpp | 2 +- .../collision_detection/collision_matrix.hpp | 2 +- .../moveit_core/collision_detection/world.cpp | 2 +- .../moveit_core/collision_detection/world.hpp | 2 +- .../moveit_core/kinematic_constraints/utils.cpp | 2 +- .../moveit_core/kinematic_constraints/utils.hpp | 2 +- .../moveit/moveit_core/robot_state/robot_state.hpp | 2 +- 13 files changed, 32 insertions(+), 20 deletions(-) diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 1d3c6a2523..f9154b5c39 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -105,4 +105,4 @@ endif() ament_export_targets(moveit_py_utilsTargets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_ros_planning_interface) -ament_package() \ No newline at end of file +ament_package() diff --git a/moveit_py/README.md b/moveit_py/README.md index cdb6666637..79e50c1dcb 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -19,10 +19,3 @@ 9. In `initPlanRequestParameters`, the method call `params.load(node, ns)` includes `ns`, which was not used or present in Humble. Needs clarification on how namespaces are now handled for parameter loading. 10. In `controller_manager`, `:py:class:\`moveit_py.controller_manager.ExecutionStatus\`` documents the execution status. \\ TODO(@samu): verify the actual return type from the module. - - - - - - - diff --git a/moveit_py/docs/script/conf.py b/moveit_py/docs/script/conf.py index d1036c875e..ec11adfe85 100644 --- a/moveit_py/docs/script/conf.py +++ b/moveit_py/docs/script/conf.py @@ -83,4 +83,4 @@ # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ["_static"] -master_doc = "index" \ No newline at end of file +master_doc = "index" diff --git a/moveit_py/moveit/core/__init__.pyi b/moveit_py/moveit/core/__init__.pyi index 35267ed1dc..faa01f8153 100644 --- a/moveit_py/moveit/core/__init__.pyi +++ b/moveit_py/moveit/core/__init__.pyi @@ -1,4 +1,13 @@ -from . import collision_detection as collision_detection, controller_manager as controller_manager, kinematic_constraints as kinematic_constraints, planning_interface as planning_interface, planning_scene as planning_scene, robot_model as robot_model, robot_state as robot_state, robot_trajectory as robot_trajectory +from . import ( + collision_detection as collision_detection, + controller_manager as controller_manager, + kinematic_constraints as kinematic_constraints, + planning_interface as planning_interface, + planning_scene as planning_scene, + robot_model as robot_model, + robot_state as robot_state, + robot_trajectory as robot_trajectory, +) class World: def __init__(self, *args, **kwargs) -> None: ... diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index a11b599bf2..737f7677c2 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -4,12 +4,22 @@ from _typeshed import Incomplete class LockedPlanningSceneContextManagerRO: def __init__(self, *args, **kwargs) -> None: ... def __enter__(self): ... - def __exit__(self, type: type[BaseException] | None, value: BaseException | None, traceback: types.TracebackType | None): ... + def __exit__( + self, + type: type[BaseException] | None, + value: BaseException | None, + traceback: types.TracebackType | None, + ): ... class LockedPlanningSceneContextManagerRW: def __init__(self, *args, **kwargs) -> None: ... def __enter__(self): ... - def __exit__(self, type: type[BaseException] | None, value: BaseException | None, traceback: types.TracebackType | None): ... + def __exit__( + self, + type: type[BaseException] | None, + value: BaseException | None, + traceback: types.TracebackType | None, + ): ... class MoveItPy: def __init__(self, *args, **kwargs) -> None: ... diff --git a/moveit_py/package.xml b/moveit_py/package.xml index 1db1481c35..fd08509339 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -31,4 +31,4 @@ ament_cmake - \ No newline at end of file + 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 index d6d14701b6..c3be69f620 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.cpp @@ -133,4 +133,4 @@ void initAcm(py::module& m) // getEntry } // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp index 6f412c8676..50415739d2 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -50,4 +50,4 @@ namespace bind_collision_detection { void initAcm(py::module& m); } // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file +} // 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 index 72f44d17f8..37c3f54a7a 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.cpp @@ -47,4 +47,4 @@ void initWorld(py::module& m) py::class_(m, "World").def(py::init<>()); } } // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp index 45799de5fd..44fa728f28 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp @@ -50,4 +50,4 @@ namespace bind_collision_detection { void initWorld(py::module& m); } // namespace bind_collision_detection -} // namespace moveit_py \ No newline at end of file +} // 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 index fb844df7ca..3c858b4b79 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -157,4 +157,4 @@ void initKinematicConstraints(py::module& m) } } // namespace bind_kinematic_constraints -} // namespace moveit_py \ No newline at end of file +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp index fc95933146..5e8b21a0b6 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -66,4 +66,4 @@ moveit_msgs::msg::Constraints constructConstraintsFromNode(const std::shared_ptr void initKinematicConstraints(py::module& m); } // namespace bind_kinematic_constraints -} // namespace moveit_py \ No newline at end of file +} // namespace moveit_py diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp index c7a4d57a7f..3b557077ff 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp @@ -81,4 +81,4 @@ bool setToDefaultValues(moveit::core::RobotState* self, const std::string& joint void initRobotState(py::module& m); } // namespace bind_robot_state -} // namespace moveit_py \ No newline at end of file +} // namespace moveit_py From 2d5fefbf244b1c9746e7c66007ca6befe0d65ddc Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Mon, 26 May 2025 21:31:18 +0200 Subject: [PATCH 14/28] Fix authors in package xml --- moveit_py/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/moveit_py/package.xml b/moveit_py/package.xml index fd08509339..a38246a9ef 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -2,13 +2,15 @@ moveit_py - 2.13.2 + 2.5.9 Python binding for MoveIt 2 + Peter David Fagan Samuele Sandrini BSD + Peter David Fagan Samuele Sandrini ament_cmake From 959028cd34402faeef6ae8b0ffdead5e08b78a8e Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Tue, 27 May 2025 19:38:14 +0200 Subject: [PATCH 15/28] Fix after stub gen, unregistered type RobotTrajectory --- moveit_py/src/moveit/planning.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index e58c8dd098..1d26669706 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -44,6 +44,8 @@ PYBIND11_MODULE(planning, m) { m.doc() = "Python bindings for moveit_cpp functionalities."; + py::module_::import("moveit.core"); + // Provide custom function signatures py::options options; options.disable_function_signatures(); From c1457dca29c92dadc06b150d73e9f0459ffde454 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Wed, 28 May 2025 13:47:41 +0200 Subject: [PATCH 16/28] Fix unregistered type MoveItErrorCode --- .../moveit_cpp/planning_component.cpp | 29 +++++++------------ .../moveit_cpp/planning_component.hpp | 5 +++- 2 files changed, 14 insertions(+), 20 deletions(-) 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 index 563bddf20b..5697c02e42 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -147,6 +147,14 @@ bool setStartState(std::shared_ptr& planning_comp } } +moveit_msgs::msg::MoveItErrorCodes +getMotionPlanResponseErrorCode(std::shared_ptr& plan_solution) +{ + moveit_msgs::msg::MoveItErrorCodes error_code_msg = + static_cast(plan_solution->error_code); + return error_code_msg; +} + void initPlanRequestParameters(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::plan_request_parameter_vector); -// } void initPlanningComponent(py::module& m) { py::class_>(m, "PlanningComponent", @@ -330,8 +321,8 @@ void initPlanSolution(py::module& m) "The starting state used for planning.") .def_readonly("trajectory", &moveit_cpp::PlanningComponent::PlanSolution::trajectory, "The resulting planned trajectory.") - .def_readonly("error_code", &moveit_cpp::PlanningComponent::PlanSolution::error_code, - "The MoveIt error code of the planning request.") + .def_property("error_code", &moveit_py::bind_planning_component::getMotionPlanResponseErrorCode, nullptr, + py::return_value_policy::copy, "The MoveIt error code of the planning request.") .def( "__bool__", [](const moveit_cpp::PlanningComponent::PlanSolution& self) { return static_cast(self); }, "Return True if planning was successful."); diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index 6c1c9d2d3f..5ed6fccdf4 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -48,10 +48,10 @@ #include #include #include +#include #include "moveit_cpp.hpp" #include "../planning_scene_monitor/planning_scene_monitor.hpp" -#include "../../moveit_core/planning_interface/planning_response.hpp" namespace py = pybind11; @@ -75,6 +75,9 @@ void initPlanRequestParameters(py::module& m); void initPlanningComponent(py::module& m); +moveit_msgs::msg::MoveItErrorCodes +getMotionPlanResponseErrorCode(std::shared_ptr& plan_solution); + void initPlanSolution(py::module& m); } // namespace bind_planning_component } // namespace moveit_py From 3fce12b6db22c08e83cbadf43fc6c928d04fcdc3 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Wed, 28 May 2025 16:10:14 +0200 Subject: [PATCH 17/28] Fix PlanSolution properties --- .../moveit_cpp/planning_component.cpp | 28 +++++++++++++------ .../moveit_cpp/planning_component.hpp | 9 +++++- 2 files changed, 28 insertions(+), 9 deletions(-) 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 index 5697c02e42..0d19020cf6 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -147,14 +147,27 @@ bool setStartState(std::shared_ptr& planning_comp } } +moveit_msgs::msg::RobotState +getMotionPlanSolutionStartState(std::shared_ptr& plan_solution) +{ + moveit_msgs::msg::RobotState robot_state_msg = plan_solution->start_state; + return robot_state_msg; +} + moveit_msgs::msg::MoveItErrorCodes -getMotionPlanResponseErrorCode(std::shared_ptr& plan_solution) +getMotionPlanSolutionErrorCode(std::shared_ptr& plan_solution) { moveit_msgs::msg::MoveItErrorCodes error_code_msg = static_cast(plan_solution->error_code); return error_code_msg; } +std::shared_ptr +getMotionPlanSolutionTrajectory(std::shared_ptr& plan_solution) +{ + return plan_solution->trajectory; +} + void initPlanRequestParameters(py::module& m) { py::class_( m, "PlanSolution", R"( - Represents a planning solution, including the trajectory and any error codes. -)") - .def_readonly("start_state", &moveit_cpp::PlanningComponent::PlanSolution::start_state, - "The starting state used for planning.") - .def_readonly("trajectory", &moveit_cpp::PlanningComponent::PlanSolution::trajectory, - "The resulting planned trajectory.") - .def_property("error_code", &moveit_py::bind_planning_component::getMotionPlanResponseErrorCode, nullptr, + Represents a planning solution, including the trajectory and any error codes.)") + .def_property("start_state", &moveit_py::bind_planning_component::getMotionPlanSolutionStartState, nullptr, + py::return_value_policy::copy, "The starting state used for planning.") + .def_property("trajectory", &moveit_py::bind_planning_component::getMotionPlanSolutionTrajectory, nullptr, + py::return_value_policy::copy, "The resulting planned trajectory.") + .def_property("error_code", &moveit_py::bind_planning_component::getMotionPlanSolutionErrorCode, nullptr, py::return_value_policy::copy, "The MoveIt error code of the planning request.") .def( "__bool__", [](const moveit_cpp::PlanningComponent::PlanSolution& self) { return static_cast(self); }, diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index 5ed6fccdf4..22d0a0908a 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include "moveit_cpp.hpp" #include "../planning_scene_monitor/planning_scene_monitor.hpp" @@ -76,7 +77,13 @@ void initPlanRequestParameters(py::module& m); void initPlanningComponent(py::module& m); moveit_msgs::msg::MoveItErrorCodes -getMotionPlanResponseErrorCode(std::shared_ptr& plan_solution); +getMotionPlanSolutionErrorCode(std::shared_ptr& plan_solution); + +moveit_msgs::msg::RobotState +getMotionPlanSolutionStartState(std::shared_ptr& plan_solution); + +std::shared_ptr +getMotionPlanSolutionTrajectory(std::shared_ptr& plan_solution); void initPlanSolution(py::module& m); } // namespace bind_planning_component From b4ea55a76c93f8b2a03684c7182c1c259e5f5aa9 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Wed, 28 May 2025 21:39:14 +0200 Subject: [PATCH 18/28] Added readme to moveit_py --- moveit_py/README.md | 40 +++++++++++++++++++++++++++------------- moveit_py/banner.png | Bin 0 -> 168492 bytes 2 files changed, 27 insertions(+), 13 deletions(-) create mode 100644 moveit_py/banner.png diff --git a/moveit_py/README.md b/moveit_py/README.md index 79e50c1dcb..e903761ee5 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -1,21 +1,35 @@ -## Notes on Python Bindings Modifications +# MoveIt 2 Python Library + -1. `planning_interface::MotionPlanResponse` does not have the `start_state` and `planner_id` attributes. I commented out the related property definitions with their setters and getters. +`moveit_py` 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. -2. In `planning_scene.cpp`, the method `PlanningScene::setAllowedCollisionMatrix` was not implemented in `moveit_core::planning_scene`. I copied the implementation, but since full compilation would be required, I commented out the setter method and replaced it with a read-only version. +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: -3. In `trajectory_tools`, newer versions include `applyTOTGTimeParameterization`, which is not available here. It applies the Time-Optimal Trajectory Generation; currently commented out, but we could copy the implementation if needed. + ├── 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 -4. In `planning_scene_monitor.cpp` (part of `moveit_ros`, now in `moveit_py`), `process_attached_collision_object` and `process_collision_object` were defined but are not actual methods of `PlanningSceneMonitor`—only of `PlanningScene`. Consider implementing them in `PlanningSceneMonitor` as well. +## 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). -5. In `moveit.cpp`, `moveit/utils/logger.hpp` was used, but it no longer exists. It seems only `rclcpp`'s logger is now used. I followed the logging pattern from other modules. +## Contribution Guidelines +Community contributions are welcome. -6. In `trajectory_execution_manager`, commented out the `execution_duration_monitoring` getter and other missing getters that are not defined in the `.h/.cpp` files. +For detailed contribution guidelines please consult the official [MoveIt contribution guidelines](https://moveit.ros.org/documentation/contributing/). -7. In `planning_component.hpp`, removed all references to `MultiPipelinePlanRequestParameters` and `plan` branches based on `single_plan_request_param`; retained only `parameters` as done in MoveIt 2. +## Citing the Library +If you use this library in your work please use the following citation: +```bibtex +@software{fagan2023moveitpy, + author = {Fagan, Peter David}, + title = {{MoveIt 2 Python Library: A Software Library for Robotics Education and Research}}, + url = {https://github.com/moveit/moveit2/tree/main/moveit_py}, + year = {2023} +} +``` -8. In `planning_component.hpp/.cpp`, `plan()` used to return a `planning_interface::MotionPlanResponse`, but in Humble it returns a `moveit_cpp::PlanningComponent::PlanSolution`. Although `MotionPlanResponse` still exists in `planning_response.hpp`, the Python equivalent for `PlanSolution` is missing and needs to be implemented and exposed via bindings. - -9. In `initPlanRequestParameters`, the method call `params.load(node, ns)` includes `ns`, which was not used or present in Humble. Needs clarification on how namespaces are now handled for parameter loading. - -10. In `controller_manager`, `:py:class:\`moveit_py.controller_manager.ExecutionStatus\`` documents the execution status. \\ TODO(@samu): verify the actual return type from the module. +## 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 Goerner (@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 0000000000000000000000000000000000000000..e1ea199c18d57fe3fd08d72b5f6a0fd8cbac75ed GIT binary patch literal 168492 zcmagF2{hDy-v+GgyRolhUm9x&jmeUcC2RIQ8L4a$nXzRzmP~e%y%5S05`(f0Lm?sj zt&_cC?0fHTx}WEL&w1YGzMs?SV5Xz_e3$EUUDxOOC77A$G1BqUk&%%x8tB7sk&#jL zkda+nr=bE~@m{q@0beLQw2igN$m%fkr>>O1&q8kcw~Wcif+fhvB4WtMj)9jVR>{Z$ z70AfeZQXhXHR;-MOg;C;LPCQ`AF)*PE6vBi zi_`%I#=6wYbSxmS)cQjmH{dst8NjtIkyAg<4ubggn&jxhbC;?jDx*YK#S>ZG9e%YA zU7g5N_56)JazFoia`$uF#!SeU`{h~ovZ?HCk~FMhF#Jj7VDgc$IYKy9!jY5V;Jx*G zG+sgd@r@gypt=L5=*zuY&u(NKGjz>=AsDOYeo;u)d7?L_Y3IyeW7lw6yWgMjtDL;1 z06zW6LeQ6Qak$uMQgQP}OO7Hca-V zn-tvyLcb2o*>^et7yllkWv0DdL5UO;emB2UhVe8nZZ-B}vW29tAZFD9%M8DjQ5N*X z8KFv+`a=2q8Z-?#C-*5EuNtK^Poww;7^3wB6M~-*wk7^-WSa)0_HySz>~_YlT>Tv* z%L1Qu$}xcpTVxe#=&D>OZC9GC({nG4ejl3J{_@>iPhCRysI!3kz+js;Dj)-7HSV`S z<0tT~`*UpBX8|;B;{p*GCImKxovUT?P%RSAp~RCsB=(8@B5~0jgqg6ACmxP|BGASd zaR0be#%0c_hDEu9tjd|B8Jv#i$1+n49N+KJ`i9kK-GxrQ`C5pW{jhqM&a@S^^sKHT z-MclBBhQczwa1mYUorJ&qYb~V|8iz_(*~zrcq62AYRHW!?Pe!26%G!=CksFWr3kNC z-cw=?`4H_N`a>1RD_dj+32?^(0qFYhQCn}19wsWf!u}w!As-1NA{U9>gfqMt@ZmL5 z_HQ&6BC*)t$f=vK0@9m%kCQU^s4=b=347SwQaP#qH4vN%GQ+QT|KVdZpdvw;DMBK!NC3x^ZKMnIgCtNT+Z|f1#Y8j6dNT0;Nxi(fRXhn>aUSd1JbeGPOq+A)&wuxBspl3V_(J63o% zan3Rq^8uC)Y^8OJB4?r^rxO&xtI4)@6MCX5H0M$xqY${J-(Z?(Z@k{HI-WXf8lLYe zg$>+U)xM$EqN5nI$DD87;a^WQHcD)ezNwe1? z@sv~~1L@7uuZ2`#Q=!$10*O5|1!jWvNpf}0TTXDgF+b zO$&M@!ZmcrZY@6~VQGuxxBGB5Rjea8JBRAtPL9`Qa!Fmx>Ibbz#SarApHmv z0@s*=H7DWgq>2xek+8@jdUlP}|NjO8yZUe)IM#GIFuMtkb}_l2$2I@B`u8<^Y{V6U z1MC+~r2o;i{Gcd&2ky>T|1gafB*HfJX_GIt?m9u&k@F2~FU~G5vF|eh|AlW|YwTA1 z3hR7rP+!PM9hYD1GSQwp-tGiOGS?u*%5 zINR5q^W&do5mD^l=1dJ2*v(sqac5+&2pC1z=sq=V*`SX~&J6r^Agm<0>c#9Ub#0Lt z3>;k~R)z$Tms}T%+3mn{GN+`Pp#U`SZ^{$xLr!VTwCT_!YX$e7o zqEMlvRbiue=MwRf6@_fK%_#j?mXovJHWJLmX?85s+Tt(Jg>O4)G?+(H{XA{#e#FMw z&(GW=IDLPu74xD$E)XPKqWx=hQq@YBtxO9c8n0TNcyBeP);|idl`>lI8UpXEhRgb( zmW}ckK6N6@X>lcxbVDB3Fufp`?a@qps90rb#b?#j&vcoa&sRd&?pLkDN+3!#p(1<6 zWz4+P2$fBgn%K}L>0+LonZPvEu93b~+W*|gXVVF)Srv)kUW zEvDXkB~%seOGBOSj}y3fT}8~`8#gFODqrCUg^Q})t)IIzVq5V$Bot)U^d`hb1_$!;jhi9^(2NYuP0Ho8||6Ug5ch`;mv)6MM?D(4RWfDeOIuT~g?Se4a!u0`y~I3hlcmQ^%u zX9?~m$4iA2?wCllaDvm#gvnhO3=z{(qMl~l;DY@vJi#W&^02UWu5^mGX|nuyrXp_H z%&B(gCB`!uU6~gogAomgRrO^r=E#+n=n`3_3u9N#ihtS_zIKL4bn}@Hd*bI6*Wbsz z*J@-JzIHk#3@xPm{6@Qlb-_q`Y4ubCKAMzFDY9wx_#XnTC|BbMPDuj)a^Y;bbe+bq zs#G2|&0_WUGS996Q~)6XG$Us&G;U{LKva~7JX5@JCTEf50_}Lrf?1i6B_0y`3A7aE zW{2?q4VWo9UsIzJFGb)MheLulKXdK2CNI$r-M?DXc+YGy-RHFAca6wif0pQ-FxGDp znyXC_7GV=5M~FAJ<-f{)Hs0GwzhY!Esll_Wf$WT5ZRn#-_j_Ub@{I`Ra7(*h=HvAY zt1(t%U1syMOFsD&cD>ZERs^zjWj5S(q>~4hGo9SMI2nlx0IB2r?7C@6jd@mQ%hkxV zQfHF#b+SQLBM>+%c9t@AsJ&iK4!P;O9WZl8!`|l_6~?fRfu?BEp3m-f6S|hfz?<>} zH9^v8F!vS#4@q*7zU?N=x%@X&Cs#yXAKOd!P)KED6j2nyeHcy)+{+fP6QO5!YQd!C z2i39bAvt`(nFg{c3Hk5k1YB7ymsr;1we#p;-i$YKrSbP$OOwJoV9?B?b1M~Ut1148 zUJTQg%vWTmArW83x~=od*$$oZp`=(C8Bk<%ksV1ZlQ9oqq5F?3uqXaoSJKo+32%98_pXbp|i8_`H8oyiI_jvZ|1^#_M-}Wa{rqueg>*5IwcI@WGcF1WE zVu?pZ1#kP$_O2k0JZPjTF-v;NhX*5+`^+A;>GFyx_vopfs;mE6j-Dk>tDxmyv|>nJ zo{01VF0fDhZxt3ycFm}%FL1!8^qNM6J7ocrSX2i!&S^H%R>UYb+W)i`-=qlichEE#LSoS|bJ8E5nG` z!-AJmbyn^sF$mM9TkSh!&Qzcu=w&VBT^th0vY{FomaBaEw0x+7#n`ul zg)yFWpRpo*$9xGMyE8voji$RMw(S%bTblY>2T^TK+p6@YR#{{ZrTW4B_?1+^({SM8#q@>9hS;{jz!|94icA=m;T$6AV5$0OD_^|JB|6s(1#%< zKhz9Jr0PPfso4KyQ-2}+MKJfz3gaY`3vj1kl&mDOlYzf{qFlrxXRjie0fxdD#$z|1 zJP(YwOYhsAX+}JiW;ZuGY8l96jAkigOFc%6>aML<9HY_I(B5n{_r)+y>wGhmXCmyASY+uz$)0|EXUKtv6wd-Ll7~8 zCC5VGA^CJBfgS*N4+%D^3jxvp@eYC%v(>j_(&pC8tJAynfWSS|uKBLuMFdjJ3 zfCoxJ%I_f=c&_R0fkp?>mXum+$8__)w^$i|- zDDIk_i4)#5i0@P7_pmKi3;6PA1ZnvMlxee@x5P7m(z&ZET=}^LD~deHC=|ae-+BF> zvGYZ6#*K;7Ql*zxw)S>ack*4aNc_?zH(31Yc3R>#nJ8HBjgB)5hHDgPqkH=-mX2LnXM+fNn?A48rjd0a zR0V`sXLhh9u9}tkaBR!djkSW1Gl%ZeV(%`Ou|bli6^<<1y>rh68l=qankh2nt4vcF zEho1?F#oM4X*q!eytKN>fY(3kG&WsIS%OwHyw0mk7yS7VgjU0f#isMe&gwEh z4`{Ta0=6hM0ePy8Lhi6(R;Y=G_dX=x^nin<0sNkuuyE3T*>DmM)4K??o2pWwBL9mb zp}4s4jU-Ay%T0VXPLM34%DsMD+Dd!Y{dA9 zJXj1|Gpj**71+app;FV-rW=j9NUSCrH=vzK&PtOGz~*NfzGReyZNW-9`P0>iqAn+A ziF~8h(seWOS1tM_UeBeDO7Mvy_AN!w*F8)2aw0bT{yW!WntMSPMT|@<=gchaMu%Nr zEFjB_n-F^vE!u43cYRw?_@z{JIsd_TuSHw5t2}=fl!1{5#;K2KYeD9}%$)cWeA0Sf zy_e#N-mZ+(PlA|cxNIdCLjwlh+07rc>LW!8hG8wumx#!4r~Ypw#ty4-1eErk7EA*x zOR8q4!2z&0fGgPtG)Ald6a|YEqN6L+|NkjX5;m&s37KA|M7J#AZ7C}HUM~7bM@t2* z)vCO+HHcyzf8RKo&e+-@st+g!8@yJzH}68kucinVyQBwcv?Wr;G1Y=|EvR46H3}J z)POj!i6kH4Pyq$(-h?si{2_porS2B8U7L3N|D{v%SdlX>sDXJD7S?%SvsZGxLXbKcTjTP#;_rP2b>Y0(ozDOC1w&C$zFoP+CK?&jju}O za-VTSyNb4CdXdMJn`N1JDh5;M+!en;b-z!h)HvZ}IwJDCJ^@#itphu+H6m?SQr5tZ zQPVX*g#_Y6ZOovH#C5>p(R`5&+*N)XfSgf8GWD!DVS6^SkC!j&@+|JAo{&$FNX z^v5;7u)NuDKIO+o`}gw;MEz{JWi+L2J6cYw*WHglBxklIjwut7M**W=N<<_| zfEANHnx_v`L#h#gpx5cDfbwee$3X$VK=H4N4k+jT_o$;Bful>3WQa?2c*qW~W=7um zXFKcF6sI$Nqfq`%gN`K=IIQoRENh&w)KjLac?E$j`{W*P%Gc5iy|x}AtB@EzO;E;;FKUl_Xmch)PjXB=uQ)x5 z|1zvyDQpX$7Mr$#U)SzW<`Y?fBc`t9rX1ug7;x>rn1JT%&DOSbRbSWbI;H5xdFp;H zRkgw}f~&UFgaCCE^fO-%0F!}Ik(-t$K!aFW8UyS+z*uP6t-_EFuhe8<#?Z9Mzm{fa zA#9OI5k_(<1U&ovC=;Wtp~P|%PLx8x|vLtxi* zyT0;s#kA?o*6C5S1hQNKgI^^py2dQ7i?*nk>VDmrXRi;t_1N|EG~mAan&m~SSfCDb z2TzhbtJ9n+f_Ly<;Z;64Rojw_#F4cGTt`(BNn4nQ^{=Bz-=yvnBao*7>-*Ax&iwNz zSJ8cnbUSC$d;f1`rPY01tCw|2&0Zb_!mYI#>2v0fv$f!d-S>*$30B19nZL0Z>7{YW(oRg-$W`@9 zFw)Pj@JAKjnZaA63@R-Wj|arS@=SoirAW-s1!@8Vj>(jWJwQq|oIrnj>pY+%H68yL z5P*%8<4z8Ku8fq_Drg8tp{VpnF2t+;j55k|cPp=Ix>U`S+mrUduBFKH6OP=8jm!6g z;;)?XMdd`bgr-bMYmFhLR48&!;;Y7Ny~IG0j&rV`R-+@2Km}efBiQuDh#`0 zW_A_gDhuE35mny|1&QT-ftCDH_d#3ujA)eHe9R&3o*Mqvvp~%tOz*Bn+CmTvW!20_ zJUm>YGbHs5Mn4keVZj`j>HsWF0yw)%z*a#4E>zV>P+L?ZA}?xlNrjeBt@;~L?sF@ku1-TUHxQd!~9*tdwz&e&>DH?p3An%`ITm13BE zT-C0b3tc3Wzxws)tWvHl8nG*_St$UcS4!=P^STOIXh3#oPKHZWTFfeID<*^z%#Q+- zJGA|)LJP4;0wca!cXdn~-arSQ8uIm6SavpU#Te2V3XW$L1*1+XzZ~UBKE;2RYXJcT zAVb*!-MeuaB1=TJKW4*lrISpZL#h0&K4U-&KLPgf2cVMP7FGl!0RqCKE%_gaU8m%O zxMX)dOR&JWqv8VnY4}7SMEA*MuL&Rol++lMmHb zI58ZlZ-YKgOYoc}g)&!e-mqy!$u=W@Fw*@h4jCNdnAKJWyf4Bp(ez)bbw$YzHPEh& z?z3|Q!YV9V(VsY|s=c%o$B>?xYh!Iy<&fGEJ+(?LKwy-gc(!*NlxjM0`erAre@hRz zc0UqyQe;I=!^2jF_<_{Y3xy(nOhp%^maH0NLin zVOrmOkzYBd#<5bkz28<6$T3C@a{)ZQk8e>-pGs4a{b13uiD~xpyNXQ*N>^dX;&%#8 z1Noa?<-)4I1w|UN_JY3Lj3Y<(8KtYwoc4-@?#W?7-r0J5f8Kxf?i9hpHZKbCeEi1g zO9u13#Bc{Je@Hz)3I7RXL}VRQck1#HA3zKM1qjI9Jiwe2L<8iMMDju{!R7!5sEGQH zYKk2GMjIikhw2Q0JJrjERD$C@A3VEaXy#Pn)1|1m(QNsx#=B*Et0f)BvAiH9bjP4V zl);c6npU^Wb`)MJQ-PBGu;^uKHVp^e{Ulz(CQ!c`RcX~;?f%p6UaME0?vDx7n%{Tb zK?qnxai@T(?Wpkzp4X-4;<#yw(9P|lBbz4Xn59%A``6f_EmQ5S6qMvI_2mW65-2T1 zlSX8Gm?UNOVFj9iifn(J1dNG7VZUe}nKI*Iacnd#w`+Q#s(rL#zq0QO`B=~|S;+@o`mg@PwfPDJ0sxvz z%(JPfBxLBfx_?b*aDU+vxMRRxkJXh-ISGBk%W1lo2XZp1`X(SgfUqm!251QXCH(4xDbk!yqcSWU@T5pQ2k46~fI=RdD&fP0wM&3EU2q0JfW`p%k9QFw zBDbO0;o;XZxn0nbWG@x>p_MxJrr9UIdO40pnY58N`bXxdlKF%7#oF=tI=E$>6nkU! z-tx#uHoQ6mDKiL1uriK(s`3O^eJyIJ9Jj|&jy4QhjJ#PrEY<0~?~wMo?-g&!eO>~B z1=-d4+Exm`E-Y2}dsxx$%Rx)8mwA%u*szi7uY`IvjI?86b!L8gPLjiDc$pugb=pRU|KCl85QBZ)mu)!r|at&f6F=+L$Z+BE6Z|hTNZz57zmcL3{ zw5K%V&HejWd2$aVb;zO@@5L`It-TZR`4Q*Z%^D@SKFt>SK%a8aS~R{sIBqyT1Q$?-(Al6QxXq^`HLtxS*35pR(oh-#=Y+ zZ4vUl93}UxJm+FmXDZ7gHDlB$_1X(9AtQR_Sr(cYwad#}-2@Dka<8f2%SRU@6=a8z zIzlXq6vUt?x#8k}%Z)oFAzD8c9Y$^zmCD6t^mpY+Krlablfsu)n-$FW>YZm*diDyC za#j<533V>P1qYCj3oVJW$kDzggDV&*#4ICo)N%6aCU$AN4CGyC2oZ6V>``nmUg5DJgJ**q~5ij9xGFv9@bGw|FEMTJEXfS`n5?- z|F$EawBiy~8>YzrjI+^vX@i>!fk%6eh@Ch#7{dt93XIfMN2bR#IIS8uS;c^;Qe zl}$L(gqu3S?Ne{-N2bLX$o64o>2^VBfSibuDY)P%C^eeg4&V|ri^MyMJL_;@yr4HB zNlyaxvu%=3y?K_8JaxT_#w88xXl(t9XZ$CZg~ffWcp7gsrkL>lRO+Szh+e}t+}V@G zYD1|&*{}IhlKrFCee&rNQf=rjx+g6UT&%)6S^^}t&vZZR=-KIQ+aC2}$&% zghM(owQ0xp{HULM%tQD1**#l2jwAUezHJFrfTvQ$PMRyjuxf)T<4_ zE+xj@CEjHF_O`Y@pq+fg*qF!W$!x9a=;?1bjwV%su4VbwY%Hhg%&5?`Uz_Hb3gs)d zTqP^64ZrIv+#t#^e-szmFHA?fgBT;mKBjfU*Nx&oBL z+M1-t{egK&4Zt*(Hdu5W!EbC&`sv$nb~qzErqK*oEDx~PAh+=e;(Km9xp7r!j2PYzx4JPIrqMAX|dAn z?I3rbTW(Jkxf7v$m7lY0rI}^#yrefgzlAO>vfL+suJtW_df>Qlnsbcw+A>}JF@ZKu zkCFU{)09KWE_tT0dc($8c3s_*{_fjN)EZ zQ=VR#&?J)Uw)xU_Z{Z3fk4PnTE@g?A8ZAez^35-q?P8tdV)&DRl>nVPTD)jGe&G!_Ph(vOy>Jd|lUe!;*zpU(#GRO;XL z71+6JDbS*Fj$(d=+~AsN_>l=PiRKje5)0*D07wwjA-ZHQtFpfJvLZ=0?Y2T!AcW=$YV+@p zav5uD{jFpyqh~Gn^RiqYb?8rb;m3qk|5mm1SCZWVZ1sl#LmJ4`1qus&#sCBCcd1n#!8r#DZPkFzvl@&n{&5YJ>_OkyhS=LG?! z(%pXbl%2Aq<8$nItn2JWk*p-wS!UO{gx}8;T!WLyMGCH(*AEP)D+aSM8n9axyj3iz z_S0QiESmf4VTU5~0D26#e;UmxLScOoz$|D67;(DOr2|nf;$f|_V&LJczm!EXpUIAv zLY|=vZ#(-4Z4%*D!Q`w*Imtq+{fkrF9IJ?5ernr@Go&!vF<~vJ9;ito4h+!6P}twoRpiizlz?e z4EHF(j@F#_&;4bBsRreML2C+5A=eZF|2YTjk&)~`%VV=Q(+-*d-T=O>bK6KUx3JHD zm^;UsT1HGb*{ReplzB(82C&NWUDOi8d{F^Y-;@wIk`!i})zhVx{2Z*um4aWt8T5px zli}|Febd~PDkWNX`((;Pa$y1kzy9dx?6ZTf<@xA-c*@lED@^DwN{UDBTGmn2pu6rv z>363bAM?%&1Odk@8%d#5#ua{Hx(fTHNUDry_l} zYI;TAWJG~~*N@bvNxu%>qm5wQFW!3?}TNcPuTHkOBwn|=`SThn#SvvSIE-z_>m zhkxXg&{CFbNj|NdL$Y5@Of%TOM%>lO4H{AcuYZ4NDV{v~vqn3xs{2=VLi}5QJ?-xj zNhx)Tl`m?8r1|&?%1vhdQPb7X>?FMCJ8fyjvYx%quVc`u_uoq$ERKoG!%=jYQz#BO ztMcOImOdaoMz{Jn#(a3L0#@sW(G38{0S1zJ6F5915m~R-FJ47I3q=hqc>}(s#w75s z#sE!XFGvx^@b6(B7;Zh#`hMws-YsgJPg8}*Fd(XdfyI?! z;Slc50M1>ba;w^EFtUpRIhXeU8Cl2RXUo}BOzu?QZ=Vo6r~QxrHeS+=_U#=jsS^3r zIaFcN4~x10hLA6#Dw9XL@q_i-Rd@VfT>Jf^vqoMoj8N}st$Mkf=X<10 zxVf~Gm8T;|0dLk{)GGqa0ZDMpJ7d39t!yt+#qn#Hm8$@R0QIq93OQ#1>IDR}fGv!v zlt$&KUuGU*%a=hpt(PLwFHO{V6|`8=K6u6t4d{`sGUU!Hg1SpmPR0t!3!~R$|cIU$uV zq(T38D4$u>aeQ_ zUsTDXkbjC%a2?Y%*sd^u>dx@}A}BEIC>IC+VFJ9VX+79EkeJ-r5UGTb$X6-iU>Qhi z)wtRHAF>&6eI$%d*wyP{9ln zPH0DVZ9V)=#3DmHD8qU;pK^&HtCV6UE8=R5*nK7Xm=0Y_pbyTNM|R>5`W6E@)CK0BVZxc(tv64 z5owRB@Iai2=?CSF92G#6UC^e`n~1Mwon4aREDjIc7>-3J$qsFbC>~!UFn(fvnO+*v z_j1{rx(3{Z-?}NN3rAchTc!E*@P*`^g7B(GO8UA7B|~S3VV)G-mow1Oov8P7{c$B* z%b;H?qr(=%Hwt_z;!Ji1B%$(Xxgx-UM#ewLk;{B&2N~{1Av$9c`Z`;Otrjkd2#3M_ zzP;$4=A7L!NxocsF55{CTtE_f!)6-(B_t=qfo6s&Kd)_ydnNjpg`Ikf0(-4t!Y}@R z#5ouqJEr8C3YkQ&Bjm!ZXj@t1e~N4;Wj*1YCHhT#YS@(t>G!iOxNqlT+_mn*t7tqL zY!)O?zhqL1>HFrFL2$60!M&<69j*80g{r0OOg1cYX?+oK5G(uw%rSQSV(~!m<|Oz9 zFtSjtFJx!I=-mU{Utkio(=!R62eD6$3PAEH1K9b!=}^bZj9^n|Hz0Qb7@=w@>FH7_ zH~TJ_S1?7v$oKs2d)o3XX8G|#(R7g|I#jvmogsfdDogwzeCNWH8u{U0u@(^2_EYbB zDG&U(DtBh6VwuSR-xX^$dp<7myV<)vG4x6OrsEH}_|kt~jwBGUw=pLUP#?7vw+qaq zZo_qfczB>wrnV$PS~odhIZ4W$IIs3-o*>2ce@x?lqCw@Y*wvoC`ue_?%xMV+K~_fg z!QC8&5gjB*O;&9^V()b)t@{A|gBq^NISouQ^rRTj<^yMM>Hf<0 zC)@xZ2Xq#5b~+MkKcju5J3yLfGx`3dwx)c*u~{|yNwJ_7e(Ksie4^?mGyKNx%5DOF zeo+1D!0L_no{F0>3;Bou*>A`M0p^7qne^-&iZi+EW~gZ{N%(Q=U~8VWJ|if%Z4Is} z3_n39gW;fWWwim}!8`PfAge6=_wJLh_2EMNqP~RFi={r9PRSF=p}UwbjEIr?PL+Fs zW3u&Ei$pr!Glm~UbGBh0Ne?~BtcR=exBWu1LEB3!>eMUFM!B;WFA|ZLXIL;#vVp#f zJOR}I-&odBK~h=LJHR7c2cjY?@=3_hQy>h!{h+g-{NJZtNLB+}<|T4wr^_)q@Ix9t zLv;Q5XF?obSv0{%xcD6tNE!&H1U8Qh#GIG|b}UNw&8>ia)sbg6{A}O%HR!5*GqTO= z+AWgaz4-!7=h8LJWJPc9Z0Ekh?}L-?>Aa?Khher z59SSxD1VYHc_2V>KkD}uZ#tTeeoC~5|-4u#N$P{X{J6WRpCAK9LC_QktM3E83LGIY64TDZLG-8f9SDS0aT ziXAj(0mVV+zI>h(mkBbcQS>FIShf5%c{W+W%ERY=Ok=8+s*pbr1O8g)`o$q-u=k~Y zE&%!!erhHuz0wn6mYs|}b=s^}djHUycWjqVU4k;}hHU~3hcs>f+j?}vvqu0Mw+@)A zB(0$VX<{@B42FO(I3*Be`enI9nhyO3u7^szA2A_}%Wc$|i#`k+YX^v@{#4kU85ISw0Ygxh{;w9AvCpYO`!Esd20 zb}X8%c*Uk2;MWx?$N|PQhnHuHVMbhKt{i-0aZ+FYwxJBL#14>&sR3>tAhoiyf$*J6 z2)q!Og-rlMv3A9hT}~??Xn0NSHz^bo`Kco4lD5iVzZl|rWSFfj#=-#Z7=JpZyYv;%>H zY5+5!P_AnV%J_F9$K*Hw$ngP+Yx|q``S({5*k@z(bW4h)7yQ{0qi&!AxPww20EbcMeAlu-Gn+W-M zm$Bk8x4lzbF=YgNPkM_YFENL4i$%xuT3KBo$Z zy}mf&Fnrj@R;H)doPR$`lEKc=e`70|g(yc0QGd%K2ir^7?^0XloIDV^)r4->z5cEH ziUi=RAD<&XZJ}sQ&}#y{5S{!H=#2Ugnv!|lanZ+TYQ#7e!LNZX_-Q-0~YP~ z?j_V>OE%@J!KtxD@8x{2!uS;GU7Z7(!%o6+Nvlozq!hef@Yo7}n;uGshGEPd0#1Ms+##sGh%O=e^p z_;yX&uy}xcGK=z0Wj{62b&7B(?Yf4@6e&2zYIuap1hFi z@o~{kjbcpLjTX|f%G$WaaUX{O&-OH;X5x`ew^?Y%Fp4PEipu?>Sb)7NtbU;}UTmwJxgygQnzCzl%}c%?~~ z$W&05smEEP0biM^AZo>WpjSt0OlHZ&GL5D4^BSS5L4X;U1N)S7;;6*#!bXWsFdEQ6TDhy}H&*6}CtSoPRP$ z=~Hu2J}4r1`qMBJ$^Dp{X`W0a;+zb5+6%ApydB7Qc;IreqowoVFKnb2oxVg$%d7v_ zq5rS+$%0xmQ%?$6i)<2nBLgKz&oeM~-+{WK`O*L$U%J^`tq-4Tl-q#X!R=btnIwfC zEQz^k54eT#?!rn1Qh>xwZ3_8qss5gwBpj*3(de-f7aJJ zToWv@ciQU3!ebV3kC=L;v{&_?y%~s=-?v$V)Tz zC~Jvlpo4C61zzJ|++nd~s>$=SMWz&~yw;MyM#-`ED%Bx3zOF%KDhq2L?{r5lU$WtV zs)|Vr{7C2*DAO5Xp*8oW|JngTc4i*-i!GjOqxZi&pd(2ZHT?*s}7wUm5ljmPEPP>KHGf4)6F%)e{!1LJ~t!OI{ z59fh?AHVVRUtL2o3Gm_|K!lL=B*8suax>3XVYWX*-6!ig)t&Pce!*^EWZ z<9X(z-RHe%rMM!z@O!MD2az)pwN3QZzqxH^F6`S&1Ye_~qRwxR#PNOXyL@|+ft>#E zn4FyF`Q-m}`J$NbZI~$`WM07Ehy>ud5ydYbycQ>zRNoonwk$M})}Hk5#1~l8FoFk6 zMw$e5519oxW1u?j6l1ci5OD~5lHqEJp988hhGNfPb_+t?Wlwen+6rbqT)5Qr#%lgE zIQojej>K|3RaPREMk~$f&-%zAIqv2Lnn8|;-0Dcn&TH&^$q_du+Av=3e9awHuuw+q zKkgMlWCM398vfZzJZiYmMW$#FC+!h23A+v91|`^TAFnCmryg$SR=bN~Vud%GE2aN= zmhwT)&_}@SKKJ7D^uUxki2DFmeFPeSWd>CZRQQvu^J9naK_c7d2$;j-?|6~9fEQu^ z$U9|NHKeG#{*h`dN%BbV%>aHNR$~%{7Xl`)%jQHR$PP6HS~3tDxLGpw!$yOM{IF|2 z)QKlK#Llk(jpQFpFnID61w$o+0wPHFK=tZ@G%$bnhNUQQOH#JzTtN%@loZ>yLoTzV zx|CtnIzANnUKdx6Kx-fZS@l}o7Zi$SU6J*lwtl(Y_Qn*C1?fmgq)7ma)!yPY|A+W! zK3g#N@ZRJ{FDCt*2wZ|7*|2~@Xn5)Z;homym5&NWFyv>227X&kowlWUoQK4XCWjkP z%Ft&N+iP9L>{fxL^&{PUi+b*(I(Kz0|Gt{cRbT9c*w`9p(-XjH*JyidUP60nSX^=l zNR^>?I^ApzB|xBCqtRb{vjFFg9C%#d2#v!@j+zghjlQ*5Y3}WM8kh-4L?9G{;gSsW zA+1Arax*EAcky4~8fC4Edx@@u;UZ#Co`kxPE z#2nBENR`o=Ta&qRZJ0!@EOr5@l>vr#)s~IV7W`s4+O}-Ed!UeOLnqJtjF<0QJOg*m zvYF1&)(b5(LKa8zDk)!?c(SD4oGd*q`0|h(Qoqs1x7SW+u%#xOz7VVvHzp95=^fVw z?HayA#(?-Lp>fq?Ue$*2biXn(De0}MjR=j!2VbtG`qWBO}$6a-Kw&nsUAn9UZ2^K)Dj8B|pId1n?# z?S53}>W_(9FksvF-m%}=#)C37er_KsT(#_qjcs&5yx{-yh3zhU+}`TtllARxmazBQ z+f=WzQQ6qJI~hxAzJIbuUah|8$mkt-irgDi$|j~P!GNfwyo6p36LLpyNgOO#;C#au zh>T=9>CKid6S3Ybw2_k2L5Qwy5C${uYUvC>)$w1lArAyCV#;c%U?0Sh!!yD`mUHG`#k6jTNcHM8iJqD4h@1=Ol z?HjB8pkO=Pke7eLpHrD3l>1-praIV0TCRpoup zN;WY;BR};zbvxcy_$t3uxKY3G0QogZCW{bflpF%y^0}7~Cqn0`_kzVv|D-WN)$-Qx zCNL)`6;PZP5G^@Q`(mtE&;X;>Y{B_265|1{%(;CA>kT|-5xO9tI@5vsl_@={{CW7e z7o7xfGq#4*DoFhA&FB9@?1ep2>P8Ugcv#$10KDt}8@#)2)BL;=`G#|N^+PfH1@bj9 z1)GOkc~ue`_Ec%KA8O+gC~6hxLj!r58|kpKJrj~%8ck<7b#S4cn{ zA1F|L=|1_;QZ3edPuhVK_Pbl8Wr@~Bm=*GI=y?t<*r5A?jV%`}i<$@^fBJMV?KNB4?J!Q9x|V zcFI#|Z)QBi%s7%g-L!~WEb~5>UTEG{tA`RD7vTq);DsYpFz&_h}pUqs$LGZ z$HW&nk4SE@oz`th9YNwc@GQy#rMw``O|~o#MZubbSgD$MTAOaqe3|K*t|B}KDF$T# z)3D7t$iC6ivAO#%uE zu3hq7%BhCpbqx(M1KDy5Q;nw2`d_9R9s5pn_YeYux$wc4XxE-5<^B8K`TdC)*N@6PX=R=-vy~ae*na2t({>rVVKX=8pP;6JIKz`K zOT?)qM)KDs%#+)utQfAbT!#0lz+4${q9*RI0D7Mk>jf#+*-KKlDsXKT^|*YW7dJfA z{gLwE1YtKyBPsM8<56NY@}_Sw4R_5}f~rBEJFd2g(s&RnH(9bkhHz{soz>&ETM~EG z4n(&Eg-lm&hoPTw=bo0hQenC6->z7fQ#0sMadY2(p7UAB(ebuz&Xs33lW<#wM%e~D zwmBERWbiE0Q>7WOr7T5UeYiocz<;qdkXkd@NTEs4@G`BVBiY@0s=>1!Pe<2FQ329) z1-cbM7AE2^X@{_E)>$?ZnXxgeckd2U&D>M&yse;BGl#o}h`hS5mc2S1k*n~`NGx?= ziL|gLCar2V` z148i-3RCE05EWE8o;VRSlwM*$InjU{T_Rk|Hh96H{BB!sFIhYfr zUuBv(d3K^xqVRbMi~(*MugB_a2V zw2w3?PLVU0g@NZf+Es|*bKwN%Z%~;;Pk_H?IlRn=5v*;4Nse=LfDRA>Wa=Stzaqva zf}9j}R!GNp?#X1neT7WRNfAF+ZY>Dq;N8ejCVdG;+pG(G)Crlt6dKf-eq)tK_;?h^ zP6_27SGfMsmGOH$iV1%bG1GRf`puh#qa6<5`dsGT1w_a_Q)LEVO9nFs4i5=c&7nt% zU-H$H;3AO#LVYv7)>P}wcNQ-A^e&scjmyGV)r75!3+|y>OqW;j!Mu+IkG_YZ+LPZh z8mC+2q1*j9sm#PagreSzCBTJ|o5K$pYwPL~JcevYUxM41rklw02j6{kkN}K>yF;~1N41>i?uwzPV;q7Cu z*5qrwUh&1_R0Td{PxPO5$?zWjsE!OBZ}Tv+^gZ`pe#f%%Sy0jv*Qu~^na3^)3%0~J z$j*~OrhrfT? zvC~n?QoBLBIG^tEc?h^JsilxlH<`b-B(9I~{+{W;l$#bT>}>>EdRp0LG4}FDPR$Bu z8oK(qf&`zOf}r_w?T3pNFRq2`|GFmQ7iwBE*K4lTyJIhoc&^Va&~d8Wv9{>xT3?wm zS+cj2VGMJ21}^Rr-Ujs~%e{DXZN9wFXJPrXLvzrd3lH{qTk_j?ziTAGzjk{SqvJ7P zg65y{KM`~1`x9#K8e)Kh_YyYS;J&>5*w# zi@hlbJNg72i$y4sS$mNDIK%gWn7}PydJYrvXpfllpdQQqkY;QoE`^h17X{%s19u8v zyc$FIN{|^T=Mp1#Kb$)Wn(+YVO&|Nx(DbzARNXG+)m}==!dXoE>akckcid*8Ct8%TWyjp|keX|Z z$mNuqe@&1-EgJJVb=@k#mNSpDUJD5ewtQ_}i}pIY5G}p-f&`xS8Sm~bJ^XF^ST}aD zOuwZ8A2RcdC%vO{8GrATae28{S6BCzA3?y`JV_Z!9KUxxRrnxYbu!of12!3124lE| z96@OQOM4bDQv;_5SOWiIZ5*F_?JtvD9o6pahD%@J4P{A(3y604bnTb#N||J+vnP5r zSH5%k_I@9%);7+X&(5{xL`csrrLv^FBUc_iTs=HbATWHg1c8w%Sy$_ob4NXld(O;~ zGSleZ=lG!^OFz1v;58)bvoP-WM<%sB<%R?^T8MG#xg{QyPpD}@tA-oIqWJL$# zn_hfw=u?y3g<~R$rC2&hKYdxS+|1J&uPTz7&gWcXF}J{qN)kYZKSSm#%uQWkzpkHC zEQOV&w;f=>*vYiUj=oEQ|NMttl1r>WT#5~(%Zk>yy`h`wnQ`5UiV_gVhe z17R&zEF`uV#H1D*s#yA}AlmXDOvts*#tq>y8*|B0&0;D}y3RydYfQfyH+6rzw z(8%yN!RhC)wIF77*+)R*WKKV}qek>-*|4LAOGMe)W%f9a^R^m$w+2Z&2L}^J zM;+hOuY_RJ?;l7?P|1(|QPM>nyU6^03}~3`I=;w#wd25tXsG_W%J<76m8u z6znYRw-@$$&ZxO=8Qp|EzdQ@kKwx(Jvd4u{>-%E~?>(vw28NTl;6BcjXT7GvZu*_O2Y8wLqjeY03xdbXj2FXM z$`-e$eZ3A{NSYj2NKcwy@rVT3PlDlt)D=GSPt4ZQ*zmK?JDz99Pr}Z3qjy0l0a!Ye z1bO2~z54DkmuY7UGwq(BTC#>Y_ppQKG;Y9gcuebJg4AxmAj!XdJcC9j0LXZLr?ZhV4;9_ZbbX{A5MCc{ zx`i!Q(owEy?o|&dw!?=eF|m+3iu z4CrNftyE%!a)M*X`nsZm8k>v=9RG}0092({61%@L)YLTEt3FeBRJh8{Rg9-@G$V%k z1#oa*Wkpq0R99T~!dcw-?&c{G{lHJ(JDuP3X9OcGaMs-D*W9=S{qk3H)!g<&g|n>< ze>Ss>c&o?@%kqDR5xyU+Z`>NLn9s*q*uBO5@VkSfKiJfBz~57(`82?8)a(~wRppNP z-0#Qmp$pu47tw$F-dLRpdne8l-C&CmL_dNO$bAzE{oV%GOzm0xmH(`J$;H6iZO^NW zIX7VC^vdGTgG9?1yBujVYme#S_eE%E+zeb|)`hHNW|Qy7IW(_!guUr4PS(zeVQTwR zo3!KS6%iaeSC&}K?v3XI9zjOgpT~tOUPAsH!L)0jjT5*+wn*LvsId{w@*BjHzlX4Y zBMCa$9BEyB$@dbCteV*Lwm~o>cTRM5KCLW|ROCO?-c#WD2Q1+KV0Ic+xh`(g?C!RGat?)#x&st+BA4bC=V8)m7$%W{3W zg6vue5*&!+U=D7@8n7ZxvZbERYIgEBiVhR{6hk8PwXe293;o>^ z%98Sr-53vSLxsm_6lwnmD`i9pl34KDeC)m7-`}rsq5{_7REjVZHn`?TT(1t!nohX* z5nB8)r}d)Tk9g(^3&txr4mvwK#dqFJs|iu2rlnDfQHc*Y!2k~eaI5=QPn z%9-&XLoudzv`tbK&?>?sZpH9Lze3Xrk z(NjK=+|O!@`ZHRR#KQ4S7zkxeZ$j4ciLx(UX#e zKinfW{wFECUp0LA#aT5hL9#7fcth8C773iAL z2+TxaZrkuxx0;eL`a#NcO4>Gd-fgx@3j0$8RTm06e~Zj>H*p_t^&lzqq*Iiu3DikV ze|yo%?qAn=Jv1-9m>BqiwT1CkH{XM2GB%`e7e-yc2otQvKm~o-kLXDQ_A3x&JdloJ zgd3MntVW0P{4SU2hSsV2M>~FdX&Z?wrL01D690@d>*2SLMK^{W`}g;@&h3I!hRMXJ zMxln3v9bIXhE3l4-Q5KOe#%<4yDn4Qnd6^nl$|LuPl~tPkIJu`OE*ydfzj<_(9T+QF}U`JOSi9jU{g= z%^@9pM;|6I3biQaMKm=L;RoMk)JHP-x;QL#C#$A*uehXUXQQSJPPe9fw7}pr4QU-O z8z0^|e1}HneFzdfeS*E@Lg(sg3Dy*zQ?U4^HHwj(FfND57XN@5?eI+n5Kaa3I0cgE zmhlUUK_O9yrbFny_@`ez(!o#eDgF>%=+R$e%9(T@u2TIiMk*$mBj6OXU&Gz~ z*W&3$@7?IIdMnNAuXm&BB$` z@G9qz&7rn47nFO+B!4kwblC&BMGzAbsGoZbrwH-)YltQLyf*GDP!< z#0Kj%#b#z2Msq)F8YX9>ct90(zkguB)WhSbFrBQcV#J}g{KyB=lyy~}@YN zp5`3V;8eI#4#^j=>`!fE4CWWKN=jq--3%D{BbG&Z{^WnJY(M0Y@B6d0MWu8dU1czK z^j+r987Egef@kuwIKs*mS-p>c)i*Td+XBSG0Ui@^XmxCG!3MIMA7d6iUlir=x5`zf z-sDaqE~8bBuv0l2C(H3Ct?g_;PkAMY-8fxw!XksvZjiyqhlbV{jA1}lPjrV6qOi=7 z!!DQ(1xmuvL+epGF*&x9K~Hu89yNh4k+!cF2YzZ7h59rhw5yiF+=;4`1#~!;29bAJMuiN_jRqcF6sm~OOUezw<%(Bc~T@suuW?x0qJ}et{s6J`htK$NK zG|r?QJ$Oj>UH1Wf)EvI7@W%`t=??os+6?%stv32K}zxFcVLf8NeuV8 z5C?b3*8N(;PLQaj?`dbE?RA3tXlgxr&!zc^>2J;C(=0{dy`axFGG1-#B9l1NBr! z1YtBNlkW02qSl8{J8eY42EJ4i79ebCqM-FuCgL&;1a^N&BqmL~DmpN>DW9crxF4>{ zsIs?5qzAy=%3lhPu>7nPu)34&j7sibhO$vbyT>io`TaKiwh*)k*G&!u^ci@ppVt!i z)tHxgid0tA>bHW|V7G%d+uYfCx2a;=gOT9Uh=o_%c%d^U+iCGkP3yZaEcQtwftL`0 zXnfx?pAJu+!7>@2!W~9W#;((MGO%F2uz9Bty^^rZCd2438b*61eCAXcp2XJB{S~;l zxbK(!^%GyaYQ4g-b!$O%x#GQ!7fl0?0gYVoqwalv{{30GiqTtLrm#twY7&WQle*?M zOxbbJ8wQ6|w{+Ako&elwp|6wd9b)&56Fum>xfuXQ z@c|hKFx*|5kHq-R%8={qSpg(g4TDU+jmbawaaOq$N^iFPFEggL0I+}Y&*5y8=TR1Y z$oKb=slLi#y`5tk($dczXsFc%^=QJC@S1yK@m2?V<5a8&DP(gtPv~l18P?pCSj=2Uy^k>4{sc5eA6ngZ78@jS-k_ba1}LZ_9M(U+bv8d?Q&jeP^rf0ifEnd z#8n+_Pg}a5A=z+X#P}XNXsM3btkwwYdY>cgdR8rG(sk#qBBPHKwi9ER2U%T%719d1 zz%9pRniCKCwG%!-{oj>&iOm%G;LdtmYYs8QGBBmdPhTi=GZf9rW+aqtO&4H4?;R*{ zx=;t#`obi8D$kp^RBwS+SJG_R|Mx01GM{nioT>@=w@5urOnymwfb!tcesk5G%r6x9 zKwVvExrj#a5nQ{BBR3?&jAkW8Oq{T~g0||81xVB&gmp^Bq_7Dz9^ViG_Og7*&(qYi zfk48L;40j-&1H(XNxH#=H52CpWt1lKWi)So+%MyJ9)%eMXKp8P(hfRZd+FD4xfpw2 zEjRI&2Ls?HwmgpK=A&oX-x;sA=lqcMKE6gfcw){D8SES#^TupVO-*H$R~pT0OZjw` z{hzOY;=@PRR5UloW(WL?acntfe*3m8&s~a8I*u>)*rH8_J+sx$na0)CwR^=CY~)k^ zYX2&&N4zBl)l{=8U(I-Gh>acXW4MgGExYa-UH1G|yD`yd7|nR#E{Qux%s4Yb!{vT& zJQhq>J2>+famwv-Vm_Jpz(&bVbSlAPP#2VzM#qzhx2BfV6}Qp|o|mL1A`pGF!(#pG zp?+r3CNGuUaWTc%yY!)~U_ujf!ihU}yZ!Fen{xtbNSKbegV#Y|6Ty^`rL_l)JG{fm zb7G$Q9d__2ag5qWXZZDrXnG+kRr%iENWw>-#ZA1b-pmb=AzmO%*$E6%JxPK|Hx4ZJ zy^KWj_WHgi*o;42+BGGgCuwxYtOx6Wdg*qMk;??KC*Fzw@1H_790ZJ=Fer+g`HMWw z&&EVDbqVvy9!)D8Et|wu*MtGmti3L8g;|_%ijqdi3nZb=V5b=iOn>6$EYmn&3)D-m*3f+Tf*h%KUvz3 z<*l*L8?m0-T`T}%5DVsD?d9+ELzB#JcAq|lUk_UFbEc?p4&886btv0i%&i^8La5uo zt6X9^UQewA*T2Vsm}N~#!3eJZ`c!AYrI@8<9)h8)1)t=wE0N>U`$qH!MM?#XK}0J?QQQdk)M@Ntqn}AMaXE z_^&ixK3ex&*tNA!v58Y=HvInvtx??fVJa$kAU@QPTp2<{m|Dx=3@cnUkWmNN_U z-pl`9t}VvR4F7ajDO6VbE%J1{x)OOnI%2Z2pT-bJ45M z?-V4m$&fPai5+ciI4b!Rm9MXlnD58Yh8i1N_FsB>A_4z=bp$&>`G@Uf^Sk{dwNp=Z zGZgl&4|CbHf4vAPD>oOW(Rl7fzkJ>>pLnl$xksx>3iH)$VesMjv&|~vpfmz6I|R7J zWDg&9o4=?g>+2^5gvW+Muqsn-SjJ7yPvGoLFC6jz`zSZzSQamVDhS?3nO_zsefQ=D zN)c9`V%pyqoQAj4y$(8qASNc**9Q+!DJf7k%DEqp@2f%<n<~ zyhgGbHk7eai-;;t-*^=e3*x&x1Uu|&e z?P%Vlr0@v{1Y7WbHd^IV$Rn1R#ksnIo-AWyR!QV)caRaMGjoYV_~X9YKoi9{mMD9^K)QouAeOw9NJBkJ zBzrrB`Q2Zd17Wb;6>Hy=PYbjCB3LAP+8unSuhAx4N^?w|-*iru@MO{w3l6VDY!J?! zvIBQlIU(`i!kh;ZkN$j48hLM8u$FU8(fxHwgZA_XgGxm_Umf>j)B!$Yrb4!zHL#~G zY?j3?n!sT>MrC=E);Z`6b%_k+RgxuI-8)^bLk*sc1nK(v6B85iMPu?6x=*;0C~O_I zijPEA`U~MEi@{wbCH7Md$5_ISS0Q6#fD&TbVhGW6q1nUx*lYPwteE{EQI?Li-uxXnNPj*MW>l<)aT)QD+4&AF9 zT>Zzbdr(~=xj@|FZ7gB!pcV-eFU%*?hV9L8RSBq}@x4V--|m-xEQ(J4L5Xs@A2p%M z(Qu3zT2ekE-5FKSkysGkwcpFIY)D0&>`3o=?Yy+>EYp?otq;uw+9-BMTQxCWdo=fM zbhQUa$MX3Y`_1s2)LvG1+4n41|$=ZA{B`McnaKZuK44)u5lQ#CQ1F+N8dD0o=lW<=ZUa(6FnZ`K9ev zjYxvt^t!vM98FO_J?$6`ECq46NC;*n`S>akb`&RM#yGUJK?i7G>s)t+8k7C?k4ihH zLwUWonF1}B?MEa#?M>tvl-U&1uBK26RW zO8gJssI07rIg?a4X`z8YL$XEgaPYi-UhU`qbq6$c#; z7pel7)B7my-xM~q6gmEt2rwwKe7)-KJ28!xE1G=w_!EYl^b-RLs`;c7w1&%jmqsrw zS$_HOP40L{HL)vB4u5A`8$2p}04kux9ny_CJFw)q1WZ}e2No{sT~jkN6a)vME8k_;NG(%wxpDiL``VV;*28749gE zpko<Ivj3DA9z*cfT>)wyhd*Iv0rv{m%DmU;T>%=m##unCwN}zFl|3g16Ypgg zYvZV-xBOEbZjTAF5W6j z+WKI?tk{>Eqb#~Vyp9xEVTxU7xPMu|8mcD90Z{=#2<&CYI!0DTP zVM5z|{eJn@A8}q5?s?5zL}Lp`E3h^|${r%YNj~W%AEhs(KzILfa(-Ky*7DmiAtqtwodJe08}E(6u)O~Ycw~kN z0rMJCNG8dnW1lwQV&uZe@okU6Aa6k?y#9CB?Y0i_yNW%xX|?2TWqlE+kbi>B6kgLC zhSa6xiYkl9AuJ~^MaT%9+bK+lHBnbdC2g%{x4F#y&t@Qe->>J1*MYuN2-o4X7u0-MwA zcr$A9BNv+99HV6Ta)RlPHn+CT1P>S^Bj^qGzmVMu{U<{DeT+z3TN~)xaQaLI_C&(5 z@a|*UV}H^B*NR2BR?%ZXic;Ok(xWRwBi1z+?$o!hG{m-~C?eSQP9rV( zpEpqY!KtVnQWxuOK6P6Pq^75%z`*vQ$HHzW`b{yzY<^93eW*uOM1my6A~mB8aVTSC zK8013!k5vVZ4F{^6DeEf9i3X?m@PPPAU#Br26zo(R?cNg1V~eurQ=wxli(z5AMp8~ z61d`xbM9P7lxyM8SgBI+%dmHteAq3MrJn#^*3@eea&)j%fxG>COw zJF0fKlm2+1T+<^2pIu2jgXzgMy)|oE?sfOp9P)H#^z!@Y=M2lm^*3$|x0l!wv1R7X zEwfo#eDGh@6^^Oi|9f5;={^velww^hL}K?jOO{=o)*!ZiB3dzC%*$e=a@FRGGoJ|i zoFmZE2IeQ5??9%7BE_0gi;t0X*iNVdz}yyTu@|q!rkQIzJ<99rBY+lZYGbojIF=XU z(-g*`p0$8>Y(Z7M8PY8K6RVmm&l>SWDsD;OS*qy$&kun8b-pf`uO^UFTebn+=>(gV zNLTp+7dBR`(30l?ZQ&%9Bi13>MKPI~l5S#>B1E1hr>%k{{ev_WaOrRZp70y1(AHSQ z6}fI1+;2ZrU!!|ak_pCfXT>)y6_iP*B}H*u?}5LZ0!G}#9eP-9Uq`x@a{>+W(xPm@U*c1xrwJ_S1}Ujca6R9X5=jGhT6RdAGQM4HZ) zeHZ@HJ0WlcUv&fUn!fN=5iI6Nvg6&+gLgcQaVk3mD^5Rw{YRxbEQQy2KA4Q>hV_dl z_3i}q1*)2FWsDq2`YMNaj(0Cki2{DQGYC6gglugACl_4!kcFMHqlFYpZ<|%MeG_D_n`#!bb^{(FGV_YkN*Z_&iogdHp2< z`ezd#+{jf+qLkx<*yDqc@j<#147A*UUn7ezUKQ)<=+ee=z_N1{W7i?f_2^piN6=*DSw&(}VAEE^2iy&SYS-%VxY&n>j-H817A8W*;IUCAT>=IuohV38^uvn?D?hrLd|5`XTsw zb^C9LkWr&<$D3^b$hYIA9a1!rA&(a(*-WFsW(BN0WVH^{opI7r?TZEKs6+A;BaV2L z0=R%a^4E)#&$_9k@G^ zodx#5NxRQ_CAs5kt(9BBY3wnKUR?)0RV%L1Ieu)s6 zL&P$L6g_ZT<_L>K5@vx-Ta6NE2hXCSx>xqXB|5-ukuDTIo-N@)RjCnNFLHz_ZOByh zVRaqx@?vyuGdp6j$3IU^O@F)#{3JK=kn@oi!=3FK*LcKL16YX9t1}IsbxH3IO$SH z1fJl63=t&izKPJ=JDFN5|Gr{V@#6yy|9bZs>hXeWP(xWz$tMO6K95T=5H}g7D@;+i zQ4HJ8Nr980^Zh=~VBwy^^tgLB8^t??uKZC6ZT9ax3$jaRJ$Gyeif!}zUe>-pelg`A zLLkdCHsgfDpZv6P&i_rWwub59t{<9U z**=>7p8O*@Zyr@Lh&i;{helJ9hL;h%0c4c4{M8@)lTR|H?}kWr7inLnz%| zkU`Duub~JZvqLH&<)i`rn?Y`%a7xZS@SwV(tpz`kY`!X5b@O$~xaERmpz&hqk;e)3 zV_tG0?07*4TtN}?UdxO9i`e7u?aO*Fqr_n>Sq;o;HMyqxRB^&;5Ml*Nok9AO=~s~J zlLt3hF9SU{6dSv$r0E3|)kG|sfW`BO9^jEY)G$-pDjCAIFW}+MJWb)kbY#9bmJcd% zXmOFBW>mBV6Y>cV=|(95S}QII6_Gc-$gGRe<=v2D2*`OriM)27!ynpyRtlLC# zhI@?8MNu%jTN0bqIn~>2LQ$tiR+{2_hTDZ< z!Kp&>DE?k@mNXbfb9P};l(OYC6&qi!4Q?t5E%Tt`Mq`VYb2wv8RH(Swte*`V2RtAE z$aQV*yZ|2y5&>R?UgRmiMtA*P)?mR0;$Mr}|0RDz?Zl@E4~jcVQ;?X>i;x06{(BPr zI!SCW!ww>4b|$f#i4%LsuJ1|HO#7cd!1E)-CoJr_s$mCBh_P>ZBfAq^8>gDQC_>7O z?)>}r?Src}u3`hLCrQ~pQUUhiOHtm8{?d;lrviz8SOw~+|2cntu6wcw}H+TUJM zb9C>5mwST5IVmI9hy=X3xp_>itnee`pNMDEoIZcl=dts!L~CTj-E@*(qC;XU^U+|g z7MB_L6%eoZYNOIrmG^IVG4QXDT*DB?FKWJk7Ks`UvX4O#;nSsl+dT;(q^e|>5_`MA zB8_*2<_&tgqQ*@_l8Qqh%=%UfB6Iic+#V}NJ)7Rk&~m~d6%-rH!RL=~OUeAI+u=VD zi2E(EQqLufs3b1rS4=5}WdAsWZSznmKjHP8NGGe?pgQF3(0MNy^{M~AA~p8D3f^2T z3j=Htnnr~g@+8?5g1VgtF&8~+Dt`p@YvcfRjw7}S^cwRGp+1PSmKTNP_I;i?k_1pL z8YO??^fCzRgFvvLq~uX!KvQgyj`Wky8X`jBOR_N1@WOG16ZC9z*X;Ebq^7pkG-iRs zyPbFx+zK9W;NWRc%6~1b3&00mC=4zzkRpDbr(f;f0WJaqt~@QSyXgajbkeI~iy|;m z0&oKg$2V4w&z-oNc6WcRecZUOouMF99hr$(I->dFIYwVu=!hxYL!{2+~34j?xjJ$(XywQ z!VoqVGr`tg;hcS}bPwqrrEWiaNWJkw(-5!vsgkiURQT$83W}-fD=nDlwyHaAYcFC7 zvl8~}prf%0RrqIm+Y&x2VJH1DH@GPoK~M&@pkQZQ){8bW6pDXJ2dCDeJQ&!$Q*2!5 zBYe_xm$EXVfJNig=tVn(mQsVSh~ zP72gbd6I>N0d>0Y&&K&(T)DHfS-&39<#ae)AhGLWO2UdL6nWkb$R|TyKH|*h1Tl(~ z10KZp`rh3$i>F>ay(n#3)1u$s-v>J)a5w(?bJ>OHLX&FP@JXP}>MkiJibY|~hgN3c zBJLmA^Sm1olf@ChWOubOj%+!l0v2xY1MHWfcCKwC`PL>v#0ExYb&b|{Qt%XO5F_w^ z4N-Q1!zxReq)c5R?0HE~etkR`*7J4Uohm$|@f^yvwPIg1$j1dX0Z^5gFCH=V z!AU|k-0Wq@rp)cfcOvWJi^A{;gQKHZwVlV~z)=)POpUs`YiO;}j7yg{B|Gpy2HD_zyClI!<7+G8 zhY9V6yvX2mUAV4y7bz8=^YqF(Cr~zpk@ zA|)1ScdW8k!yG2U%6pF)Xl6Pqfu@&U_;}sgQ;a|$a%yXSO9}gP=97IdIE+NI$X3i|OQ2}Ko+QbhZ7M<)12X>&k)XQu2_Z{C($bQa#?uF6}YsG!>jjDHv zpC*MSA+@nkonjQayP%_#%GV|GG5_2czeakveh8~Yvj;wtWb=s~it;))PQbVYkZ(O} z9CpW$kAID}4FENPKO-Ok;mR{R>vPvwEt7tG40~6?g{6f@nS#vHQdC9b8`eT#aiyA& zg@nUz!ID5=x|jk>Fe4j3j)h?w&>Dj2iw(bZy*QOe>IdBxCX0ztwConw*01DV(t>*uE-{s-rfO+X9V3BqBd zv3eO0U`BY}Me@0>x;nJ#O^PO0QmkGv*6aUL2VEQEyO+n(IeOoitCa~%J`wg)5v4v% z;gi-Oo#K{UGl_1ku?LzTNd5q6{oq^Z#B4l72A+sRpl=>`}IiaJ-`|?@KCn=#ITI<$e2+|v$rzdL{j;bb(^uG6b#i}in`EGmv**Gk3 zY6w4!Q8PGQB?4|!&u&+x6&UZjzsR5Y?QLdvd=$LWrvqa!ym*yT*PQp-1aihW{Y z&Xhh!IaR#sbIvC_7(ar<+2ND9qS3Y7X@|V&?VM?wJoH-b=vEH+33|(Hcn2g$%>?PU z9A{e4NP?M|$6rlZ6W|HT;L5M55>2Yo)Ekh7K4A`hf(^c^ssw=Q(6PA*6!$hXq{?RP z`IlLBADL(}Vnbe*G#`*+yuB*wN$=ITC_;JtlHsN5Xo$^Ef0Y__)%2U7D7pq{8N*vg zK-3tvtpy(6k;&^$u}q+fZKEL-Bi6WmHS-fS4C@oFoXN^;@hV@zm>WM}6+f`7!AS)d zc!}KXhg!XPqrpw13BEX~;#E#rq_Tl(0Yf>7$Ty5BHfbxB&Bko&6X^uq|3FCfhDg`I zK;~|?5`RMcQ2@&|kGO|OO^nA4!^QObt-G5~Q!t^RfqSybm-vAQrAdJBkCZWVw;0R|QG&tDtQpg6XdgyXCw;pSij!WiNTaSupSa1Luu*i7oEB>>vt#eJ0r0 z7d&_v^c2`O33e8A$NvqXw{v31jD)X5o=}D)J#>NH{Z5;;hqBne8f@5oz?v8>g?e(Y zcS?tqB47laB7dGWC`GMprD05808R`c9F=Lq<$3h;F7?Z&kmRkI*`~??lgV4q{5s>l zy0CJZ^NY9qpw|`!wwW4R(km(c_$e|+p+r+%ZtPQN$dd>7YvC(cx@;0DWmY1cuQL4| zD+kNVpHDl`I{pzJ-ta)lT`+a8K3!4Joq?Y)rXHS4{(d`lNgnAQt(aGP`~@>rjVw~1 zzmNC%p$i$s2fHRi*H;?>;v-w+YZ9o;`b}tEVUbO{;BllY8&9 zaRWP1g(y^?iX=kWlv4oXL^U)o|D1(ZRX@54}e}&`jVT6E&Y6s&geh5lZPWsE>Z~ImixV362WA zE>4iB!>?{FMIH??K25QJ7@wXt+}wV31E$L!0~&bP))V+K;Ea<_o3}S393#E)B~hxZ zrdW!sH+Bm*uw3(mKamPI`xagALKqcAeB;j?%n%J;TDlKDVE^`UnVZ=C%}nnMuM6Fl z|AG^ghd!5?U~ri_BGzQI455I}q=Vr3SGYc~=}s}a)N@lrglNH}J7H&+*x+pi9~IDr ziuayb^ElZ7C5am3rJ!gu8Yr71X>A8{=7r{|JdXCYXkhY>r$^_;tK3fjZ!p&YxF&ZJ zd+yAO+GYICoORTcm@zF)nOGM>^%YzGD*vg&X((SBhb(=fsE-`qP!iB+b zd*+>#fXbF{AuBO@sJ$?HDiT>yd*&gukfR}Mr;Mg+ZTAJQYq{HR2_vnUrC^iAb!kjP?$!&>e=~CHQf{sawP5 z?}_t$L>38~YFOYI6=(pSsO7xheG~rsY1kOBk3a2jT-dSOVPc&(X=mLc>_*}lP+rl9-^ee7+i`)^87SGmwLv^Np7nsft$ZwYSa`t zBy~wOUTTN}GYD0b0^_};r+_TY0a~#3+Z=RUoJHDKgdb1%g5B+<7hs~t|N1~F-VWWF zF6=l}v3O*!b^Lvm?5r)ztjUN4tQq>~hSlQ(*XVhZ_X7j=7fZZ>9n%eDemhhz6?jrm zY5Dj44##Lu){A(ZPxmbU?TePU>w?A>Y<1yjxeA{Y1=zP96kH9|HN9O43T(vptHZU3 z73yWj^2|_X!xF&Gj153^TLzKq`XHJ{nzv_ zvHf(i2CMxqY8Q2(snEzx*|{-G#>#bvqQd*aj?H;?iLo98Ne}r+d-a&q{=B6JY8H|k ztEJAYj9*28b};JsxM9Mb=xbaKda?Y6fnxnwLS!9^N<0U zq7Vvr`9BbP%04>xZ(mv>U&5nHQqu94Ew?`}33V^)GVNf(&uS7LU71Z_kOq_+mbiGs z#NWS90Em;ymUiN#8NZ;_F11I4A>QlkK-6KXF2dJy$JDb7=dHzg?dui%l$=sESofD- zB7s-Hc3Bx;V4rbo?=MsMhHbUp#N=eh#<(NK)}F-CsLb5;Eu}b``o`U_bg?zz*_tGz*ll2)<8)RSoHhAaQmv! zr#|d@926X2`i%WzTa6TArwJ^6fxyY_i>)1z?IUBE%zt1j)zkXzAUsnMo1+V!`3dHvW2|3 zQj>rH(o?^y;aWEX?~4=1|2)k%nb5btM|L(^blyhG^CQfhY4TqX3J3@wW>jVEnF1*X ztSB94iWjf1jIX1Cm16F2Lm1GI<&PLxXP!p$O(ch2nS{)jlRb0UBn%r#IG>$c&mXrkaPC_4Wxe7Q%ju z_CG!V3-w_l%atFn97BlPwv_bUEiNuKZ$nMX-|zW#!ptJyeh!rF)@%EF0H-o;bGgV;gbn zt-eNmW9h!Bzv=dsk}olr8j*x| z5(S4@=uDi-Yx+!14Ys#fNE1T8q+Lu0K5%qO7Qbqk^(B{=A<*uEQ#ziW!f++y;q8Z2tbfn*lSr4>2_Xr@Aw{$V9sn!b5M zmadNm0dNslE5HP+a5buv=RRpxjTMgt>5U!pH*aV?x2HM=(rYocTaOh{h%;Ge+#ufJ z+fVrXY208Y0J}VAKlOr85S=4)gAY^Aw8LlZqQ%3FaoyLiI~KdtF?N=C=yY*w!Hsfm zbW<0nZDNv=dVqr0FELWr*`g&ZFJX~C=_GbL+a`xgTra*slZUt$dXpZW?DY-pPOfAx z^oFx6Y)&rK-mUFqwrHFh+F`=<3qhs4OISuUksmHL_2-kwJ5s?)U-i^CI+yCl8z;0CbVq#p`LR;uQn2MAOrs4UyVz&{c z?(-t@&L!GPRU=3zxHD&`ukf4UMu~vtYkk%P`ELe_^#(rZaj&jVeB+EU+;$H3^XDzs zLDERx*XboeOK1Ko%3;BvPG=4P=khp|yk7N#P$Yl%DY%J{$11ix3)^BC0y#Yvr! zuMB02A3W;Ac;O#k6pXmE=*zQSnoX@s2>}gFaVz-+ZY_3>3NJRH$l|qrITfYRHX-%hA*V#gE>OFl*U+o zu#Z%hHXfQi^t3Kf*S}&ga#8j5Ii=bmzkx7I}G3Bhh!Id*qY8 zzXldjZq!GQHl~K|k;?;}{f62UNl;Ej0_(^=9_8AXV1rKN^Y zr|eG3C_}R{E^vmw_y(#;&?ocZEW4}o_YTYsurw3^RB6^g?g8;}aFDTDBzz7YmSFjL zcEcQm|KgY>JHf)`bbJLVZZT47nC|SZIU*ZOy_S?vVy*LCg*!2V9E?%Zx|zQ@v9QBM z2$^(abXmX=_0@FCC5PYH8i$DIHX~8!J2w#l%^&*q#m%}MGlM)oGiMr5djH4MS%y{F zbzNJeq`Ol@KtMo1x|Bw`ySuwXq@+uvy9A`A8>AbgyPHjKns;$O-|_hy1=-iN)|_*U zan5B}+g=E5ux~lWLhZ@s9?mmVr{47_zNkox7Sy=UOrvPOd$z~RXg`taaV~d4{Hqi$ z^f1zTpT?!VtR=@TFY=)La+8H!XP#dwR`q^QFt$sL0TjOVQDE53o+2|tGUV_=e2P;R z5rhgl_MsnkG32fOoqBI^w>~{WuDeJ5qQKzW4FHjRL49rlYQk&sTT;#lF?=wTrB6WC z99I2(ZcU%?7u4a~S1s5wNYG5(@{mHg%#EizzX!Qy?}qIWD=Ng?52QI#{#~=L*^fj# z5mCE3FhPDJK?<)%6BcM?{l<#dBfKm7WLXo2?loN%x-j!$vUdi3h0@v@2xaMRT{PPh zF=yD}u)-+2X#O3q7PF&F^gBNyXfqP4C-Kun(mqd7*5Cm0!SKZ>e1h*@E~EQ5*~eVU zeVs0^3Fi4@uGgLohj}XS<)ZNsX5oh>KR%s+nzQ+BIUi38w#g;{9pXs#Dz$!}$(DYl zl8pIH0V0$)WTCEps%`t*(1#h|8=B7l^6us8G#NukH3w6u~gxP8T6|%vSUem^GtX1 z-{^IPnO)ttmKJ`dxhRzA%!`<%`Q;8yzXyDvI}0~W@%+xsa)?rk2F^Nad3roBC|L=9 z43D3T9;~&2mQ}bm1%dxDpPq+*2nM-^Dop&49jM1Lh z3lqThXS|As5S>wdTINuN-u>Yg&G+FzZ@qf73}`b%MauN-(yI#3go-hWi*aiXg%VB{ zVzlm_-;^zPMHB2XQuzw>iSqObF{3;BDKrEj$!QC~PQbxgS5aBx|Cr^c&LkKBx7?to zq$NCgz(|5PNsBF+$7TV7vnv9$T_C5;4_934e@QP~)6GKIf{^1`ebu*a5R<46GW9}U+U(Q6{DENBC#F^ zHNJ8V-bI;2k-Kr&-K^jBSQNubR_4eSd3_!k{b`22?U*u;+2)0kkU^pE5=W#cfraS3q(esx&VG-ubtOewI1&r$1rQS zy94^sln^TzXLt`lq~PBH+A}ex|70nytgFwM)UkBtU}I}gt=?&e$#$hRV7Sx<^_KkL z=@;hrM&~Y#;~{Bsv!99bbek)|Cpa*2g1BfhJiuqV=L}L99p?#neBRfD&qd3*8N=-I z+^2Hhx1+qvt;98zg0Uko!nxz0OD21^r#W2RTq|2z0!*ueMg^a637B<-ij;Xa_d2Ur zF;Wp}&S~Q%0o@|6$L;VJc`dobAjkqLluXOvMpM{nvlF8=yH|!$_%{1Ta%zP@U{EKb z_ipUIKS~q!?I2&D1Png6h#(VmVT!~YO&B6&T{92l-mdwBkIz*E6e8I95P|{1n1J0g z?mV;X3@jj^3$)X<%hI&bH}fJGR>c9zli<_M3>X4=d-?q&01rd2PwBJO*g8koO5fPy zR%5n~fW5#A!wULjlR~4ohr2y!@*@mnDm1cBR*gKYYKX6{=mEM3BrqSle$$=&nM#$N zxqmk(PJ{kJW>SPHvtvja=Ehrgx3yN1bk+9-V3M)D030pZ$ri1ls%5gU*D)T5Lo z^6Di-O`;KILSw{Rl8>WJ=nW^2llJ0^;E})yLXIZjUJX-1SNZji2r+u*HPm4@o@z}z zm#c`09Kz|^=WRu0@#_C;zq0Uv#PvjD)b6^-+cXag+k~c%YsQFU!GapnemvIh7VZCt z*_TJ?`xNSLJ%ASd`4!L?-c`O*o&tH6D^VQ1wXTU`+o-FZ&$x&FIjBtotpvOd3JHOM}Mr> z&hL%{65e_z z-i`dOZ(g#NOkq!i$t%(m^69vP@AagyF@_Z&4aYW$-Ub(|d>?w-z|-PbfTXiV(Jb%t zl(Ulg8*lvDqZ6P4UJ5?7Ut+S!Ag&00JSpfL+Y7bcbuZP{FsJ8y>t%&TY;xp*iEe2F z3)M^gYM{?zcpYMe3ZU%<#IKswg#YDS{^~?IiOA+N@T8gKL0^D27wfy3&G;!LL1oh^ ze;pIu{2RA*tEg#-1U5+TlQl$jb^92Jq}Xg$kv8Er2Wu`;LtuvEv|auy$7u&X{V974 z9F3D;Qm{XOvkmD-cr_R`EIwu@&3FGZ@Qg(Mc;q>M38;o4h#2J0Y|46%b9|6l46v+0 zx3VT1ae^c;*})*LWx&xd``mdjvaoBR@%cQWzK}1U89#D$IGU#gIJKCwWt0E|Fvnm6 z+y`NhoRa3iIr1QwD(4&JNhb%87*HpI!BiY+2sSt+h>V^+4XRdPjEi)<)DoiEvhCS* z63n{QFlhTU*SStr=5zJ#`4Tns@R)->GU)JujT*qG3Ccw<0E7b+i>`o>=!(XwZ=SiK zx^3mIC$wh|$yjZYfiym;qs|y9R+q%&0=^O1A4S>(+abQ3oi$dOKyiBojpqXCN8aZ` zbcPt1AS%edL=)7=2@V-gB)!K6vLMh=R<1a)og#^AcITE(0d;oZRX|{3 zhN;$wn&9GplXK>ExdwP3(9olQKq)|N@rUU(u!AjYdLNEHOkP&>P@6=9tg+ge<3;&X z(t%SP$>GF(>SJgb<~=LVN$MmJ{6#au+ZV`_KYIVu{aHK$)+C;}&pmw1aBem#R%<#P1JDR(I(#-7$>f2&9GXn*-~ z>m<-@UT%`&LcV0WqP5Z=O#8=g&RJw01IWt-JT5pMOP9^{O-%3v?;pg0^&ZYEA*^Zq zkhbj@t&@_>!CE!7Ak?rj5SYEG($QC7u+s_L2^OO^z}y*Jkk;1L>m32&;EAVQY;eLD z%`6A;H#o0KxJ%}8*&KAnoMp+e2eBNIN|=>?eRBi((zT9Ual$ABz#om;Se=v;?4Fn~ z+|5RUd>W?)0)JRF@P&pay~7Qnzc6XF!{#511pozgKHu=cK2=DL6 zgaj2n_mdak&F;(vQ;Y3OEB2I9;WKz&hGImL95?ELT`xfyKbH2ZD}0_(8q;Uu&wtrp z3yZa*B^zC!F0%FLYv2PN<~cVPN%OlBnh+)2+22ePGh(a&s$izXm%_B33r9y^fCW|) zd=4d@Ri#~sMa>52517t~I_e857|ZVaufFUwd!@lexFF)=SA}SoV}s#{D?eV28od+iQpE^q zjoxoX@TdHLfcefMe;CSiAnUyZp8UWL{g>uYG;7r;CpuBYCj66OVVnmnc?q`rNi&3z z{1@lfqN+TKAXy3oC*b@r&Gq=V{;JO=chYw4a^4EWL=3n{DbrO+toZ`Us_&OmaPU8>ae^JZTsd=l#dIuc8$4R}D>piwZTS$Ef z_T&jG7Y(pLyI<@gf%cZP2bdDTgK(7n9tT(_t?^Nu&gq6mKz@6C9(|BFd*KLtP}x8= zB5`sTa`CVMwd!Jv7lmAgK=;*1)WKd+10-1zC*Kh%)9;k~`BVkk#Ovt~#VZg{!G65I zEMHkmwQGGIgpD27%Jp35y%?-gu&X`Uh)e~fzV3BYUue_HHMu$|Jn&|lM2=b4$lI}x z9sgM-ySEBPf^D!ScG_ZTxv_DARrVJ8E8AAxt@X3F3o=h?B!V6X=fG$jI=1ZU6G!zkFREJUfNlm;RVhMxPmEX_OZORD}PEYOn+beOz;aiK^NiZ?MNn zP=f}QP$WOs>R07l*slVi-!!hj<|7JXkZ)1zS$3YJ2}EqcU@6?FimUs^rO;z z1`pfcjWl*I-Y75bnCjsG0}a(CSfsO~E*i~R*NBt(DiRlrawMv5j@D*t58EE5=) ztET7U&)jzZGLcya=x}Skg;dX=-RGnEe{6)djp#*i!({a@Uy`(hBw^+D0?B?saYZBzxwZ-CW9zvt-@v#fQS3%`xKR2;H{|Ix62S4G!VbVSB&fjuIW%8gwVT4Nz5V|M{!k`%n~ z{_z^HMtO7{ow-+h_bd%89W)W14leCi_GkueYpmh736IJBGpQ&`EA6{b1s>c}>#%t_ z#Zu;7FU%w8r^`W4=KZgFf8(-T`}R+{2r;o_F#!7=))-wVcO1u-F#dKSpC7g&h{SecV3u*`0Yn6yInT#5Z{0RbIwg=R3L-_D{2 zDWV8Dtk8qeW4;jS1G)kkr?E7G|1x1CJ|Q0p@0>jnkFhg`=k5QJ<=@_KC}6sben-kv zlKe5?1YD}SmyH#{aTzEX6}GwKm?WuMv-5)g7{H*K8oM&aYp@C&M$fqE#pW9vkcf@4 zHS_CYu@+_<;g$V{9&sN>BR!N54oDhM3=9ku{C~8xeu+d+;2`^tfw6jXzH@!t)V8)Q zR|j4Wr+*|a+;aFC^1(b9_$pm)@E2IUhPy6z?x*W-9f7QBirhzVRv&kd**!NmMs>p} zDNP+OImA3|u-TiunbR12sh8;zP#^04%6?f|5VO(SfYxP7ka+&2Ek#y7c2gE&O$N5( z_WOG<3x26sw7KA+kkFe7Y=p$u=0tr3z-Aznd*CV8Paa0p!;A5!TVqw4h)WA0>?Xhf zCxEgE=6IMymFX&VhKL31p#snYFqSIm{fZ>a1!<(rx_WrR5RhiOO5j&}F)|E8K!A!% zH4WRF+>1E%+A9WUT6Wyqq3$y86E$_=2v2j{0pI3TK=HVTI>jUrwllFOLF{{`5P02< zW9VfQ^)4bv+zYu>knco(;h2&+r^Cil4(SW*usX%QVq_{ORA>oxw>cT;nPk#9K+1r} zDks2TUc2D6R!A38`7ZI}?H6TkgFuboz(LK7^W`n7c^a}9{umT`;lDUz{DiOqd`4gs zMJx*uscFuf$!sZ`dQ^3rH$UlJrs)0AkohShmm3L`HwlBR_w zb5t#FqBkt08ov&Y42_&{fQSs5G;MQrA-t}y;KI1Pl(~oaJ-#_!ZVBLcyiRNWrH)lKbloP=J>iaaHBuVQ*d1gwtX4)AFw}*1 zO2YtXQd0VRQXbrmwNnr}YVz{h{j)%bX44AGW9OAKfwD+KTZH=5Cb~m}!J3Q;JQ4u0 z13-lmG~IQ+-Jgo{W!73TA5sOQ?pzBpF89*TLtT?{j5AVG_If1m@~^j@ExC&Nt+ zp#Ji=R*xBQ(AidBfJ97iiP_fYf;1Z7iEY+h5h^L&$r`|@?k4hnxEwvXl_8?}s#;Qd zB#UQdY~bZZQc_Y5uKF3!2*-|HEB1!)JR|)xpY933R1h=4@;!%6cM$k}=d+oyaB|kQ z#d4B`3qdJ!g2VwbpLyjT&+p;D4dl{A%4hg36G$1j^vypmsO$Iy0rX7{2#y%|k^}8s zk)%AH<=F$9rA?o$&e+M0cH~BUdFOL@XbrY={35`I?MZzo*DYOJ%k6c)J>p&$wzHP2U|Tr}ODdu2-(WD7IeG2M2}|Vb#@i312|GOfhw) zMHhp2f!EvxFzBOTr=f5(sGCl}YV^!8F*a=E9cTB?gV|&|=GXES{sFD=gmTelvTYOB zBp2MZZ6JieJiI<}MyanEIB_R4ug(4Td$0>GMh2{JiU5^;R;J{HU;gpycc^A35uOz;HaaWYzwON&g-f^#@eZBMzlN~>i$NE06Lyd4EeADl( zafRBYa&m>yra%}#&bjeeouMECq4xHdwyi8DmNS3NH%aT+4%34Qx9wiD3k|G&eHZY$ zOKhrR>?QE$Mj27Y>T_}K1P7Bs@Cbw5xn#!W^K+&%!My7?1MQ^de_hV-u#7WrO8|8L zyKM%Xx@l^G|7QdInKGYmbIC6~m&kRS-HCkb2<^!1IX~5Lx>Ynbh5;2jhm$W2j%FtS zoLK+|17buRkB?o^>MM&PzT86CiH?Ccm9g zVoa6>7G#Z(w+$cz%1#WPn^3#f8+^h0>o?&0`Rj77qtj-z78*E>E>a|?!J2#!7=4uh zd6RVy97!NR{H!G%o@r<|I%BDo`v!i_#?c`f67Ell7wYJ z%fo(`$M!By$s%v^9kr4qHTE&xWR7yI=bnzi680L1N4Z= zGLTvKMAuk2t=ChL?RVbVL-8sW^a(X@r%{&ijoCn9TSmK_FA8GnPX=qiw7|F5;FACD zB6Wnv8BFW=r@WV;#G9f_#G;aj(F8@p1sJ3z>N0b`l&df=%cjT_KT4w`r%OD;AC%hN zDdp3ZRUi=LTODfE`o6C;O)oZFUQh3(9Pfyg35z8o3B9!zp8aGpn{H&V6>K9GTI=8Y zR*RiacovQhXPT1HPJK6+MMKmD?`i}b+vZ6FToCT~NI|mv%wGi6XO#U(;HngL!5 zH$vLGhV0k9AH`bYD(-BQkLiEx>G7>GeOsS_-ob*d&Dzc;d*NKY;yh4pv%c+Jf^?QT zlEM?ZgF<$m!@AgHSPCYBon{?y?wvaZq6Tv{%0q?F=6 zXW5^JR?j}BbQb2iUzq9{t~5$WX&SnnK~6hhz8f&GtP2)h^yx7Dhf@YQ0C3I6O4Asm zcpIf!zsf%~Jpm%BZ?iA#(_Q8ho>jSS?hMLoz1orx8ZtbWKw8iT2YD+hgA^9j^Rc=E zE)LQK4=+fxk)wA!tHCSUN!|^^#jgCCnvc)NV>LO0okC6H^H&25gSdY#*FS7)I5`EoIz$ zZucmqn0=w#$uy1)d}TgoNWNE7iNG^pgp2G!jr|ygm(0*(0fq%&5C!D3aG)ZUUtig+ zWUCf+RFSsY2NvwKkrMjxf)o=l*B40>cPGCz{Q_XO2?}ZHq5_%XH1RM-sGy!`dDafl zYBX_bJkgeP5nGO5Jbc24x;(x^o~l&4`t@hSt{(>B0fwWl)_&*M=h?V$=O(nZvhP96 z=X+?f2!TG^!%CIM4&dCwf|j88c_?z8Yz8?j@x6=MMCa#Dt!}fT4x-OSGH7K>b+~I` zKrH{nOu0y}2+|U5dV0oV!EV?1+ISBpGf6M7B;yR+I;nxiQy$ zMt#*@c2~~4)6`Ygab^)4ga033tE&c!at z7kkRs;92`yBUirG+2FK$ar>(W_veTBfh>SK1~fWgOge&w-WaeG8vt9&iXT{u26?(< zo}FFn)MsBB^91(BA~@C}+=)F@P{dUwqdMO30i0V^11czlQ3)%Ez&-+@HehvM5N+dZ z*Pj(G=WNef3E7fnX16X}=)O&L;qyLuMWNRcX3Y@*RHiM*$s%@BvnAiLUz{+J+34cA zuQi7e{%77+ESf9YW6-R{0_Mb%)s8pM?+I@cZ{Ghm0dqvq z$g7}g%ua0_dtlwcP z6q5$*!F=9-0>r=Xh7KGTkBXnU2p5|sYWLgI)!N$@VaaV+{xqQ;_Z#y)FvTMPX|t{I zB{1v$I1?{L`p=pxORy`?h!EI-?e&Rvkx5zm%;Z4f{TJ~TKFrg_$t^r~v>AIHm)#$i zbix5*9*|x0kl$vh*6EFj$;tZTV4<)s3d0miii<`-M7F`ULYF!h^%_+yVe4Tvd3l>H}FU>X7Y!F$-h23an4doy_%FM#9c3Ht?zU0a{Hv& z=n1fCKTnOcJqf#bRcC7hz}6B1;9Uj=jzzzd7Yhr#6^z|?o>%YZ*!DnpkK6lZYv+1* zK>WC%%HS(^+A_}T#*@L+{%uePhbUHlx8^AMqcA#&2u?L%$g^F%MiP8<$C1nA7cRtE zHz-`U*s`ewOPHl7q`#I*KF0*Kv1%SWU}eE8-V#`K7?f$eL{CS1)zT69Hn>l)f8S z#IrcfyQP$6A*ID)Ez4QT^dK1dy?dFbJ8B>bBI1RQEDHQRBfz*YbbCt^>1jlzhS5Y3kY_jS_6kD#?i7y>tFL%y7j)2{%Z!M6^b8YiMthkvY@E z;|2+O%)dEzZr1Gk5G+>hU$7e$jVM$yl_%eKw%*MEjK>WU5RP#7j6Nx99t;|$i z(;`#y(eQ|H;|Avgo#LBHmecwO#sw{AxKHhnbFApzR|vuqy37N4=Wr>Jn^lfXpQ^9# zFeP#%Cuiwa!f`;Z$x~uIWyhhR)^H@c!Vm zw8SG_u?X$%rw|46`RwsZhRnIfA-+OWi#Lctw-^%!ZeA!BZzhm+dN;7l-%%a#@`M#! z%G2#%-2GUBVB%a2Kqr_dR0bc<9v)lT%8r{z-bY3<-XD>hr+k&0IQ2rK@6H;v|Bgly z^~tkJ3TJ}Xbfav}H2Z+3+tB|goeVhoQl>Xfh<=>^JuHYbwJsa@kPHHqGY4;Rk->IH zZl7EBAVZ5brQdb7D0e?o3Y5x}8^8iLNSoh-kZi854|Gx#+O}}Sp{1n-B)!D)*2gFS zqqLkV<1-B4*k>2BT@QT~D2xT}>UxIK~22_8a&1#4eYl%DBgq?0hpb)fvS+=>ckx6u^Sg zGGZ&k5a823+w*=U7HBh2vH@nv(B?8pU@HJCcv8~LH(I0y=Uj+UIo!K zq|7R%Y{IhU9sU>Pk|}@4WY?b!Ufc|@DhqfvGWCuZ^A+Exj#^E93E}}O0!1hZQ2}4R zZ>mI1U00VTSvu-R|8JRsV=t*?jI6JZMPm|6y7V_xcDzcS+e)s)d=w~WM zN{?k5-qy^F;Xej0IxGuq2Hdej-fvR*>#c(xr=_1txrm1IGIpWlAB=J}Vm%x~bcUln zpv{~mg4He<#YX?G=On!h66sy#Nn{TLlCuSl?`>6k40xQ^Iwk96KM(*ixBY2qAmvw5 z|7bNh&6Fs)Z*9wXv-HU9n!Ts&aZ$w<#)DLXd_g~Rat}_{H2f!tVtirkE(|OCMy}97 zfI%i5l|PM9JUK;b1A2Gr(b88nbmR0~I}ja|kc_bBd}>kMxtZJnz00IE2Oyl)7F%+p z0>UTQ36h-3>9-B^nSTc^z2-O%FoGy3PTtpRcc6)mRV?~DlHQ~|_z~BH)!A^({`-(} ziUhT{zVNICFk%7>Xpi3G89bO*o6)AGn|qO6H)A_Wo1XrCX*QC!wI1Y@Cg2lxbK|k@ zkgU)0eFf5ca7h(rsD~I&?Q!BR;w`}3q5u6tIc2Nbtlsg475LWIb~63edQ%0@Z>LDk zg8Jokj`u1{<>^(P=kFy}2WbK=fsBJjqBkBb*y9p^`FpG;7 z=@V|Yu56B%ntFR~5lXL;#xs0Y&+n2_)7K`t&%rXfVb>oclt3Vzp5eUSPPMxMdssal zZa)qqi9o*R2yqdcIvDtWNn|?fK;-1|vkmI*?r!@fARhhLL3^>asC(BUSZR~!$Z4$X^y2k<)$o=3<#qosf$=(Ee7)`qa#_<<8y#k~Ug>4-MHZ6&0Tgr}Hw z&JHe`WnqTTL%3>R+L-vfwQl~mV>)=>WU^mRWc2= zQmb%st*%SfGKv4S_dlLJ#OhdG)$PqJjap`WQ-V`S>8+drR{qev1o z%Yu8ZP*wPj@3^UPYq#$Lq$*20OLtzDyJk)A+Rm&eMbuYw;!C&Bt{xmx91#C%dRoy5 zkCJUFt&AlYbHU0~6PiN(){CRm6XIOQlg2e}YDy}$#Ab_a{)Siz2-h`rk|)|*4FMM0b_ zF{4t>Ih+nT@UuZd!9-wJK)Rvo^!HCty0A{rYXj#IQ1#o)tjt18-O3t~u60BeDDlH% zl1D0o>cDU*sqm&9|Sw4OYV(h&lHRu|9csz0Eb8pf8*72g!99JXi_238WP`EXT zKu|zALYCizbys0cf%$yOX!(Cf!Fe%e%8Hj|I zJK35fCnsA_#eTP-ZKovpj2YO{Y&1DZlxq$XgqL*vJW>0WgQTw6javM_5|ETgNEvZT!*=$E*6X)WjhEMmT9*Pbapmx zMA3i3jG+##w;ub>#5fc$O(R25??G^MZQuee)0v*tih%eV%IsWyNoS_m_jaJju-pH) z&x26eXzNekQbO@Y>PwsuAETo!c^PBp9MLO8I6mQJ7|1s=L0iGbt%E3Q;zA15L3A$) zO%o+YDMVM}DmHN%tPb`vGCQ@$uD^Nn6nQv>S0uw?zA~Q?#raK{e7)?`4biip@cJJw z8drRBS@wf0KMIah(xcNlv;7`cVBUAwCBD1tl1^%xV)0}%GUs3pno7g#a^@;-Ye7$T zC_*aTx11n!GqS=)&-0})-ymcT*cBccaw|FaP_MtMN0(*z<7;a1?(VWJ^LNgR95~uS z5o$^Cv)!)}O{M3GFe>AS6R)rSacq`9Hgv&__rY)Dz~9G3tsn}NJE$nY6Wf>c)ZLR6(bM=}}OMZ*&iAd7iQtkVAY2JK>_ zM`{z4T)td2WW{*;!?>KULt+>!@%GVHF4x>0(Ef$k6Z=^RH-dF!Q@}soT!#Xb!AO|PY*sg5Ts2l_2IrJN4}2+XEz}= zc{I+epc5%<8JoXiEy*7`2%lWFvgH|pcb}{>@^Jn%r2y2h3%nXY5^D!x$XV@`KgWk8 zp9$s~j-f>V@<0Z64fL9b<~Sk!GjhSpZXj8;12Vs$sZ@I|*bZH7{hk`rSC_2B1-Wk( zAV&h?vE~stDB;i99Pj&aanLY1F!Kya@-BZTJpaJYA2hm?uU#x(syw$m_d0QK+>+sL zhMyOb#guNIBio|;CULO3vJxpYp$gctmkOl&fW8oUjoe&B?(mB79i>e$t zOmqYd4UIL5rwhc94E}Jpm;c(yqY>Li>j+HlwJJ6c4Q4r~>B8!wgbz=7ydCs*^!&yf z?ub)L4Y0mH{2Dd5fC@t&FfT%fsNu>o@$3K4Rm5P&@KK`A3E`1UuSwfwE&r_WD#P)c}T<~Pb}jqiHv zGla>O<(+onVCfLNx>@V7P?x5t;Ybh3p#vfjKt+MPw5SSM^g7mCe|u^PKBhN=tZ~L7 z2jG_+*AG`+U2R;Th#LlPM*L4mK}EN`>nbn-ueC| zeU0aoHara`dXTzy(nh?Tz~+2S39udj6xcib@E=lJk0b>~7~d)_n0t*Ci>N#F^$jmV zFU0~bBhh^CPsPdk>g7sRlV6#ra^Cw25{zW-Ov(vtSC0x9SnN@sihR?j{NB4EPdCQI zgT4gDB_It{G-LH|Uq!b~*M-Be5G5r?fsqima85faz7*eIsNGd0kS^@{0pQ$dmUrvn6V~jc(ccb&XBe>1;Ux){=jT^=aM5bRcrcd1P%PMvyd&4e{GBXyA_*$YiNwofE^Xbw(b296_P~dSD&i)4 zP+ozDD(&k5uG1^ov_2x}-;Vl+<3BspKJyhTMH@gs3$yR3*7}7gx z58&((zX^ZuZ`qZlJ~3{=P@(b24;iVWNZxyYsCdi=}?zd{VCNd77DE>2X3+fcId&dj18JU}vYj z6pokscp%0FhC>Nb37@ki0EtcMGTTL+93$dYj69dLauim0H+q^gXSf1xO>uEmb#>QL zlUsw|)9Zs&ExIFt^kcFwO?L)V)awqQYhI^#T*L6b?zl_it&?Ua{wSKjCDCet2?x`^ z@ftt8@{erDCLo^B0#6)OEoALOj$;fhD*+#re7XSmCQV!;CE08Kpz?6jUN(ub3{dW z;^b)&Ix27%(cLZDxhB8zIA6H|<`Q7%H7NXmTF7i|V>@YGcLg0BYUGzjP!Ng}8Oapv zEzI;C+R60k?2o7Y-faj=hHBc=6+vc48gs(R&)>Z_S*WfnFu1*qA1M}Knk(mArgJ#0 zP^vOFJDa0iHf9?w@Rg;EY$bbRX?dAOk1oPZsQs_w_W8|sv0TuYbgn(hekieiz2sbF zda&y&{zJ;dv0>5PEK88>d#W2FmM#0m<4x@^e{Hc-k4pn;JXi_p_0O^BuD=9}$c8GII+_W@VTyGW~cLG){ zACE4Ed83e$Kj(rNIbcVEJzvzZZhWPdCn%Nb?rWmaqbb?R_%3yAI#b93e(*n%9F~ z9?riN(@_^zkDHAE+uF(sus_f7nKRk|pg=+0iojgnX#DpX;0*~H=2tN8x z7AAi0&Jo@sgkv}`r=4)Hq#qs_U}a!1)6iSdck&|1|42=-h-o$gW%^w?*5S`AIA)nMa;#1kqLZ5Wl3lEsJ~*Jv+=j|6@4eBCr^oZSzyl?K(4ssEax z`;#}&W5Fz2&WX}g)k^W!uPa$~4<}t1*Z1dg_4aG`wq@K-KRPyMPrvDJ5{8RV0smcP zRn-v4y8(ZnUAm$0g`G*OCE>q%UfuxNeP_e2eAaQ3q4>Gfb=!kw1nU zinp_>K8LM*u~zJ4lx<_V%s^BfEbUH<3UP;l=O4wVUmU-8+Zm76xx9oo4sCc~82%Nz zZr0?I6k@n*6;;nP@J6-LUbYgM>7CU#?|@-by5aN4X{}R~;9fUL?sFeTv90y$eR;4% zQGvms%p9B4TS-bHtol~Cr@i2I@xhnp?{fLe5k+8lII_{*`Fd*9I>CA&!+2P24c^0ujSdXL=dwsI|-1 zrN5kXi!Y_+AA=NW}(`Z1pU~(ro&X$ycG|Fd9E4sn?sh zYtuNDcohugGWm8oS`dw$JmyRLhu*!AJOg+BJAu1Hw#N;>do$S#J_pLrW)r|$1rAEt zOpW#aNwa46o3J=lXXj<-T+LFjv|PBwmaDsGGE;;OcR3YD7Q`GCiDo)zXxUkXTzTM%eUO-xNavvMcUl~qhYZ3n{3DdKV&=&r>P zFbcR5`AXg=k7(eh!2x%G93>{#MIsV^eSMj<9V z@vmumqqKDi?y~pBth>VBU$4EpFV|Zgu~Hwj{HH~XKQTEM_*D)&SqAz3j^U&qIW2|5 zd7XLL#mva zd`ixaFDt^Q8F^f-HCo{{L_=__jU(#+6=Q<@dKEQ`ghXtG4_{+&?~fbjduaMuppSy_ z-TW*+Gcg8=b`rt$4s`W_?IrDmm~#@P^CQmHRER#;0QI!mNf&={k?*yEYAnCo+3n zpJ0`*{GKrXhdxs1>xde5wVSVYxbHY-w|fqfnh|n>58vZyCngm3*gs31Fq%M-BTRbs<}F+@k1xJujsXR zp46lU2EI`kg7+~A33&IuPcTwcysqBjQQocX@wB7Bd<~gfoXwlFjUwouA+dn#JLTFL zP2X4n;09x`v#?Tq=m#1*`t93T0yuwpuD=%->`KX~YxaZ+*b$fZ>VaF7H@kjvwBITe znI_HI$JlM`_|yKoJBAC^vfksPY-f0|jd%DsuV1>yZV@I+o0{jXWVN~u@`F)MGY&3j z({8l=9gA3b66Yy3m&$JDeROc)zxfI}ojdhaHJ4+6FFPhLalJp;S-0`iLjG~UgE8aW z>SHJpvgCudkRK&%UDqnR(B);t4Nd&|b@S5RRCe{;=_KUKOEXir&jK(G-+=$EYUAjK z3;mh#m^LA8)jw$u1}`<$t1rQGZ2`s{BuJe5(A-%HlDDaIXSuf%TFWEL8`MYc46%s; z+dCB=ydxRu8oa)w_!Y~w>0u%+YdIu9>_g>RQ?9HM(#R&^im2_)o*=0c-1xT>SawMG zl)G6ib;Htsvy&$~w3$O#XSdP3+k3OrFEBUWMV;wU?UL-0Lm`%VxN9-yMEzczkyb{5})C|CU=+4rpe&dwsMcpq95+k>f(Cgg|3?Sw<5qvcIqeGj? zAivK=BW^=Fzne%W1)iugrfy}3Y0(laSGprm;-vCBCu!w8fI0#^o??GjDq1zkW}*Z{ zV~u&b^FdhpzFMxqdyi^-_tSs~?acKjOt`K+-t>MJ{*R*6#G+>sbYVQ6X^c_1!LrqJ zIoeD|+}%BuYCISDKpC*7{ziw4xRWt#hLM&3r%1IJSofakaos{It91FU)w|HTICV#uPH_$tW zj#7#$oEE>q^$0oB`yQ*`xYJQ269>Fb;=Y;D^`x}vvN)05F?{5jDTe%rYcXy~IrQwr zF7yekkz!;rlegQd4xLdmE@Yw>N=2%3i^~PdMJXli!QNCa$t9`qBU#zlT6VJd>3MZ$ zT)-&=s#WlQM4C=6dXqWR<>)CWP-CK2fh9Mj;mP|ml=ZI74OY1WR>XglXPNf+7RGK4 z_(2V`cYlUmjON3M?aTGi-e-*70Tk^?$eQXYDlYdT+&B5h-xrivDF2b1&dk$GEH?3(j}U3~_cAX|X5=7GWw#en%f02|0A zDZs*Ha@}u#lLq~V2T0?0jFpYf;GeFoY!dhf$V?Dk1}^@t^^>QY;5G*5@2S|o?Y83s zz7-*vm01aHy!G+Th&!=|(TCyj@j{<-t5VHWrv^Yk!1M|gzd)7oq!Bxwgh>TR2}9V4 z*k4F|M4qyWpD8yy18}|O%nS$$3e3go1+49#{oRSX39$#Sgv8Oq2poPicVUV@j)5th zBv^uF3w9us1^Dd_pk6jioYB33D@P0z$~X}{$6oiK0l2rPbjj*{q59lh|M(vNA~r&J z4k#wJ1}1{R1EIS9O>_(VELzN9$ut3HkjV)119UT1+e*Xn8ie3zsp+c0`4+3eJ4$mw zhdex1j+Wq0kC)c<>OCoK+;iDN%7gaxlhw0pIbbXqV~nNEuGTQ0!BfgDWds#idl%1& ziY`T1)(AkL0C!u z>W#h>S^f2M{sWWE#@a3Hn60h7F52h~_HceJd@}P+rWTAR@}YNYhP zQ>nX8{bTa^divTl!h7|Q$y>P-UT{sdV0(A`Pm<2(m zR*otmea=&94NIO6)$?dYf*HB8I7qbe)FhZRgB-tNsBCK@r=94p5^@+p@9k;OUDtG? zLA9=JcsCv(f7h3nZue~tnj=5EqUJEJUU+~kRc@^^gGUtEf?II1!uW#Qu}0U#)its= zI`jBunCU>%+6AdlnwWZx(HxTbXR!gGKQE83z42XlQb23bE4)Y|mx5TNoPJcAxZh8d zrYw0=!KZO6)!8aX=ETl$M_T0#Z1vDb>Y5m%APa(ry{}S{p38Md;LbDKlH|fOozGqq zoSFERo!e(Ly*VK7;;n==!Ot@CqaSHm1>eedo0rF~w_?^XZchqFWK?tK#Q2eQrD6jEb60+w{o=v3b*lT0j0dZFgoxE*sC8>H`3URt~;X7PI* z42EfA67^vR@fr>PT10D|Yc9Ewla~6U|@0+zNF++sq&tXVcIC!8avkr4y=ZKtsEgEjBQ(S{K8)Qw}-=nq)Q`_xfG&u!57*Q@1*?m$$(=@|3Vwlyf&5(dmwSt7VEE6Kp5=l?OPbSe z%cAfcM#gYX=nriMm?1wI{mTeEa2%;yG3HpcW=d|Ty$Dh`u*IrAyvm<|dNWYIE!8x! zwT%?2J386eCm=|Hb{lhhggB*qxvT9>VG34Su}95lBBbY{*5X!ErwWceY8iOKw2I(9Xu5iRbaaeYcATAOk9LuT)7JHRpTA{lD@<hwKh9bj98Nw200jHL*Q}y3FM?k^Y7mq9 z-p^z{GSf~)Ir#wXCwNAoxZ#e3*k z=u+ba*NXQUCN`yLj;?dm`~dun9YCK9lzv0Ee0D&zI(fC>TRfMe%@{LESSUpPx)wci zG=!!1+d8Me_{TdmFOZQvH@mnoH(|bk?vVnv{k)ItT|?*GgrkP5pt`Hem_zEs{fjl! zDFPW$LO8Z}^^WfK~XrZ zlHHiPMm&f{2fFpaT-oT^#l;L%IvYTqcL6#%J|4)He&(jaWhnB&-)QJeodW;Blyy#| zI|qLqDi=_3-?5KdXb(wx)yPJY+q+2 zf-)8Sj33`sm;|sLeKv15CSLsQWj>x0h;bqCKB`au-(^|sVAz4*MGHA|-6MH9#$MtJ zgP7@9+J77Bbm81Vnq~a@?hR)N<&rvH?m+YeA+ry;W=|o$dmx@pRNQyBNxdCbE(MsG zi{0gqyg7XB+{2n;KWhd`*Y`feZWK}PGA)tKGV>(PdPn4iiqCT9g1t3hDnTiQBCZG$ z1jnAP&9O0tHfgr~5I4GL@pk5YyIQwhwBPC$#aUK2f5X~s?Q)`SF~kW=-`6OBkx92K zi&0HGNOUU(s_3r`4NJF zWatV1++7S)2YnhF=*R<`c*6#;B;vW-{w12g(QP`4Xh7w4eEc1ARY-+ukrJG5x#5hg zsWsz%z4t<+O2E01W3?BnK#Ijh=r>VDF?0M5y~|n&R)t0t4G!_0QYDw} z{2FTg{(j}g>|(deEna_CM>ef;z4zd@{vx>sy=2mSCW=xcG5u1u-|R)m3rFar>LjbL z2$&O1+6Qv1;{ScrPKTB!9~8%33BB$uNIa(9Aldv%)O{-)M)=Ffn=JAyhg)*VBF_%C zRsG(CqfA)yh*XxZCF%PF6z_VYWQ#Z1MF@;=w!eyP(=#*Ri+)dqq{&~9SCuiqv;rmc z31^ok=5mdR^%LiKhD%2F$3xETkY0zs&C`38$)-}OsXA}#oVsE*7}~_G9m5{hBFAC* z@u9_lt&)XHB>h#o!rIxbMUCvn7{{)Q3IQO-`g(fkK?gfyQP1@B%N3uir;pPYNa^}M z{dquU%;kc=k5(S+XcaCu+(2)ze!BVY$HeYaRpZ982|H4OL}10)R0uL_veX~FziBu! z2PIA7!mq%D2TqEEPO+AIFj)am36vUvd*7zSe%!9EfZ&U5=aN)yw!8 zVUbL>mzLL!gBB#!^KU>9Lp=CJs^l46*r_e`gqD*+&_IeZaEpAoy`lg`78blLGAvlJ zuA?5WaFlxlTB^G);78!2mFwSSI3D~R(Hk(U0#cF`t#Sj^GWFjC5%%>G>ovFb@&xkA zc{+M}AED4-6-Msw0p~vt-1U#XPmQ6J;+joIsh(H*IE4w=_VT4Ff!!7atyk;%IOWdi zZ+jiZAvGI>?e4zDd+tF!X=Dc9z0)ey?B8-yXEI%HVOP+s*naSG{Vl1?;z9eTa%gd1 z(GXT<-K(5WE*<3g4yH2Y@e_^Du$?d3<*;>nj&)DwANvHu+jG%}J=egC%e4ay`G9Xg z_w3^!vwB6idOv|UQ7PWu@!hj`EXoZ{a~n3{!(NpEu_AZ=lP>a_eAd=W?d+IwrlRxb z`pS#*s&(0pJUbW1@BatfccWSeHuwKZ#9n>4rCIbtZm5hBJ^fx7l}|kRCX%Fi9r+ysq>&#ah0p=O3iO~we5`cu#?JHZVe8sH15o<9g{UOh`6`UEIWxWZ_gaozlw7r8YIe3{6~IL&+qi8{cV%i7KwD2 zg%Q%vEjM|it+V58QkZ(h?L0KiU#(NeFXjsr+!kv!8%{a}PS>K^7gQ0|2rhY1)f%O)I-y z;1w@ar_UTDC9tI5coHltbKbsvg^wvKM;C__*pba`shRmGqtauoYOH-c(*Wf;nTL-P zIXntv!;o}AFbX?$z1raBR+3bmtRrpD&B4I#6KG7+I>kG_KJje966gkBBpVQV9+7J+t6sjBh_@vidxLVbqDID#7nVjL+D#hHV= zhI~b!dd!MvjC=DS#hCurIHGWDs4T%1xr3t`mWIY_0yJ<9l7M9H^0?8iGrN~LjDN0f z`aW?22*39eQLxdY_8r{d(=)slybIG^-109-7KgP`ATZlaCYJ;Ev)@2zA{Z~Y=#%RV zNA|u&$|$ie*>bhQBzN8C(P}RtX9}l2DFF57e_f^~`AD zgw>anzo@|QKeAuMN4!{jbEsypLg@A32Zs4%?zH?cBHDGvxsV{g)NI#k-5&YnO-E7D zw7(7&#b=#gmdK#cg%obs6;)Cm6%Q9-wACF}u?TtRtAsb5of_%o6diEvclSXW4j{Ts*DFzmTIb~H^@;F z8O<6Vm@w^l;|e(tBR+9s^ZCk7Tz7FbQ7XoPUy+4mVNi!_;59ueV*jTApg{$SriBke z>fO2z&^LQ+b5@l#HQl8j+MbYHd&k)DE{H`ZC0}~}dnxVj&e?H#s`D7AqL~1?_vle% zdUqQk|FI^4b;P3X4gZ5o|1yu6ZyPT1cSU+2sWlN$Yn# zO|pp8!y}d<{{yB5*kRy%kfg%-`KFVfC{3(DQod+hbDl9Jc=}!uYw$fEquxXgkbC!Vot#k?ihm(7OG@J+XCjg8F zJP6n#9$6fCv32!#T&xxeCzoyay?m=Uq?{PkcQA<4$sgpAG*KmfGUy~nF< zaBnX~qHab9f2d5JzDF|l>s`S6_wRqGh53w3t$*w#B`_iZ0x-`i)1gj@>o>!F!s6J7 zRPuNf(e6}N2|5_C&)iNjTTUL}8aOZ;M2$G|S-vfe?6eR@*&sXjY(|ol7~ee?wbEhu zI0d|1{Vz_DfyZ4{9XXg>co!DEY%OvXo+HPC2WDaXh;Eq?BQA%7zrGj5G+Nd>HXy}D z(EAL@*VAkRn|aE0Psx_HDvi*Xl$)N`<6pU85 zvuP|p&zZO| zV%2lhw)G)fux_y?0-Q))Ve_8!FO~#>bj}#+NxdSsvF}p>)%cVwLv7Bd89qEZivneKvK~E_P3mGZG_o9JxEun z(;=CBi@r{dmg*-b%EPtf-%rRRis*l7kI8Zat^TV=2tuEEeOcW7LwH-((eu|XZCc0_ z%ur1K5ZTvPjuRDA?_Jh28mqiGOY+)fB(_K1b-ub>!cU*Fjr4rCvLr>}u;(eh z1XN2_g~CgC4cB_Be*$`viT&MY=Sh~-sI>ekGprUpkQeC4rN^$uXk-Eexj54XLc89M z{HCAp=0Dezk%|CUK)wp2TB&CGg8TmbcN`JCN-`Soq=!i<+Ed?pi~c#`M~^@k_C8Ot zVO7qZbNS%~K5xGCouu2>#(|?N4iTZwNK(GPtDV?#^!So|dyQwq1kkhLg1t~V4k1-1 z>Mw{0e<~H3lDoFguxbp4glE#N!*hUkO#8zJhQz`5_DOw%W`U63@C)2TlcC{ZAf`T$ zhR*KZZhRDe4@iC8ny7J2`Z!7~gti%3T9S_K3jvu-MpQ%uzt8T%b{j89zncv7kO7cF zrUlZV=Ex08%D3EU90AuW*n9tgP{NN4spJ%;{4CUYhS&PwN*YNM8D_}z?iw08Z#VZM z6Z&^>Rt?gp>ATfBIP_ay$Wsv=$3W=ZXu^&0uQQ0Rh{2BrPEnAsrtzXTU7or}``Wv; zf{ZD5T3%XpcCKHo?*&G#nmZwuDn(kLv7*kok$755wzgw8@V#)rrYQB5Y0KL@evO62Kot%CoyS{Cws{ z?&vrRCTA=o0{uW8g>K)A^Wfhpc|GvfQH;%qWKeaGRf`3$PMd=U?=C5PFvG-R*dUf7 z=PF4{7M;(IC5E-AUSKb1?*}b$kWNoC-p}Usk!j9QB@d0HtDe8T2i06$Vu#OCr(DM& znI(%8%xy))QLq^yL9w>U>$AtjaJPAJ`t?ydFHgjwvK>vum8Ze#^t;HQAD?qN8U*|; zaHNcmLn>S0A=b=4RVO`;Wwzn6#}-O3wVB_I7UABpeTs-9ik;qgzkE9;y*h{Hji8F@ zx7%*VLkM!4jBCmk5P!cR;+ZtCyRw*(~OZksX0|8^g zz{nOBp3nmW4q6B>1jb^?hP#p#vl+JJsIcTzzp(b<-K9FaZs~JMyS~-|o*6Gs^2@YH z-MAs)2s4}ZmP2b2K=kOA%9I-9mR4WdZ=@#4=9eM(f0hWb+$a)CLbpmP3A|yW0(L`r zS`3(W`--$Os6JEtT318_sZc0XtkIQLB@m_UGO1Vgmd^x=n&NNA$QIr37M@@igbEPGjrJ5 zi4q{c*XdIV^_(;$^rAzeRiz?%nG6Z_t}Hq0jX?CPXT(DfP&SSpSS++paO;SoEYgWv z={7qg)`Wg9G5e@_q-n}GK`9jFFX4ol>SeYQ(pw|XfFNXIQM;16bps~EmZNCL?KMUp z-MQSu7B{k%)Pr0&xAPjU9&8qg{dMb%?}gy7>|r$qq~CY);n$W1AMdEWObro?B5OTZCUGuwv4Q=t%GC6!$@dW%I$w$l-1+{R2@6tz zF0j3v05-2}X}Yk6n_$z{McYkjX}7j1x8}U;;ozTEMTz4>ym;}OESf(T79~UeE6h}3 zEj6SnfUWlTe&OqpeJ18Ref@_5tT^-GwF}tmfx>urI>}r+#1kAe_WsFN!BZY%rSGdo z53?FkGX5A)QL^nU4xI2m3b9&-=NGri$e0QpMd^VI#r{ zbikcP7q5Vs2G^XvR3(+XAHeV`FDjBLHItHzXO6Fn!KKe#Q#mEs*y;jMvjPS!o=OdR9F*jyMoN>vUu#(j zdtIiPRdu+n_@JOh@pq>M6j`YkYk*g6WLi@>;}?gM!eJmIwLI{%t+onBK;}>W!)!k` zVdohW=NWR>YA~FAu6Wr(T}igR(Jvj)k!2FXCfhIC7iC{FZ)0!%n&6#G+0e3YbV?PC zlL{b~+m8TZknoAK;>y7Jz}*WfjOqokm7XZ%ajiu=E^phptNU=={nQ`&4p!xWwqIA! zBtWr0dvag#L6_h;ic|t8qF*!8!;CB<;Y;Lz)-|`(`wVZb19u?@tJZlB-X2F5{xQBL z_!_&AFWwEwR+?jxNRzYGx45|xpGV|H*ANP?FzKaCyh=bPy#L>5*=+~4tU2PMH_tpp+E~EWx4&^iN^b)`!^eP^*>ho&MZm#bQ z7gKC^(Z0T_GjLVy?h==+)c5pK`n7|Kh21;ubc+@nUfDu5CziT7QCtyf|IeAA@|vah z*fX8$ovS_!P<_K&Ao1Egm3g30E=u2>j*Uad%+WDMn}wHqd^qeXV=AtQ92orz)pPWi zCM;IWDy4M7*KY5Gw0trDr7B%Q?Uj{ zB&EJPv({k8D^v&EGjLmsyHH=vf98QqaPKLkd)6$sHS1tRI5}C7|5h}pnhmCvM|%1S z6cmcK0i}NDmK7Ke%*}fpzF!G;lVube^uJ>vG4&J-ToL2{vc5&L zS>LMkr#YBQAGk@tCz{Y=%z1c_uVlzT1 zgnpWDs1YnbkXTvT&9_%?@JfcB-|pa*?)h3Y9VL<5Ckh@iwq+j3kVF>KFj(SYleS!E zVHXb@d?nSlhO$JBN7WUP7IW8_-B>?T99#On&-9vaABl~RlQ|~wqV|(KO+WW0e=ZO* zj}UW_RkduEZ0%P%0|8yh&Yz!=d@yo_7N?}#Y9fQ}g6oe#_;W1e89FW-RYJ-kyAI#& zyvifjC&@M~_->7iYi*kdur)IbnLUXva3`W!E6%(cvd>(e{_~byA9ZuF6Gf5ch;>e7 z-?Z6aWFJVh>hX)_bK&IhZ>Vqg)}=x+*;8@!OJOD~ecGQ}m+X@??`e(d+>jNRH)K(7Wwxu#q=-UJMv;rx6QDsRAWZ zqeQ6&W9o#h7|EcWXCH`@kb3S%CC8Zz$gjhD(L=MZ6=CJT^aMP|uQErkV9%-|Sg-Fy z#hwd{6uyU{hIHm}LMTUU(@>oWK< z$geK7bn~4Se1Nzx#Y)7Xy86XOEK$eyHk5wUtldd#%z;g`DFsX(9{1L-p1v;Rr`L$e z8NzE3Y^}!DUV}dnz5a<9ljT&f!<5-Tn$$o4?(U$cqjmUDVIl|B0q`9OdwbjWOfOaUrTQ_mZC>JnlpDa<=fp7j;zN{XqK+7Z3Q{0d zm>-Zk4gTHWdS9z-A2O$_Me3W+e(*fAW9k~Qr}4A~p+ACFpEqzrgQP}=!o6Ld{c|B6 zVM872LT=VKFD}{ZIOMV;jH_NRJ-1gIL8xMPcJQ9O;VdedYWXg|>xSp$)p;qJ=1?Rk zQNtfavt2)XV&=*@B6@b*5_hui-gH}>yL?n;G8BaWKMLdzABs}QZ~(6Vg_wN@U-a5; zV^j_~{iD$P7epsZEksKI2YFpgDMXVL*q>{;zuv=H1JwvnZ=@kV)}jg5Rt$rysu0J1 zt6EPJbEgjL!?b;ktKIxRM_X_|`k)BV=8Gf=kU>5}Qfev<8OFHHWQY9rAQ5m%rhPD2 z%ly~bW^%nxJb3;s93(UWp6FD)$n@P+O3)52YDmBjwb#ZKNx81pDWHmxH#PMGe)h_K zJLNIHa$lNogE+spH>J3=s$04-UFU0F|9A+2^sh0ZN)~`H^yE;IAw@G%4|14P%@JnJ zSqREx&suPUgezb~;Z~~Aiq6kwL<8+itG$_a6%7s2xx2#M8KZEc>wO*3zpKx{1A`kT zMP{EH2>GPMn#qtl2w1!*a*h4+=hQv70PMm;MLG^05tB_Z+mK!WUKYKwtZI7MOvZ4; zN|w0L-sjKK^VDksfom^M&6FV(7$5i_d~sfpVRG@rzaqn0JE>oI1O2U*r5(rscYp^u zS{#g62Z{po6O=qIq#i4bI_7h`%iGmcYVT$nm4xyk3GOEuZc_qB|CQ+71+VE^nyl(8 zpY!c}pZ&o|c5pe2){_S#f>Fv!+(U^D$nzC@*OHyW5hn`cnAbBXQqEteIKng4SDx)E zPGpY-9OR4)6CPjC{9O=92OD;GO8i95?^_Ph&{*lCv!j-{!j!dht6C(58sEm>S9EtT zagIX~!wTAsbywF-|K20l*V2ANMZa|li;S!GNWv=3qBPxO1e$+R(2Wd&og>zjJ$Fe z?sFv8i8_$UdA>FM#XzSiGjqurV7*p(3Sd>%C>84dDy*ag3PB&c$=r^XUU5_&F`{~bEa;Jd*TCjjeNxncI#Q5N-cA?z35@ITg9sXH}71}&;8u{U{{sPGM9Kn=N zk1CW_X7+X(rNf5*EbG*4o>az_kLk_C=olkEAodYv<5S4qOlzZd+Q0FSlx?gHjSD2#}4( zh<07V>shWMdq_i4%)cvf*swIs?2fo^ojr?kmG5Pd-19 zmy%EizhK8lhaWW4B|g7f1b`Xtn+b<;1raDYtivQ#)o<}A`E5Cws?Nk?RWqOQerRax zQ(ea3CK^zJZGr4vVZW1KC!tKZ{wj>W9-}}ES_TFNPTNC;Vf*eHQ_wSI(|$AO``(q; zArnT@ANZ$6_&o-K`Ej56;{d=Kr~%k#a`c5-op-X;$|~c)d_1E{Fx3hP7T}Q%q~}Xl z0Kux4n?%AkLaio-k1E$|;VCDeQRtRC&ow*DM>=dc#08Ho&imxofYOQm^zVBA?%#*% z^~gIT!8b2>(H!6nBZSz%8-h^|by3*F(lF-hX!tq$^2pU)r*1u{MUDKgH+5KO-)S0H z8;?(P>OIw{aB)$uhgD0H$7YP|{z1SVC*92ORWwn~%?2p6z& z-O57L+;sH~hoT~XZ zzCMX*rU3F_AbA8jtI_Q${lpo#sTEWAT$Q=U^(kGD+FBnq9<3@ZZuHqd6h4q>NeZl{ zo>`stMBGZbY8(YMB$)URQBoXfe{ptkrgUoh!P?x5XBR7>v;DE7+vG<=L?h43H(rUv zQ*-eI&A>&Rbbm@b(s1ugf#jfO{!;-yf^e*qFokSXH@!^U7I;=Ri7K?ed>##<;-8VJ zn#&!sE6k|g8_XxUqCDS{!kp6XCc^D=v ziPj^^o2FQ;@MlYlBnZB#k&hF^h~1#0hy!#FF2*y4w=}ob(lrRh9_ek1{vafvRO@H_ zL_v+9fGajSA{CX#+E6u^a>2Gn!OHq8>^m;`maE&?|0A&6C=TJ?7#Q!uh0rwPc;~|y z1&dCh!uOC%>oh_7(kkC8Y{55Nc7|#EswS|1@_SZz{ACw{G1|4a>JHmSs;*?cHD-%~ zpry93?92wg)u-faN^%`(4=SI#yzdv~A~*Px@S3M+-xniCoxAKJni?Of+;QVg?fe{+ zYiv9xpf1DsW5ZW%D+ zLPgfhTy>@v*CMPB=p{07$&vi{D0kE4B0{4VKcT^;!}o_ALU$quo+1X+G3>i8DJqQK zpMCAggj(_6Xq2flz6@8lp6YN(f}T&|OVcS(gIaHOCLjo0NiB!WFKf1*e0@s!0;ES5 zOcpVk?L>K*k|U}TpdtqX6y#6k*OhJ32#)o-kG=5EMSBFr#|}!{O$>s5epJV+kt4bW zxp@{l^A7W!`u(zcN3Fa=QG~||h!t7wb2@*IOWH%sk;~EpUK3OQt5e^EUJ?$K%p4Vw z+tcUthA+ltk*7@>eAh{lhh35PEU~c{I~T75vz*w7E5o4(E7Y4$M!E!(G(!Hj_WMXVzToj--XM&EBAiYTcYYPzu@dlp5bT$3Ihu};mO=3H)5Uy?X*LK9A7Duvh}k;hY^!gNyjodrvDQk?Og$hq`ncu6W3f6=QeOJr zJ|zoMjS$U<8x@?Co=hQMW4`G#<5;rgBmfFgB0W2GdliL(k%TG2Tm*+F)#oekLTclsWEg$O8~MlFK~d z+vaRnE*|AgYcA#Y$&XEo|fF>D)d zXWL4wYD4fC)F4UP*yqL-voSJC#4$TP{?TBR+nc)aohv0nBXZCO9C!sPhky*NF+Vr9 zP{3dP%5_)r`65V7_L&3uKPRp2wxuuIA3$~@@9%^?`-o`!UN?&Yd{uw03B-pr67NcC z2S*7iVuaBXjAwj7!1-9Zd{%ap=Q`-1&Cl9c`Nu}!tE1?CgNt6zn*JW#aW@2=aOwZQ zubwM~sFB;XeQ8??XTo{C}(@G&B`ih0Qr>QeJt`fA8gM)o*+Z=q25(y^Lc<;CZHd@Dk`}t!iYNN^vk!>S0sH&eo}3SCk+DPEw&8?;s&C>KQIDi*rp(fS@OT zW-ZoN*nLLZK==eRu>V=pAf2b_tT@VgBPm6NhZQH+*JT{+JhxjTJ>%N>I)P1b`ZayP z$`K977mVm8%j>I&0Q(!zF?lh+E7g>=6tXsP7cuyA@^@K~F`U))4|{>2O`^+T%lI{$ zScjYP+T63r4@`U63%iK1TC^|xqcvBR6T!Zeq#QtY<2t|iUuM4Bo*d*Rz2d*JBlWq9Us=C`uD#W*(Hv9edjF0TA`%O<4wf)Dh6yI@RyB7rtRM z|9TuYq)i^Riy&3gWC4fVh4;h|h$QngMP^&c!`nqq&??ve16(aEtgK*> zRg%`aoXCy8&bK51c^bfQSG1YISYhN6eLs*yObCWX}9&yT669_cv#J)-Vu{07MjJ={9nKkpvlOV_zZS+XA5 zWa>5!b83)lTH<_2OCfrPix5o9CmI40 zD=9Pfn&+nrx!EB#9XK*UzFLhYJuLGlAl-|KJ(0lu0psqlx&780>E(Nzy_|R4#mrnQ zZtLD~ay}tn^(C{Ni8O8eo{IH@MSIwY?JrZ%huMRb0F6OiJ!$b`*>ERP^s#*6kb_3+4~p@q~K z??+qB6isw|%{JT0}O0 zHw-9$OKG2x*`17j)wo-g&u)CpL)tS;Kw=ID06rr9;U;dRE_(GOIBI-a;IIE;xiatc zX!aGl&p)0dIaNGlZ{L`_uM+WpGs`-s-Xn&E{+3?KnJ9&Dk(!v53H76hS}2iJm+b(k zs|X~f0MxMiErQbST*(A`rdK|tlw@0n_&4x;gBq2CL#Xz>%E5BMxw|mP>Vjm74lmvb zw=FgZHAdf`{?>*_(Bec$3%KqfflyOXtuj3jSYXB$J1 zmzLjH%={=Bwm}LAut`@{#Q}@$xE0g%8E<4CE<-rZIf^_T=3bY0yIAFs8K2Hgi!4${ zXnwz9-gBga$nQ7ztTVvegey(e1fX}RKtd*(JtYHmF=D8)$r*&3)NyC0))Xtf+$gxN%6#fRpa2hfIFp~uC5d27)D{0>O zikK#&DS(+^0u(lW09-!09;9J*rUyp}ZKL*JFG!aA7Rmr@PVP6y2lp?5_}mZByAn0o zRr8t>Tu^U&(p;M@ z2}rKdy19n3=Ivy;7!+GCRY$D!(o+2v0y4#y<22LN&9>nM|8&3`GjRAraylr&?*H>zv&J%Zs; zD%aq+{tV;{n+)c{1%bsF#FP}59;VMiNn+$?XN48J$n)kWTq8YmN8nRDdo?$IUtoO} zTV)W5rw4ARuS^c!`a4aNVaSyY!jYcNZ_nKj{j)NqsO5zzdXF-Nm2~;Z%$VXhoLJ?l z`M=+k&Dp53yaZ1C>JJCmrWzWiKz9zJx}0tgeQIlYE$hv!f-Vr7b zD3YVu8rOXV$VlV`W>L+2cFpaFz)(kR6DF>1z%7}NW`O(S&xK%9E#m=iyi+oW>L}E! zO?Vg&R?PnR4TRc*(A4!xZRw^`6-I)-ZFFucOzx#Qx9b07=KTpn3j2b~0k@ky2+5bs zMvI#oVl*ye?crj&dZXN zEkUZj3_%*8#mA4r?-31a{W`8JCK%?pNq%`-U~N|R;SHai{<_q8_oQSgwC*~_chKxU+h*t#8l+SEq~OCrMP!bA+eWH zUqsIveX|rZ&x~?|aFB&_#F>~JELB%2S)bn>3a6#xhCW-#s3zgjDpi5$ia1*2DYiM~ z9!>y6v8o>c-`&QR$6NPAx7z7(fBQd?=p+EiZ9*aSvwPNX>fJ)_GiT=?u>8KwN*VPL zB&u*kgUqhQMGP%A!#}yvq95(-5mCCS2vg-=xtVFF!JU-ZL&0+#wCBc>kT zP~W6oU7>o>CpV}C_uhz1gX_i0(M1d-@AX~ECY1+LIeTaT+D^K;L&H5 zF68EMaY`4jHl+&71(&QbCoD0c-g6TT1Xtii$|z;Nh5m}tA_xLHC*H(NZQqWl645rw zOs%&#zLFkENH|jdfTG^t$Jic8Uk|*p2iZgN-lCK_jPrv1B0KlXR&}ov6bK^dNCL4i z`OksF1Up79Nzp7^u4{P>0k;1a%k~ohg&#GEQ<$D+O_i6~s22(b)N43lUz*`a=a=Md zs!sC_z^*f4i`}R-l{GfrueF+}CAcCscKruIcxAFbf#K+UeNtxYJudq#qZm03ys;^k7My4&fw6by4g2osJ8Yl0L>TWyqvZ$;x^)-`5l; zvL&)Bb33x|(Dkp5*V*aZ?&?ag`OjdD5EBi0xWT_WRWql7aoO~m)K|TmWqx)3dZFzZ zz9)Pp1Jzev94kUk5W%kz>7g7OX$ubr3nRaVNY<~{krgV`%@jY7VTbi{|E8`xt{I5z zv7aTSC_K)<5qR)P@`#?Ks9HLRYglWs7hpbi~=|)CEptk77$d& zAN>~kdGyE}3Iz_VQOC7k9^-`s2yL020cG&&7eQdFDAdbYqKGkcm~1AYssonfPNT=G zt%v$A76;y8->c6XC$Yfl;Nuwvf~a_U1k-tav_D#0h*YZ_Xr>xx%K`uPazXS}6#0qo zH(EB3O|32{&rU#AKTR1-UJe%wH1C~y9vLvwsquGEKi!@ZzW5;s`i@i(D_3SQg}eZ@o+LJg;Ho(~RMkbtAH|ESn~ z638X~27!?+e15MH30j+>P7Pt_@m3#4@+VGdb|OInLHV{7Wfl^eSG~`J5x1Pkl0T2z z0l5ZayuxG=4}tXDAiP>jMin^og*|S*-(pjoo^GPgA0*_S${S!i>1~0y1XhFk_Aq<~ zta#pvK;W^d(ahB>CA~&wc#=8OJER!wch~Fp(bp)$f;J#slHrJ2O0^3fFPE>TsBE4z z@@REuxWwQ|RRZJVwfDb|Bhn$bXeeR57z{Bh+hS@AF-2Bhm4phWy2HV_s_DidZ#5HQ zOgQdBcA7;;euKDza<|o9^JBKUwM4>Wdi3FhXCdNTOA%ShR5a+`z7)QG2%;b{QC_@{ zob$h`8bH*}yxwB>w7i$`Xe^cLpB2&Jw2NI-y|2{mrE1Z!Tu=zm=CT!ZwCc5q-HJ|L5yE}Nw{Oqvnk1lV4lP>!GMn-sZ(;<0@gi%IsB8Ah*?w=TW3#X;`78D6dNokPo?k?$)ZV-@`l5V6n-J9-BcXzyt-~YU49EYDALv+BipLMT! z&w0%`cI$1eKt;Ni=2a}jJ1-zyP)sEtczJ6tH!A`P@sVS5`fWr=x?+IQlr4ifq%NPR z@$hiO!P3%Fz7*9qR4t-&>p=||mHa_O4!ETH|3=coj)TFvd?<}~uy)DgGgl+b1wG$q zX*i?{5m0RP!JYyX^I0sRDQU9z3OAXVmXyHk7@hzSB7{B0$(?@*^n zdN5Th6@imr_WmTxG%4%_2UgeS<@K#tXtIo*(9Ue#?w_I}fOesjWCZdfbI!hk(z$1# z*mfR2y@>m5D){IF7)UQduK`?{%3lXRx42TIc(5@91o*LSgePnk?xLXix5)U ze4|b5Qz}vhkKColQ&L9T*wfQr$E&`h62ydYLpSVRq(r`W9W0WQrm4AnQ*90?A{2xX z6rrSZ=A}^A+}(!b3`@>Js2OdMA6K6|l*O*+2Q%QFC@K;>t)c^8wpG&c!Ism5f!k*F~%28$?$XqmY~dn9WvbL z7hJW3kROy^VmL2mr@wxyypfON0;C)OyC_k}D%n#Y!uU($woZD!i1~II1M@48e`o9v zj>|?PRg}_&l{c@cJx-^C(Ql8Fhh!!47e_{&UoU!ImS;p6Lx`~Sj(@g|RRK8^Eq{7l z)c5+g&k{1}=#BH^uU7Zn>k{A}53ssUtAPbA7%KS-jm9ZlUwPb=fse?w?oxfTKL|Y2 zFTPGXzFM!13xOY~@+Dii`Pqt;=q)wK{-)9Xcd@JlZg3#tMl1yx3yOymsUSbi&+s?- zs<+kNs$?bN@kGBCtQ475fUO0z>!G!nhWmGEmx<64Qvg_0hzMgZ+fI2X`HSG}FPaIw zt|th`2H1}`NatRzE>&;6FRd6adEykd5jKZ2VU=HnbWKe|R2hvVB6Uxy=PV_2v9C2V zBy-)$zuvQ&3Ah>Hh(0e{dzUT?#0!h<#-;NiYOi|DJSE1gouZ16LpcQbGGd@}8?f7w zQ?v~=Onrz-bHfKE4JGRO=H{uIlRe0eZG|g7+F+He$|=SNZ0nMd;Wh-~lLv#C^0>zU z!Z(iex+pl5sIv@^>Y(>02wMGqXVA#oQ{5KZ z5d$orOda%1$pnv%2UQ{pBTcs zY_Ri^K^B(-*&}@{bsYUu{;gj8L%EJx@h4>e9b5X$+uwUvi@UIz6UTLQX zQ`&z%N0%8+Yyv zYg+MZ01+wqX3nx1QMwC|SOCUQ2YmgYQ6^806-(aqIR#vnTvDzgjmzPUdm5jkStjhZ zeAKL(x(3eyFswoXDI?c&>_~Ftf%ldFx-au^N=32&;DCBJ8p}#15R^`O0sG zEn`8lt3oLoYL~Es>$LPTYkARqzg9Z4pg_*DeLpcU;z7vUS4Bgv)Cs+W#7-%i78?U4 zE&cUXe(}+aYn_t<`SR`lXx#tL3cu)Z480GvRed`VYof12HB;~)t$xtarkhr^(>AbO z1M@J0)&2O1u{+nu43YnpDM*jzQeR?!THn2N@DKPTv^O0HWk{6AFQbbbSc8XbT>K7d zr!X>nNk*2QUpgtaxV-cIBDV|}3Q>tq>&Kyi7r(RNvc-a z0&~E6Sz~qt^F^SabgqI(PIPk*3XLJ~WBvzL;julT`kLAL-P3+NFc5?6y;RujLFAo3h-c09&~1vq%Zt2CL=9|d3pb%uTh?_Ot^s%DBy6PSv6*F zeG(DHVVW#Ee<;k^r3a>W@ZoP(wS6SQR&|Jq4Wa* zPajuzatEvSiyNL34M#6h>eu3}d`imsg>9!ySH~ektqbo38>Z~|&I4WP@$%p9dHO|J zEin)bm96voF6m%iIUq7;0_*X`CT8XBC*D0Tn(j_%Spz|l(X-$2_2Si-n9 zkLqZ1p#S}3*zm9)P37Sw^x(5}v5^l*e;n&*p+XUM_3ho=NPxeDE2T9m-NEHGxRt-_ z{9nZw&^uEc_+(h$h~Q!@ZmmHqFEtaXLX_x37$KOVx^<{{X*r-@jyaw#k-;7Pt;~4A z6cJ$C#{lu@=;#=y2)g4%Sfjfe8*=BK{gnzY%o;^Ff9aI50LC3~MH^+sx;Ej`q3$L|NAd>%%~e|hpr1qZ2aB(m=Z+h!{v@9# z06$)$%F$-y6NWi4l`meMhHsB`?+m@U6p*2QMus$b;|XeDT`jo!UgL`R!!2}h?0np> z+h&zMzagoGLH^Y>0Q9zyq%o|I=g$BjJf;J>=r4gt^x}@}PZ@C}LVMZ_f5~$HINg|V z)3yUI)yn?~<91bu=b)vEUhR@ZcsHU52?R=h&VdU4!PvKbi;aS!sp|81mlZw!-Bg`? z4|%lu{uwP^*q@TJQ7eX6(~k(ec)CtTNUh#>xYndmUwDW&I#=PqBJ2wOd6oM8cR?DI z9_jpguY0~ni1))VaG9joOuO!g`W5IZTD?*^1oHSQ>m9>d&CSvUZynNq|&; zd@9(2*9)T@Ycvi9h!qxx7UL#-;wv*~AV8~`pq7*g0lSA*-5WC*wxONUB`tNDLr(%fCA3J| zL}oDnvJ1XO1JPR?vF302|MO5-)zVBxil2b+T(qI<;f#PJxQmv2136E50CUp85CvXc zk!<-j)je*B=d&gqsf>5$q<3>#b?3w<%IOPD8m7y z4)u~=i^-7eVB#naA)zp5KcaFVxSYCzD1jkh&4W@ho5z2j%^btxeo*}Q{MN`Q?|NM5 z&KEQw#Es{p1x@qxW+8u)#{~y<@$N1s&6*N6;Q0=K4?PgeaR}f=<(=pbFNEb6+b!ZI zUcB%_mN;3)`tZWJ&GHO~&vtV0`5A9&im5~r8G6`rABJ$sz ztO79>y`xSyhk$GtIWIx=B9NI=%TTY3087BnhnDX%;_mD*a+Ybz%A2AoI^57TJH$Az z!x?$`@6=P65+~SUVqo2oWwZ5V6HK6>6OSJHDnQuRs5x>nRmyUpS{@e!pq>G`iV5Qm zPe}zyGDbehXGi{b>G`{S-p$D1rdofiPNOnuO1FVgqLwAS*Iafh)kgX9%9A1yZzYH0 zD8C=BpM#YNgrxoFvYbsKJ5IfcZTKee$jU5&d~Dvy8yC6Hfsteznm--4U}nRFMD$;>``bR;XXINa5T%gi`w&-C}cN{qz;=dLR5NwQ$19sc1 z?;froOHaDjQ%6f0zyIvvR<#Bsr~3~8W6|OPezd?FQ^N!^IpQPt&CHlDJlHFs0ZKUQ z=ey>VfV@I6LAD#|Fk86f26`NXtlid-;EJip*Ka05=mFke!22OhJc>*a%s@iicxRNZ zqI!RysGXpo2i)$(4c@1!AcPN^fEO)eKb0a~O|6q$=X2Q#`M-`@Q>+CWee3kO3dvx) z-Q~8vI?m5@Ioo2Z_t0fjbBBOcJuzCP#|CPq_XS;8*eQ;?V7D;$esQ3sTc5vS=jZ{q{Q4|jB6wYdb+W0698#`9*SOA@0{ zk`1p60{(~cjSJm_MiMcNpbBpHH{}!%sxvc4r1Q!uPn%>XDWVyn|G6t?B zV-j5`hR)6gI=3T$Sq3l67BFPSwmFjra9i&@QuDuN$`^BwR6P(~!hsz@q6sIU|8LgY z-V>5}pS|rhi1|p)Ix46_|JrVb7!VrvFAONUnqm^`h=I@57l?7uT%20Y0-^c%H&#TD z{m*5v?_MWs1Q-WGtLLfEkm~i*>t5-6H9_|TaP$KoHg>WopsgDk<#7nOo^@8Z%}J)( zRSPrg{L(D70foX3>pBk+nx)GgaY-r$Zf>Ew+@+^a2@Sa|lN@t&BEH!S%DDgla7hOi zj4V0w#D1XJQ_pI*9fH#a_~KrWRoGAUBBO46z9E0&t!>V#{_(#9x#ANbiWl97cV}#kIbRrGuiYxKChFXm@FVHkoN64S3 zMe8dy--$tO`=>q-ao6=-m-{ykmDR6H6+D-&UIC$ixA#nQ=UfkQpBG^ ziol=w_-o?IS@=gPlMj3YX6@^?*qj*gVvFOnYf|iPvYO>x z2u1I2X^^wQ4oMR1wRDa+6u<%6&-FF$!x@J6+ANd-H&hhxrc?NSX+NjxeS#Q+Evoc8 z5Ky@SV1Zc1C`nE$i=+(`270qrG&AIaY6D&|_GAjYu+?*P?oa!rt$yU>ZVe9a83~Ir zr3w8H7Xq?rtJKlJmnJ5q#dgzR$iMquna%_cjll!J$o?;nvW?FUA*a6U!A*tNERdYODETRL25RA59X^{(}@6uz%PWnBKUqwD* zf*MKEJ3%tR%whTbY1>NJ>(@mxE-i)jy8%O)hwbSqg#S@3*tl)~DAJ?Xz!3eOM{oaV z14l?}Bl*!aukUu9&y;?8hR1Xh;ctEp$A{VdtB&c-orn~>2J7a(WEYDu$%0W0-Z8N6 z;@q;+G%)s8$?4;W)#r$rzN?M$&E`Rxl-f{=j__P#qHCTyAkU;rcE3Z@g*Il1HqzCi*j-KQ5 zgIsw@Yv(^RB<(@B7(iL^Rzw`iZwWvEBz{{Osccx^ix`9F--XV&NV-BrC*U%=oS(b~ ztu^4qkgXbGLbz^#8PSp{SlxMhsIv(Ol2j6Q9;6ZayDkD53&i1g2G!5d(v5OttJUA5 zqngZ)RIku(kEPH46d0JNAlc~zP-;OHkRm~6wt!+yZj$@19b>*g`a&HGtV|)|KO7Twycg)VAhhy95(SWcVfSV=4iR}yG>_yCWui&(o>?s_7 z?G)Z>8=kI9!xtoqz?&=$?W1GlzZ;RAg1TF`p2&Kz^0*R0wt3mDboO@i=3Bz;?M4Th zWz*D&-kDdHRLYNBKU55PUrPXvM!6y!CxZi{HHijS70=&ET@|pyrdnHR_iF>|b#FJF zWf67qv7qyzJtVQq2Hv4rk2k#VdlqkYfWw9sAa#EBUs_D6MTimpeZGI}k~Cxh@?W>a zGT+YhFG#pdBCtd|W(*S=Y$s+nCPFrb+*wjNwiuuXCe2PEE-V7lM_rgPdvTFu15H6DN=e6=8JjuvJD*fB0byLb0_E zwl=sz9@)~_VvL~R(=(GUsF`>X+-p>1PfS=mKC>heqne%<{^`ROg7*nMHA zC>LNjx^}(VBi2kpzBU=3`fpMM*Ve^P#I0pz&M%wbE{NyEbTsm@*B?2Z@2`ufjW)Bh~n63`AR1FgQNUp6=FX(u_= zE&U@6b@(aW$2L2od0$>E@F&&e=BK@Qo7uJ+*S0Q7DZB@|IWu0rxM`z|9NPRIU^TvT z%38?gyyKA3ka`GK4`F(R0zA$^(7O+}p)XvTu{k)#JKfXt#IJ{Vn4SDTl|aE17t4Y- ziaQvLOT>xS(QWvVDl*uIHUv~f0Ui6m)NS|F=yxel!dbs?5_9d4iN(HW6i*3pRWAAF z`mp*AEKh1n9b(xd{)Rbd<&T>D9NxUX-kh6nI_F@~Zpj|C2#4nDz7VHK6R7T~x9D zK;^pvz#P!Z-|5i4xh|Ns)R3(>&;UXB(KuelLgSpy%LYzKBsA%@f9|NDPTQR9TcnIQr#2 zxG1#exf6%3S;un#v{IbF4;hN@}p-n6tjq}%X_P+`Bft6Z@eNu9=0&Q~u! z-Wt^xygiG}I{V_^J@7~RerH~>J`b4afJK_k`|-rw>d`LMti;L2A`M#y_e*|^X1!f< zZmuE~_Hjjl^xib@eIJ0Nf$Z3Su{)8HMF`zw6ua$6wS;ahNv4i)WUpqkM|6K83oC#e zV;=Dz5ED#AauY{>5?)~i>Ziz?7QyL>!Xo;(P{Rx%@1c_!4nSZ;8@T0>;BWa{tx~8| z=Xz?~a(}>YwRjT%nyLWmbK%H z+T3v>@eJ$>z<+=-QDDA60B8Acg_m~^_wqL4h7g8O$>Nd`3p!*#yZbHZSqw)a!(msW zT&xUM8vq*=C;iPP2{a0Dy4kdKA3XHa@slEW#C)M= zwy~60#=r-f8rDWECJF(8@8Aw}AZ3-adi&Q&&Q6uaBdE9e2CBi)iif`BuRT;xALmTM zb9g$N2|-@+d&$iiEj|ZFTSQC;=zBKH{IfM=b`7YG7!6!>o6Y}kWI`?ro=Ma=A5;hi zu^e`&V62>@D{0)+?TG}zeZmbvT<(l(J7j6g-&~%h^OZ)uV>Zk-1;G<1Cp(a_QkOB_ z?&UkW@#wJ&2l^+^-pTG+Hn=6lE@UY&j$3fij0H~ZzM}^DE!*tT9uXl65Y;^XAs`^nAkWjr=!R2 zZ>Y(ys`s-cTwsPWP80IB%C1TVmib~R;Pq#JD0!@t1$RKw6D!dHvAMF7m`>&t@;OvHKlgZ{AGu+Mcdo`AA*pq1Dm3O z@)KDFMZWmXi9JtPh6~$x22@c?Hhq9Fs84U6%_&7pKkY{etOj~rIm@}FY9;7jM!Ms1 zDYChpCtH%2vo}}1ooJz9JVK|q(spQcBS@ch`ELxI1}G?LsW?|AGd*vebU%R};N?Wr zzCy43(r(Wd4F8OQ!s@)GO6cLCdP(=iBwoB3nleH8Oi{#@`}85jf6PxR(5BeBBs9r0 zoh3;Sq}?@e%CmT5z|&Pz;{>p!RkJ0d4exQhpo-cj*I)7#fYb`Eykr4Cm3hU|e|R>& z-G!G&uxt}6n0ev*{1k93LD;<)&`o*mjY5i5%FZIrl7BFbS#Z(eNzLC}k6n)~jh@cC&&SiyobEpIpFc&i&xKB~1U0IyPHxc2U=jC(!$cg^P zji$j*jm0z>l){FI%F55(57pZIN*_OOzszU3#IIZHlzjwBaR4FKNdMV=h6Fv-%WaY~ zU9zORII-+0zxssz($#G#(|5;qYWmMolOgEYJ*Xjcn%H_557d58;(Z+bcGwP+SdgtoMNYL+Kc^CAugnjyAQSG)W953i#D3= zWJMy(79J-6bfL-XIwc5$ELt}I$gSBuasd$ArXu_}XUnyI7^m{-eDDLw_w+=t#ahI+R`ad+w% zUCBWngy1noUep8QO%U%5STd~y-#zgCtz7&FZAjBIay^u`_#W{2Eqk(Qd)7#2PLP}J zU4&yg2w#Jy<8`}ar@3pB4_N+uY`_6d7_oYO-96S{5LKhfC`lg=vUPGLppvb3%g3uK z?emCPF?h9EEj9PIw+n!m)7AY$h^VK-npG`u5UY_2dJO11-f%xXbr~Pr#Ts*E8DIVW z{K@TjOcZo;NE?~Gk~rDhZff-IxDliS@xkFkeK)t1E!O<$mQ<(5CCTNEq_{8&xDXe= z@%*p$_c!mKQe;6LIMBhmBe!=T=?UWDlialhaY1Xp|K=#7<_lNKKbfi^QDpWz6Ljq3 z1b5lNCjNhW8Tp3d)tiSdCb^cpZa?UF96Pr>uF!z>fn>j)L9|5u2ht}NGC5{dwYF-s zvOF%h!kO=yPne_gcQs^y%Fv@He<@f2CV!zCQ>lTOVeJ1ycz60w!$C5aVpvCT6G8?9 zZa?s}0h^k;(!YCjzv+Ke7O%9pJHILW|63U{zuMITBaE%~G(wt~ z_RassiTKt~Jkl8rqIDR1u*7rzsJ>C6oV;SP?~-p~s#RAGUWXh1K|s(EEnfqomCj-N z;~nBblV5s_Z2w-~d348TcE)l6jK@v!r(r1BG#DX#JcIcUSl%3Dd@su?jLljHGU>qJ zFepoA+}yin`UNOZ)zzYf3ZIgny(jz#ES*+HDXNU2*6nfXKwmUq|F^dXc)jsAKWN|n z`{n8Wut%_Te?%D;h5{mX(S?PDAyv6WpgS!UIQs2QCt2?954|`4`HTOtwje~Wc!@n6 z*nzcSZ0!07JR=%vVle^`vz#xO2*9_!trYWR%SGXLXg09r>YHv;C9-I(-Cf&v>Q#Jp zp3r!zup&mUtgGt@+hmzuoX_~eFR76$M|-mTk1~&aJWasS55V1oaY%)L1zM5*XCry=_x$XWxj<|%i4rsS4A5%O%m2DFarS~{k8BT#k$)rt%iGj_e% z)l+lwg*+x`{1b>lz;Pp?wTzEAXY!Ks`aN3k)F{ZTEiim{CjfQ`vmfFpQ_xU${GO~{ zJfqdwi$6*z?yk{2&YyX?AkD;x<=Q&Lg**#|%S{#or?*ya zHClT^6H*obfEqI@{a2G_Q`_15;c@TYwVVp#x%twI@?0WP_`smYJ*;mGyw-Z`^t$@BE^Xg0TBRZHtQ zlG*XEuLjl1P-|r}+W<*gM=F`yGmhIl=LoD%SI1B@M9{aTs;MblvhkBt1^!r-SU;JQ zTJjxkSQn9-Yi{p8%?lwxQm{YkTxHdr7tp2`#q#&EqUfS60x(l`^_D{>!PVHz)9QVQXnx>!PuJYNK|K^T|~f# zBn1?<6K(rgYZBNBEtYCGIw^Z>5Ps*n;%;P^@b753O98O66M3T%&;JC8^(` zNXIZzI+v`sMWf>krX1YYO~ig@BR@dnG}N-7#(wXDCYf!Dceml?Xr?h)k5{eiZKa>? zc)3e|3P0OV&9_fpRIB))CuCQb?oTsj-Mm(5{#8Y*`5ZAhMd>#hw3mROi5Ls&G01)r zIxoZ`{s&qkSLirIb5+KodNtnqT)V|AE4C}21HR%`yizMwOeZ`3LBU$n`EJ{A$zoj{|if7F=PT92|`bX7w{Yro?|wDR00vf^ zVRcSMlm9GJYpHr)s4T7+OrsQB+*~6jHK4V}=J10vW#%wtx-5W#FHLVBa*|%M{7_2> z5UUIkYmT(54`)y*U9I2r`qBEZ`xr`gx6G^AV)we*ZQm$agnBpKMpKDd?;*jp2}WWK z8O;%Q&b>Y501>W4k+D(2KwlRM-{Uc!ZYfa%4gyXhjYbGA&0JkN%fxN-UcDQ&Q^aRj z7ll=FCM4Z>csW5Mk`jo(J1|3dmfM{(n100@;3kSBC zVFCZ83~}mMQTAkEzWDYt*@mHqElL8Nh6Sq}s9DZ}m9D-$3Sd}u*bfUul|4ee|1Ei6 zx&vAicFMjMvM4X?%C7fc8mZNafL#J8vVfAWE(5D3vHMUmv-*A zj`e;OP#&V}aeLSr=E4}ML?7Y67vMs%C=olgYt1+Rpv?hpqTvw{D`$mswvYFhFd1t% zOhv_5?xD>+X#G;)fj!p8ez?*<>Q{}4zCw>;5Fwoz3-}Mi!q#ZA)rcz)XVYq5y9Kx% zYt6Si5O*ntoX?x|mnANN#)Svh5Oc#{W1P0bwkWq@>GosN_B?kk`CMXjgEQq?7=;AJRe1NnCBl%vS+c9udU`iQA{dtfX8NNkzZbpOTVnwJGm}*CQcAz$;ZFPk=DJ$P6kpNm7YY3*P&MP<(l(NQ?I>DJ7*T zTA2O|YRAX7nULi1{3GbL1eM}TCG&r0Wz5y;Bwe#e>xGM(7 z2_I-~GIRXbYe|-r=W9R_qUbyyd3(qor15pp56nd~xWDss$y0mvnp8WlT(C6OEvt4% zlYaFZb=xerF&^+clUX&NBjn}hBQeFW=9yBY4t{@pAQ&J`5-sEeh{Nv+v~V9AXS&B$ zU$va?AGWcgT3A`F_}&idnwi1XYs%7&RV7+g8CHMmn4IKFNK8EL0H@j0G1YR}d}2(EOC9hPSMQn&~<^zV7d50KPllJkhC5opq%%u!^e>Yf%dQaVr{NIHC@b07af(O&eNzHy#Cuj8sMc`T=G{~urXO|&Lhm2YDdK@thK zz%Yhl$kuLc#hkI#@!rExt(C7RPJNH5YGi! zQQ?!Nk&w{`R;MfUT~-1TZC0wK?BaROKLicWS)!b)JV`J>hO>S7=(%k(p zbDxw%oAq;8-@*d5R|)%A6}4M-O|^JNq*-3PTA>33O~J47nwmiKDSVsjK4nd}ee6=L z*1&kEGQgmv;&&M2kRq_^l4thoIyvD1vNirt#)p8yLTDqTkS!H@x5VIRK90q8s+*qCEyU?rY6`z24kiiBR||6#nQ4Lf5rAcm|%s z`SFFDU#ViYwz|)kZWk}*3*#zkYC<)>W_14Zw@*q+`bZzYCqXtPBQ$-C^T!D+V~7Tg zP(UMhb$VC%?iBvfO|F#7IW){=J2%nhAzsDyyiXaVa;p5bF{QfM-t+oCeWm8h#Rv`S z4O%UxSGk?BYL98nHpw& zaSD`>I^XFb%PPh%Oljz2QHP#Fh-@^}G0awFx5u*Qg<_Z&#!WHF$SALBX(^*!g|tpp z?|br8aR;e4RJ=y{vNs^RWAT`;>uB+G@0vsm%Pvzpw9ehiaDD&!mk<5pd_m+~cG~#8 zXt#lUJn>5O9PvOr^P$Ux4Q$W1#qrY**H|%=e{YHH zT3TD@tT-aYuCDe^Prs4GdADY-|D|(%LH6|=HTFxOVS*lUIF=>{(nMx0u}c1sLmH<1 zphfU7Nc9)1s%v*Ku(bMtkJZ4yUdO|XXHr^{NQLT0>eObe_eN?=6T4_i^vYln0Qzi6 zr23^)HC5Z%+u6BlxfX`G?WAB+c#Qfv;+J8#8{)7wP>>1-W8^*rq=azbRQ4w*6r8e@OZ4f z=!S&#Y@YNOj0JbC26st6!&A3J8BDLrNjEk_F*n~;RaNa|C~NY=*Yxx!@ai32{TVerW4^T95sx7xOingz% zVpwwcecnr|7`pH%e6XO19yB5`L>Z4d%=e2Ny>$V{Z1Blc-(S%Zyc5!PS{eM>O#jfXeFQV?3%Pkg zVToKl^1ENKVJH`6M}73+&uuakn)ipaVQp$#5pxU$zw5=@A0|1o&+xc>S|6eV%7lzp zS|47hs@BR`U+}Z1t3B_i!sKvv5>rnXtscs=Y$U6C`<*bUC$-LUs7=EPdC{ZTR|CUA zfx0_8`Mm}0He+bXcT3CHVBKD6sm7h>+uMsR2}=)QONP>Aoz`xGKiNFA+}7nuos+4eK9BvZg|#^xy6fb zX?2FAU3v~fM-_0?GjgKYIEtGxZE#Zk?qr^*E!zHAj zeUasBbLTG9S5F|R?}qT1Vjl|WE)Uic@-$#14IIiQn@ov2OtTC);5J~Az&nZK0FF9m z`QN{O=TOIqU4AU&l+8!|ybuLbxLk#oy#{u)G&zpB>HDo;7u?HRDn>(a8B|8h8h)4K z=2~+%nG({j*@1IemoZgT0BCunQl&x#7D z^`I~-P7(%Ib*#DwwQrWOu60w8LSdq54KSxPLC05MKoIjV7WSIHyDGKJ&(F8HyJAag zT1W6Y%OT5B*T9||S%zhq*UXtr78Ni-fo~(ED_2z0LuBlGu@L;1nc|kJJk)I2B}--|K9x_lx;PjkJQTwzP{FXS^jrx`v|{jvv#Dr zW>=ryQ}Z2AC}|TJRMoWq&Gxzdvh31h3tzWOn&FPH;Btp)@tX*dZZU@Dj++9X4|unK zewzrH+@Rh;lS%!kq`-U;dFfqG@FKai;e-B|G)^)uYchPO8hQLrsni0T*FuZiGY(SywJI}sUsRjp%YuxPg zlL`MKjC$a-{H})Ef<+fgqvrxkNH%-Qe#T5gbrCM3)dJ{XY#`g(tVfIF@M`}1*5WOno(E0dR!Ee`>`K-qelS4+x3B7VM!bw)^y zCi$cn0W+%WCkvh&eFSCd;5=stK2@OZcMlI@Aj@~u-yU|{bn^iij9dw48luzqAl-Rt z`rsZl-)(8Ebu|W^{VJ{*)Z5-g1jtBsM zR%yD4vNYw@rh7G&kyXLkdvRZLW2S{T{BOzX$UtXN+gj%j<5600TXt#~9tY>G?Zwb| zx(wQ`dBG$R_nf4mZQJfxO@L&5DEQsA+uztXU)9EV+wwEln{kVhlpC0YY&{;{ka}!A z7r1J>uY*nO)8auEhO7KCoIi3)TG-pe7Z)>t1?MzVV~H=xnSJFy^j;CT>k3Ari$U^O zS#znz^{J>s?M!K>q0Ur(n`bz&IAHAf)Lii{zjn*7z-;N%%}bg*b`a!T!3xw(td;*7 zR1|K`FF`F)RKEFk$-q~;5s5`T<945$EcHIuht(h4sR(Fgp$mCk`X#(`NhkK|yD-m` zfr~{XPh|fYyq#`>F&l*+|7r79;-bkdU1T)sGDjTqZ$KA$l;k!O&cv!JrdPT~y8H?3 zB>}D`F69r*)i5KJ0&d^L(x+QI)7^ep8x+cA_5npL@8ucG{(phZ|*_P9tpBqO# z3!?V5Ftg$376+|4z5wa)=TE7wxjVUW90a{t6&I9k;<>!p4B~sG|ch$7czm?bjEF0=DX=ayO8GI zU44UD*0{J9Cr9y-l04+9Pwvw5|4o8DpM^S#)8@MA`HAhd@HJK_1Sr7`NdzC zY1w$)IyY-Wc6JCSxw+)_Wf-P}jQ8|oIFi3)%k}63O=5a_x|Ftpb}4A_h1|mV#h676 zI1qe+3U6U)&C3$ZI|xG=IHXy%U9;x}^!ByWYuS{_pZG|?_izH{U=sdN_rrQ3jZ_Wr zbq*N2CqMJ)44~I4S!gLyB;aNwy9fk&Fh_9!SuP8itnL1CDa;=kfyYjLVxb6jmoO>tw)GfvqKm&8kK&fWxt@Q6K19+69#v+Y z=Fs)rby>+l?YSPY{`$8mCzBzm3RtOc`i;P3&AM1gOT7zmzO(pl+tGghzRhfjFw2^_*{NWULkFYSf_W#yX6>Z=!Z&Gx_Tv)o z-X&=e-h*=(qqdr2Y--V4K`zs9xh@K@tKL+vw3nUQ?m&7-ZA24r8Ef;6@vY#r*oF6z zegzJ%kaVrM-lXSkZ9CNE^6y7Xp2-S{qXl`-Fy$DIQAf(Ax}5GKk$ym%cN}PYpuh|x zh-~CqumCs58M3zxc3Hb$nrZ+3T+2(;s-KnJ=^Q+l0dD_#2W!7D&O#-)+;kG?wu8DE zTlcownOk>rw_-foKQw3@&%%WI?k*7!o;og^y5?G}c;<^$8NtWVXVpb_s{MCQG7iOX z=vGpyWG?H6_aT+W!{6=gQBb5%o^%87w_=6#dv@>eUpI!tCCZ3T>2$w+Z6rYYxsWF^ zwsz64w(0t84C@#NEINQ5h69xJju!m?e2aSom+?F!^lbX82 z^;)gqu#q}h5?pcHlR1oSF7^o4PI;#|5(0alo|-2G$H@4J?Z#O4XLT&T7+gg^HfkQr zzNAlS`1;#pZoP4Fc$UjhDMKyM;V(7c<_8j~C1`SlQ%->ErF7y|S6)_g4q+!9P4cuE^th zLdD$T^)gYr)o(rCuqu_wn-0g3$tcN6hafna`foj3I6ew39tyW;XvocR0%DbV+Za-L zxz)efc&>VPw=yY`4=S@X?`xgXNvKmKM@#b_yl1R~IGpYWcj_m%V!P{X(^u-MhqhuD zAKd%LWw|riE&|t4ae|Zax+~kHM@9HXd-HTPzKYztl(A<|1@}?iYM4#jyCl1bIwqR3 zNQ0{&`p_T}-w_P*|FQKZ;82HO-?&ngU1Z-vlIl+E!tYd})lDZm*&Y1^I6ne*0b>UQvuB0(_19~!|? z18PhVIQ5Y)X_7j9^fUY_N7AX4=TKIf{o1Q84OmY2{+`!TZ-u>}xOfo$TqiGJF5OAV~3HADH{p2O}`d^c-)f6SI8 zK&UF5^ed>=q<(v$W|4W zBS!p2=MGCOD7`GjNW1(i7c7hrBZhzcet+4e&@5`xv37v_L~93KFc(~U#Or$X0VB<% zX479$ojX>~?%9%R&<5$hZIT9=cbwc0r82yyGj_;ZT{~X(K`%|1RDqfF`6a=7WuR8fbZJS(!7_dRrgDA`m_;xbW|Z-68M#J=Ki*)s~I2L z>Gb!F&Ab_x21KQn0WLbcdPRKXYhFu=7b9IQvOxl7W5fw81I9ZCy)ZmxH#=alR93R5 zVaT7E@T|z9)W*|}6tu&S^J6rKx^_~$?!<@N`1|{Fhu9fPURA#Pb7aS)>qUix`|^$1 z{f}afRSck7lkT_U0~ng!8?;8jXB7#fb=ONal&gArdgmL@cYY|4%f7({Mj#~@r};JA z_L$o=vJs_}PMU=HG4twHghU&z2z2ib0wy`9I$C|TV7RT^1mKlxt^p@}67Y-se(OKq zPWM6w)Cx>^Xt#(at{~G_Sfo8Dj7KnM?yGv#&v3hVDyyOklbzn+JND$XuR_=`5 ztJ=4mB~k9RM0RhPE>R;ppW~nLQiIuzaq2lPo2KkGFHp{3 z2Cj~F(HK^fI_Z->=e{dBk6_T*-f5sT$Pq*OC_#VR{c^VBE!Lf-RkQ2c-MAOIxVjr{ zo-wY1X#K5qcA`sj??sSe4_2e>-szsO`^HK1<{_hL^>qH}9?wyDOd9O0phsr?a0xMK zaLUX_b>Edc?CTCJTsh=C{uLe8JFnWW`?yBPk+HAi`8+a(U>dIK+t6e86Jq zEetc|S~m*^{UcyJo}r;3E^YTUe=gtOxkHvPm+Qb5uQ8))H6f#?mgjm$QJg$O#G>9r@gk45iRuyit{$Z%n1T!}6eR1m<@70nQfmYopX;mdyM7|T86R`aTqw@k zMMOkYIcMqolz+!+=jlnfJ*U}#GB7eSe)}r6c|;&KI;OJ)DN-XSUF)kn)yg5Qu}7T; zrrAGo`J~F3(6Q1kC|$wo^|=mzt#>B!yl*BfHI?4Bf`06JBrec3y{ZDjjnOi<0Lmow zIb_QE3D>?)eFB%4HS*oY#s=fS6P=^~)Ov-0G&&2E>j2{a_GZkY33_m9ZFNVGzm@8B zl-&>=tH7*8hTdrIV|&TG(Qn@>t*ok?G6x0ckJwQ@wN5qd*fwrgGl$yIkTl2oTEiqw zSMhG!o1>0C*&5Ve(h3JA?ZVQWuS0tuu=HJ7vU$uuqc$;nnW8unYgDYa;zMeka0Cd2 z!a7uEzHaQCu&_;26l*?DUWU8el2@;N$^{6CHwUi#BRcO5O?VQSnO&EL@0ElqNQu5OsUWAR%Qlzp}+;C^i*$dWBI$6J#77+;0M*+lHpKL1Q(@uhGLJZ}M8z!dn!#fCR-op0n~((Xmu@JbU5iC!pOC3^l7N1tzdO@|V) zN`%$cngD7k`hsi}Dis}Yp1U`06Px+8BWuEGk+4rDwlyT=aa)p{%_jsY?YaF6x5ZyK zfewIt2|XbKvc!8}dC%hB%WSgg>FJ;Q`@&$-@v?ZjD&Q7`8eAF9?Q_GBO=-Ci;L2nV z*;C@wCLh3NFk@pg#bTDP#rhHLHgw%~ICj^}MWdF5_;Jn^Gt=v0MUT15@VOZkTCaV0 zji2C?DLSgg?W%lPhD+s?X+Tl(F)TU6NW%THihWFSC;TY`;>ay$(X_|AL|*8)soaK5@SA!e~Iqz|#wn0JcVxrZF;tg0NEYWW|RW)`*45plN5D(_Sxdwl=4a zxVX6De#g}AIFVcTc%s@t?w_Ex)Dhnj&b8CY}PA}T7X6luXQAO>APPYiuuikZRzBqy;4m25O^TGmOv+vyQn zg=PBo10oqJQVDGzXKH;Kux7)nVtt_w40PSG3+wS5xHNCt-I`^>={Q#;5IB_G(Xr^} z`h(sIFg!hFRy=w)V3+kzR}rmFQR0A;G@a3*q|k!71!uWhC3g0;8@JMP9)x_EF`$>O zX?>Q6nQ}pq*37{fV^|?Ez*3^3J!vcrkjT%nTPnk`t~FD*EMstA7#wD}f zLbbp}6y1Jt&FRTeo>t`}$8Rk%Sfoes$7XJ93f|&P{DkYzAB{&U(35qvrD>$1f9;2yF=B+tV%+Rc?}$-05H1O}s)C32>o|>bNJKx3at`#Yv;= z{^>Z-l=!iq33j9$=R2TlP8EXw_r>jH!y7pg0>QLS;R$MfEyzz$mH-MOu(2;fS^Pt@ zi~Hn#8N}nsIy3%AQ1Jt?CVObs-ZlT$F*(n~T6+Z0FBKQ_;Zz{}bs`q-17PAbATXMj zmzSeG3iS{721ho+a*Sqj9jyK9Jw{EWlH~7XsK_P*?T$<=m|1G&=}da0%)I?#%ETPi z+QQbSyJ=Yoy^|Wwk<`nU%-GV?Bg-W1OYJocXWXgV(qfW^QG>(^a8ohF*1`#6g@2<| zXDpn@f$zK(aqWF8I}jCvRU=(fw<~`)mczO0P}N#@g(M{-MC`Ses!d&6gYXFf;7q!B zZ*{mt+VWTm(FQ8tAo{SUhjWO3R1i)Av%-wP3i=q|;?;VI(b%c+-B9#mRTxQu?bS|w z2Ac_zV@+)vmmRj%wb=RL0%_nD`6T>L1cg(3G$S*DkC(vYF2L!0t1o~5qUideidxs? z*vgqVdyG7P6BVUdy~78wLU_Fu9pd`*hQqYy7y={#syTWmKOm37j076r=d|7)E;In$ zaeHv$d}q$ROb-IvtEXlxfB@}SaqoiH+nE3k(agN!o4Jq+@B10sjNTE~#I4gNt0h0i zpZCe<@+}vVYxI3uvIttls;n-{Gwsf6qta)}XQ-odZ)RK50N3WwQ#tdQ)&OGRDe^r{ ztI+0`1!f7cxzF=J#E3(8jt%SGr1*Najnup^W2#DLZ%OHhU6J-5<%x{Co;gB*ULDnJ zk>?@%C2F_&J{5sqptLEXfnQJ=(K19Z>z@{+qICtsw44240Q!qsCsViJIe`61?$@_J z{srpbI00%w$%hlEnYShUY)W-Xy#~2moSZU8Z*ZCGizUbv=>Hr(q#oQD&NP5=vcxWp zmt_H20+=9MGrwhd`N|)InrkH%XpCyjxvO7YgRk9?$2cSk0I*O+AP@^NwM~fRy#^e> zzidu@sf$>w1vJj=DjsWSfkjc)uU}sg_Lvd|8(V=`^9BykfCVbl#I5l)rOXkExLT5{ z-$^kb_99bL15Q@wu*?>|dyjxXBL-G0oFbBDE98~_#sA9Cd9IL`8=1XUMqBET*L?Mq z78uOi_4*5h$oOMO@o&wqp0ur3!AMB!xyFE$PoIB{r}#c=lCz>C;PSG~K*+qIs=1KC z$%f{`ApT@PMbfgr?`SU<5QiN_kJpL{#dIQ+gzUcda#bghW}00Ob<*g;owWz$`X7%uwsOds>3g5XWcm0wHaY8fX4&fOpzt>oP^ z|LB52g8ag2^kwZfT9iVBRO2=&Q^rG_{(w-6P9mV=u@Lbu8|#he>GUV&sN2CTMT%Zm z32qm>P+^Y;DiV3HUDL{2gXfcbhL>(@&TDjs4!F7`An{V=Y9)&v3dsCs)DV0~2nb8v zPEOqoYU_pvJy@7QI1?U8ww@Xq@{P4UmboPezk-gjn*doe=Oyo|uOOSz0%{R^>shz{(sw$0t;a8LC0*{QKSf(a$gGxP@1@ zV@BRObABv4FkjQVB2ctzxpSXw?qvE;{%=(JUj|-s#$OuvIiG`0>F38QgF4ODWT7Z) zz&e@BmiJ1&flr}tLoBMkW|T>;gTj?l=B*rbjb>^&d^O;$Mj+ioh(%?3!}lxE*YIgu zj|Fvz7+DTYw6v}zaY(L19ptj<=;Z}Hi-d6#}2G1Jg@eV*36IrS%WDfa@ z9Jd6F0~jq!#wztLBt%KS$WwIFa}L8s^8_dhpF8?8*lW!MN$-3oJqL}n88YU!vUwXvl6br#UQxe(^o$fNJB7#J4afVCF13^! zp~xO#%M%0^umV-^o;Bt9i`>`GkMYQurNV96V;AOsNP;;RnQ?u0;%|sQGQ{}Y7N!dg zGUg4H|MDhFg?tH!99ojL#>(8VQq?tGFKg1eC~t)hnEX)(+DvK>3GY8_K=Vkx(|~aS zo{^$i_=#C9Y~T~YW5=uXnU|{o@o(Egq4deR4N7(vnGIC zQ?bJC%Z!WXa+y@5WTBWiNkxUvUBu5a4i#{&_diQ|Fo2o3Mu#XXya{TM=A|?c@VZ_} z6MSwG`nwJ>gFW<%wWA%ZpQ1`IRf3rxLB+`!?nTi{2oX<9lQU%q3Cg}W>CL9BW`$@( zm9v&o4Kz}K2zzASdHuE*!A3jPEip6dgCGqS`({6{94*NU5<}tB;B7FRI<7qYvx^Q> zxi3NLmgT52GCrIgC2h-nL&=#P8D^QMpNwYriO1eDoTE)LUvA!m+d5N4nsj87Xe`_0 zJLCO+!8n6RG&Pvte;-mz)bG?#7CBgKn739wQ2EWI4Aix)Y*1^`^U>xx()X!$C2Z#h zDhOC4o#)48*%E|Zm~8pn@ygOP710k4%sRRj4Z=DwF!wFl_0dj#;FJL6MF(uc0$4UD z6)n)U?>tCC(u;s9i`&T`t7)G#`uYt`^2J{fRN{k@r!vq&d88fMq~m25X-7To574cUGWiZv!&iGGmRC08!P; z&T?LyD+S`R;bBf3;|jEgZX1JkU>W5VFih%KYh>xd(U^eEYae=$I^~Z#N)xy_C~~p) zbob`sh#KoJ2@HRlr+LYGSeh4F!rcDMGp(+Y%{K>`0A<_QJ++CKLN!}TJJCG{IcH8s zA0z)&I#VEUrlV*0rYl4iCj$rH{Z|-xR)i<{s$yK)tX1<8fH)k?y$W+%Gq-<@nehT2 z+Vm+ewTqOcx)M2kTIcYZz83|~l0h@ncvB!jLIPqq+o;c?6?EC|bv-Cu#NG0$G5-H5 zLR>8z>HRsbd0P%Ga({xvRu~bo3=$xw6h!Ov4@tl{-th470Ev7n`Y#qdDsjGdos;*R zAhV*0Z|Q7g>*aCSe$hBTjRF^qbremcI+!{Rl!aUFt8U%b6=sN(DvTp+IwXp@zga9op(5F__Mr>~r zff*Z{(NYZ21fPm&fEKhE_GAOW5ZNv$CKif4$1H%(X$h~nhVLNvLm^8HoM>WAQhFFA zyq=ImrqOhH&R1QhDz3guouIV5?ldV9rw*n071M`@`OWCPeoYWUOkLjAf4ZkNw&m^G zz1}p2^)`_-a7*9-&LAzy#W`X#8&!Hf@p7|O@O)3$_tqjU|9c=z*5RVWCo-9Drv7Gl zohM1W(e#0-_Z|=EPig*gpDkJERhILWsfY9X!hgxL1U?@tWUzY56Tj`@x3;ZLm~p2q^}12oFKD=yYtQFF``p-kys!NuN|F!IIb=_||F z&nTfqV2&DET8rq3iVooVwo0VSYRiHQi&kt7-bSE}X{e~~GD+bW6;NeJWcS;NLGiam zcLZV2?Q!){V16!5*yG;p^~GB>$uvOl6_wL!_to_)P9Hfng}7JlSQV<=5c;Ah8dkvv zW+&ame=)7d{_!y{i;5INUMx*nOga=m0r|!^;Ma)Gn;IG#&)t#laIdtWe-Yi$RzPnI zHh0=@EJQ1vB-ErUf;Mw7_5A*AHP`3>DqErD zrD1YjLGCUtWFhibRAQLGq^rDd-@k_g>oBu4ZU#M8I9{{55o`)|2Aa;SX9Y+L*USM& zRT&kaUgu>|cohGb3n9mYP=-5^gjlnHJzuSR>|E+p;^~OaPBrjA1cglGU3dZ4ATrz+ zXGI4L`uoa9pNsKWtoZi>G(C`P!)RLlOZG|uf=hYAn=wE-IyA+LG7 z@`$~mWmK~s~N%7vW_ z-!uib{>%u-0$N`3=2uU)dwFQy*bX-T{SXm&{4L09)?vORi%UZf9$#oy@n_J0*z#|B z$R*a427cnpIC9oY=0q3Ht%)E66s9q+0S?0}0bo4m;85Bhd9zK7o!_yD;Vz`;agMe^~>tXc~yrZO2-h z-7m5ts&lIEz4*ihR<=a%ae;q;JznwAj%&HpOXOmE%`-kx1|TlJ?c|zV2B`L`s)xLM z0ycI&^bd-PaH_~p<3e_EHDC_U%ppwE|2MFp>C}{QrR{$y&T;@b$ zGr_wb@(7e<65DWXh*>}<{q!E792CH1)8LBM;36Fob$z6ubIjQAjTeaE@qthUD3%m+ zbmfX>LfvQkONvSH0W?h2Uzqu@SD?@Xy(t!@S>U2fIDsaWZQgfP z{f^s*Iz9*IE|-dY{%VRwtP4SE&v*B-d!wGeY1&^m z8FS>amO=masH1Y>=+wnzQHU)SZ{L9}Su9S?ljau*NhvEtb-kVZqt(HZ!Oq0J-nAHp zC^il$`d(+ZUtkW}q+C5UaurWeiEPkHpL`|iSrhFYdcxfOW@o}TO%T^R1es~sd4@G^ z0W&|vEv~I{y)h*>?#Lx+7IgW%y>hg~j3?GbV^K?Xk<98jC}g-b^FZSl6+xQ^*Q`I| zpI+&pnpqcX_OXf7`+kp|649)8O`xEqlQOp5HCuf6G;1dE+u$dHnV5{5K|8fz-3&ph zhH|Y|n&G29bOrBLC~wQ3tRcZDMyeH%dY}5UG&3fgk2sQ!INk<~DA+a&UVsM3sj1uG zxbYnLe^m!&s?mfwiSI+^%vnXQQ zt>|VqxJMWcGzf9wBf z9@ZLtQgyE^lcx1-ilSiZ+bOD7sqC+q;)-9^_5h`%)@++cY5SiMrntV!y+7MR8mkMEK4ef>h2dti zxFx~gAxsc!{piHZVteSjT_35Qp+DkF%0sP;FREDT%*p&R#W>n-%0tM;N8;6we)*i(5V2Q;& zIt>w+&6O_|_v{ZZW@JBvglAbx$4H-T1jOhT3`ghp^kx7Rb{k`#P6buOkjt)C)fCEY z#wTV`x-WhLLo#z=U;3ro=iO2i3(}=MGrJ;kfnoUwW>>$t&U%^^zuWLvS8iI1Yw0N>KuW4#*L z0^c_%Rs9(Cle#XsZ&oa4+n6{#$~4pnhcvuna!54DpFpCzuem_sbMx*w9^M-~HAOjn zgi_fEU*-gwX==s={|TeBKgNJcXy|dpNE>q)Nly}qE$Y6a@2ATC2#TqaXW#3H9(mn7 z>4MCr5 z@V&I%t(~dh6^_Y2RERG#%AU3Gt7Cq&^MW>MONm_4YfA*V?_G6YH#v$AvFYwdGrin@ z)OcK;)HBX|HoNaw(KK6vhS}Fchnzew(P#BF{9#fEH0$VjTlcKRu2W6*1Lm zF%!!8A`D^G&=C*BUvc?XS#1=H#deXz4E#K%%d=ceU-QYb=xYI~2+qOKO>Bb9l-#n# z3q1ld4Ll4Beo4vIwbEzf`3x_`r1Xq`=Y|-SRF!?BX{mZh479s#rmiBdpCk3eSnxIQ zoT11ThTpvtwsY|7Cl}&Dv?X{C(wvQ*iNVR+1}!I zm;dgI;MvB}roUgy;|9-Vu-;y2{jqbV@cPa3sUr9U{LB8%7uMa9Fe5CoKNeF;i1j{) zp`i@fth?1W*6oGVf3dj0kgQE8vJq1IcR&Lv1e*F91xeWucv`*m-fqOkP%eWe3tCg zPzMVstSqMLqoyTtkXH1n361D&TnZf{`zl9SN#cUB7~$G+pm>ww@cdH3ZB_7_$;0PW zL9C5rX6E>x_=pV|Usm&db*U~hD0A~1X+GU*wrvC@4tjhBzUM^*UtO+7ZZ)5Y^}oGN z^_buJxv=YEqYu7o-AJ?PuArbe=%}MKrSMRHxM)v{$`7z@*EN?OzUW%P_(I;5DLhr8 z$vfoXxi4`Mr7ZQ0lZ|8_KfmJ)4AGC|z zDuj^vb=R#N8WkAj)SH?xOKpB=?bOnOJjE{O8pS{PMSzU?=)@4s7DswcqxOIzQE%~R zN#5w}W*)0+om+yz!-mMH7;%VcvoS-;oGV+BVE;=7K~3W5k0D3pA^0thW>5W4+gQkKePFKFOOs3tk7!{YQ@> z7Nlo;*82mod91r5>6VbjOJ?-AZGX}5MJLDRR*Wh4L71j`vgy_l`%c0kk7 z@h)3@l7hW&l2yCISXjw7Kh&0;m+<>rzqCXlnDO16CbprS_3`@*Nr)yXTdzWC7Lhp3 zEA7WuBIus>TqVo{Z4G#(7UE^)*PV<&U$%UM)t8CN=T`(Z*yzRM-s4Xs2AL70@Z+^( z7y&O1f=BbV$K(5iwjcJxe7nsADi}-ril@Hw)E#tge{&O`_`Dgn;mVFRg(Vkv@$00T zdlnwXq?T_KL%W~bix0f=Or$C{!W=B?i$x)RA;WwL9mH$wpS2(4u^06U2dg&S^?2+R zD0#LQJoZpyy*+Y^n;7feK=Ly-c|-@}5^%N$N{dsOth=PxljmqG^O`U60d8nSf$Mh}F-UGZ;Cwrm+R z$sAo`hOOJ})m?obh{9qsDYOLPqT;oJ)*d8XridH1+6YUB~-l zS-)x_&q8LD^=By$mp0ll$y>tVhbdTR#fdh!=hj#(+}~|wqn!|Vx-@}kj-a}TYg3b|Sd2m>NQ0;&kCeLo%?2_epjE5V znClAXNj3Zbu$7Z{Ls67mQ6on>4p~oz$&N^o(fR1aJ6RDW_~BQY1*JBqmxOR#Q}oYy z$n`fL7Y{^mzA_=;E%I=-UaQkbCddmVTpNt&^|I=OM6FB2rL5Zs9LwGQS+3{mm?t;K zIJ3R=hY`6v7sZfeZ3-O3)Ob-J{!Kyzynd&yB`%Qe3o9$%u{EoH5ZvGU=(QOar$mbE z0HKBCyn*{;RFc8(o4iN`y0OTv!V1zF!CAJ$ZGk)#DZRPUB!yZUyPqpGsRk?|bxvLI+D|?m=8x0@^!kP~ffqT;%AAz5jk;`-U{6dc+e_Z%b2U zNmqS4-n5ET$4AMT)B3>-cxUPmi`Wu&XW$PXpu&HHF(2JrL?CD(Hrj_`y@KJ7uqO?x z?H(R@13{86ul%1N@n7NMCPS_^6GcPOBJ#mZxpZ^=4ePM?3F#J|N1;lQJH1;C7EsJv zbjh8O8NxzoZp9K>(v0IttC7owa9+3PHB2L`FL&Nr&htW3zl!& zqFfP^nytBYu*c`|tqvr&stV8tOP+o8P6F<+exABNe)r1NyF$?~4-Q{gKX^7GTesr%L2r!AG*!%C^H#y!#?0NR7^ln4?Yle8hH8GwJw+ev)VQmv1 zvw%~Q8w|BO1Jy0l4wRHu(H(2pTT%_xq;u|m7gjX7mA5M;X(Xjf*C{oR+?UWKi0CNo z4vO=U#pd4IysU{Sb2OIUvJ1@qen%>3<+y>&uOO-cx%+YUoX3%yx$k|6+6$f2pX)V9 zs@HOjDz;mrSo{fBR3YS>Qo34vCrMt!HTsGAO@rFrt*~;k%Vq~1rIiL=KR(=hwUUFR z!|Qntms*10^?s84$jsR)?V zGMVq#*O$IYf_0TnbPZ@N<@icYWPy<4kfh>u2+V_rV{+^Qy-X9YZD*hOl84A(iUc3l z_c7y~*O4*4Qbej9Zoc%zGSwD&!&XnoCkc~QsE{2s_7Lt>6mdwNdQN>*X2!*=Lg!$k zICZ!rebNvp*YHZ;_Hfg(kdmsfdcYZ zO@_iRT!8!BP{w`K@PPGpmF`$>#oM{shn<2_d2q{li?PVcoC78HrRVGJvWkNY;h}up z)oaUm5>+dSg&bdLBb*?*@qGmv!xkUm=MCx;R&-gm2MF?Kd2H!-AdtWMuM3Ti2#a@y z`QXt0h#4&G{EpcA-wnlaNF(6@tNsdA>$c1TJ$vH-#iP@wwU%{UO~KV6h0{83;q{-H z6(=jtqre;V`FDTbg8Sn=BenhxUe5p`Xm#;b-~^>8*{=?9-}=9Z3->B6K5T<3AoCp0 zV1!~mlboGIx;UbPY#<+Mn)l~|O>c5tV*!k_Ea7N)!z`D*q2v7IMf;ODLUO;PJyfs? zC*oZ`=|%4FpGc>=J0o>7wnp^bN<(iuX1OP3^yIQ%#H&5{@@Z0u3me^YvMgR4yDq~0 zAR~XqcI$PKWo{DzqEOv{duGGGzUxdgdZ{M%+=tw!GjFI=r$GQd*xkALpb+@(vSZwh zpp74MH9cHf*;c|m{NzVChkqX55(~102Is*(i-V<`1J`l*QGfOsL~*-)^`1~qwSlJw z^qmTM!pu)!t(HXXl`z+*y{{9UU)d9zClDdWA}_ujq=}n_v9MR`CEe}1c6y;?aLljM z|D*IT)_46A33-c_T)K?>uS{0R1xbbTMtfv8gPr6?dlb~%Fl86Gm(YStmsj~lr&^H`_GnWB8FU$TQ)9KPixRX<&-A-7R{@Mh=5@8;xmv4TMYd0aZ(q6tML`l)g$`Y*TrA}>br+gjZU}4**9-mZ3 zn`E{p7Zr`PFCt;L(|w9j5Z6g`cnVv;b?B#JAdA>7Wb2(nva)`j2LJ6;g0nag7B~W5 z#=|8q1&FNidi!||D|l+|%9^=y6JNmfy7Z)EC;s295hG!>BYK0wY^MCH>NcI*d#*|j zb(9@++gP`Nn@6nQf$$bH6k=bF%0X6ANp$>`$yX(s7^VxZC%2%a)LG~7ix5P!C_;lY zP&)E2E+UW@Iv_}X>j&Ub8+jdjl4f^Pp5<)My?iK^6_`;=2>cQKZCvow@xHmGK{c|f%84HmmGCK zCruvuTE{7|zgGIa^v$f;URD+zxjc@-B4^0aM%)8+spt>>I^TW^?zVY;&<=Cb`7X|B z2Ibd%cb|1YWw^aY&^fb(>~JZI{GaRI5>?+<`XL59Zi^>NzN?1J6E}yTqd?B_dNokUH10G6Jq-3ewRn^ z1D*4L9q8m}Ij^yyw|&rf=T0Ke9rf_(3b}5UVdL3t4xasMIe9TsKhXy|T$}q!YOtEM z=2uR7Bigy8uy%LIt{Oe|7`QORlo4N1Rb(018_D>5sicHgR2WTIpT{HuYuvg6A>k|8^b-(6tFo%_5jDS_8T zjOzCo?^{;z))UoE=eaM;Rhpm)GrJAY^MxXjnUE;0x=m=25w!mJL6&&{7SmUXup9i- zQF&*msloko(ei6A)NDbN#t!th%ytR+8EoJ&s^WMxbfD$|! zfPdRogIH+Lj2v&Yqq@a6;g4un!};m%EB}v^!KKj3EXqLZI_4sSkPb$7MPyI9Nt28h z1T;xAeTIp2Y<{t39XO=W61!pD{vuFXZkuAX*3V7Ft$Z#NG11x&g5JfwOY|CjY~w znJua$fN3k7eIa>(d$7ReA_(Z%N_$bv!Mj5nV;zj6^0g#4^LSBC_9KZTN=;Yftx?_w zsN@TmvksDRmjr_HmFcdoi}8=~YR4?ymD`xR`5cHhCDqf1{R4fr-G%GydZmS07W?bN zKl)DCc_P1}d0+{o8%AmsY$7g&jFdC=cA3%65`)0x_X;muJ6lhb~dgXd$W2kwPr|J(kdcc%;$<=C0hC z&I9Yei$XV$_z~8dBH*yqb4gCgd@=oP3PVQh_PcF-@K$Y&Ug|^X4*l$hK{mwy0PBr8 zLyB31Z;HMIsDZq(34}9%O2JEH^k3L|dlp6w01g8yq(vN+levBRyS8d4B=pPW#L9OW|KTr)oDYjnb`~&7^ zds3b^6u3$OJ_kYnWIf6mILQ&*_mhBJd5TSuP$T1ed-!7Jgh1Ll9<80Ze)!;%-+i{K z`q1%ilF6F`BeLfoqzjKD28~5-V8cv4vQ0&Nknvkth-Gm_!glku-4rK)-_1I3$zg>| z1G;f^ea0rrWoDHtZ#cmmZ4VmFUR{7#sh@%p$c;2 znpI7Tk|AN6#>LVhkueyvei%#Wp1iVsbH-e4awdpAv)b6hr7-fdy#+alY|3#Rc#G!T zAcl4qB#zw;GiL@l43yyTd@o9#6J#7I+R(KygHyS?3QvQJ!WEXpUY?%rl^|bHkQpah zm<4~ls}bfKS4TG4%d>yHc;p}cDA~euBhD+uiRm3_sM@6*Hihif*A4bFo4U)tr&vlZ zzZZS^*py=qOIMSo6++M6?)*hBN|jvZ3E%a0Y0D+g034~&!U6cSm+}z!3EilyGURV` z-+KF22>fDQ=pH0J(i(R}|G(TL{Uq}`-HXq#6z}a4sOgKgUdg#@Y5=U1g=Uzf$K%#5~(ELoyy8IwRylz80a8 z17!+);pU?7^ffD0`uV(oq_u}VH*PSaf`eyMSGa6SOLQhf?71ULyrR>hFB&n}+|yfX z5es(QHD~906(X{{7g3&Yf7O~TeZ9VSRz|5_spwuu>4t_gNnDI}89Ulkf>>zD;v8fP z0Q_dmbzfKoTsY45&d^_QF%i%?+={foEP^u+wj-seblSI7>NcPF8AFq+)abUATU~V2 zljrr7#aeasowmmKc@ku-Hd)#f+cSqwp*OckDB>fSAa&~(KWOz+t>o>Aa&+Wc$LK1* zn(K4@Q0!Jp%8wFYKk@>A5;EJ(WmU;|8#V9`KOwfVSy>SnUaYqt)))8J@bU1UCqG+- zkodXUoc|9Z<{%&9CcBY3^H`3VlWw$xI7#d+;719QBBz@&z3@X`=jXpn)0q)k<@SvJ zN{W0xlfseEc8}+*i$zqn2NkWTXVEdvp;`;2RNVX|xWfIHF4ta6)ud%s?dw*pUc9Lo zvo*C7APXTNJ`L2p{ zplRWDErNx~YDVE~RE=vAcYX0-#<@#NBl)*);EyJVzm@LDgzlw_=tix-iL{obe|L(Mcz)E)nf9zX8Z3d5=%jpz z3e5?N5#`fzV6W>~-8bsgWP-7*3ij$f4XB=8CL1!8Tj2g3siB_mSJ=ffa`?fAU5hok zUGA}(n8w0SHf9(sRlIm%ezrVS!)#Fr8@C-2C}$g{Op?kzJxr z$0UWZ*ZC?+m=krz43sQpP^6wy?C7;$yZp-L!g!0)f9ua`5M&n4>(jNXIrg-vh$`fE z{-g?FFw2rs`>Xi9*H7V(<1pH`;??0AEuk4R2XcRYB{aQt4P2XeI{7IB^7is*C_}y{ zQF>njedtpzJDS|M|L|h79WLT_T6ZyE52rn7j}0BQD4oUD4aFuW(`T}>8kid_gOb)P z3rKgw4hJhh+JoJmn$R4T1;B8G_8oBaN4FkA;1p+Jw_8=h$(G@sxA^EnUoZ8sO@2Hz zQxbrwsd z>SMq~k9~}RDMFGPxBwR4a=?*x-VooR*g0vq|G0bS*>WNL;@b%e=NazM;4s-PGy&yr z6~<8)4T!YKMTaapf9LNoBGx;_g3}89X_7cVo6w?n;M@4q7hX9%{5@8)q`CW_Pbcn6 zci8u88l~s3WM-gX-%{G%)A#mam}Jht;y3#^bdDPirAR6dk4wN(9>|nc){< zkh7cqr$v_JUXNViAL7My@w6Oz=cwd_7n81&NKC!nz zNZT^t+p09);4I>=h4ww6LK1P{#+l(_aCQB`K+RI4-!4&cL##h~L$2s+$U&*u4QEgj zll`+yl}?OnfdBW+-|wvwH(BM(B<*GX><%vU(5KU%ydm!Mv-!Ti43SjCs;p6x(!=5& z3*}!dR(T~v^4tcsin`wv>l5dCE78k#D1Zx5SxelS)IH`G0HDiQO!ZmXkez!J?kc zB@HOmpoKA9J)-aD{`OX!q%~m;Uv3x7ue6Js@;%y%)DGe5vx$8a6~-d5=3`Dh`=c)W zz4pKZZ93qeJZ3CFVlIu#@&F|S{#8&~IfK%JG9YI{&)T~vb`V1FCmyvSttQ#WBHvZ| zhCm}bmhWcq`-Q)wiX2|LTgp$bq{AnmTQYW2uVAcGt8n95y=}4##keLUsl`Dlop3-* z5$gU*ds#NTBj*FvY!Rfhi&LbebCz*LmZK<`ryMi)ge!H=Sjm<0yzr2`Ms^1b@U!C> zIk;wfNK>pL8c=5HLyx`Cn$M34u505S5gI2=qGsaR@65e>>wb|ne%_kKaRd6u9AbI4 zKPDf!$fKF~XYF#z9<)Ecv-K)3=(Sf}rN2a5UfhMBCp59XFUFgPU|BQ3%<RVMR^x;p>b6<-QTR_+zUkJST_k!HwLcMu z5UO_VSWw7lh!BOHZSEBDpp1gQ1*JKAqTTc-nCTRoX!(X>JG?0gbi4_E4p&6R%*@qo zMGsDN4FhX`g*f}xL#gxb*M-YX2?Lo!xqlBtF5;)n$NKV*S8SEwx zPpM_?Mef6X2e0_P&xgc3GQnNn1&|d{%(g2B(qdhspu1pB_TKppM%u8`th6OvhpxqT zCv4`S)bi~Ch2vEE;S_opLu=E;bVjDA1&I^y=t6toNmfR4a?{z-Q?w8y@LJ%>d4}R6 znu?@OHH(2)Ag#m&S91V627Cwy0W(sH1WbY}s%%|qGx{}L=uak$JmDgZOkhS6T)-Ppmpj_G+4M^7Od1uwAU3O7 zikZ2w=t*~?-?UgWWpP{~C+oe_+^rD>Wmo$1u0!JZzq6O-m-ce+vbsm@xs!V;R>tJ^ zrsfP*C<4f!M+%fawzHJ*0fW*x-_Pl!HPvU#%bB`1>vWOWn1}E-&fDM_B={p?ZV$Nq z8W3c4=kJ^X<3>zd2`Q5l2(9vTE;`!NkB7UrQ(yaHiYD`#v=MJkQ>K zo08_j1FINH#ToVOTEOAUK`i=?09GytgJ10Q>6dz(n-QGt?fb3|N^gQ9nlCnnnFsy| zmBI%mRhA;Qw1r}CI35Q7p7yjag1;rfrc#C^d5 zNHzw;og;)(N`)Q*J65>Edob8dUdKNB`Ddc%OUsN`yoA!XTqkKP=ng6$o>>2V%*!lkZJP zsFy1;gNzSI+@617Ev=D_Vf0JBcMe7_F^Kv(YjFMeza0Es7THTTkb$S{?GtjTff< zBJz*R1l#g-81X%s`?r#H$_sd`c5O+s_?zZbi_>A_i7l~$xcQ>2pvV{ ziLC5ni$ii0S(UwxWF0%{7)f^a$mST8WM*@0MdmTG$1#47qw)TH|M*>(%T+GUb37i8 z`?%e1_xnA&g%b0TzO7dJ@Z&&-m?N)^%pz|} zf~!s~KURYtA9cH2Vj!TG)M44%*TJp?Sw8`e&9T(`nZhm{mCwHVFs5$FJh~@1?7`*G z6}k3KvhdICgLlQoU2qD64iK!D1ciggCq);17nQN~3!?)$X~9&^I9@Le?e&lrp2Usu z<@#YCp}wF_XH|z7#c`GcEzbHQqrzOk`M`>w=!s~2N3MI~O9$CLb0 z-(1>nF1D=nz8=?Ka$_t>_L`|^H0RCrx!K}I4!g58LAd;7cCURNk7ZY7{P2QZ^PZv5 z_jdC`Pd>?0AB7JOx`GA=LHKlVf9pLv)E#iv0~%1-eGmv!AMW;W&U>Yg1odsz9JtFP zt_w!qNd2(|*}#|RUKL)vJu+}@F@G37$;!XNgHe$>w69v=w!O^ei7!X@xgCmKKr64aVl_yVjyV_>*#V zIr@4bS5xx~5WJtpe(De0c~6}y?Z$=O{SbxS5a@HXN+#8 zXbHq9ZW(K!ISE20$CFqejkDSW37#ofT6g1yZ9?{M##rLh8s%NHUu$P9uXjJ=uW==J z^!x?RJ;&1YBWWP3(Q}OUdOzZeXy5m^M6%VfzrbFDbK#$yG|SX!g6_?& zl3D$?u;zAr>z(`L@ts{ybUhW3VG+Dp3+QIg6l%pm_d8Y}9PPsP*G|<`Ev)Jb6H=!V z2tbIzORI$|EIr&|132vV(qctvYFwei3AItgrThBOo!5A&ke1nB&rU!mlBPjKM^GLW z;G9PxHvUZ0-+d>lb9?DXD6QdfF1a{co%2(2kL?n_6`HL=O2iV+v*cPkoRv6cRdFeu zRoJ+;ZOi;}uiHUeE`O0=-Hwr7GV*ZzD=FydW%nIhQL{9gC1WgwLQ>5$Q|oW)GFOgm zZscM$4AFB6KjGiG{lk+GBU`;%R)W7aEN{YfqkWx{3diTnJ@v#0$Zw# z*OsKG3&fsg+fQGG0ML{0)36phUjIoqKXorn`_Tc`FHZ6(z7x3@A+@reNhRt0@j>|# zRc3+tVR=@MUHPfJV>R`RLAvar*VZj`9OnG5SI z{mZX4x{9$djno`bnMMwmKF*@aO&O5ySTkF_Yv5@9k2k@&A`3DYFu+@fpo!Mqtlzsk zKvQ<9ZW(Vc#V13oL35}vXwdl3WZb4^Y|**KATy$PiT3860-0#U6yeB7A4N@G(O1fd z3dlK4Bz=iVD5)*+G(d84INH;ziCk_&u{n?vxj%>}SAt ztNTP#O(x_brCgt(g4)AIK6l9&R%7L&OzQUAGuTKa3t$bMT&|>#-x}T}pEYz#@YF|; zN}*#=RFcaXp5}VJzI$@YNfvmq5*t+7iknqwLU%8p>_(23rHh8c57mq)!a73A&VFdV z^Xygo$r21*R9XSsc!T9Tevgw@irHr>mRbp z2i=a)MLVT^hOWU0*uvfkod2y`1_(x6ifgi@OfZ+Fzo9V3u$VT9R!F<~XZA8ZKlWdB#=*Z37OE=Bv-x{Ng5Wr%KxO_|8!B5B7B z*^D;O$z@c~v^h(R;(>-s0nmd+t+=$>&UGh1k6p8MRrcCzM>f zqvCAELxDS}0BX*stXn59#k=9?_m-B2P9#}?)(A7Mr5Z2SY0}OyHU*)Id#|-n=pc!2 zvPtCYn@JKmCDv*^2B#fRr$2`<}n z>O5@sSel}shZOwi+*QvL6C5k{6nR8Cu{S?j-meIqz_aYHbz+Bol%Ny7J}~A<|JTt# z)5AOf_AI6y;H!kVdfa}_U=?(O*Z-h4$wn_ldx|Ki>Cysu5ByG=z=EXfz!tV|wQ6hS zm99-=p%A@^1UhbkQ`)gTx1%xWpavukNlyKXMc2QFqNr_#Ub?<||qXg!WS-EgX3-@Sbj5k+T(5Q>qj+P>cxYTaXz_$wICQY6KFlZ<9Qs;=QQ1LK8nC*A6j*IK(Y zT@xO)Ye5fwDMM*hB|kS$9tuhK#Dr>Z);kwrJeMUe9j_i+eShYR}pr%Y|z-ox+p5vnO$Y%6G}GF7{9C zM@PK0QB4_bJ_`cojYY^M@zZhCM1KIZHA?2-Cd0@QJ6A|=6jt}dm*t9usWy*Fdr{qs0vJ~?FiE$bNc_$8b*w;NHQ6mMUz#dA_`AI?&92u*z|Bk&gRJ4_ zMG$wlFK`No0?vp)6*P(sQpRY4*t;ovUJgZ7#rFryA&V&fYyRxvyZe*71zHbW6c0S1 z(nl+97Du~1+9Q9}pzFj0$m~tz@6goavue8gWS!fOge>Ty$2cK?%<`S}Nu4))!#CA+ zbjS|C?cm;%ZXBeX>8H}iQB?o9d@?k+=-(7H7Izb%;#vn&x~U6Ce!R)wU;Nc$P>`O~ z^AL87Ja7JuPrnT|EOK+B&O|<=EAR&IKo0W94c!%($2Mxc>6h%W-|FvykMxvL9BA`mCr;uV|z| zdY-O_@c7u$&&t}+0>p?@UVBt+$rh^ZilmrSd4Wz;$EN{@=~9)3P0^qc^uWz+RE9EX z4P_B>*14cV1}mY2L$Z5Dcxifqk_BE!SF)z@L4D$wU8b%kvxiG$g67s%8!b2@C&zHD zerIBH4}ffIF~^%SewC9m(4!{b+cvfH)`^8afM$ZS5cE94w!9~Tf%chx@luYRMwNxWv;=KkGt6STuiGyuXR^oynp4Gf6N(mq|7X^AZYP+y2Ba|zFWjlQttn8m0a(s7(z&3aT za3Uye`rv?;fI$7=&5VXzcE7%R(CX0l*zuV@AET}v8vtCIP zvSjMg>$kuCg>2E-^>X+JXtd?DWbn;$UPB` zJe>5=R+CBp?gktj=yd%1gTa{rQ5+XR3ujL;66I!V_z3Q8PB;DprhdAF`hA=K#esh2 zT+U+T%qIR>584)v=8#7vIiDXW$3rMp4I7d%1MD}rcG|viu6aed1i!(Qr|NWSvDUCP zO^E5S^>8E^Chhdxg<`reU9OI!{9SK4Fe9(<2c#irX3%)X z;QbLKZPi@f`FHIAIGrj8_sNE%1npiv;=49!sGa(`ERt))zt3&JOIeDrwU9Q{(z#c| zY$+@y7ZoKG7SE#vtdpGEepIB(W7EZ~N82{(dX9z_m3X0tHz|_dy0W#dp!$d1UUc8x zsyTwdom;P4J8xfYuq;(EPU?!uHFdzi^xygPe1NFeB{}yREA6;51YML|T<;ThUeTm( zpIf}f>THf(b_!`DgcrLD$sWmNwXh!x=6D(aguTVx+?+lH(WqG=z|w)Jr4T3F2e|2m z8ohP`U<_4}a0Y=Na`RwUtuh*#;YT`M-E$?6Ptqkc)CZycAw4FtB!@RGBS!PZ{AtO_aXH+j zpBonj=Gtx47qN1iY`M8v))Gn2K(`r_*{C02VcIL%NIn6^YN?t76ano1+I-JmRCppz z1pBjA7Lbd<1Ynn!O$l`8)L@hix8z|q`{%1eE;hbVTAQ2LnUD+n!zgop`46ar{8Vbr zU~Ii$8k{28+G}!Y&%S4dCcPk~$Lc2=nt0_vv!`5$%CVfuhggL`xhOJ=3ies20ei=x zul^~{^H{|j1%92^rLwQrFcy)LLRam3DuURcBx86REjK8&{KLwfZjntAwu;yf%f*?n z1Hc76caz4F^XM>fm4=IP6RRXiX=m!9anXKC#MF;v?gp+ML^oG$_2M~a_@#bPwtZNKCH7g+wE!&jC z5Xib`>$Nv8^lsSajP5i(N6FOfr`Ro0R93ugL$%CgHMESVxth`sjBBY*Ar%%EHGoYmuelJDX`-F2%0&7qi^=gW8>l@A! z8kl=SBMit|e;5RQ;o0Y)s~Eckk&&VH@Xt}BFy;Pe-bIB&2~>@@++r*3o{)+ zL7f{MAGt5%CRHoVAK*b`|2ma&^HbyZ8K#FOd(rTUWQvWA;XK=yp0|cYFNOU`(DJtH zemK-bV1xIjl}WCB?FJf5LRo+eZ~`PFx7mTF9{ee&*~Plh06qlfANGKjhoZUL@et-T zg0LI(vhUyQwx{dCeJ=6qbaigepSloeQ^~hIn%y!r7aWyHLn+UD%X#DZ+>B4 z@8!?}?+7+ck|7`@v|jQc=?Kc-rEtNQ+jID9VN~lozmz!mWLzk8PZdzw+e8tclnl4J`IzDjQrnN$O(sphP)TlbM5r~l_-rI~MRoQOtk#l;0*vLUn^enOL{QqpQ; zp$5MCK2KKy%g&%+M%`T^zZq1WRo9=3TGLjsQuq&#c~t625M=MANTt z!*-`m^_Z~-uJmYlO-W%8DN(!9V14|0IgSez8O%TZ9*9-V;r{NGXEQ5>q^bvSb4gc2 zJ0F?M8q=d&bMlukdYLllr^B|UYp-B)Ug~jLKX&rD;X7Y#NC|jz~^ytt7vC zbyJ0)196|Tbc`!GF~4T^AekMEL)KOikR1S0HOnGL-m?o20!=F*VnA@TDBzh)7VQ`I z;LWd%v^cghtM;1KY3y|xx$P_8o1enUgImZ4G#MJ8B4a<-Mzi#MyWI!U7m3acV-dWZ zAcpC|F5gQ)F3#=IR+|~PVxA$Fh5Fd_?3Z>(>XSo===ZGbFH3C)UjLDc^JJ4c4MxyT zBeNGXJR4lp-+97u1F>yC15|u0s&dkf0<+B-ex7mECMSpN&wEoT;O1vSczw?jSQj0u z7qfp7h$>?9@E@U#w&_-=gDFF5PZ&}C7 z76OZN?HPe{>{vy5c60rX`{YS!1EL?u!iH1dmrN@=2j{Hl(T_tUUFia&9X=L?Yhv?O zQeuWfSbnZtW|s2Yx;o7EzD5VTTYW6)D!0g=mu9Gqd4h10$!lTzqNzZOM=5=8d;hZg zYNh$Xk~4h)v`y&6W685n%apcKzE`1NqE<>I7dA~^#Euqv$72%55sU@q%5QiFl78h{ z)~?J=XdUC~>UDCa*bghCY|$FLTE`>7YwtypyQDQ zcs>UFNV4W&mtzqKvp~}^IU4_OBIA)3G2+&BJGnAT+J|erlANX3CF#eW!RYnYoVx7! z0=2K%^NeR-cpIeMpW{HrQ`7N7Y5Y|dez;wich%v@$FpX-MI;HbAI83Gc6HNvK zZ0n!2D0&94vj!QAUx@CzSkWgRH9}ebcjBdiWnku~XheM}e4gFXBdn{pSuCi=Px&I* zQ=eu)Ds;!#pbN%J%7(|iw!hrc4};y;+t=UOyL8sa(zRTn+9#_~kz+)$wbJLx(^6t=wFO zo8EqIoEhwPrDc{tVAN7IgFkmDL=i2Q?Dm~IX7b>BiO1LUmr9EIR1G%SY0i`STcqjg zpr*)LoPS3viwZJ#E~lbLt-p_FTl9ZQfj%7oExQrag0}e~p6nn4P3CL6pijV}qk4S| z2mFED`utfS_N{YRSG4)@iULG{U|JOcJWz+S_1U+Y|PF+Wp|~rhiHh8Gdgo zV}g-GD`JGL)5;nZ3>VKg+{z@GZxbhFZd}QNRZ-W~kQDe4K}VSpVZxfaBb*1z-iC;1z`y=wq) zv^p2VHx_I*4)$%H_~EWNc;}`Pyh=^)>h_neq&Xj_O)1lyKMM+ZcqP{h+TXW#UkTI2 z)cjm-UbzR~ThQO^E{HKnO0#{+6?Q>lW8tj+3@>JoFlGg9MGX8dOvl-_t z38_){@*f88@d+&(jQyqWbqwBzhdY#9yEq7W$@iaO%j-b}|t&Y5hzYs71YPs@P0D*wA_%T8WJ-i)W`q=jt5M{= z#?svAqz8lfejAqf|0Euuejvs|WK!h41XFBdX|P;FU7N@@US!RQbl0pnZ}Lb~z2bp+ zcN7)=358Se>dJg7j%EjFVtm)TCjAG{KeV7m|HG&Ru-C&afkbR7&SGiDLVog_i6L-w zsgoRpV8+``nHrAtjxPaS)_`pFx&KwdouOC`vdAw$y^~*aamiMRZ6x;PlmGef?*Zr&#EMEIOW1b{ck8$f_!2i2JS)eVRCl zie67*X@pvGF@~~I-$r*fMJEYv!p&{8_0bgmEC?+r*UZ`L&=uL{mRqqVIb;wgGrqLM z8*UDCCP~Y!gRDw?bscie4p^^p&R$;x&L!73zRw<2f2=o|%x@WB=cP#!o43gA6mbsO z{8dP9|2)!ee&;CUb@YH%R>4~;Tx2g3$201KHajL_fQ=y6lMg8j3=K{XWPu3A{9u87 z8c^a~@Amq$PTS`-d~+9kKi~jPoL8k9bixb?lQ|)NPMy>?NPEN6K|SXFnjBp$`()T- zFUY#co5=a+(GO@FO9XR1sbKGq+8ge)>s2mE&h&pvvD&cI8i3TeME3d&TvcN_Osk}9 z6fyOkS8VO9*<8W4-c?TWsi-WJgC0vRY_roV_Z3kB$r)x2vP?ND6aK1Jb4TlE{h}u* zKe&0pG9_jS7F}dsC^E9UHF=Qq5m?gir?q7L+y#S@ch}e(4-TR~j-HmZxo~`#e2`Eu zAqf(hWVv=FsF^Sbc1{DaAVA+tcSKazL1ZB8rS}sTCm`sB+KkB_4qmeVmyB*PTv~!vAOSyJZxd`;2-=mfX0h(VC$r6-qz!kZz_!BQKE~D(9Sew z$@ua_6s6+wuyCI0n{!3mFH}4hTpDVB?j(*VUQrA+B&|-&FR^h`zy}JJr0(Z-y|v@3 zQr$l#oTRXtta|)PBZ*dzpNTm^Z<#6RV(xksXgl$Bz84yf&sU@Baf^v`i%+B|*}0bg zs8ef^JZq<99&-oFG_R*LbI66K$f+e!7@=Dh=6aT$(M$KiKS>(&n1b$g9bGXn91I*; zZ!|})Xlb(uuk@}aOdj5l#aA~_)Y=op1CcQFsN?v7El|*P0%ryQ%9CX-aizFJr4Q7m zdF=o|>Il+w>5gguc*E2-O?m3lO~^Vem+Dd@`_Zppl51azA=beu?3I3U&0)GO-$O!A z)88~!AxS{aWKl1gWlXdy3_mQfM7IsiK9U|&Lk+cPg~k;-lTJ0qQ&k6{k;9l{h5?&+ zmpDb|2mLFn5c_$i(}!bmQAhj7bX_t#X$*}dnCNYn@&KCO$ zzipDr{97Q$;?=)Manqy0~?r!yB`#_P?fXRe|2$psN$DRI|uU=)vq`kz> zUV14q+l2l6Re={R?K<2yOwk-~&xv~|+A(jH(Ut^cMj5ioy*+(DSU356k9no3+3G1; znOeJC$%#eT(R6c%hO3`ms19=`K2yw$5ZSL;nHwn-LW?>c?DJX7=juMBl|dwfruZtG zhd6A5uFpd}?r?k(ug*7zk|BUoAD56_xD!!!d+!WTph(uQeREvi4=04GfHQWHY*mBG z?z)cEuYyd{6cp%8U~z+~4FFbHbZLpDzGIjK37h{A2d0!ftHFbIqJ&m5|ELY1SpN73 zx)bMDsS6Cg&+-1FFNJKvQz6Mdj@DwkyM0|LK)3YojJ~7-0pZC-{JXEJn*^aBTg2m=MN%Kn@l7UXXPtsYYhgENUTfV_ z4M-4M1BT|u8?g_^^~YbLj+sC^`~a@e#WqRg&W-Ii87FQeY)O(WFb5$ zFqFGLUIMKur^4CB&~*e7OhWqbgR){8zfzN`zqjA|T5xz-UHKgMFKY)+m zlxT^ND?X2NXy&5Wn}F`!&{}M7=6KIALFsQbmOM}{DUf@4C`s4?A>Qp;`XE|65bY4vx@I6)jMGZqY8-aNrZ>-$GZb} zns_8fF8!>&P*RgSbo;9;thH!m^KDM8=nL;oguQ^I?4q?+FjCSbE(g0X;e{rfHg;h- zMp#j`85~aHdc|ABmUh9$1HNqVbPgsCfJW|f>5h!O10jD-@b?l|k6GR1uZ#D7RnQWw z0BG4^2%0;@`+%ajlMJF{d7)b?YX!0eNI|DV(0#RJJ!D^$A4$u3QmE{_dS<@shjhM_ z3j*Obnsjyl73Fl(6i=RJLXCYD+O4!mWiQ2^et(~>IQa6wB5CR=W z@Qs6N+h%M@g|t3JXMRv`j*9RlH?|U6;BRT}U>E8h?>HJKShc&uiy9vysYDFf)IO^x z?&6MD{O>J#0H0L_bkg$C%tW$4t8mA*9Vq019cgdtzXAvk2`K<89aZMP<+2|d4hID< z7t_Ql1g5P(CD!IN9_rS6N70Q9OP$AF#FaeGH1C`oQIoIm7N&uODu$uMjVTwA$q+$LYi zK{<Vsi$590cpof0($3Lyd=&T?d8FfmwODCe5mZ85WSOVVY7M8++ao84-?+#6}a zOq*0UqT`~DZ}%AJV>MeDD71!adg6+s)IpW^NhiP0ghOp!JLh{YvtSb6-*?MvpK5ta z%;efZ`_7SIXL18m=%px`eKGH!hREx`d6$w(QqPH&RqwctSqf0iZ2#TOOjiLquy>ys ze#nLS&i2|qy{z?lIMu(b$C{x1oqKnH)&^41p31W&9K=L`Rbk)1?H;CQe$;0E<3ND% z|Ev@Xf!ub?h}2bx7=OH-qH*c01j6-sz!j~yl$C@Qp^I?)GxjDc9UO!pnYH0CM{61g zq~!VRefGkR%@L4==pGP7ex5sCiUOmYD`kM7jAO(nBA|p+K(`YV&cWX^J-sL)!RmRevi}?Ic-9`v%@tK*jUT z)U0anoGGucqHbcFTkUh%cAb?_pvc@N8Mk*D2ib6iz&26E`7-n5pa=6<~oWvw|y;azeb1 z$j!fibn}i4PF!|BRolg@&wuXMP`V>|L7n+NIw~NSoseq;Q72Rj!JT9dogQg!z{jDQ z*qycuKTRKKZ}>neTvWlB_=zD+=s#>$)p&IB5{qc zWCZaoZI=aN77q=KZ$!MRBi#IXpF%TCB;kpIdk4Laf|EZl9VK+Tcu)DPQWg{tq_1k` zNB8C>32jsm?k^KCJ=d|*`ev@0HhQxu2p8=hFX7H)sxUjm?!Of03*$EffwGqOU& zRMrv3Z{8TxJlxTeVZP_ZzBkkdM6{=S{wZ5&g+eKbvWo9R7yi>9SvfW>Q}6$*ukU<5 zCy#YXX8xtwfT#0v3Tot#AVpW%Bo(!vYKE@=_r>yq-@<;2)I=z-;Hl+1xQ4A(zXetB zL@(#TyfPmCW@}WZ<(@{vttHoIui85`T}#5|Q`6E_ykO;=!(LBdpg`-={VWkM3VuBCSK zJpu1yk}gWUM}%Keh|BI|-FhcFs7CWBG0-OYms3;8n+cn(f;RdhRYX5)Ty@Ll-~dni=FW_D<>#$-vA0 za{#FC&A`7{UI&usWUw)Tu7{jZt@x)vAX=VhJb8S}%w-f_1c`d@bqVs=$wuy?({%Gg zhpX&X?KE8ECv)QnPpuP>BzC<+v=-t1|L${-b^+4j6`w*?qi5sQKWw{yvh&Ia z_wsjp%H=J!(4C0rlAyk{=7(L!R)3ySfnnU~2~17+R(N+1_hTl8X)5%>&7^Vv<%c6F zB&URj5k(uiN{S4;h~+W!PTa`Rev*8#%a4bww3w}SMui5Qg_A^_8BRD8%|HgltZI@O za$ffMk#B&zpqOj6+7$WAJ+l;FRRbk?_OFWjA2_Aa1_|^(76yHB{OiJrI z@+Hdm=jmv3n$ypfC)Nx)y4cAI>ZpA|dA|XO0#CL@(5sWGZcoE;tJ5obr#2C;zS(61 z7`i(Z>MuQUd_{NI>GAEaVKOQum6SbjhBOz+fvzNGu%B8cxI4;12tMPjj8AilJ_8qE zP6)=Qn`@pRDP0ry#b2vk_~hhokVLrFX@!T8ZQy0CCe1$An=2YCEKdyUmzkRvo6Na9 zrK(5OFqt=`WC%CgQ4h{HI}CGrMG!x+a*Z*_prlLo>~_))F$5Zj;LV&WzUm0bz}jpFC3 zkwo{TR;pmq)XlD4YX?!!lZ(awiKfML(02 zM=Xz&s!j6hIBH`kJ!e)9y&S#xX`ko9?(-iufN%)73xX9{f2o%Ayf||xC0HIL-K4bym9n!|#w9+Lx4F3HR9oCsg zDp#|rdyXKGoYTv*O5?M~2M@c>N4GAc7+@wL7kA`UXn+q?K#)jh{b!2!q%cP`Rn|g3 z$U#3F*0DUZtQ^U)680GStGChqL%xYPVbdukgMD?>*~hL3Xw9;b5%-^Lj!gh00_Ppz zM?X{a34W;M`7c#R-e<0LlhpzVPM`ETheAiZUu!1l`CoG$Q9@|m$XC>oe*vaLzjY!wwMG4g9_cVZo!DikXg5DKwN{WQczj$=S{ShF7yW=+k7;pH{lfD3rnnQh^~DunwE*maRS`!0mjq;h?(1{% zwyXdA@b4Y+GL<*f$wl)CRTRzBe_Q)M8ix@DqTLyDrf+2-hXm8+4#M2P!3h%C&8Ig^ z3jWWN`V+c`d1+@FhDz{?V%UNf0$PO5MvFYo`gJd%*kG6jZakcXcp3wL?*L0|S?nJb zM{J=F&C1Oxar(|DTjL2GFy6i!N4|m0$7>kk0#|FGRg~VgJjQm9cy%5B(%p?a=BmZt zCA_>}Pu{qlPauDm=th^6wfRPu4y~vsgsHHyA22t-)+WraahH_re_A#}fnZ8`sjU?$ z{D8Id?s>K+Fgv2`dOO6k(NH0~J>xU4d~$Bp-L7+v`=rLs7IueA_~C%!b>XD_B1WIq zy{Amh{e(F6<-F6@h|57E^*ic7QY}=DMGo3~Zlf>Cn0V7nocqqbyq zU3i_Db}j_(u}A#jKH*k=TO0X<=?W&!(wzE+&Iz3JxBnK51aSk6g|bObkLAAkv|*ep77ju&fdaA1fbD^x!UEr_er2MA+yG}@%(xJ30-UGDewO#t zx4gI6OGx{nNuGb#Athss7Oi(a?|cZe(T(eXA0TGV+gzT`r=6)L|EyJLG;EvX$}r!rbu+_IpO^lHG_OX5L&7+3V;O7kzrR8V zr(S;i)%KmW5toy*Qfw>YzrLrl@sHco5-L8+PAM@&OLSQ*gn>o^K55ix4<&p4b6{$W zvf8}-sWFb`-`%#{@2w%cJM!hZK$7_2elgkBl(nh*4Oy04cl*|Kfi)dW+PnUX7u>#NmJ} zJxLD7(j?}*6#=UF2vk%`)oV2<=q4UK+^sTj{e=+Wol5-OGDqcARGRyh{Nf-M;gvq%R z*4h!JNY*)URxN(x8)WCzpXh{hdok>O5oHjplagRb|9og#L~?0!+6`X%uNRr;evmTD z{Iekcensihd#(O}d}e)W)g|G)XZYGS94RzbLLF9?PsQprkSB1%NYOXR)lHIUVEMit z+W$5o?D@R{Dpf6=ocl@B=Mx_3BpX0Af(XEqQ<}!(!m##3Zb8rev zbIN?1%q@wczV{sVvFkp!t^uN$@-lVtQp&2BO6cpJn?qaJ1#V2s$7=$~xk(4}d?w;u z@BflWAYpKD?nuwE+a8pO%dqY9cJx8cetX-tTfW25sT5=1<#0()(XpoqBIF&N@(bmNDZPKsFOTxA<#J2A!OqQZNMl2vuBqMQB560HA8cKtDewFV2+My#;6pa_ z%Oljf21&x|Z2rlH4i5%u^mT-ZQ}f_nW?>;U`#c1HQnxDT>D1kOT!VMN*Jq`X>c2?S9i`MAy|HH7ehf)G#1{mQd$S1W>U4bM!Rw zc3?6^HkL53rm3y%Tx?t%m<9{T4^&RhRYa*V_xTXHus`;e7NETULDci`u*ieEfv+D5WWqZME*T#2az?7X<1eh}n z8dQ*6lLR*>6WKEMO{8OWKD@X$r$`+R``~1>QR{$JXkrK$`t%od0ny-_hovE4h|$1+ zKKLXHdwWV-TU#Sj(=XtT7q|8!pt`!cU_dr_5`Tu1U>nT2Ap4hC1}u?zUjF>JoCOT! z;zysWsI2@@ULHvJEMZ|wA0IXx4rgR+{ORl0uZ5D}TeqE9_31aiUoR(k-&`Ligkz2DQ}zYk1_={8vj1ZAHWc$bb~w#OUu(?VPRFS%PfVa z)i1z>@{5XARg;DsDtoMN5MLuyT0?YD9X=t0q8(0gt{b+RPERyFsW6na^Vle0hQPeo z4L|HhQFVWvm;Tu;9Kt%XDqf+2H(FbI>XaV-m$!2-8c+LLMqy+|4Rvy#6qmusmpEZC z%u~Sd$@0ieFvRaZ7)%Dn)bqu)`!f(wH0E=9;&!;VzOUi8ds9o<;rN--M5)bQSjIOF zdm1s5tVfLQbw#(?>VMTlxaJbYZp|)94m;33c$pK^gyN3 z2!qj@wOVl#`+gJq8*h@1M=h84&7mRjAqpM0A(!rOJzUurVi0|)))XD?CL5K&^9l_7 zbe}uUQMj0(Kmi6#Bf( z_O~G$cN?lxIZ%c@+xEzfCUG)dx}kYb`8@IObIPTZ2Zl%4rkUD4C>{CoFw^hb$MxB%dBA!A`Do!l=A2l z2j`5485FuH|^!!``&_q-aLNl{7-c+u>2pBjsyb3Gbnk)Fvlips_mFB5J!bM2w0-` zOxCxZgGM}L#mx{i4CYnU8ho8WLkbHGii=@Qy$>mIXh|^vjiFibC9R(By7zT?=p*@+ z$8AaGMm^aGpFoOPp07`yK!Ppt=i(DD-s!kO zD@wV^llR@P=8s{Wid>Yo?4Mom^gV8!zi?AViEJb`{2bv_jX%$SrP|}TsF)CVvCA4ArtqTg zBcAlEGsJ`KF8OHIAwKK}}3f{8}1+EyCiVbeK;Q#nknGB}}N6+UTXRSEIGB+uou_$6l#OVD|3z zR7>`V)fS=$OsuFnbJ(&x6=r3UX7d%doZc8-sGjJgGFwM=rk_tE|NSwXy%mCGqYU8c z-zow#zp=!M^<7huO9APhcNfQ7QhMKTr0|GLKah>sVpuL~S>LrPAT_UWy+lgwZJSdZF{fvC{~M!g zU^+!+IC>`)p3N+^(qS?uW2!IuQpvGY10uBy zZy$Bhv;8I)#^3?Ye?>kq^4G|D25kE+ANTWFk?K>ihts!iB(ayd|1ab~s1TYa*BX2} zT&bnW!P;j9>9*bJfpeAQ9asd{jkNNzWQvipIrlFc1TS$W9roBlQn6!6a;RIt(j=mZ z&ER8S-GmFLg5$(~^_FY&vlJu=)OL}CIRwO8Z5VEmUeAKW4k#6je;q*c6c*a0k65Sm zT7NEij^GNC=Lt~{6{7rYI1r90kmd|Y;VX0>GAvirKsyQ>2VGZLf%&~}DN(RBT{9Pl zZlMuPH&>BMtMbmDnYr0p1cO4Th#jbRQ=S!swg_=0VwoS7oUx}yJB<;COun>}R}smN z!?|xy@z?K+u+$&#vlJE?_E?u68rB9^m(>O?1FLzD&UWIM|!heb=iGgbULQBlZa~q~?@q1E_!V>sqdRyL~|369L7b-vpp;R^_=7lKG zHA!5-OKl^H{vL|760M+CwS_YjNp)5~WcyXfn{`$V9+sZW7vTl_x>t#>&~bW&Jgy6j zE3%&&4zu0;k?B%fQ4--9L;Iknc!Z&_2o1{kT<3?^Tx$Md^Uta%C30{%Dnu^* zl475G9g(}^gxtIHhc@S)x+|ui5_ipN?#Z~_wa22c1TWlTdBNmS3xxr<9pS)a z$(ioF@Gq>*m;2d^t~R@QE!|F16{h!$5;FjrsEj^%%`W3S6FaLGLgOLWE}w2?QDE9k zZYaLyuV!0SVWyawD=OdNIo+U2`#|b=n~u|UI{8(PS;cgWQNYkj)HOXErC|9z_qBC? ziqr8eV9N6s?Tc!h=#;QKzeDa}uX!_((nSBhaiy1Ych5dOy4P^AgtZUf-gZe`G`m`% z^7%P*Y1_e>2Oy^JWFkZ*EH);$%O}398&Gzc8#g-{ifP-H9T=+H*_4k|)s}#W^Ci_POROq7~LDM0^Qi}$mGMD`k;Z?ZeT5D*ez^ZAA! z81+_1crT4@29&@{^NTMAdr1{8pHq4~zMCaq7FR*~VVvD476i>9GvCg=!&HUq2?n*w z6I-^PIS=N+#YnaO$v?bE(0WM7%z*-KH_7lEWM8A)C(Ak5Kfa6d1xH%%Qrq=&;Q=7% zCA6}_U3^QD=|4JWCV|u+Ij3V5?K$Rr?%5}GdMNoLBtu0NhZ2ej*fvInN-zlr`=JIN zCzO0QKE8wZsi(um=Ixp5(P~_As$f752-u2^?WbmNth+yEZkiekjS7Y?Yw(w!m>Ht) z6?f?3g&w|)N)VB2{v9#XK<)*gE4o+Sa72nOG&$0|P|0mB?tHM(t6M23$5Cpu$m=d& zX8v^Tq%|k7Nhl@Ys@fB8n!*cGOdaKJ>|aenrN)O$_kZGYkkS75DdhT+z(YbakN>Gg$;ZqGuxNSX}?Y>D8F zH`ns*41P_`=>LS=eJiakFBEg3$`)MMxfev^kIV1B0h6+S{yYn4u-nz9U)R?mkIUaY zW2-i$iiwEOs(Kh&Qq4|~zR~Wgf905%zxbqN zpq_N3!q}PT@DG{{&oApr7#06oFV~Rv^`JCA{Gp$y_@S;@ym}KX+lba zASxTR2Xj2ZW#ivVSU&|n;T{G9K=X>~L$|hEW+b;)C!@S&qd?kG`|C3~s(HHQVd*XJ z-QwP~p{XgZ*x1c(mQ$W#q3+&xD`fM6} z*4V4}DBvZZg>e#J;WyRO!1`C@Qr<9Go)B<7EDEzB3aJVy4Q^ihculv;iYLmME3(Sw zbJ?)56=QqI(4lAfIiUfea1~Ivgx0F)xi)Qj3a;65yVf&x-}y zpK2m3EdB$D;83L_``6}XIL_F_q%lmm9tDy`1+M=ZMMSP&_fy~kU!2_SAX7AR|8bU( z8G-a{!I_1Qjt)$lR>{=y7o%SzoBNJ((2EM>{ELC#xc^_a*u9~AF|hqpbR4(5PbY3A zb&`Ns`GelYEb55q4}GO7UoFMCzSPFIWQxefegO;RnI3U3ed}wQ5l5JgYfZLb(3bCt z@AZ5<9GIlcOpT7$B}NnZ_aVu0Tn;bPKzF{WsFg2@nDP4mkEpMJimL6t2eIfBkOol@ zNu^U#Q9?Q-1Vlg>O1fV<1XPq1ke2Q)$w4WR7+|EOTT=3OM!*01&tkpHwLZS#-us;A zoW1wi=RSP6XhV{xKxbEro1;)ho`l7X`!wMlEKv*+;cntl2?+_(5h_=iZ#?FteD%6j zAGj)JvEej(;ONNTSS6*U|LrXHXBccePleO7rEF{GDIqTVOH%jMBhIOMx}_c%cR{@2!_pRQ13!mUS8hy{BDsWf0FXhsxzFtYXU4EPDEvO`?=8MA5Z+7 zc6iQ$h$qy=c`hrY$IFQN=xdMs<=Y`Pjj`qqM(-?Ph6kIa>@$m~PcbY|>f3OG8PB_twh%`iM{kDdk@p!Xi(<080sM{T7M+ zEbq;KCe-Zn>Y)jj#}WBC0&g`#IY>>t83QSqZ;ja2jg`9m{v} z?f?PhpNsj|~&$3ZdKE z4s>w5{Ji7!#r{;}+FF4{LS!Ui!_i7U+=;N$5((xeD=VS(b@z9TJZo+rb6(F*?FY4l zRQw-;(jz%{;+eY~r7u4C+;iuO08%-W;2*2TY;mEd?5HHms`1tVRkK{`{cD5Nk8>N} z5w_j$;eGQOiP0XqS)X7y-LA5_0*8#v;*@kn{Z6xfdC+hnhu|G&49W_<2*(@4W5wt_ zM!u)cm=ptv2^1hum9GCq-WM{Tuup$alXY9pinHHa8KxchQmVVw`N!wXZ;AYW^+fDtm^JZyWlC_u9Ns4A5nD}&BsJU`XgQ2qyr!rP)rqa)WnS_OLC>$ia4 z7uXD|dlbwPcE{V(7f4se{a{r0T9_`%OthsVw5o|uYI zV74E;FI>VY`lvo(Kid|3tMne{cs3TZ8^kllb1 zG0Srd?fT_J{L|_Co+G8At7RO#`Bwj}=X#d)rmN4p9_2xfg<&ty{!4#!_q1t0 z*g7TfaPsSeeo>gbzm8sQoYcu2QFe~_XgFeA-RW}dRQfD!5+f0`&6dIQx~fqOAf_&S z6;+JcCzkv)0hD3j33>=M5)O%`z`OMR(M_n=urf>uN-Rme8+BE1$~0^UIIJ7O3XV`d zKG=M~9Y}w-R#&eLt%y%$joD^QT4sz* zqAl|O$!9Fxzkab|GCXW^*x+y}$Jy2(PeP`C!X3vaC>Si0=4T@qAt!TM@EkBID*^Rw zN+xOgGWIfjnJPqaGM)6&ie{5*j$a=RZNLv%ou%Tn4Vvcl8 z<|?xgbcu8t)?dp?v=Gl6GQ#&92|sN3$HzY8dTS}va+MENjudBUr|6Tlt{flD8vz+3 zA^pF9ydD?oDrmM_KOXa#n3&9TUP!P?6k5Q6%Pcai4~Fan6$nreD;X}b92siVn(}JM z{q#Jw@qEpewR`YlKeFO_#pTLD`M{8_g4f&v62lYa$Jj^58>`6@^*I(QTIS~R&!5ZH zWNO4BxToXy(KWGZr1%XMDKvDM^_NZYIQxH;wg@*_j<9r!%cZ2~-)XmjMw$i-F!*wLsO6;?sn5BzS4R^nV9OhI02(TVy#L}LJu!sm8WF26x zXFoBOvX5AC+IV84@JKn`5c<}n@yPa~>P-Wb*o~^yiN;Ia z$&qe-$w-A0HPO8FPs;!T3R(@rGh6cvQZ4Pv6Yimr&D&>dLa&ol)4YF}R8^}HK3efnW93H6> zf!ANpG~ciuV&xip8`}cmI}pE>iBPf@vk4TksW-{)`Z%wvdeqZbH_cHz$)sNtWz|+e zf3lZ}C;MSb)z0yud}V?WT9E7gs>F$xddKZV6Y_Txo~x~q_KKeIZ&H6RFw+_ABnd$$%$M_7r7G+>jCd2> zn-qDR`eJNf9HBa%qlh${^x+ZJ7F%r!Lb(34cK1m$H&wHd&cK6XY_@lwNKFrp_~W5B z-F+N_$ExKQ1_(Gs(Puqg{fT1wQGwFG~BdDncHG>;pf-l9CE(PyF)oYMPqh6mS{n$lOgJ z2sf{w={8ujqTGpU>lF>I?$Vu_j$gTpEN&F-0_{>=R73G<((13_WTwV$-fB(y%six&*hE#{SkDCdEt#KY)*b2)z8U=G<2H{)G=2}||S za#_P|aq8reMVBl#0!PP>7-sQQTfyx3zMYjxd1kEMO5At;s<_too# zWPQhzfc9bGStbAXrqr^4j`|3aTWG-@AD+s15>QX%YBHRAfBANSpv(%=hS9y_` zU^h>VRdK@ZiPfobXA}3)PbBXuAFcRhlkJR4& z@UZd3oieH+^xYicTkh(isv0g^4@b{*i(%D%kBcQA<)e#{b zp5zbv=TZQOdqZKSwzkb>-QaL2Vv^mEp-`%Di@bo zU8l0tWx4*hS9;<}Xq$Yc-p;!*&~?kBT7R``+Pn6Mm=$G?9G?C3^jb4BsS-mDMQf0o zPR66iNZ~V3@LW%!R9CKiti8DTEc{W{1w#~RqylL~$200qcsa&ZDQ=N9=FU$Gh*xes z_6ZtG5}K_mrOHX;_QS&K%!Oh_fwfptMWUOE$kcw92^FG%(?p7=hMAkIb(^a%eprMf zqcITT8ws_vbS5O~1p0N=Mz?$1$5VDs^*R~YCVUI#_zRP@ z8huju(C+)LV@UTgVTMQD?U|UI{mzZL=VtF)lBcYE0#%XM$BmroGhmC?t}9wq-$pw> zxY>S)JEpZYhRD8EJ+kp?I zkv;sbV?F#%sV+eoXR($?P-y7YXKFUg0UCHkkF$=_vK_oQwr7zQOPVpn192_+91EvE ziv|}z{=2u(&SMyd^D8XmTHDxA@!kNSuq}1Ic#i4ta>oT^TeX<^Gu@9hhkbk!eZDRA zOpo%%GQVl*Xjl5E>y&p0;0KTboJ75IxbzFPDA|SF#ly_fi9?X@wS(kKZ|eTK;x@K3 zD>?L)wdbVSB)Zp(%J>$-?(JzjTb1O>pK{9K>^+B{j1nguvb!V=J{Ml9yKOga zMqE4bmR66KSLW>`^!whP#^^h%MVLYPFnM< z!gbq?Zrjc4<eCZWI#LI@3P2yB`q{ROBDaO6g z&*Z~3o2rSI?I%x@7O7QN4>G+!n$r?H|BMq5n9n?D^Hq*F`(kB1~zYYebNDbAIpr{n%%#cC|`-)a*Kya>_3LMlHjgbaPTH3Nq2AsmIKDiia*j z&c|Iftb?EJl>Yolx^r95N-PGAaN+W5%1hH+{WM%YExfTrhZLRA;BiPg71r7l>Df`VCV=o=v8uZkD0DsAadke%e-;8 zDC91@QQ%8ZJjK|{Hh=ld`1}a>e198nBr>3{5tAJMeFhdx;KA*%S}!D%*^hEYuuFC} zNiGOlh!AyqY<%`6jM1f?+?S=R*2_Q#yxp3q7_B%pM%{TDI+DP2^$9C>IBQYe9sK-C zkM9(?WlXy_?Ek~XEtMaUO}fJ@=;S-@VQH=Fl$n!*UQeH}jrKO6fPH^Kd6235WXCgp z(Y=Hdzi3q^)|}x6i#~o8A_KMBQiTwXy0`ZyJpH1O;ydk$Q;V+-bh@#Zy33xS_lM4l z<~J=o@J{$X>UE~Y3iO(cN=gpxv~6^=JbvACG_H8N!aY_cVeijQ;?3KKBpMb`D_4*# z%Y56_XBV=2RmC>HBD5pqG?0bO0;^#mY@d+_)Rz=HlOYaTSDi!wg_-5N(x$ld_Tj~M ztPbB11cUr0dXy}0uSS!S7G6R+e1BQ_-L~2!abYOpR>lB3{10+!&(&5$UZXu?S$ z6ulI>WqQ-X=#6xeWy-v~HiG_`f3#R_F0(w|oUqLK=Yc$40H@J_M$6REn@uUqHr?81h+?{k^QB*RGZ(%gSOR3$_@p? zFm6RM3ivhUG2eOktOr>4a8P;gyh$5(2#jz~)@;J|iE zA_6%FFj#f};4Y%5g)f!YEbMHjvk+gv;3VFe({fI;C{N>NM5- zqrJh@3wohsp){fOQI)3;Y$gwEwnXNl(CN#P>1u~M5a$w#EsTGbPzdq<e&-2Tf1tF zm%oV*D`(vq3{ZsJtlOK%L5cc>E;*JO`NLp~36QR_()px1XPlh!ko67x^uBo8Jr4yp z(zG=6A?t-UUZ!vsIkg6%>C%nMSM)F2ojqIF+Y{jC=C=3pdL0u(4m3P8J#>A2olTA6 zmEl1j7lH2=m#upwvBt0Igt+=X=|x}-?oU-A;dM{JS9d34HC(p%MZHeDI@f_P}n075XXQE_t?qp)(Jj<=0B=y{y?+KY_(K3c^6DG;dtZ=KQBk!Z z^vf*XYjOk?78XudJ(ONLJfz;7YJN~YCYL{)cxZ5Lk)fU&c=k~zU+ZKaZ3lFzvtR1G z;d|*W`Fp#`h8Fn9APR4woC9|5myZ$HCaYXYR$)67@|(8!R<$06I2t_2&BIe3d`5kl zEM9^+MM|0n+ft~r+Af}wL*$t1rQ^jT`S3gU&l~f!^MT&i)Yi6ibX+26 zyZX+|r{!kMSEiV+7jKs>cAa|+j#*o_hc>kU6+Kz0ho@x_m&1D6+bpQ!-~UXhbF0cuzRHqQIeV#z(w!+^RgP zviB&_$~)CHmD2E*h57}D1n-2!R|`{%@vK}p4=gB4Ub}0uRhBMMgc6$@9+iG}77yz~ z=kRWJr2Ddx6yEE0FcIUnK?cm%SRU96iQVGPH#88atFcP>T+LNcM@eHy+gs$Gqu}hj zWcO{zDEgrj#er_Lik|fah(RhUD)!FKK}kup28M<+zkid(nk&=W8(o%?ey&HE*t?L7lv9H_ zH0iV_kZhUv4-RI8iQau>g+$G&XzdzQC-r%m(F|P`;6FM)Ki`N>m+x>!Ej!t}xCA$! zuG<~{Sxg;RcUiPGv#_`_<$XX;ArIfA``OLG-91d|{7*krG+0gdtWFj*Y!ZhvVAMpFgJ;7A}M62cj+1f_cY3lOm=Jz{;Rn1nol} zJ3aM==1HddZD446v8dk-WuYxQP3C%-cmei_NoMe8cXnREtxAmUpUPO&PcZy(gIha0 zlXP))Ev>JorRX}TEIeWTp}~LXIfkk0LAKQq|ICo9tJVfaMw5!cK_Vf2)>{X^+0TMF z#Kgn^HRJ!N05~8et2)0Jk`iTJbYAgLjJTF)__Mhj!=rex+-O3T7LDtZr9 z=`l!X_qhS8UbxuB-f(p`Z?eO@Fu<`q#HC*v>G-{zF5act{>6mPDL}m>qwk6N?1Jl+ zuPB0IMZITsQKC__N@<7BR~L=0*mL+9Uii53xs7*LVDgmh($sG$#k))f*#6_H!C{w~ z6tHu#*wKDR7oNCcOK(brFnpX728rYI@XHnoy)zo^Jr9Y8SDor7vNCaEAuaMJ^?ur9 zV`BqCHaa#2`xhH13Cr6q{hQ)YRW9>hxpJ_&=TWkdnA+vGa)~cp?ebs@6ypz%=KJ~| z)lVoCqwxD41PrQjw>leE=yQ!$MM6IhazuZs)QtqC%NlL#8yhSj<1$1SAMtWUn(JYf zzoDUhY)sD{J1|u=QSj2dtTjk5qM|annytuaMkrGLJ6>r|z!Z!A?6f5=-Y;7|*0AF_ zmReT^gOICbRD8VEijY{#*I7>*N|O6C^=} z^6&IBjzp^_L>P?gfe9=epH=tz$n@mm*m^rXJ-{={5S;6EF`mR8^8SLrF2TBu|Iq5b zrbyu|&Q^}_+Mb7Hr9S4#)YY|u_5)%2gyX|^OMZr%q?EBzyhgd2{GG*y@AUYav-cIoO*uD}SP!~)~sg>gNI4R#g%KBub|K`4a$SZGMpYYLhy)CKZ#l=PN&V7OFp@~%Z zocn??LaOpbp*9W{6y9eZ2Hj5P0Fl+0|M z;*DEBl@YZ%O}$ZI_CBVQf$?q!)r*x*#bc6N+f^_=IZ)<6v4G7b>bqLdpF01m=$WHti+N*!V@8N}zci}iyC7=r zn>vf|wNI#3Rn(~G70W5?Bc0RT_>cO|n<*dvHS*@>%s!^1q-%1dsptOGi+P@mZ`uZs zy3q)kqnZkfgAok8j@PR}A2oE-Z#3WEURt{9d$yArVD@u$kJ2vQG9V#448DuUxM_NA zjq{=Ukrnm%;`3{ZHwO8k1OqS)0by@#eSM}Qg31lrOr0tk%c^>Z)y=1bj8Z=B9Ubz# zr_YN~Pm}ydd7+@<3qXS~DmvQ1-rkRj$LN`_FJqXT>0Hjj4r)nMM5N{GSKOlF;sAk( zlf9oKH%SHa;Sc2J-vMw2*{{KE`vP1@J8Tl6vJ$1FRld&9>_Xhiu0K|j5ntNYy6@)r z7<2|Lm_(0>2W6QT=U4SCQ@1s_#P|~bI5JUt4FXi~qPxAsC$n)tJs)*Mg*mFX;xh2g z)&~jo0RRm$_zc+J!8vEdVg5v_F{fNF#gSFbL4%~YcDU&RH2*9f^9An*Uv}y zFB^$Ima%cuD8JS2f6Vp8b}#2;MISb5BWGAVf2xFfJ*ShM!gnIxrQVzEx{H%jAS}hu z%gRU<&e|ecio}zOc&4j>2wL>b@>BMEcEtZdpu!`2(!$rPbz9=Z>n>vP3`Od>)6ize zv;}Gn6dH|;BlurSeaJ}F{*7vOG^Fxp!t|DCB5m76&0@_jt(F-Y8O_ES`?Q1*Gd?NQ zW;Ex@Ghc^p0_fnMwezJ*yZDu%GtC_H_5L7#S6RdU<`utZTtdD_2EK*7=Le3J`i7Q; z4bHqZwjWcs$~p1j>r?w7|4A-a&H$GeydGhC=k5lczuJ^BKM3Pmgx$ZN;4Ait5>AUF z&1(wH8I!DZ9D2x%Qv~pu3i5pz9I-XiDV-U1f5>a;Z|jop39dIK44*TW`=)(f(5$?e;mLFgsF+SW2*)&+W5=OqA3Qj=}@u zYSpfWpxl;4)jM~U?|dqpy?D>s!0iKr*|Evu*S*Dd;`&G}#;57#JLIO7Rbg|Ew9P4~ zZN>-Jyc%9k|H>vrUN7|-l4WhDR8)#Ym_4eWGn$jKTL>JzTeTWlBW5RK6Wp#Fzg{;!_iecH&nZ-3ElgvnW|X*%}(3z_UF>Ll4}v^+p&sAqs#qoSg= z{_I#lN0Xl>^2$UNd-xkx-0<|*-ciu3qO(}maDhbOSW5s@e%*z+`z|neSTeG(_J9o` z(h1bM3mBh3dWNXnULIugI@-m>r1RQZJ?DcLrh1f+6)vG!mhl;V&6Axc%4IC_x>Jtvskb(JKZ8--m5@ z)ftEspq|;aH3GnA(rJzfnp(-V&yK%evI5j+B}yB48896`O~=RxCHNM|&32huDJ&`Z zR>#fT`hAslC~$JFJ*PB0dj6x-{o#&^Z_T%Dbj$XJ`%$T_Z}j&Uv!C%nxsX8Na3VY0 zD+A6Crnt9JF&rv)==tkM?n}S6Y^yQ+ax#=EqZW?+1Zy4Z&Eeu$0kjk_1?gaOY7Xi> z;F%>S(dfI>9ExP=1MIWghg|}Vke%TvIZDD5qQV z-BeyDhop>>UiTY~fYto$;#UH(-CXZ4YPAvEjy^w(4wYkhLSfD|!b7K7jB)^2%jkPT zDs{R$81tCvk9;;J9RZaZeb>K;Dq~+LF5QEjF$tJcBKDy>N&gKS5_tKx@maU8v}kfLXE$d!FwsKNs5;JPDe;SDoL;rBQG=!_t;f|w z-b}yUQ?8))o@|aEIt{l+Bd?w8RfP9vQR*AgMv1Wy@(VBv-WK6snbPIt6S$r0T)eJ4 zwh>r?f*`(C7_!h;mVEn#VkqH(-9{kwMl%}&*ivWdmQJquU42M;p9EWa)i+D*B=;*& zZ>Sm$j*k8r88pxTI z7THz@NRLByP)b`MCUX%@4mw&Y#&vRb21$wp8V8XIZ4JMALXpZ@nO1-8~6yTVrySt-#EpD*k1)f()Wxmvn}>Rmj=a6;p*cs&hrKy#UEYm zAJ6X%!X8@P^A})(-&7hjSOS434p2Uv9Uwy?PshM8yS2r8IHr3OT?G?Au+P949t1_x z>+IxuTwEOFfBaLU{Xy-v=4f-h;BD}A(%^0`}n#&aJ`GErz>6QKu2sHdAXorA$ z$80ErWUy-uHd>~L(?j?-bq6$Po_ahwmXY&b-7%)5UE)&>7snpVv-xH)w9D^~#fg;7 z9cVj0sJ1>qFW&8gogg-HI8Hf`?T~o?n8(i=dxeNe0ZZ@lm-JEI^GCaIZ71mFQ(DL@ zU_O9bi*0gIfgb|#XG*#1mP4ZqQ;D&LwNPk`+KN`tD{0#Xk= zi*H}t3$+1X%ZW3Dq_ZEcw-x5Yjss*OWyyqXo3r7`$xc7)z5z=oM7Sv=c;Fa^ z^Kr&oi&E#;8;$I&5DzlcfVW{>nLF;!%g3I*)Nf|G=QepD&cj{;83?%FCzuh^dAA8LJOFe%WL@VHJIz|*wW-&$MaQg-r%Y+OJ3@*wy$on?SXQ)v`bWFo6uK!d^ zut|?VR*vznVoeUP%V5CipFbi{q13qJ7@sEsiFI&tx+t%#_SuHm)Y!A5;dBoUhOvXnWhdm_LqW_K_yW**3zIAXDuXF5P4LFshi^)hL24l71T+R$YQ@>c>wjPZugQs2fVhT4 z@)pE_#zy)Ao<*v!dBZQ?e8f8z>o%rZh1>vLJd%r$WtqO7=$`aX0PO(Yr)1%18n^uTdrlyh3%%Pk)&yi3rjo4ox(o8v>)wJJlD ze3p6(hJyZ;O8tM)V$w4ubY6M;`NQRsf>ROtSy9W=Qc)a-=O->fF^;r*`-6IH{!0w? zL>0bkRP7Zgtn~bif$^=JXi+ICWb@f-GlrkGXpKhvaHfZ}fa$nRO(CIjV*D1;3R0vC z#4G(`H9S`@Jl7+-To=*70(=)(25e8Cqh+=V^m-P-b7KR8Pe#QVPuMSN2$ZrbVEOmA zar&Z?LHI)%`vxJvQeB59;Kvw^EUm5%v1|HsdCF_2x7B)C>9M(F74Y#XpJPHlso__L ztqrMpJ2`r2a3JsRyS!1^i!jdsgEC+fjEeXVFyLmJ8!DLeI2(c+^WnR{M4l|{;vjC3 zO#MVBkWE27hs&&JMf4)C6^xVRjswHzPnTqjeVHV%A^+fDi0Qm*vx zJVOgA4Pwv`6u>ReDwzZtEIF1}R`5X5`TG3sVS%`dTET$jGzNdGN}UG6h-Z**h~m3I zumi)K0=++7}&xox8O z?g=L)1QZo3&AkKyNIvNUOQUlV8xquec2d~!y)Uq_;4grc5}Z~Yz&>H~FCBEIHtm2I zjpc`l!jO=V7LPb$@Up_3?F<;`Y7C8yGsbNCPIn-SgSZb@3I$6t70i72*#*1gVG%`& z!j^}#l;^IFP`wu2|9T);MyusxmL!o5ATd(15}o#6B$eQz)44g$iNQ|;`rG2fQLD3= zKRQ*B0$vTa=|7jEJbzErZ~1%R#EL{tjUun4REMVz$s%0!lU!el4o2;e0ozsJvw1hO z-p}tn``&akc~LbfyA*%a@RREWCn+p{qKCN_hS2n&0`>Z#CO|(`Ugf{}>MG15Z!h%H zVyHjHhN+MdEooW2D(YnTK%4=YuTu=52_xMXYEyB9|FMn-`VGcF$k_%~s&e`Bs-$uK ziUMf003NnR ztBZ{OQeQ05f&hl0hGD224AO?!YQ&m<&I=C+z&Syk9~pxP#`wIDt(jz9wY#RiJ_6cc zslI2|qvobhPQ-UT=TsApXWFiS9NNN50Jh_4f1+n+nme3e*ufB$xoD}`#Z+G-%L!g6 z`H>2x&CSupO-Fde#?9c6x|;pji{Q-jmul+Af1RXMpQGD{h7ARkosfFL&Ok&=9MHcf z+wcIozWlV59VxXTP47%5vSYmpp8u>{7di+NRP1+A+thV)9r$7##ZhC$ZMW@|9yvYa zw_)e^9(xx~9q4`Jo`R7^8axk(z$u5H)dpY>LscdwCZ3V-C#%Q~R(v2*3QNB#uV(M& zc2$~W3!|A}+7vm1xQ2B1lSN+qXirpL+2vSrv zwK`y2+8-cbx_U0Bd0q=BHL9tS(u4dBUVnZ|YbbtM4^oc`0FC?v7XNS$Wh_he?ztXJ zmw?^56V@62awzD}mb(tw9Q&w9F>&!g6Kd(BG2<9+0zk*>(Dpg?g!Bn+8X8#T$I3c{ zd8pOH-}Cqw84vC9dFm_FJE%vnNuVMSXS*n8sS}TE>izjzi7GahCArw(d(Hqi6=Ic$ zPlf#3?UaG))Y}UQVrkS$ElfM|-lM{3W!eyQ#o3RY&U9*t&(31kCAED=5Y4r!6^i2$ zv{@d+jND0wXg$JMfi1?LW|AS3$rm+314kRH79YO6WGtg{iTXu-jL|(zo*Rlj@K?FF zJ2yEYPi`vPpA=ix7WgV|abohtT7U8~`J!Uz1VclqA+2bMN?|I62Jx{5yT`Y3<%rs% z2tXY`>(c;GaEEt5K|mm8RDF-gjhcPvmAWg4PONAS_V?fPFGAxQrurXcsV}9+r{-Z? zcdTTgIgWmr2wG@vZ#}x?;8r^{NYwWiz|B`O9B#R zrPP2rw?m{(u%Vc85wz(hO(A`LtH+vW4q9QI?xAD(2y+o=V5L9;0BXK5Pmt%+e@b_( z$C$KlWTWr9>~4mU(MG;4E7SUlV3Q(=Mly;|kbwRI7Y@|uBG$<~k1wvo6TIExN6j@genqR8@(ZLg zFE3h<>)ODpg<`_0^6~&Eh_D)IPuRg+DvR2C@JHmmxrnaCBa(|XMG-G6>%LP|G#U{D zj)txw*gi`T$I|^d(6xiWLt>0zjMNCt-r&k_>O{t0XnK+a^URN|IGeEj}rQ9 z(9MRd3gSe7k~`FwQP3YS0?y$UHuS59YM-c4fa4t_0Hf`iF?P<fqW`U6_N|y0e&6kGZeEcn zkPE?aZ~ZYf6}lFpPJBEJ(R0U}&B*F0p9ndYQkS4~2w$wzSI&QR9XzcQpnO0hN#sd^ zT|+RvefUc{d)bQ{5EJG#ArS5WiGxObagOG2;O!TY3O9ClyX5A?KWvNOS*XugPe@U5 zjt}JhHE~&|nT@jxC>e~@*T75fsJ5AKOK)4-M*iH8z1eP?D#XQC`NWuwE;@+J)NJ23 zcKx!k3~T#Fr9}6!1jqKg$w{K=+@#on&rY{eD4`vy=)#1nU)>cC2Cx-#cJmV8z|(+K z03w|<2(1#{&mrWOmv6(&4bh?|gu@ ztHA^J8EZZRX$mx?G!305FjPQggdX;lF;yV1K=>dTe1ay$gl+#sFz_9ILU1qs>%?PY zx_DXUm!$7QT1aQdG~J+h03wym3CJJjppb7_Ubf=(W{WVLu}ZwGJm_ChyIKb?_yoHJ zgzaDB)*Xl$95-&Qjgax?7U@K!aB$Lzg{DQ>-zHKq!FmTP9X%DQGunG`VnooXr%LM!eJOpYClwTsllGJvd>C?SpUwIG~h9T!+ ziZb-7AqqfsLOGstD82(A2A~0ZkROyPDvk!s_5f;~n9cm*QQ@$KgT}Q8`IGHvU%5i< zHW*o2axp-G@fufx>+S$+k|6i)=$w!0ge+s*OexY~Zqj5@r|G3xdXr0+RIQ;jwBPZtiv}hHqYv9G0K02ZS6kcl(1@gru_Hch)PRt|mf@)!(TnZ)aP9`eA&qf9f$_dTuB>; zIt-DBDJmQvz+3fLsw(356s?xlTd3lRx4=RGnGwVRnPMInxGW%Q09tN<_^dVjp*XGT z>q+GfFa}L6Ag5_k2iaD^K6YU#WKrf^{bsL|h^qUy?LF{xDQ>W&SK1HqZ?C!&&Uc;! zG|9DT>dA0q-=G{|3SYe^EW(&$eE(JA>0U=BMTBYpYgp&;aR5Qm^%F!|6gyyl!DPJn z1!b*atxbl3PDqcy6A1x!M8e!5G;5e>B8!}7q_wnw6hfOwE<+833=ag~-;@e*5hseZ zP{$2`4C8o$S;p_UdO)Hg6}F+d2bHeKTmKP-JS1uG-UAZB@H*(^4OP7T`x2WOc=Vve zgBtiSXVCg|%)m7-Pln_oOxM5<)Q|7Y(Fk;~RU+}b5>^;F7~AH#k$$C2MJJT3@E&UK znFu$`E#wFxhQtB}!*)>tzt68wRsWYXQ06tnAwUkYf>+^4`y9)Z{gh7EX8Ri&}cW0F87?;2o35Uz3=1K-#G1eJpC z3=}EHyrxS4@bzK?`iIk-qr(I{>dquRS5j6S4h9G0R{a56iKQ$~)FSefH^c_a91V!G z*FTMwtG#p?5D?LQ9I=z5`aa9ScV*PiV!ydatRBxP`e=~-M>$LP-QR!2L3;UQH7aL*8Z(9jy2jvsk3`u@Ob z5b3tO{I{v9oo6k=F5m&PBJoxDu1t}yq-^fpW)2>Ro_I{#`^Eq-kvhibce50SbLB(R zmqmdvnLyO+(&_>ws^K;|`Q5%F8|(QkfH*fVY^t-8su})Q)J3vM80_wcUS})*+45hy zRc`ScTs{IRi`c-?FLNq5ysF24A450`5LQr$K_?gyz-_BS{1buRch6@PUy|GXc-0?E z@g0-mk4_2$FG!h5y(}LdNykz>Rc?_g=FC|7vF@T)sI8W(2|QPpY8;*h7;3;e0#8Ff z=Rjdkf+6}w%vVmYzD;m$!AK2&hRGX53c+kwFTi7Vto^B9r20UPc$H5>MO=qmg6OZ* zngxwys!F1l$@2#0N*A%-v9jdle;2rPL5An-fg!^u@wE z#YXX!b~{GvC0&xmbK4$Wr)~wO^al)4M`zQEW|RGR1G3BdMBgGA^d-a|&ILXWCA=Be znHBj$FXyGcjLi?1&E~5bC?rkX3;fCnk$qPi85<=jg+1%Z^*RRIIlos;LUUyQWi02t z_TmWF)NHP=X+U54sDEz#=2s#!Gi3oi6$>-v+Qvs*zVoKR3q3n$Iv(05Q-k5@Owfp3 zrQ}KniyOQeVb?=Ku43$D?|bZ93Y=J1`TFL|+8Lxir%GB{m`NS*VdRnvh57cO_z3}K zz)xOYux&p$f@lco7#ZRHTp`^2KQ^gyKj+msXk?QSdVDjxuj<|$TWV+08L@~sPdnJW z?d!9=UCLg?I)_KnZR0z>?^nO-(Cb(hZ?1xBAzz$+WpTTPz|Q*c*hJORL^Xa8?_qg` z*s>d872k$L{9fb7{f{jN&qvOw4WAbZp3SC({3xe`(J9M=L&lW~@j8}_H}EEj!A+Ia zB^Rum3vLryt`07?I3<19aq^{$k7CUMQn1E+4Z1Q1xEn|W1Y-gXlSeRgFF;ZfO*YDJ zIfZ5tXwX67rokrv8vZhk!XzCMYykD`2 zqv&@|Ns&FTkWzvxO^iaX^!=Q1LP_t>4oUQfrBXaqC_1zQRv#pK_!BEIe?>8mWu5-=&E zPmGEC5*dj+{l*@=efy{76^#Y!(bg8%z4d=R^5^YOayE{2E_m_BAkdqKYJI1nc=6ix z-6#6lkr6L_l}ooTm4eESt1 z=>?WLe;Ltm%^p8iqFWt_e%=GBhP@h4`Z{4a>x9Qgt$=`?1)xCB-5AqGBOYC|PKS?T zIxolucA%Z@WN7TJ*0a;_EU_8J(!YC?%9Db=0(>elyU@lr7GMC}D?m!6GRR6MQRUL#>0TMB@42UQdvIa!+I6u%7hPT{W}03mP_7l*GMmC`B=*xVbkXnCOiBH;n%~$3TNs1t$81{53chzR zN|U@%x`lDKg~@5xsdlHc;{muL3zJ*IX8~adx_21^(;}&%3LdZb78dD~OlML$XN1|f z@G&61f~*J(z!DINk0Key>8+LWx@di;b~T_CeFXXPXLG#8M^2t+KG<_}4T$R}VnNwZ%GoI9TAIUH|&5-P_Q-Ka~D!>`F z*gZUcgVd!7GY4JryymlwvM$G?qeaB@K<4G{F+_r_R4%j5@fq z>{Rd8|4Z>Q%@59kNPJ8RP^*O=)tKQMZGgiNx47u-?`!%?`b0IY*j$f$o@~DmifAk3 zcXDP@o6|eZB~R9u;#xpFGx=(DfTDO+56e>+nwJM~4nQ3~v1w9v7%)`ZxKuV4eR`?e zBepy#{(=NJVRExymF$Gv`i%K+{#Dxyl$4#Wp>K z3hl~CneU^_Db&>(#{1x52WAr}3H++UsvqJB73sZwEMQ7lI(-(8>|CF+m$8{Gr7n+6 z_MJlDxLmVOSaUA|cZN8_1AZEL7?4;EZqEF|6K;MtL2HweRKmvMhhvv&~vaRrm z-y~*-fefHxfuj)vK3G6iU{>VOq=;d~1#^Lx7JK;b=RYo$aRP^6CJPHR(&$-C-AN$2 z1?k5WBb;exT8RC%L>Hp-BVRluQGWbh`4MyPdmkE&kND*idV_Y8fs0DYWRVX&C-CnU zTFcnPwvXCS=*e-sm|piGnqr`M@=%_1*e*xqH|Lj1tX1$r{N#y>oG4ZV-3Zyn+8)f9 zD#W~ygPW`^wY=ac@n-RxF^=W+{~4FQ!Dv#UCScnvb-g8nOm;kiHotUse5&Y?uyl3+ z4q(3jcsCBNG8~uB4h|iW28)4kB_LDi z#PKWRPjrg@Mo~OwCBskM@oZ!Oy3P~Glpe{uGRqSsPdacHdW4wJ1ByY9UzkGZy1_b< ziHwW)@mLGg{p|d;74CK;L{h=1WMV9O;4Zl-*(~Wb|*q} zr!_IM{GY652CAQ=-T*#aHcO%(U*i^Gru5pj)v#L!{!oq`>Q^$#bR9vdMiD0{5pS*s z?l4~EBDDu{`DSQAm@u$G6HNrCQh0J$hy8&;9gK$Qd|RGRCG{b635E_T&R%9d;IlKe zwErHg<6^_zzmw8)a#F!ygOty=$gmz07ka|3Cfi{v_}}^2Df8K%B#Gx_r0nkk&vL&l zQ@t&~|8#2c>EO~04maAab*gp)auO{p&Tt8~bat88`79;k1z{r4KC%%qPn@KtqZ<6Ai zMhA#j-F4_HyQYwM&EI&_LKEB>K`j<_%-D8)R$cV|G{l$7wemf~#wmEx=DFLr_T7f%*zOE+6zL;ifcP*E(iVTtqy5bsbQL7Qo{0_rUM zffAgSsRsI;G&;Zyz~opf&~f>ICs_qrd)!WaRh;-ya|Ir*|I5Rr*KqG$nlUrJ&1=WV zSgty!w6ahNZw-Wl&&;Oi8TXXYaq%zj$_bt|h7q+kZ znz-u>skV4;uJ7GP6E-$8|1i0LrAYQxRqjoH`}PaA0HJyjuj6z-C6)_%WWG{DB7|Q= zH>uj{d~pgizN``!t_v)L?|NDl&mb+V%)a85zC}A?3#UchLdjGG*VpgPk(}WiFvFUZ{ZVx{I9gjS=O|@fz z4NuP319xjuu5`Q1)T!=RgvUN0x%jy(Fu)Y=e9MK(*P}6DlPKbi9QN$ipDs8JL!pdO3XvfR zDVc{z$dpQm5Sc3^BJ=zn`}TXDwcfS9@B0hBz3vscr@i-eUFUTU$8nqz;PZe3lzJk0 z9v(h089U=%Zcpm*=Pol&?v=K$uxd(b)#L5qd3(|`{_SUvrKs0ElLh;1&s^Qwv%#a7 zoO5|7=}7n0U;{Y)SO3UbhJ_a!2m7@@NNyabs?a^1`?7hO1sV1G?pA-pb}~{j9afV~ z*&Y>=6dpuKz40ehDN6-rXi}!jNxMGor2QnZtYSEOpi-0fJ?pS9jn+j*sp0XRhg9 z+MV5L3iZuL&vs4Zy&PzHG&L1n5Ny|}T5`3dcQ#F>;w4R#PQuqaj|(n+nY!!Z)W);e z(j&pdbL?>7_Oj0Xjv-d}5(m?a0#h8FzSy3#SzR&;yP|(&H)}<(<+`3NKPG1Q!wMH+ zbUD(+rWEz13G7B{kt;Qc{bA8mfJt>fro6h(Or*N^ok;qG}tUAxs`YPm1qi1}V!dZXZ z`8`=Az@Tlc-Q3F_b0aK}H$iZny6QebW)9ZFH(#kQyXFeLwYz<5tE89di+c_5=^6ar zOgM({lb%Q4jc(!#h*G1_IjnZ-H0yrpWJ_7>vR|C3Sha3SzcC0HB%S3MdQ0U|MiFO5 znq(iBJ?^nHAYP%(6*V?Gb%I1ZR5zg3t4dAIcE9LKkkFZnY)LSYJyQUf2G)>YmmS z3R1e*w%7oEyR=63VJ}v(c0cAK1Md&2so%BlxUNu1W?pP#p*lh!yTcx{Fpg=*esIxM z`7`sPw|?#{TYb%>+=085Hz^4ExqyHNCn4xym14u$_1Q|ovTm0|JndU=)eRD_UZ~f) zI-Iv`KcRL{`$)LPkoxA$mW9>Z)FSzVD66XX@2}n>l4;Z#t;)jkm3=6kGSN)1S)V&d zQLX<#D~EX;Wod(tJ9Tuek0Wfna+gA|Q|+Vks0<>aZoVK=+!FriH|&!`rsr6s-$?c1OZDV_(Bz#fl45@$-ur zvlK%sv{zPXrxYmSza|`GE&oB}WVR>l)xK{o68aup6*P?F~E zEgOMBYRdg*xT(Uaxb{pcko)opor_2F$lwQ7d4aDPC!{DnZtLdI)xox^Nl zi_JS`6sFtQTn-&Bd0oSh-TQIeg(xz2v%U%Su(y3p{iJLrQ8-AGz{moIC3QESHA8|} zdjY)R{G=Q~frOCW&Qhkg^6&dQm&Gmz(zD%LiLuag=j`3*)P{Df?AAx#+T6UuHyk!= zE~=DjwsiT>vP!fb)t}?#Zz;ZS?#**zw8-d;&26*}lzB+@T2KqVWGW>19|QkaH4OSS z|J|%jY$0OfegM;Y4k+Wo9Zv&w=1NoYnydRy@XgKxgo_v%a0m_EF7c7TFQc>R-R ziCgZij+W`)w6!Z@GKGI>d9=qabvWO#_#+iR^@Bls@-dQ`zG~U_<*!H=pDOzIk1w(b zZF6%fnS3?6Z#Z|)_mzMK-Dtn|_XB_Wn$|_-835Xhq?K(9k%emdP9{5tCv)%qfb@Bd zl!idkIAPWWJV=xa4B3{lL8}8B7UwE0cuO8}-Pd7Gm$T!4NAYHN?_+{_`k3PLw4+Yi z^~EAZ&;DeQ`4kS1nx*SM_Sb06?$2r5`$}tdapOA8ZbxO7i)Cf;?j}rm6zv-(W>_2r z$vCbpcWgBT1Y#19E(5_-+VQI?A*f@_O^l1pj&nLbPdB6IRZQDC}o^?1aDE4+p&UBvch8#hDp1tD|J!8Fm zWgYL%?!K$4>MOYZgmWJ$YcWuAWPiEBaB9_e8K3Ua2RCKLILQ8t%wx01I{L1C1zTz% zP;i9sG0z7i75#q2LQGMnwRro!Ab--aljj(c_{(2M_&thj*&K17>){ahe0xadBhX+P zZ}YE{WM?oNO`hmdUf;v_BP%L+d&-OOXPp&v4ZpQF44s)`w48}rsTZLdV_r~`yfC$L z!uY5?H$%4b;=$*9&1`xDcUM(3^68GfcW+kMd-gqO2nmB{4m-3gRwc5C2MTI5N{2qD z5b!vKaEdKZ?3u824AE_ZNHbiNr~jBkNCtqTB)WtgKGM8F0&T>~Pr4Ag7Br(m^V~tMi5L6??IEjms#0tRW@X$>BG^*)RzQlaR1iZA`xM?%q*xEL3ADVTf*OW~^@dwYPYE%395R>BuRxO~%ptf~*0fU==Oj{Q;l* zRMgBtJdv{{Jq{(1;30H1>)#){K+TTsQG8Y8r2=hBsd^mOnT z4>!*hQV}L;ZM1WQ`O`l(a=GiG{Y{?mUb5IA5iFAYbfK%Zba(GriWv!|zpJ$0 zMM)9Oy-km)l}=FDQLs$=n>?)tb>E@$td04VXhnbS)Gq=_`r{4CUQLQjtNGgb!oS>k zx6~ThtOCv=aL?z}q!S%6OlvC*fvx6g^RzwGtUkFm*k9c(&h8y?s}z9^Q%Br$8BT~M*D z-nP3AE;S-tP6<)i#&RQSK4u8I7CMxd0ZZ=_ad|}8Qh|l7M^6zZP1;CR zC}n%fE|&c_zW4JdlzT5;+;Hg8T=`Xbl1y(m=~vmTp5@}lPfyNXp?r7te z`)PQitLagI8>Kg!PG4kg+G|a&RS+?hqbtG1ca2VYeaU2`+?6OWk$1I#$PmI>88hfd zh6mXM{Qfs8b_rq{df0LGjv?kOxo*ZrBb?*6e1_87Ltj->k2d}{rOj-FMQel4G^^5I zFO%)fN5fzit<}}FMrzX@bTVmV^vMutRRvz5ZJcsWXLRjvpX+bI@XDz0_8gj}A+6)z zN!mf>;dQ1Bf>T*y_9A+=&zgsyF{y53_5ZnPNMiSHPP^JkxwSQn^dxvfrS^=ujznh> zkzsRk0*;8GagaxtL+4_ub_)kX>}K}kwO9UFTejELu)^4XF(;LDO86O-&7^Ak<^LZ0 zlCn`+DTv}$&NY*~d$eurCer($8%88m#wS%(Ukqfr=7&&6Bml^7`Jy}H8F&P19J4;Z zEDJ78I>IOFT1yM5+*7TBJFP#KxhQ#=Ii>bkfL7z+uRrVGz>g7rSFp+C$nw6Agq@HS7Iz*jA^RB z_PeRb`z!k)@0;JPRBw2DTPX&ava~5OOMbulqQ}3J!jUKZK;rb7d8%aG{=OFI)nOBU zd@MM?Ky-5`k?lw>S(DNr2oj|x#{sVbm_xIf$&@|fJU5INTOdDMpyXCfxWMEn#zyU& zhTn}t{+6ernz9`XXGZy*(g=~a?6CQsvvS*n=zRo(-dDN4`F?EMR-Wj8wJuJKmZyD8 zXy5!g+4;2NjGBr=d~(eGxAm{vsCfR=Mi1WYTm^QpBFfo5D72%ou^?qfmC&o^g7WLs zq{~}QGWxt1sBsx6-tEwN1gSJKdEBH(ORKr{q9-@J3nNa=WA&+b}sDua6NJ4G5Ig*VpTQzBghnxpj-%hcjwADDnZR3VD%pj}9vl?tQF>vjP-W zSXs3y++>5!opVVU*WJPDd{|jFeOs&~OA~EoaX!T?WtJkSl4T|){^U!tMiMBV+u|`tmaC) z?m1e$bGu{KSw^n*-O~IH2kPXH3TZIv1HaY!v=z^G^mJZv(mOg9(B=GB=>F~Xm0b#9 z_EtlNp|Wnio?PE*8l_6we)UESj@J4S_h0Al*U~`%1fI@#w<>gZuCjJ=x79*Etz)pW z#8dMG#cvt@3~g3l-aDWxf!q4k?zjji^DBwmiyBg7TfXV+-p~M}S`- zt0J3NUfvoh#`X;}Nj9lFr`~eiz8M#{6*ZML^L23|6L!;B#Pr_1&=&!=v3ui)jJrUB zlFUV=qS-X&_Gc@`mKW{GH(Z0NhEXXfvv}v!drSt@WHSs*u7#gwS0mR2aw6@6z7;Tc z797^*fPNnVj)rjflO*I8l{@mzW;!|ax4>?e5P}1ETq)^mO96|Fz@LGSNJcKH7Rp?p zhJO*C93c86I)tCqR>|jTY}li$+h5b@gfh0vxW-93)zo^Wim-i=+KS3KKYf{E+ulWg z$xP=%u1WGqVG*r$G2`v;y04BB+Wwk2Zv@ZRyEvP==T3*tLg$NctHjmaxyb$`_y z@w2FMlMm4he9hbI0k?QK`gWR3IJ+<1HTiN!s(AlJ&#AVt%}sGet&PRGCtAxFOb70M zZFkL~E!gMEoGpTze%N7dK6gtjgzoYRqP!p1OmF{@{jW1VKWyXd)emlWc%mYF-uA4D zs*~vQ;~``TZw+P;7a{ua!#lNXb-{n;BOh&IZo@e>TECy$AM05r_PEI*%yV$wfYt+J z&tGNRm3JRqO`&-NIO5ah;g=qTD#Ji8K#&32{J<-}8i78Ba<-vgdOC~RJ>rrDl1v^e z+`|nxc3SYd zqdZ$w4@4*A13-Rzwy7Gu-~iQ%vvJu%hh_Kh*!d?Sl>s)B0ZH`OWr4jZDPgR2R>^qw z^T2vs8dGI^ds4J*41ww+&W{lTLJu_u2&vGheS2J%O^ggL6`}H)ryC?D@M@R(5&d&k zvRBkLt9N~hfTO(QmSq3I0k(79X|)%-kgtiPe&U^#L95kGjf}&dXcqV2Hpk#Fc)Jo(%N1K<9@ez<@zsXy3z^f*LTmK>r;9gCwqd z2L{C>eXb40H{*fm@ssBno~PZup?e3ir!j{ae6RbbOU;%tG)$zFT34M4f{}^n2MZbW(aDp6fy#g}V$Z#&&JP3)XRvsF z16ltOYI7R*Rr1%+&lpT>(r~~o4fa!Yu*VP5>`yWR_sKGwo^)+>#q&R8BB6c+_TClK zJLxlR17`*&17X0S>2l?@MXTyYqM$)-pZD|fK_MbR5^!6RC_dV6vleczQ@saGt1(p) zNtq(~Kw|Quzf_ZA`3>o9C6Mr{GO zpcB3tE_!6I889BK6JhK67eE()EJomgOV45_u{TTJr?rt_cuoy(G|n(clabb{92?sW zG%$ANZ5m&UH<2Z74Q`S>Tt-dlH?t;&Tit|)HdsM}w9Of+Kfe0Z3KE69|S%4=wgj!Y{ zQOmm0v#_j&UmA3UQU+qXq;s58ECNU*yY&I9tc?W*T^l{l0nIx!M6b9qw;e2qw0-Ak z>vBQ>*~C8jV{)Eu7r~x5x93q}0EYpztHFIE5aW;y2vxr(w*$o%fU2acg(58T2<{7n zKnk6D5Ru-o1K$=*Bgi!nwCSa#8;*)$K!}1;2V8Zutdyp*k-k2N3Iy1ej*d=}X+=v5 z=PSKVf(KSCKRm<+2}FU7ZQBAV&$Pvcx5R|ctb2&!mLf>z;{G!I=Rc|k)IU^+iEJ@8 zK(NV&{GZF;)nu>F5iGH-gA8OBd778m>1xfaW*uerUy6rn*=loCXt1~m+UL`<`lwmt zh+(S+OQa9z?!)=Uu)rNdyOq!ckc3&Vdh#^Y695}G19*O%$uqpTLo}+Kuxm* zSGVrzlL9mGz+FN<1)_7!(wjV14ZuwYpwL|Olcet?^ct}$q_sGuZ7=4CJv9`2mm@|n ziGagKcsG^+lD7`v^pF*SN5?*@rgJ8^hhnASw&Q=~1rTr7}lY?(@t{5f__Ir2j0=U1um`|9V7dUs4fLJND`BA^0ib(@b=pC3w~^*N%NQ09wLY@c zo*>9r1P=)ug_G4uz;j}5Kw1O7>tX$`<{25Tz1sU0Kt1S_@SbOc>|UB17H#k*iE)sf z?wlZOD69M?vI}brLyD|_f^=4F=)k{}z?}0vo?r6z)f_n+s&H6^iFBsC^1*{|VoOu4 zSi*e@5)Tw&wiD^hV8O4}ngO9V?0FAvlPPF8t{I?pX2hY!FFJ9mZKIxxPXX0?hLhJY zQP1oiz^aHz!6bpFgq29fh(9fG9O&<-{M%hx3A_?;$|{ZvBH7@%ZP~H~ag3Z{2WTw7 z^kQNI35IuJ7irW}Twn-;+y$5#05>6k%s(D7Z%<(q>5n@A00NNc01eZC@q+Q^IZfaX z05sA6+F0A+yoK;5J00_kGZN*KjOOfvs|TC`15RoiLC;FesrCZ-(x6Hmdof9>%

2|zTuc=P5=T2>o~vR+>66BT`yW=2ik zcrMOV%v$kcv!R+HG(uo$2naMBJ?q@p+08ovv%!tEiQ!By8YahUK*?hafVRx}_9y*n zs7y$TEm&)J&KrTwaTt*tc4K`93C!3+@5TLa88wBdH16}B^Q<hcmw~ z+)#Px?WFR-;m@*8!|Yhfy&9SPms3q44B3xb*<%a&E*2r(x1113ngUJZ`$R)r2W?7W z_JdPBqq9Fxd5cSVmO&n;btgyeb3g_00Ad2XuB>jId}B4yhbm&6z7||iA#Bp4^6DWp z<2*cqFoyQ7phN1DHzKKCsr|$~;MNH`%l@1^dj7^&WZN--FMzjXB6a zC<(a7|oz0x!>+puBIT^tZ8O|z}PoX z8}`JjW{s%`J-68XPT<`=Vk1&oF=RE~JcG*&R6&CMJyz1kU(9O{rj7 z;CblGa0XTlXjMEHUWWt(V(3e-vILjA^AFlF96ID{8-qJ`ZE;5Y?dr;6V)%UfmRg4U zyS!w<*JdIIm5BKjJQ8?SVDd5Vk+@iX{qmG3ydZgieABPqkue0x7Mev=kO2XfSL-P~ z|J>_kv(*vrK7u#2L2=0k-hSF3YppXl=O8JJx0S7M5ONA2xjhty{x_&qAY2kGfwr}K zvX;d)xT1fXgd$Gh-`QOBo2WQnT_$ktHg!{}w}Pt5C!QjUezub>M}y)Z=%(P|)7W>3ZQlmV4kjXQq_2nopG&c_#(m^D zzycuzT=7ywICjQaeO3m$i5Adu4S?LEPZW`WIPw&9N9A|6-@v&CARmhp4_tri`a_T; zz$8Ujq_S9a7jYauo!AUPt4=3m`!K=q^-u-p!34}Ubog3wgy%@JG$8wgei&33gOYmD zl&nQM2uXZ+dq9re;y1@+#9C;S9#w95h?FX-S$Oer(sE>>ji3J+Rwut4kty`+gcah?%$~NC%nKWl5aXyg>s8 zz@{SHIOyP|*n$)eCx);fn3)8Ye`MiHuq8-~P%E07sO9q=o+<7c)9r{7WWej@2uXHC zSy(y&{g|C1^pr>|i9RF*1J`J}i_h3YFf7Zg>#&22 zcbKDkZTlv8$6z@uE%&U-1F0tz;og{J2!a^MS>2`A?+_ZAgo8l%Asg-s!neV8*L#Q0 zOGzOC;p&Fgt3>3PdYc?{A4-DISV-@*+d2tGhBK!-(+Y}*VhVllzBm@(KJZ03S2`b4 za(pu7)$@JFb?uKSzG2l{L?yGUbG24IAk@z4?f}pCgvj#P#xRXqy>N;hbb>3?s@;_L zEAp#&0~5Aj4+7K(`Ai_nA=mKy)bA5^nr+j)De0h(p}=-E=mb1F!XKl8P|O3J)MkWb z0HZ?twF=t>p-%%TVjJnm90?@d*EYg(xEQDn@OjFrdpjaHzlE(0zN>KtE#!AlOVDDK zd2hDiRp+P*d$oCeHY5|kbtE^4yVY`s5F^@mE#XK8_aIq07!)Q4f@1^f>ec%eh?_we zugK7SF8ABnlm_&ahC%R^5IqwWui-1d>?+N?M?rGB#ZMhr0;zp`tPVS0zRmGi4Hjyo zo-kqr$y~mS2;Ue?yh~L;P6Ce`s}pc|yaA_jZVyAHB&VqKeBcjo#@A#*;Z#5@upibP zn5>p+%KL)EBEs|_3=LYtfloX&ElHm9P=Z59?uJeuqUjZr{nv8i-nnLk_I&=+gD13u z=;_xba@_o(NZC~Ala^Vm*KnV#?g8e)Ehw9_(_p`V34`&+&BQ)~-|-g!InOo0h^*m( z5)&_j7YLOqkatkvc-lGo&iS!_u&VDD#7iul5Z<)SuF&oR$sT$S@OV&&c(6RpC=Qnh z4hOyl>kIiID2ajhP;vbMl%p{SD!+f(8yfS-4b3m68BJ`#JJ4$-Qk0LQ2VT(4ZCR@EMTwuSxM910?nU03POpk z3kwn~WJ1fcv(Rbc-_PSeF24qHKgE{t4}biiCdA0TNdNJ$DgVomU0Hc}sc?0s2i@sG zWiVtRZ*UveZtj8x6@OCQoX39O_@9YZ#zT*mGt0t`-aR(_DV8O6o7As*HfEm5h2H#+ z>U`oN+h{*Uotpf9u!GCx(~*=Z?sYz_Lgiuu4kv zU&#yH(re6J)=N2+IW;S{0j3q^5Zijif<9cy<~(!L80D4ChA>EofA9cD5jO#XHzI-0 zkw9X?h!NU*xJcXtgSQ1lLgYL*hLTXrMGp|dK!BQ^_gsLkXBSj2TwOU{ZWt^(TKr1e zln1~mewv><7niOag#y(&%ZAW|&mx?`#LeZWR~ecPTbfNjvm5asCIjJyVxHjp@U6H@ z>a(E>uLb@oM!>woG9npuY0J(lmP~WYH%2oVZDNEFR)92vIP2DP*L_;C(QiO{La|mo zZehg0;DtWawRM$y%>!pD};`}^_>8)h#u?%c~!iMyM^CG)-Pd3BciI=kkDH|A1e*SfcJ zR=#1q`b*?CjakZ#BltQ0fIWQa&Wct&NuZ6!XeU`ynhq1{#n=Z!TpQe^a-w^N;>twW zi?Pq8Z_>Iik^u&H-{n)xk!3e?P$CJIaY} zo19In%iflhJ3qf^(6GhCiob*Y7VXd>rKv;w|2PsIDFjm4Ok}(->FGRpwqEy@FR}%& zLS&I22oYfWGw6DDeW}V2KC0q!v*Nb66U5F7M+-%Z7h}Bj$fv!}2>~DS36eIDW`Xt> zgDKY}0wSC(a_EF`*2s)V(B;8MvP?YTU))eA`(3GQFUT847bvCGd@o zG!dBwZbAQ@&vN7%N+Q5D>FbeL;8;gP>{8(C!UJh^J&{e6nDXe!;j$ya)nW?|9GR=l zEVvh7lU4w_jku+L+XK<3RkzD2WqN}GA8Rzn00)gpd+#CF=2 zrow7ExAs+azx50*TP!Ivag92{rp5{>kUCn^HdL&!Uc$Ld-0U2_ECl3M2>d^GddxBS9gL%~vKT11O5(lU;R^T#e$l)!V$m&nZ8Gwkdwp6 zn~S!nk()yp7`&Nfw8RR`JX`uMVP$G3LO63pVyy_vk#Ef?17)JRKtZ$4lHzJnSezz% z+2aS!SBxq(o>O!_x>_%h#KG3$wMT4$Q+yFMc(E`UD)G>*2$aH&=|c$)tKJm~*63zTWX^JtfDx zVIBG)t|#OoKyW7%p`K^!V*^3#38^c{+H&A*3FQwi+Ef-IJ!?$4cIAP$NIGocT3i6D zAs&%3y(x5j$Y}lMtsBt*0fi>tE^^v!i^kLB-uvx2OX)^Y6SlH>=docXRIQvIzTd1!g~XZbDO~SsHbB zs9mDUBD@X`G~m?(2khev|Fu2PVW+{epM$3T1QLFPyPNZ7#rw+!p_-)%Q($C1?t$|L zHxIeaWnQMqjTR6ei#(5_!nr+xAd)tO3ux$3Qq3V0VAHARq^#jdUX0#0Dc^?x;3$M<>H+@-T>ue+7=U(Zv8O;OkB zDAiFkW%|62^*SbYnVzUa{P~9LJ0eeP3HTRt9b*sIx)usf(Bk+#y^vmV@P64VJU+p_ zCPwerGa0wd88;L*pwtRAHt=;Jo70c{FJ{3gU8W~?lVOM*oX<5vqO{5~+kC5-zJ9`~ zoWm@B=Alo^Ru{{T!Z<waAa(p^_k=I8QQ-*UHL$IXaWPK)r@^I z9K?JvPH^=wni9n7)4^O0=t7X^5rOGw>XgOfEdUTB)lhrk3gAA zI6N>Z03Pg1hP?H4Qi`R0sTH(6Q6N||ompfkf^UjX&|vfNQr_#JqY0{XXu)o3WqLL% z3>$(2Ou#l*h>>6(FEM3QE*@>Tqp0>m>?7hoq65UaJrylMa7wTXV4sJ7yvxkwi}s3L z3~ZmT$muddTxWE28z=?MdHQ??L`<>M)@AiLdX2@G2D~yq&c@nw?oGGG(=S7bU;gwb zeW^PC%wM-O)VPH1i?Y-TYG(BBUL25f<%$QB;pD}{0dpkMD^F{?}8K`LXI5qoC<6|9)R3n;-ok0 zN1k`L{ToVFn@8w?8@vFfBlD%V*TPkM`(4867jlh|UIddc^zG117<8Mi2?=qqqzu!=lppQ@ zBBkWpfezD+;9X3{3hfpZe_$tm%0%AKtxVX!eztMUD9ho3>YV*da{K8LBS5>Q)rxyW zoNDr#-nTSI7g!f7{*^V_$fkE|-FqXYpy^fd0ILGUCr;6k?6DKknl_hHtgeI|vMAYD zuy?6^+)b7F>NZM5UwGU zMbzA7<9g-C!OLU_4}EOC#v7)87|_;usRU zZhDfyPAjwA@{2Yr*8TblO8%wte-+ z!pJvqM3lW}1T=Iny_#&x1;&zya@f})(Qog4cRv%={0#;50e_-}p>2n6{2{*k!a+DO z%)|{{5Y+3-i-rN(Rp8}a3P;XAS!55eGb!)e>#8g|){E8pLmvJ63VYDq&N9V(w>`?A zPKMv*Zq(=Ik>!+m^nf~Pm5Ix2I@oGJSF5}Ikjp<#C(AR6oOUjEy%_o(DJI;)kA^P} zOX-a8DcrI{6w=f(qiS1ARsg4Fm^N=IS9HmCkjI-Jj?ihY^p#$5)ZOgv}(AD zuDhdP^`` z(KAW$=Qk_%!%In{LT_fhEHJ+rA<8IxdtPxb6m+irT@vA3e*TBIL^5SW79|D55w~o4 z2RWb(hecu|U7Y>A9+LyXfF&Fagga*g6hoFMvtMO8Ef}$@(cfaxJR1G+=Qu?3i1fO# z_=985dbdDa>(=CAQi7Jo{agEJ0hPm4A(OU`;1@6#vD`oZ9WgZA64?M+n)mEgIATwN z>PTpd&arf{Qx%!0(au73p-Qu%a|E*Pd-_%6kB5v*R?FUCIa&*92ncHUrN5kOi`}$T zHf)bw328&P^eD6EBGGRzbF-^41Z6_bN|bhf z@pYZvqay;WsPfQ>yp>#%{waH6Vt7(7)a%$s?E@$tAk_lz4%v(&Pn`#Ie=By6Ok1yT z^UAd?mK}!>5%wz>Q7E$FU&6T?jYSY}7&^_Zj8hF0u?2q>j3Plq_AT4^Zd@+}3nEW8 zc0P)pC>AwQMF@&S+kH$p@v#ijAIOYgx-tpx-^`2xrul++q9Q-?iism0;S2D#Fpapl zwu;@KR}0sQGq!sK^;2EW`5Pu)c=rtp$Tbn0JOTn?)pc#;}6AGUCj!OqvusZ~g#9_? z1J90o>fgCfja`w&=vS1d_uN*Lk3{oFpESj5ANt#_`3^sI29gcC7SW5Q2+wqAht*BK zIaJ*=Xr0wwbWub=ndJa~Xpi^X_+8#T%YNg+DYj6@Etjr;KFXZZwB)j^SIH=IZG|6b z2hq7sa`#DL>eY1jgaj|{Uz_Ncb`9M5t9Nb1Sb0EKH8{C6V8~sMNF81Nb&kDZl<=cs zZxZRlq-PW3*N+hex&8gIlhC&cZT^&NdiBTgzPy`bcUIhz+4=3by50x0-CeDjsx3Vj ze`v(IMV$YihdEzBjTIQqNouT0D}+u#}(-KSIF6n81PxB$YgtX`H8# z)F!3*^lqTn5^mKJ{~Vh3!TqV(o`S6X!F`mLO9>R7Q8<&111r*|L|{xD;u{DbPxa$v z9`+KG@ki)3>tWu#uL?N<>+V;F6wd!N_A#aSE<>7_kx8k)?)WLUEMkVvg|nq5?^zke z_rcdG8(1A>fSCa*Fi_Qyi%1M1>|i6pj(|wVOLp%beL2aJeQpnm+ z{=2k)dEZpn_1U9>^wwI+p&nx`GE0lO4p&uF92|Hsx;4)})r|g4%j+x}+#_%VAlm~( zQ$Ee+$BX%7mMYWI9GP$*>H{H9R>`tn6E~*q?(jz+Y{T( zsoKy)!eP8;4X}dic#q7M$S?OvjuCyrX$I`m!WhJF95!{)WPXb}l>Np6q}#^rY2&XC zt_0MMdG?ek?s)AwcyKRpNCe}Ir+X|fx#O{K6DCma&T5a+?(dn?ZIHB%BhD5_(he)# z|2{SFe`qx>JFCH|vEU-3x$u2M6lC~Jp7hEsd|y>dfM4|L{fej!D^@90EtJHdsye11 zR#2=F%C8ZuqGk?fQaVAUOBrtIW3%)&;T27hWBEmr0Zo<@so}9ziRNwArr*f7Kuf^j z#lG1XOVBblhXM;6AdqSC-)#^gPtTzVb!R1U%6m?v0ehvMksdly1MMFnb5qK=Fr~>0 ziSr4hPx`YUGVexC6rSAK%m!0-$lV+2k>>z$bwA0dXq#_i$z{Bv+hoQeEA}b3pe?Ve z9cYcBty1fRhrPCmEo@C31rjvz9K{aTl0)nHuTmUrD${UQx=S@WyDY%{XR$Lb=G5ev z;t|y;CB(f@&Vd(E*cn&CPdgQCiJTib;gR71#yfGuv#*_fU!6$#HDXe^I#7HJVX=0hd*KQDcul(ms$<;Qa&m*n8kanhZ7`?EG@PgE7S7 zC#F3VO|hxuFx|e7Mg}yq`=yX43e)i{ZZ2R75KSDVn((F#IlY_|C24sq%Y6JaCH3%g8&jGz@fRgP<_n#<&gozn5qDvWhdGF^>(hUVR;mp za2b)KC*rJ)Z+>Jv&4lWnTy@s<*~9FWwrG%@i=23Q!zHX2Rdie3@>tIW=7!Q#VA_&`S-_jI8?*7awzvUne;R{+O zHA&ouS4$hic8 ziDPKncv%5{^1dswT@rehAtSoYtur&bnXB9D;egKsZR5pJteG9s@nDqJ#u6u^6T-~f^Of+{a!O3V`Tm|tfcVeR1` zp>&6I2IxRKKfF`c?7TenU3TJr_%|lBYJb4Zm}S$9_+k*f(jJ$>37|{Y7}x?&j?_GB zm)msdx{tk9`x6nl%reI|>c=O_GO}xb6@=ZQd=IccZgA6OwD$nw+Af~}UO?;{l1*%= z5sVc;m=VQqU^$`EbkLQ|t$=T}wys5}m5z}vgFU2w)!MU2zC4}eMm3w_`}pA?DW1kq zKk@py0~y1atgcF3W7>uEwhI90{_1t-CD6r0dp;EHEAj#@5Y!=0NPxkbhM4Sy)~2V9 z4S53=_vI(uqE*jEC}T-EF6(pkP-tt;M$7W92kpMtLo#e-4y$~+u>a;5L*i(+las*- zo~#c(?#%73<+(@se3kE0*mU~DY%Q(fVbxJb;R9v;qeqVtbzt=82to4aq$411s3;@p z14uKGeE7?|w$DVYEqGlGpEJ}gn|xfU4@_o=PyT3kO|Q@dK$8`b44FwEPWaal*~k%tHV4cMc;r zUb}fORF8=?&M`L1zb!HN_I@Cf@-kCn0V#v@o51tU&u(#YwdB@c16L`@52 zD`b(Mbev+RSu45_6+wh-=N_%vlBvk5Q;lq=&$Yz#lqd&* z4MKWn7c#ws(Q9T(puSsei9nYSRfaYq(QmypmTY5H8`jYB#CJcSY;jcTD$@$qUqG44 zcO<|ctFeJ6s%3LqP1D8f4RZCXac8cu3_UAkZZ%s}PdYkxMM4d~946J#d>x z$#c&i(Ozj%$=3v0shO=T%e;*nXoD=*_vTSdQreVKZC0e5_NMv7*8BPX`|yS8f;qmM zLv21bFFi5?ZK$v}lXb$Zh@PfUvsZ&TM6P$>00N6Eon(!RezdKqQCzB2%;NuV*xr+! zTw@_+5kNyVl%_~&6=`ZCVUQ`%Iy)BoKX>L9|MTB_hWMYm)r3p z-K?N8H%3{HoV+FbMOTC^Ch%X=p``hsO)e|g=vWI@B0^C%9=f;rpH4>qy#oM zOv(Y6hExFB+#-a=zp!^uv|Q`dL}aJFBEiH2FFz)w52-O?mLXP$1=$w#xceb_{tro! znU+&5)LR?RQjYtKvGRA5?$VT>&HGvZD{sLztNUQ@^8DBrORp!p=YD_ce#H(smHXZs zY)mu*S({D)1@O(GADWja?GpLowd5T+i*DQjqlK(bIl57VuCDZ~9wN{IlZwQqii4MD z^*FYGp>6a6{AoV-=`2@y_^W+pyeV7t6?eGqoMwld-3{)-l=f2Z?>((Z_?MQqj~9IZ z#%4LYdD2V!k^9pHsqer1YSown-q0rXPzLgm23W!c3IpW@JYE6hLM9RcT(p+wtjIu& zycQ|rmnNG;1j~Wh=Q*kXkjKIU z6FHPswPOd}w>$ZU6?Qs;On^KF1`dM-p^jcRWcWd(pc_pEyi|}4nI*E+F;Tb#3jrC* z1(yFD5)-m5mKUaAVORP_+rLT`KB=@=yV7Co=KeLVQ}b4PLj0bEbpc*ff~!x;;-j~i zxi)}+i)JC_qnU)tQmQW*91CO+xoiPtTQY^}kb3ZZUG~MoMe}8#%#dtF6`0ViM4}(b z=_{s#oBwmZApFbK-gZBTL}KaDP*pw!N9-`0D#_A&v)VzL0I70U(wjy3eZP!~Hu}cw z*hcH8X;%+|Vq4gb9aUq~R0Dh^lSTCOZ2U(_2dS!7sgqY20^U$e#kqHL2y~buMTBxV ziupuuKKLGhi$HkyV!`s<$o>yGO#1prpJ0^8TI&P^j!e0Dq2RD)vB4hSF%VZE^oW#-cku=A@+c>Kf&;bl(a4 znM!j_h$L}N*B+GW(jTqkBX5Qova^@D^0%_GO zSw#-(Ei-zScC_Kk&xIbnxpo3IVTFWRv;qJm8a4`sy`<}`YnzS#61xF1)yQ%oxJhkx z&l+6=Y+*j2EJEnzgKtHaA}f~+Njymu-f)bN8S+W`>smJ-?-6?X&nK7p{p%I2Itr+c zoTf;nDjM~=^~sUy(mN~zxeb5{RzyJ+nxB=EqRM9V#RF2FfcYPanipQqnw^w_p3j_E zGw$-!BR`0ym8dGCPz)y=)zvkG1B$JY=PPnv>R*lG4dka+))kAWD=!#@bm2Lpx49=k zH$hl7ycd*~X?j<-n?sf`V%7Wl zY-?vhehS6h)Af-%68c*rlDoJPU7A)`Q)8sBH>f?%Zmu~Y_L)Y_C{oQ%hwgY%0v(H> zvv~Y>((Zr1eg+jSo_d4$@VjTo`?8}|4W?#Ob{|L%$b`eO^m)x`@(j^~1OH!6Zj+j7k z^>&T{;iT&Z4u4HKx9P5$RyCCgcr9sHg{_8OD-v(rva5}cXI0)dqHEJlqZO8W4gS_z zn=L2p?~2>`{|pwox?Q#_z=5S{D<{S2t6ykGwruKLa0+f~DtXj)UTQbJl>i#V<7@xU zTA>LdFk@K6zyStCL_`2=9fBT(=-%Y|{rmSSB*jxQJ3CG6!}8B?2YIe6y>>gxZ8_~x zIz0H#QMa<(>V5QAwsVS;4MRId!tO7u2G!PMNX#ngw&V_qHW% zb(^waiiWiUa5rlJU7{(NI(TlOfLj= zK|z1wqeNNNr2{V?H;+uCLdJQu zt)n85Z0MeFk^Ypt+<0%RTGbif^u0ktr6R4{4=wEL@3XIJSl!iOwV^O#rd^SB;4kYy zNx(!{z>R;rI?~m=7lRM}oHd%P&%V4hOStl-n7~4WTN{5;b3$I_kqGN{+X>;S?%05p z*t$ivi7|<2BV}&Q2RGG-3xc9O<^*wiz?@hAEnDR+EG(3Ql01Mf;Cv(xH}3(Ib7HKk z1bvPaQ&v4ox-Q}k!wF#*3lEv{{TUjv*&P0`6Tu(=XM(s`JZaj)Mk0|+B7eHaXL@sy zdaHlBvxkXqX}Iw@xM9AbUn*$Bel`0)gKYr``>SW~bk;qV>#sFqy}J5HalHR;*JMDq zL|E8?KU+A&3syPH1hV?t?~SOBG`-C=b;T6R%k)!mH;XEwTnoyte%Lv1yB5|m(C z*qZ!#WfT09m33ZFxFoK}eL^aCAc;n1wY_pI%_ifaR?cVsq^BXDFNW3*5dDFGQnbx| zdFE&kAYIo2FqP*JO@BxE6^quZj1T)XCi+!RcYM>IZ5UDVk#WI-X6-j)ZDA}u97-b$ z)oXvUk>3?cRsQ6W(fYi4MqlgZ4c}z%P~a(_fJS!A3mm;1_{}Fu+Sc7Mx$otU^&X!x=`M~xe@!GumCz(j= zPnq~;{d=pbM|`pa@t4T=Jb588GUc1MUiV%6XXDkCU7YY@-v{FP^-;t-szkT7zooq( z5+?lk?EH86Yj3s@&%Iy?qEJ%pC^ly{Hh*q$Dxxu6>q)zUy6IJif~^J|E%GN=+lepz zvx}J5O2-WIpC26g)bn6IOZca3I%Uc?&s~EL1`fS8AwFy7-bp5(wjIw#AGXuKV04T0 zSI87+Kcp(6mD4i)Uiv-p5;x+`N!Sc18~YxW-QD5z%yVP%Z8gSOM;6J4E|K{kI796t zi0^x|+h{BygsRcyzhnBfhC6zV^OnCAoNqg|H-5=}+5d2M$Lr(2T0&2U?)%nl zGj@OR*z3PSip1O?$sE;V@-Z;lMQy@%B!lD1zDFhgC;vQKW&Skf;4&ybS4(docg(rt z;_;%t^9NP-;0-=e+b}fD$JRgNq%q;JI6ctlP@&)`dOKB4A^YND1)U4=sV`N9Y0LXe z23P}J2cA>3{h5uTuhyk}C3DBJ(;z>^DDpgGp?-bb!$J4++y5B3Xxh->Jy{%t4cmDB z!2QkU`;3|$_zq{MD?PWn(X^C)koZhV+>Y`-V=K=rwSgDXmAZq;($%`YFE1}Xxno%p zuIBmlQlG*uKj+|rRGD|wH;8w15@)(qonb(?rj6mBZbMm+D`lJzb9VZ5R$=1CNlJWN zZgwWEZ#kx{-Ct}PT;OOao?RSyy1Bq8evoccQ2j;iN4bBlN4$>xURd||gC`%|=>;Q7 zysO0MhYjXHd-pY|6-X7CwYeN#NsmSY|Li@XkYU?SW}-Tl&tb4qaQ zTML(T<&hV!wiN`Xv>(51Ir2V7Y<^B8O#88-fvLL8)?Kqk|B7n9{r&hZ{{zeJhJ=0Z zKJ5DZ>lLved`2E zZz;1qZ(qo_c#CO+H2538Y4ZEg~?L)V=rTo-0+P@WY2R^D4b^IOqQ}W~OCMLO_yg!D=8wgXQw2_!dH;X0hjT#^3 zI6W+uyvQ?q`s%9(N63TXa4d5F{DpB+miC8Vr$!&hx%Z^M7U>)ulc<*uBeJU3+4`oBf>U{hv9IwOI?_lHFo$Xxn6Ly~vj6Ai!>N zzWeQWMVta@@i;MAMLU%uGDCUQip+b?+h5mr)HaH;FK%rgP>w%St8lHZRwU)IlaZC! z`On4mSv9pDTdpY-T8o|Eg7^DO)5ErH`p|1tvDqw^X&^qK;O)bL(oWM-5raar=Iu5Q z$SeOWuGDz@$D9xOB{{EI(l5(>IV|$U@!v!sHuG><8Hnp)*m6+Qdmo93~7( zV)~PPCA$qjd_T3w*_$ynS5+HWXVq!gObob>8zxiZZWnuhdHdV-VcMzk-XVQU(Jbd( z?{Eb*rr=|6?ffY&rkK zcHV@z>%JBBvdkF|Jol9oA7^t6cl5=n*`bPbw{M@S`W?T$IqhJklIymr=Z6aIECzu`Xll&bJLw2X0?lN4!E5w=CMQz|C;wK=!p4u`kqS zsuXTpzM!_@?WD)^mx?gUFPXZ@^MU;Q-)cU zOIgi^iKvC#A^W)L_pJvaCD-F+iCx5&v@u3uxAQNl;7s{j-_8loES=0;PG!WiO3!mg zh@Uuo9GhXAOl>;EOpvkhtIOx{gS1C-&mM6pNpUHeQ&Rh7_e#o2OYV}Cl$DgcsQ&uM|Mdn(r*pQ} Y?*H#^;NzfJAcjOk?S$$p74uvF2Sk-7eE Date: Wed, 28 May 2025 22:06:35 +0200 Subject: [PATCH 19/28] Added readme to moveit_py --- moveit_py/moveit/README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 moveit_py/moveit/README.md diff --git a/moveit_py/moveit/README.md b/moveit_py/moveit/README.md new file mode 100644 index 0000000000..23831e2705 --- /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 +``` + +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) \ No newline at end of file From 44a4d299e5bed58957e95444640a451f3fa9bdf4 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 30 May 2025 08:48:23 +0200 Subject: [PATCH 20/28] Fix line-ending issues --- moveit_py/moveit/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_py/moveit/README.md b/moveit_py/moveit/README.md index 23831e2705..4f0fe1b6cb 100644 --- a/moveit_py/moveit/README.md +++ b/moveit_py/moveit/README.md @@ -15,4 +15,4 @@ stubgen -p moveit 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) \ No newline at end of file +Note: manual edits are likely to be made to the autogenerated files for official library releases.](https://mypy.readthedocs.io/en/stable/index.html) From c8da3b44f7ec70e1c6610e5369e364d9b3292eb8 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 30 May 2025 09:48:39 +0200 Subject: [PATCH 21/28] Remove python stuff from moveit_core and moveit_commander --- moveit_commander/CHANGELOG.rst | 221 ----- moveit_commander/CMakeLists.txt | 14 - moveit_commander/COLCON_IGNORE | 0 moveit_commander/README.md | 1 - .../bin/moveit_commander_cmdline.py | 217 ----- moveit_commander/demos/pick.py | 85 -- moveit_commander/demos/plan.py | 63 -- .../demos/plan_with_constraints.py | 75 -- moveit_commander/package.xml | 38 - moveit_commander/setup.py | 8 - .../src/moveit_commander/__init__.py | 6 - .../src/moveit_commander/conversions.py | 127 --- .../src/moveit_commander/exception.py | 37 - .../src/moveit_commander/interpreter.py | 881 ------------------ .../src/moveit_commander/move_group.py | 783 ---------------- .../planning_scene_interface.py | 364 -------- .../src/moveit_commander/robot.py | 329 ------- .../moveit_commander/roscpp_initializer.py | 45 - moveit_commander/test/CMakeLists.txt | 7 - .../test/python_moveit_commander.py | 169 ---- .../test/python_moveit_commander.test | 5 - .../test/python_moveit_commander_ns.py | 115 --- .../test/python_moveit_commander_ns.test | 5 - .../python_moveit_commander_ros_namespace.py | 72 -- ...python_moveit_commander_ros_namespace.test | 9 - .../test/python_time_parameterization.py | 112 --- .../test/python_time_parameterization.test | 5 - moveit_core/CMakeLists.txt | 32 - .../doc/_templates/custom-class-template.rst | 34 - .../doc/_templates/custom-module-template.rst | 66 -- moveit_core/doc/api.rst | 6 - moveit_core/doc/conf.py | 200 ---- moveit_core/doc/index.rst | 13 - moveit_core/package.xml | 1 - moveit_core/python/CMakeLists.txt | 1 - moveit_core/python/pymoveit_core.cpp | 84 -- moveit_core/python/src/moveit/__init__.py | 2 - .../python/src/moveit/core/__init__.py | 12 - .../core/collision_detection/__init__.py | 1 - .../core/kinematic_constraints/__init__.py | 1 - .../moveit/core/planning_scene/__init__.py | 1 - .../src/moveit/core/robot_model/__init__.py | 1 - .../src/moveit/core/robot_state/__init__.py | 1 - .../src/moveit/core/transforms/__init__.py | 1 - moveit_core/python/tools/CMakeLists.txt | 17 - .../moveit/python/pybind_rosmsg_typecasters.h | 137 --- .../tools/src/pybind_rosmsg_typecasters.cpp | 69 -- 47 files changed, 4473 deletions(-) delete mode 100644 moveit_commander/CHANGELOG.rst delete mode 100644 moveit_commander/CMakeLists.txt delete mode 100644 moveit_commander/COLCON_IGNORE delete mode 100644 moveit_commander/README.md delete mode 100755 moveit_commander/bin/moveit_commander_cmdline.py delete mode 100755 moveit_commander/demos/pick.py delete mode 100755 moveit_commander/demos/plan.py delete mode 100755 moveit_commander/demos/plan_with_constraints.py delete mode 100644 moveit_commander/package.xml delete mode 100644 moveit_commander/setup.py delete mode 100644 moveit_commander/src/moveit_commander/__init__.py delete mode 100644 moveit_commander/src/moveit_commander/conversions.py delete mode 100644 moveit_commander/src/moveit_commander/exception.py delete mode 100644 moveit_commander/src/moveit_commander/interpreter.py delete mode 100644 moveit_commander/src/moveit_commander/move_group.py delete mode 100644 moveit_commander/src/moveit_commander/planning_scene_interface.py delete mode 100644 moveit_commander/src/moveit_commander/robot.py delete mode 100644 moveit_commander/src/moveit_commander/roscpp_initializer.py delete mode 100644 moveit_commander/test/CMakeLists.txt delete mode 100755 moveit_commander/test/python_moveit_commander.py delete mode 100644 moveit_commander/test/python_moveit_commander.test delete mode 100755 moveit_commander/test/python_moveit_commander_ns.py delete mode 100644 moveit_commander/test/python_moveit_commander_ns.test delete mode 100755 moveit_commander/test/python_moveit_commander_ros_namespace.py delete mode 100644 moveit_commander/test/python_moveit_commander_ros_namespace.test delete mode 100755 moveit_commander/test/python_time_parameterization.py delete mode 100644 moveit_commander/test/python_time_parameterization.test delete mode 100644 moveit_core/doc/_templates/custom-class-template.rst delete mode 100644 moveit_core/doc/_templates/custom-module-template.rst delete mode 100644 moveit_core/doc/api.rst delete mode 100644 moveit_core/doc/conf.py delete mode 100644 moveit_core/doc/index.rst delete mode 100644 moveit_core/python/CMakeLists.txt delete mode 100644 moveit_core/python/pymoveit_core.cpp delete mode 100644 moveit_core/python/src/moveit/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/collision_detection/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/kinematic_constraints/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/planning_scene/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/robot_model/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/robot_state/__init__.py delete mode 100644 moveit_core/python/src/moveit/core/transforms/__init__.py delete mode 100644 moveit_core/python/tools/CMakeLists.txt delete mode 100644 moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h delete mode 100644 moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst deleted file mode 100644 index 0b6b4fb7e2..0000000000 --- a/moveit_commander/CHANGELOG.rst +++ /dev/null @@ -1,221 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package moveit_commander -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.1.1 (2020-10-13) ------------------- -* [feature] MGC: Improve exception messages (`#2318 `_) -* [fix] ROS namespacing in moveit_commander's PSI (`#2347 `_) -* [fix] python3 issues (`#2323 `_) -* Contributors: Michael Görner, Peter Mitrano, Robert Haschke - -1.1.0 (2020-09-04) ------------------- -* [feature] Add missing variants of place from list of PlaceLocations and Poses in the python interface (`#2231 `_) -* [fix] Add default velocity/acceleration scaling factors (`#1890 `_) -* [fix] Handle the updated plan() function of MoveGroupCommander (`#1640 `_) -* [fix] Fix `failing tutorial `_ (`#1459 `_) -* [maint] Update dependencies for python3 in noetic (`#2131 `_) -* [maint] Better align MoveGroupInterface.plan() with C++ MoveGroup::plan() (`#790 `_) -* Contributors: Bence Magyar, Bjar Ne, Dave Coleman, Felix von Drigalski, Gerard Canal, Jafar Abdi, Masaki Murooka, Michael Ferguson, Michael Görner, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Ryosuke Tajima, Sean Yen, v4hn - -1.0.6 (2020-08-19) ------------------- -* [maint] Adapt repository for splitted moveit_resources layout (`#2199 `_) -* [feature] Exposed parameter wait_for_servers and getPlannerId() API in MoveGroup's Python API (`#2201 `_) -* Contributors: Gerard Canal, Robert Haschke, Michael Görner - -1.0.5 (2020-07-08) ------------------- -* [fix] Python 3 fix (`#2030 `_) -* Contributors: Henning Kayser, Michael Görner, Robert Haschke - -1.0.4 (2020-05-30) ------------------- - -1.0.3 (2020-04-26) ------------------- -* [feature] Expose reference_point_position parameter in getJacobian() (`#1595 `_) -* [maint] Improve Python 3 compatibility (`#1870 `_) - * Replaced StringIO with BytesIO for python msg serialization - * Use py_bindings_tools::ByteString as byte-based serialization buffer on C++ side -* [fix] Fix service call to utilize original name space (`#1959 `_) -* [maint] Windows compatibility: fallback to using `pyreadline` (`#1635 `_) -* [maint] Use CMAKE_CXX_STANDARD to enforce c++14 (`#1607 `_) -* [fix] Fix planning scene interface not respecting custom namespace (`#1815 `_) -* [maint] moveit_commander: python3 import fixes (`#1786 `_) -* [fix] python planning_scene_interface: fix attaching objects (`#1624 `_) -* [feature] Select time parametrization algorithm in retime_trajectory (`#1508 `_) -* Contributors: Bjar Ne, Felix von Drigalski, Masaki Murooka, Pavel-P, Raphael Druon, Robert Haschke, Ryodo Tanaka, Sean Yen, v4hn - -1.0.2 (2019-06-28) ------------------- -* [feature] Add get_jacobian_matrix to moveit_commander (`#1501 `_) -* [maintanance] Cleanup Python PlanningSceneInterface (`#1405 `_, `#789 `_) -* Contributors: Bence Magyar, Robert Haschke, Ryosuke Tajima - -1.0.1 (2019-03-08) ------------------- -* [capability] python PlanningSceneInterface.add_cylinder() (`#1372 `_) -* Contributors: Robert Haschke - -1.0.0 (2019-02-24) ------------------- -* [fix] catkin_lint issues (`#1341 `_) -* Contributors: Keerthana Subramanian Manivannan, Robert Haschke - -0.10.8 (2018-12-24) -------------------- - -0.10.7 (2018-12-13) -------------------- - -0.10.6 (2018-12-09) -------------------- - -0.10.5 (2018-11-01) -------------------- - -0.10.4 (2018-10-29) -------------------- - -0.10.3 (2018-10-29) -------------------- - -0.10.2 (2018-10-24) -------------------- -* [capability] Added plan_only flags to pick and place (`#862 `_) -* [maintenance] Python3 support (`#1103 `_, `#1054 `_) -* Contributors: David Watkins, Michael Görner, d-walsh, mike lautman - -0.10.1 (2018-05-25) -------------------- -* Get robot markers from state (`#836 `_) -* Add namespace capabilities to moveit_commander (`#835 `_) -* Constrained Cartesian planning using moveit commander (`#805 `_) -* Handle robot_description parameter in RobotCommander (`#782 `_) -* support TrajectoryConstraints in MoveGroupInterface + MoveitCommander (`#793 `_) -* API to get planner_id (`#788 `_) -* Contributors: Akiyoshi Ochiai, Bence Magyar, Bryce Willey, Dave Coleman, Michael Görner, Ryan Keating, Will Baker - -0.9.11 (2017-12-25) -------------------- - -0.9.10 (2017-12-09) -------------------- -* [fix] Bugs in moveit_commander/robot.py (`#621 `_) -* [fix] pyassimp regression workaround (`#581 `_) -* Contributors: Kei Okada, Konstantin Selyunin - -0.9.9 (2017-08-06) ------------------- - -0.9.8 (2017-06-21) ------------------- - -0.9.7 (2017-06-05) ------------------- - -0.9.6 (2017-04-12) ------------------- - -0.9.5 (2017-03-08) ------------------- -* [fix] Regression on Ubuntu Xenial; numpy.ndarray indices bug (from `#86 `_) (`#450 `_). -* [doc][moveit_commander] added description for set_start_state (`#447 `_) -* Contributors: Adam Allevato, Ravi Prakash Joshi - -0.9.4 (2017-02-06) ------------------- -* [fix] issue `#373 `_ for Kinetic (`#377 `_) (`#385 `_) -* [fix] typo in moveit_commander (`#376 `_) -* Contributors: Dave Coleman, Shingo Kitagawa - -0.9.3 (2016-11-16) ------------------- -* [maintenance] Updated package.xml maintainers and author emails `#330 `_ -* Contributors: Dave Coleman, Ian McMahon - -0.9.2 (2016-11-05) ------------------- - -0.6.1 (2016-04-28) ------------------- -* [feat] Add the possibility to choose description file `#43 `_ -* [improve] support pyassimp 3.2. Looks like they changed their import path. robot_description should not be hardcoded to allow changing the name of the description file. This is usefull when working with several robots that do not share the same description file. `#45 `_ -* [improve] add queue_size option in planning_scene_interface.py `#41 `_ -* Contributors: Dave Coleman, Isaac I.Y. Saito, Kei Okada, Michael Görner, buschbapti - -0.6.0 (2016-01-30) ------------------- -* Merge pull request #38 from 130s/doc/python_if - [RobotCommander] Fill in in-code document where missing. -* [moveit_commander/robot.py] Code cleaning; semi-PEP8. -* Merge pull request #35 from MichaelStevens/set_num_planning_attempts - adding set_num_planning_attempts to commander interface -* Merge pull request #30 from ymollard/indigo-devel - Planning scene improvements + added python wrapper for MoveGroup.asyncExecute() -* Added python wrapper for MoveGroup.asyncExecute() -* Allow to clean all objects in a row -* Allow to attash an existing object without recreating the whole CollisionObject -* Merge pull request #24 from ymollard/hydro-devel - Allowed user to change the scale of a mesh -* Merge pull request #23 from HumaRobotics/hydro-devel - Fixed arguments removal in python roscpp_initializer -* Merge pull request #26 from corot/hydro-devel - Add missing variants of place (PlaceLocation, place anywhere) -* Added a way to change the size of a mesh when grasping -* Allowed user to change the scale of a mesh -* Fixed arguments removal in python roscpp_initializer -* Contributors: Dave Coleman, Ioan A Sucan, Isaac I.Y. Saito, Michael Stevens, Philippe Capdepuy, Yoan Mollard, corot - -0.5.7 (2014-07-05) ------------------- -* Merge pull request `#21 ` from pirobot/hydro-devel - Added set_support_surface_name function to move_group.py -* Added set_support_surface_name function to move_group.py -* Contributors: Patrick Goebel, Sachin Chitta - -0.5.6 (2014-03-24) ------------------- -* Added the calls necessary to manage path constraints. -* fix joint and link acces on __getattr__ when trying to acces a joint and its paramaters throught -* Contributors: Acorn, Emili Boronat, Sachin Chitta - -0.5.5 (2014-02-27) ------------------- -* adding get for active joints -* Contributors: Acorn, Sachin Chitta - -0.5.4 (2014-02-06) ------------------- - -* Install moveit_commander_cmdline.py into package specific directory, not to global bin. -* Fix typos in comments - -0.5.3 (2014-01-03) ------------------- -* work around name bug - move group interface python programs cannot be launched from launch files if - the __name:= argument is used. This works around the problem and allows using - launch files to launch python moveit programs. -* Added Travis Continuous Integration - -0.5.2 (2013-09-23) ------------------- -* add support for setting joint targets from approximate IK -* no longer depend on manipulation_msgs -* expand functionality of MoveGroupInterface - -0.5.1 (2013-08-13) ------------------- -* make pick() more general -* use msg serialization -* use new attach / detach operations -* fix header for demo code -* Duration class bug fixed in commander conversion. - -0.5.0 (2013-07-18) ------------------- -* move msgs to common_msgs -* fixed ground command diff --git a/moveit_commander/CMakeLists.txt b/moveit_commander/CMakeLists.txt deleted file mode 100644 index cb0618035e..0000000000 --- a/moveit_commander/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 3.1.3) -project(moveit_commander) - -find_package(catkin REQUIRED) - -catkin_python_setup() - -catkin_package() - -catkin_install_python( - PROGRAMS bin/${PROJECT_NAME}_cmdline.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -add_subdirectory(test) diff --git a/moveit_commander/COLCON_IGNORE b/moveit_commander/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_commander/README.md b/moveit_commander/README.md deleted file mode 100644 index 1b8f538576..0000000000 --- a/moveit_commander/README.md +++ /dev/null @@ -1 +0,0 @@ -# MoveIt Commander diff --git a/moveit_commander/bin/moveit_commander_cmdline.py b/moveit_commander/bin/moveit_commander_cmdline.py deleted file mode 100755 index 15c2d8a5da..0000000000 --- a/moveit_commander/bin/moveit_commander_cmdline.py +++ /dev/null @@ -1,217 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import roslib -import rospy - -try: - import readline -except ImportError: - import pyreadline as readline # for Windows -import sys -import os -import signal - -import argparse -from moveit_commander import ( - MoveGroupCommandInterpreter, - MoveGroupInfoLevel, - roscpp_initialize, - roscpp_shutdown, -) - -# python3 has renamed raw_input to input: https://www.python.org/dev/peps/pep-3111 -# Here, we use the new input(). Hence, for python2, we redirect raw_input to input -try: - import __builtin__ # This is named builtin in python3 - - input = getattr(__builtin__, "raw_input") -except (ImportError, AttributeError): - pass - - -class bcolors: - HEADER = "\033[95m" - OKBLUE = "\033[94m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - - -class SimpleCompleter(object): - def __init__(self, options): - self.options = options - - def set_options(self, options): - self.options = options - - def complete(self, text, state): - response = None - cmds = readline.get_line_buffer().split() - prefix = "" - if len(cmds) > 0: - prefix = cmds[0] - if not prefix in self.options: - prefix = "" - - if state == 0: - # This is the first time for this text, so build a match list. - if text: - if len(prefix) == 0: - self.matches = sorted( - [s for s in self.options.keys() if s and s.startswith(text)] - ) - else: - self.matches = sorted( - [s for s in self.options[prefix] if s and s.startswith(text)] - ) - else: - if len(prefix) == 0: - self.matches = sorted(self.options.keys()) - else: - self.matches = self.options[prefix] - - # Return the state'th item from the match list, - # if we have that many. - try: - response = self.matches[state] + " " - except IndexError: - response = None - return response - - -def print_message(level, msg): - if level == MoveGroupInfoLevel.FAIL: - print(bcolors.FAIL + msg + bcolors.ENDC) - elif level == MoveGroupInfoLevel.WARN: - print(bcolors.WARNING + msg + bcolors.ENDC) - elif level == MoveGroupInfoLevel.SUCCESS: - print(bcolors.OKGREEN + msg + bcolors.ENDC) - elif level == MoveGroupInfoLevel.DEBUG: - print(bcolors.OKBLUE + msg + bcolors.ENDC) - else: - print(msg) - - -def get_context_keywords(interpreter): - kw = interpreter.get_keywords() - kw["quit"] = [] - return kw - - -def run_interactive(group_name): - c = MoveGroupCommandInterpreter() - if len(group_name) > 0: - c.execute("use " + group_name) - completer = SimpleCompleter(get_context_keywords(c)) - readline.set_completer(completer.complete) - - print() - print( - bcolors.HEADER - + "Waiting for commands. Type 'help' to get a list of known commands." - + bcolors.ENDC - ) - print() - readline.parse_and_bind("tab: complete") - - while not rospy.is_shutdown(): - cmd = "" - try: - name = "" - ag = c.get_active_group() - if ag != None: - name = ag.get_name() - cmd = input(bcolors.OKBLUE + name + "> " + bcolors.ENDC) - except: - break - cmdorig = cmd.strip() - if cmdorig == "": - continue - cmd = cmdorig.lower() - - if cmd == "q" or cmd == "quit" or cmd == "exit": - break - if cmd == "host": - print_message( - MoveGroupInfoLevel.INFO, - "Master is '" + os.environ["ROS_MASTER_URI"] + "'", - ) - continue - - (level, msg) = c.execute(cmdorig) - print_message(level, msg) - # update the set of keywords - completer.set_options(get_context_keywords(c)) - - -def run_service(group_name): - c = MoveGroupCommandInterpreter() - if len(group_name) > 0: - c.execute("use " + group_name) - # add service stuff - print("Running ROS service") - rospy.spin() - - -def stop_ros(reason): - rospy.signal_shutdown(reason) - roscpp_shutdown() - - -def sigint_handler(signal, frame): - stop_ros("Ctrl+C pressed") - # this won't actually exit, but trigger an exception to terminate input - sys.exit(0) - - -if __name__ == "__main__": - - signal.signal(signal.SIGINT, sigint_handler) - - roscpp_initialize(sys.argv) - - rospy.init_node( - "move_group_interface_cmdline", anonymous=True, disable_signals=True - ) - - parser = argparse.ArgumentParser( - usage="""%(prog)s [options] []""", - description="Command Line Interface to MoveIt", - ) - parser.add_argument( - "-i", - "--interactive", - action="store_true", - dest="interactive", - default=True, - help="Run the command processing script in interactive mode (default)", - ) - parser.add_argument( - "-s", - "--service", - action="store_true", - dest="service", - default=False, - help="Run the command processing script as a ROS service", - ) - parser.add_argument( - "group_name", - type=str, - default="", - nargs="?", - help="Group name to initialize the CLI for.", - ) - - opt = parser.parse_args(rospy.myargv()[1:]) - - if opt.service: - run_service(opt.group_name) - else: - run_interactive(opt.group_name) - - stop_ros("Done") - - print("Bye bye!") diff --git a/moveit_commander/demos/pick.py b/moveit_commander/demos/pick.py deleted file mode 100755 index 175400bbae..0000000000 --- a/moveit_commander/demos/pick.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Willow Garage, Inc. -# 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: Ioan Sucan - -import sys -import rospy -from moveit_commander import ( - RobotCommander, - PlanningSceneInterface, - roscpp_initialize, - roscpp_shutdown, -) -from geometry_msgs.msg import PoseStamped - -if __name__ == "__main__": - - roscpp_initialize(sys.argv) - rospy.init_node("moveit_py_demo", anonymous=True) - - scene = PlanningSceneInterface() - robot = RobotCommander() - rospy.sleep(1) - - # clean the scene - scene.remove_world_object("pole") - scene.remove_world_object("table") - scene.remove_world_object("part") - - # publish a demo scene - p = PoseStamped() - p.header.frame_id = robot.get_planning_frame() - p.pose.position.x = 0.7 - p.pose.position.y = -0.4 - p.pose.position.z = 0.85 - p.pose.orientation.w = 1.0 - scene.add_box("pole", p, (0.3, 0.1, 1.0)) - - p.pose.position.y = -0.2 - p.pose.position.z = 0.175 - scene.add_box("table", p, (0.5, 1.5, 0.35)) - - p.pose.position.x = 0.6 - p.pose.position.y = -0.7 - p.pose.position.z = 0.5 - scene.add_box("part", p, (0.15, 0.1, 0.3)) - - rospy.sleep(1) - - # pick an object - robot.right_arm.pick("part") - - rospy.spin() - roscpp_shutdown() diff --git a/moveit_commander/demos/plan.py b/moveit_commander/demos/plan.py deleted file mode 100755 index 6eee209f0c..0000000000 --- a/moveit_commander/demos/plan.py +++ /dev/null @@ -1,63 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Willow Garage, Inc. -# 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: Ioan Sucan - -import sys -import rospy -from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown -from moveit_msgs.msg import RobotState - -if __name__ == "__main__": - - roscpp_initialize(sys.argv) - rospy.init_node("moveit_py_demo", anonymous=True) - - robot = RobotCommander() - rospy.sleep(1) - - print("Current state:") - print(robot.get_current_state()) - - # plan to a random location - a = robot.right_arm - a.set_start_state(RobotState()) - r = a.get_random_joint_values() - print("Planning to random joint position: ") - print(r) - p = a.plan(r) - print("Solution:") - print(p) - - roscpp_shutdown() diff --git a/moveit_commander/demos/plan_with_constraints.py b/moveit_commander/demos/plan_with_constraints.py deleted file mode 100755 index 01b69ed1a1..0000000000 --- a/moveit_commander/demos/plan_with_constraints.py +++ /dev/null @@ -1,75 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2018, Houston Mechatronics, Inc. -# 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: Siddharth Srivatsa - -import sys -import rospy -from moveit_commander import RobotCommander, roscpp_initialize, roscpp_shutdown -from moveit_msgs.msg import RobotState, Constraints -from geometry_msgs.msg import Pose - -if __name__ == "__main__": - - roscpp_initialize(sys.argv) - rospy.init_node("moveit_py_demo", anonymous=True) - - robot = RobotCommander() - rospy.sleep(1) - - a = robot.right_arm - a.set_start_state(RobotState()) - - print("current pose:") - print(a.get_current_pose()) - c = Constraints() - - waypoints = [] - waypoints.append(a.get_current_pose().pose) - - # Move forward - wpose = Pose() - wpose.position.x = wpose.position.x + 0.1 - waypoints.append(wpose) - - # Move down - wpose.position.z -= 0.10 - waypoints.append(wpose) - - # Move to the side - wpose.position.y += 0.05 - waypoints.append(wpose) - - plan, fraction = a.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c) - print("Plan success percent: ", fraction) diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml deleted file mode 100644 index 3ab4e50426..0000000000 --- a/moveit_commander/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - - - moveit_commander - 1.1.7 - Python interfaces to MoveIt - Michael Görner - Robert Haschke - MoveIt Release Team - - BSD - - http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - Ioan Sucan - - catkin - python-catkin-pkg - python3-catkin-pkg - - python - python3 - - geometry_msgs - moveit_msgs - moveit_ros_planning_interface - python-pyassimp - python3-pyassimp - rospy - sensor_msgs - shape_msgs - tf - - rostest - moveit_resources_fanuc_moveit_config - diff --git a/moveit_commander/setup.py b/moveit_commander/setup.py deleted file mode 100644 index 441de11974..0000000000 --- a/moveit_commander/setup.py +++ /dev/null @@ -1,8 +0,0 @@ -from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup() -d["packages"] = ["moveit_commander"] -d["package_dir"] = {"": "src"} - -setup(**d) diff --git a/moveit_commander/src/moveit_commander/__init__.py b/moveit_commander/src/moveit_commander/__init__.py deleted file mode 100644 index 499093152a..0000000000 --- a/moveit_commander/src/moveit_commander/__init__.py +++ /dev/null @@ -1,6 +0,0 @@ -from .exception import * -from .roscpp_initializer import * -from .planning_scene_interface import * -from .move_group import * -from .robot import * -from .interpreter import * diff --git a/moveit_commander/src/moveit_commander/conversions.py b/moveit_commander/src/moveit_commander/conversions.py deleted file mode 100644 index 1fd154104c..0000000000 --- a/moveit_commander/src/moveit_commander/conversions.py +++ /dev/null @@ -1,127 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Ioan Sucan - -try: - # Try Python 2.7 behaviour first - from StringIO import StringIO -except ImportError: - # Use Python 3.x behaviour as fallback and choose the non-unicode version - from io import BytesIO as StringIO - -from .exception import MoveItCommanderException -from geometry_msgs.msg import Pose, PoseStamped, Transform -import rospy -import tf - - -def msg_to_string(msg): - buf = StringIO() - msg.serialize(buf) - return buf.getvalue() - - -def msg_from_string(msg, data): - msg.deserialize(data) - - -def pose_to_list(pose_msg): - pose = [] - pose.append(pose_msg.position.x) - pose.append(pose_msg.position.y) - pose.append(pose_msg.position.z) - pose.append(pose_msg.orientation.x) - pose.append(pose_msg.orientation.y) - pose.append(pose_msg.orientation.z) - pose.append(pose_msg.orientation.w) - return pose - - -def list_to_pose(pose_list): - pose_msg = Pose() - if len(pose_list) == 7: - pose_msg.position.x = pose_list[0] - pose_msg.position.y = pose_list[1] - pose_msg.position.z = pose_list[2] - pose_msg.orientation.x = pose_list[3] - pose_msg.orientation.y = pose_list[4] - pose_msg.orientation.z = pose_list[5] - pose_msg.orientation.w = pose_list[6] - elif len(pose_list) == 6: - pose_msg.position.x = pose_list[0] - pose_msg.position.y = pose_list[1] - pose_msg.position.z = pose_list[2] - q = tf.transformations.quaternion_from_euler( - pose_list[3], pose_list[4], pose_list[5] - ) - pose_msg.orientation.x = q[0] - pose_msg.orientation.y = q[1] - pose_msg.orientation.z = q[2] - pose_msg.orientation.w = q[3] - else: - raise MoveItCommanderException( - "Expected either 6 or 7 elements in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)" - ) - return pose_msg - - -def list_to_pose_stamped(pose_list, target_frame): - pose_msg = PoseStamped() - pose_msg.pose = list_to_pose(pose_list) - pose_msg.header.frame_id = target_frame - pose_msg.header.stamp = rospy.Time.now() - return pose_msg - - -def transform_to_list(trf_msg): - trf = [] - trf.append(trf_msg.translation.x) - trf.append(trf_msg.translation.y) - trf.append(trf_msg.translation.z) - trf.append(trf_msg.rotation.x) - trf.append(trf_msg.rotation.y) - trf.append(trf_msg.rotation.z) - trf.append(trf_msg.rotation.w) - return trf - - -def list_to_transform(trf_list): - trf_msg = Transform() - trf_msg.translation.x = trf_list[0] - trf_msg.translation.y = trf_list[1] - trf_msg.translation.z = trf_list[2] - trf_msg.rotation.x = trf_list[3] - trf_msg.rotation.y = trf_list[4] - trf_msg.rotation.z = trf_list[5] - trf_msg.rotation.w = trf_list[6] - return trf_msg diff --git a/moveit_commander/src/moveit_commander/exception.py b/moveit_commander/src/moveit_commander/exception.py deleted file mode 100644 index 8b48333897..0000000000 --- a/moveit_commander/src/moveit_commander/exception.py +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Ioan Sucan - - -class MoveItCommanderException(Exception): - pass diff --git a/moveit_commander/src/moveit_commander/interpreter.py b/moveit_commander/src/moveit_commander/interpreter.py deleted file mode 100644 index c7909fb0e6..0000000000 --- a/moveit_commander/src/moveit_commander/interpreter.py +++ /dev/null @@ -1,881 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Ioan Sucan - -import rospy -from moveit_commander import ( - RobotCommander, - MoveGroupCommander, - PlanningSceneInterface, - MoveItCommanderException, -) -from geometry_msgs.msg import Pose, PoseStamped -import tf -import re -import time -import os.path - - -class MoveGroupInfoLevel: - FAIL = 1 - WARN = 2 - SUCCESS = 3 - INFO = 4 - DEBUG = 5 - - -class MoveGroupCommandInterpreter(object): - """ - Interpreter for simple commands - """ - - DEFAULT_FILENAME = "move_group.cfg" - GO_DIRS = { - "up": (2, 1), - "down": (2, -1), - "z": (2, 1), - "left": (1, 1), - "right": (1, -1), - "y": (1, 1), - "forward": (0, 1), - "backward": (0, -1), - "back": (0, -1), - "x": (0, 1), - } - - def __init__(self): - self._gdict = {} - self._group_name = "" - self._prev_group_name = "" - self._planning_scene_interface = PlanningSceneInterface() - self._robot = RobotCommander() - self._last_plan = None - self._db_host = None - self._db_port = 33829 - self._trace = False - - def get_active_group(self): - if len(self._group_name) > 0: - return self._gdict[self._group_name] - else: - return None - - def get_loaded_groups(self): - return self._gdict.keys() - - def execute(self, cmd): - cmd = self.resolve_command_alias(cmd) - result = self.execute_generic_command(cmd) - if result != None: - return result - else: - if len(self._group_name) > 0: - return self.execute_group_command(self._gdict[self._group_name], cmd) - else: - return ( - MoveGroupInfoLevel.INFO, - self.get_help() - + "\n\nNo groups initialized yet. You must call the 'use' or the 'load' command first.\n", - ) - - def execute_generic_command(self, cmd): - if os.path.isfile("cmd/" + cmd): - cmd = "load cmd/" + cmd - cmdlow = cmd.lower() - if cmdlow.startswith("use"): - if cmdlow == "use": - return (MoveGroupInfoLevel.INFO, "\n".join(self.get_loaded_groups())) - clist = cmd.split() - clist[0] = clist[0].lower() - if len(clist) == 2: - if clist[0] == "use": - if clist[1] == "previous": - clist[1] = self._prev_group_name - if len(clist[1]) == 0: - return (MoveGroupInfoLevel.DEBUG, "OK") - if clist[1] in self._gdict: - self._prev_group_name = self._group_name - self._group_name = clist[1] - return (MoveGroupInfoLevel.DEBUG, "OK") - else: - try: - mg = MoveGroupCommander(clist[1]) - self._gdict[clist[1]] = mg - self._group_name = clist[1] - return (MoveGroupInfoLevel.DEBUG, "OK") - except MoveItCommanderException as e: - return ( - MoveGroupInfoLevel.FAIL, - "Initializing " + clist[1] + ": " + str(e), - ) - except: - return ( - MoveGroupInfoLevel.FAIL, - "Unable to initialize " + clist[1], - ) - elif cmdlow.startswith("trace"): - if cmdlow == "trace": - return ( - MoveGroupInfoLevel.INFO, - "trace is on" if self._trace else "trace is off", - ) - clist = cmdlow.split() - if clist[0] == "trace" and clist[1] == "on": - self._trace = True - return (MoveGroupInfoLevel.DEBUG, "OK") - if clist[0] == "trace" and clist[1] == "off": - self._trace = False - return (MoveGroupInfoLevel.DEBUG, "OK") - elif cmdlow.startswith("load"): - filename = self.DEFAULT_FILENAME - clist = cmd.split() - if len(clist) > 2: - return (MoveGroupInfoLevel.WARN, "Unable to parse load command") - if len(clist) == 2: - filename = clist[1] - if not os.path.isfile(filename): - filename = "cmd/" + filename - line_num = 0 - line_content = "" - try: - f = open(filename, "r") - for line in f: - line_num += 1 - line = line.rstrip() - line_content = line - if self._trace: - print("%s:%d: %s" % (filename, line_num, line_content)) - comment = line.find("#") - if comment != -1: - line = line[0:comment].rstrip() - if line != "": - self.execute(line.rstrip()) - line_content = "" - return (MoveGroupInfoLevel.DEBUG, "OK") - except MoveItCommanderException as e: - if line_num > 0: - return ( - MoveGroupInfoLevel.WARN, - "Error at %s:%d: %s\n%s" - % (filename, line_num, line_content, str(e)), - ) - else: - return ( - MoveGroupInfoLevel.WARN, - "Processing " + filename + ": " + str(e), - ) - except: - if line_num > 0: - return ( - MoveGroupInfoLevel.WARN, - "Error at %s:%d: %s" % (filename, line_num, line_content), - ) - else: - return (MoveGroupInfoLevel.WARN, "Unable to load " + filename) - elif cmdlow.startswith("save"): - filename = self.DEFAULT_FILENAME - clist = cmd.split() - if len(clist) > 2: - return (MoveGroupInfoLevel.WARN, "Unable to parse save command") - if len(clist) == 2: - filename = clist[1] - try: - f = open(filename, "w") - for gr in self._gdict.keys(): - f.write("use " + gr + "\n") - known = self._gdict[gr].get_remembered_joint_values() - for v in known.keys(): - f.write( - v + " = [" + " ".join([str(x) for x in known[v]]) + "]\n" - ) - if self._db_host != None: - f.write( - "database " + self._db_host + " " + str(self._db_port) + "\n" - ) - return (MoveGroupInfoLevel.DEBUG, "OK") - except: - return (MoveGroupInfoLevel.WARN, "Unable to save " + filename) - else: - return None - - def execute_group_command(self, g, cmdorig): - cmd = cmdorig.lower() - - if cmd == "stop": - g.stop() - return (MoveGroupInfoLevel.DEBUG, "OK") - - if cmd == "id": - return (MoveGroupInfoLevel.INFO, g.get_name()) - - if cmd == "help": - return (MoveGroupInfoLevel.INFO, self.get_help()) - - if cmd == "vars": - known = g.get_remembered_joint_values() - return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]") - - if cmd == "joints": - joints = g.get_joints() - return ( - MoveGroupInfoLevel.INFO, - "\n" - + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) - + "\n", - ) - - if cmd == "show": - return self.command_show(g) - - if cmd == "current": - return self.command_current(g) - - if cmd == "ground": - pose = PoseStamped() - pose.pose.position.x = 0 - pose.pose.position.y = 0 - # offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself) - pose.pose.position.z = -0.0501 - pose.pose.orientation.x = 0 - pose.pose.orientation.y = 0 - pose.pose.orientation.z = 0 - pose.pose.orientation.w = 1 - pose.header.stamp = rospy.get_rostime() - pose.header.frame_id = self._robot.get_root_link() - self._planning_scene_interface.attach_box( - self._robot.get_root_link(), "ground", pose, (3, 3, 0.1) - ) - return (MoveGroupInfoLevel.INFO, "Added ground") - - if cmd == "eef": - if len(g.get_end_effector_link()) > 0: - return (MoveGroupInfoLevel.INFO, g.get_end_effector_link()) - else: - return (MoveGroupInfoLevel.INFO, "There is no end effector defined") - - if cmd == "database": - if self._db_host == None: - return (MoveGroupInfoLevel.INFO, "Not connected to a database") - else: - return ( - MoveGroupInfoLevel.INFO, - "Connected to " + self._db_host + ":" + str(self._db_port), - ) - if cmd == "plan": - if self._last_plan != None: - return (MoveGroupInfoLevel.INFO, str(self._last_plan)) - else: - return (MoveGroupInfoLevel.INFO, "No previous plan") - - if cmd == "constrain": - g.clear_path_constraints() - return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints") - - if cmd == "tol" or cmd == "tolerance": - return ( - MoveGroupInfoLevel.INFO, - "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" - % g.get_goal_tolerance(), - ) - - if cmd == "time": - return (MoveGroupInfoLevel.INFO, str(g.get_planning_time())) - - if cmd == "execute": - if self._last_plan != None: - if g.execute(self._last_plan): - return (MoveGroupInfoLevel.SUCCESS, "Plan submitted for execution") - else: - return ( - MoveGroupInfoLevel.WARN, - "Failed to submit plan for execution", - ) - else: - return (MoveGroupInfoLevel.WARN, "No motion plan computed") - - # see if we have assignment between variables - assign_match = re.match(r"^(\w+)\s*=\s*(\w+)$", cmd) - if assign_match: - known = g.get_remembered_joint_values() - if assign_match.group(2) in known: - g.remember_joint_values( - assign_match.group(1), known[assign_match.group(2)] - ) - return ( - MoveGroupInfoLevel.SUCCESS, - assign_match.group(1) - + " is now the same as " - + assign_match.group(2), - ) - else: - return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") - - # see if we have assignment of matlab-like vector syntax - set_match = re.match(r"^(\w+)\s*=\s*\[([\d\.e\-\+\s]+)\]$", cmd) - if set_match: - try: - g.remember_joint_values( - set_match.group(1), [float(x) for x in set_match.group(2).split()] - ) - return ( - MoveGroupInfoLevel.SUCCESS, - "Remembered joint values [" - + set_match.group(2) - + "] under the name " - + set_match.group(1), - ) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse joint value [" + set_match.group(2) + "]", - ) - - # see if we have assignment of matlab-like element update - component_match = re.match( - r"^(\w+)\s*\[\s*(\d+)\s*\]\s*=\s*([\d\.e\-\+]+)$", cmd - ) - if component_match: - known = g.get_remembered_joint_values() - if component_match.group(1) in known: - try: - val = known[component_match.group(1)] - val[int(component_match.group(2))] = float(component_match.group(3)) - g.remember_joint_values(component_match.group(1), val) - return ( - MoveGroupInfoLevel.SUCCESS, - "Updated " - + component_match.group(1) - + "[" - + component_match.group(2) - + "]", - ) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse index or value in '" + cmd + "'", - ) - else: - return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") - - clist = cmdorig.split() - clist[0] = clist[0].lower() - - # if this is an unknown one-word command, it is probably a variable - if len(clist) == 1: - known = g.get_remembered_joint_values() - if cmd in known: - return ( - MoveGroupInfoLevel.INFO, - "[" + " ".join([str(x) for x in known[cmd]]) + "]", - ) - else: - return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") - - # command with one argument - if len(clist) == 2: - if clist[0] == "go": - self._last_plan = None - if clist[1] == "rand" or clist[1] == "random": - vals = g.get_random_joint_values() - g.set_joint_value_target(vals) - if g.go(): - return ( - MoveGroupInfoLevel.SUCCESS, - "Moved to random target [" - + " ".join([str(x) for x in vals]) - + "]", - ) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while moving to random target [" - + " ".join([str(x) for x in vals]) - + "]", - ) - else: - try: - g.set_named_target(clist[1]) - if g.go(): - return (MoveGroupInfoLevel.SUCCESS, "Moved to " + clist[1]) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while moving to " + clist[1], - ) - except MoveItCommanderException as e: - return ( - MoveGroupInfoLevel.WARN, - "Going to " + clist[1] + ": " + str(e), - ) - except: - return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") - if clist[0] == "plan": - self._last_plan = None - vals = None - if clist[1] == "rand" or clist[1] == "random": - vals = g.get_random_joint_values() - g.set_joint_value_target(vals) - self._last_plan = g.plan()[1] # The trajectory msg - else: - try: - g.set_named_target(clist[1]) - self._last_plan = g.plan()[1] # The trajectory msg - except MoveItCommanderException as e: - return ( - MoveGroupInfoLevel.WARN, - "Planning to " + clist[1] + ": " + str(e), - ) - except: - return (MoveGroupInfoLevel.WARN, clist[1] + " is unknown") - if self._last_plan != None: - if ( - len(self._last_plan.joint_trajectory.points) == 0 - and len(self._last_plan.multi_dof_joint_trajectory.points) == 0 - ): - self._last_plan = None - dest_str = clist[1] - if vals != None: - dest_str = "[" + " ".join([str(x) for x in vals]) + "]" - if self._last_plan != None: - return (MoveGroupInfoLevel.SUCCESS, "Planned to " + dest_str) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while planning to " + dest_str, - ) - elif clist[0] == "pick": - self._last_plan = None - if g.pick(clist[1]): - return (MoveGroupInfoLevel.SUCCESS, "Picked object " + clist[1]) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while trying to pick object " + clist[1], - ) - elif clist[0] == "place": - self._last_plan = None - if g.place(clist[1]): - return (MoveGroupInfoLevel.SUCCESS, "Placed object " + clist[1]) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while trying to place object " + clist[1], - ) - elif clist[0] == "planner": - g.set_planner_id(clist[1]) - return (MoveGroupInfoLevel.SUCCESS, "Planner is now " + clist[1]) - elif clist[0] == "record" or clist[0] == "rec": - g.remember_joint_values(clist[1]) - return ( - MoveGroupInfoLevel.SUCCESS, - "Remembered current joint values under the name " + clist[1], - ) - elif clist[0] == "rand" or clist[0] == "random": - g.remember_joint_values(clist[1], g.get_random_joint_values()) - return ( - MoveGroupInfoLevel.SUCCESS, - "Remembered random joint values under the name " + clist[1], - ) - elif clist[0] == "del" or clist[0] == "delete": - g.forget_joint_values(clist[1]) - return ( - MoveGroupInfoLevel.SUCCESS, - "Forgot joint values under the name " + clist[1], - ) - elif clist[0] == "show": - known = g.get_remembered_joint_values() - if clist[1] in known: - return ( - MoveGroupInfoLevel.INFO, - "[" + " ".join([str(x) for x in known[clist[1]]]) + "]", - ) - else: - return ( - MoveGroupInfoLevel.WARN, - "Joint values for " + clist[1] + " are not known", - ) - elif clist[0] == "tol" or clist[0] == "tolerance": - try: - g.set_goal_tolerance(float(clist[1])) - return (MoveGroupInfoLevel.SUCCESS, "OK") - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse tolerance value '" + clist[1] + "'", - ) - elif clist[0] == "time": - try: - g.set_planning_time(float(clist[1])) - return (MoveGroupInfoLevel.SUCCESS, "OK") - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse planning duration value '" + clist[1] + "'", - ) - elif clist[0] == "constrain": - try: - g.set_path_constraints(clist[1]) - return (MoveGroupInfoLevel.SUCCESS, "OK") - except: - if self._db_host != None: - return ( - MoveGroupInfoLevel.WARN, - "Constraint " + clist[1] + " is not known.", - ) - else: - return (MoveGroupInfoLevel.WARN, "Not connected to a database.") - elif clist[0] == "wait": - try: - time.sleep(float(clist[1])) - return (MoveGroupInfoLevel.SUCCESS, clist[1] + " seconds passed") - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to wait '" + clist[1] + "' seconds", - ) - elif clist[0] == "database": - try: - g.set_constraints_database(clist[1], self._db_port) - self._db_host = clist[1] - return ( - MoveGroupInfoLevel.SUCCESS, - "Connected to " + self._db_host + ":" + str(self._db_port), - ) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to connect to '" - + clist[1] - + ":" - + str(self._db_port) - + "'", - ) - else: - return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") - - if len(clist) == 3: - if clist[0] == "go" and clist[1] in self.GO_DIRS: - self._last_plan = None - try: - offset = float(clist[2]) - (axis, factor) = self.GO_DIRS[clist[1]] - return self.command_go_offset(g, offset, factor, axis, clist[1]) - except MoveItCommanderException as e: - return ( - MoveGroupInfoLevel.WARN, - "Going " + clist[1] + ": " + str(e), - ) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse distance '" + clist[2] + "'", - ) - elif clist[0] == "allow" and (clist[1] == "look" or clist[1] == "looking"): - if ( - clist[2] == "1" - or clist[2] == "true" - or clist[2] == "T" - or clist[2] == "True" - ): - g.allow_looking(True) - else: - g.allow_looking(False) - return (MoveGroupInfoLevel.DEBUG, "OK") - elif clist[0] == "allow" and ( - clist[1] == "replan" or clist[1] == "replanning" - ): - if ( - clist[2] == "1" - or clist[2] == "true" - or clist[2] == "T" - or clist[2] == "True" - ): - g.allow_replanning(True) - else: - g.allow_replanning(False) - return (MoveGroupInfoLevel.DEBUG, "OK") - elif clist[0] == "database": - try: - g.set_constraints_database(clist[1], int(clist[2])) - self._db_host = clist[1] - self._db_port = int(clist[2]) - return ( - MoveGroupInfoLevel.SUCCESS, - "Connected to " + self._db_host + ":" + str(self._db_port), - ) - except: - self._db_host = None - return ( - MoveGroupInfoLevel.WARN, - "Unable to connect to '" + clist[1] + ":" + clist[2] + "'", - ) - if len(clist) == 4: - if clist[0] == "rotate": - try: - g.set_rpy_target([float(x) for x in clist[1:]]) - if g.go(): - return (MoveGroupInfoLevel.SUCCESS, "Rotation complete") - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while rotating to " + " ".join(clist[1:]), - ) - except MoveItCommanderException as e: - return (MoveGroupInfoLevel.WARN, str(e)) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse X-Y-Z rotation values '" - + " ".join(clist[1:]) - + "'", - ) - if len(clist) >= 7: - if clist[0] == "go": - self._last_plan = None - approx = False - if len(clist) > 7: - if clist[7] == "approx" or clist[7] == "approximate": - approx = True - try: - p = Pose() - p.position.x = float(clist[1]) - p.position.y = float(clist[2]) - p.position.z = float(clist[3]) - q = tf.transformations.quaternion_from_euler( - float(clist[4]), float(clist[5]), float(clist[6]) - ) - p.orientation.x = q[0] - p.orientation.y = q[1] - p.orientation.z = q[2] - p.orientation.w = q[3] - if approx: - g.set_joint_value_target(p, True) - else: - g.set_pose_target(p) - if g.go(): - return ( - MoveGroupInfoLevel.SUCCESS, - "Moved to pose target\n%s\n" % (str(p)), - ) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while moving to pose \n%s\n" % (str(p)), - ) - except MoveItCommanderException as e: - return ( - MoveGroupInfoLevel.WARN, - "Going to pose target: %s" % (str(e)), - ) - except: - return ( - MoveGroupInfoLevel.WARN, - "Unable to parse pose '" + " ".join(clist[1:]) + "'", - ) - - return (MoveGroupInfoLevel.WARN, "Unknown command: '" + cmd + "'") - - def command_show(self, g): - known = g.get_remembered_joint_values() - res = [] - for k in known.keys(): - res.append(k + " = [" + " ".join([str(x) for x in known[k]]) + "]") - return (MoveGroupInfoLevel.INFO, "\n".join(res)) - - def command_current(self, g): - res = ( - "joints = [" - + ", ".join([str(x) for x in g.get_current_joint_values()]) - + "]" - ) - if len(g.get_end_effector_link()) > 0: - res = ( - res - + "\n\n" - + g.get_end_effector_link() - + " pose = [\n" - + str(g.get_current_pose()) - + " ]" - ) - res = ( - res - + "\n" - + g.get_end_effector_link() - + " RPY = " - + str(g.get_current_rpy()) - ) - return (MoveGroupInfoLevel.INFO, res) - - def command_go_offset(self, g, offset, factor, dimension_index, direction_name): - if g.has_end_effector_link(): - try: - g.shift_pose_target(dimension_index, factor * offset) - if g.go(): - return ( - MoveGroupInfoLevel.SUCCESS, - "Moved " + direction_name + " by " + str(offset) + " m", - ) - else: - return ( - MoveGroupInfoLevel.FAIL, - "Failed while moving " + direction_name, - ) - except MoveItCommanderException as e: - return (MoveGroupInfoLevel.WARN, str(e)) - except: - return (MoveGroupInfoLevel.WARN, "Unable to process pose update") - else: - return ( - MoveGroupInfoLevel.WARN, - "No known end effector. Cannot move " + direction_name, - ) - - def resolve_command_alias(self, cmdorig): - cmd = cmdorig.lower() - if cmd == "which": - return "id" - if cmd == "groups": - return "use" - return cmdorig - - def get_help(self): - res = [] - res.append("Known commands:") - res.append(" help show this screen") - res.append(" allow looking enable/disable looking around") - res.append(" allow replanning enable/disable replanning") - res.append(" constrain clear path constraints") - res.append( - " constrain use the constraint as a path constraint" - ) - res.append(" current show the current state of the active group") - res.append( - " database display the current database connection (if any)" - ) - res.append( - " delete forget the joint values under the name " - ) - res.append( - " eef print the name of the end effector attached to the current group" - ) - res.append(" execute execute a previously computed motion plan") - res.append( - " go plan and execute a motion to the state " - ) - res.append(" go rand plan and execute a motion to a random state") - res.append( - " go

| plan and execute a motion in direction up|down|left|right|forward|backward for distance " - ) - res.append(" ground add a ground plane to the planning scene") - res.append( - " id|which display the name of the group that is operated on" - ) - res.append( - " joints display names of the joints in the active group" - ) - res.append( - " load [] load a set of interpreted commands from a file" - ) - res.append(" pick pick up object ") - res.append(" place place object ") - res.append(" plan plan a motion to the state ") - res.append(" plan rand plan a motion to a random state") - res.append(" planner use planner to plan next motion") - res.append( - " record record the current joint values under the name " - ) - res.append( - " rotate plan and execute a motion to a specified orientation (about the X,Y,Z axes)" - ) - res.append( - " save [] save the currently known variables as a set of commands" - ) - res.append( - " show display the names and values of the known states" - ) - res.append(" show display the value of a state") - res.append(" stop stop the active group") - res.append(" time show the configured allowed planning time") - res.append(" time set the allowed planning time") - res.append( - " tolerance show the tolerance for reaching the goal region" - ) - res.append( - " tolerance set the tolerance for reaching the goal region" - ) - res.append(" trace enable/disable replanning or looking around") - res.append( - " use switch to using the group named (and load it if necessary)" - ) - res.append(" use|groups show the group names that are already loaded") - res.append(" vars display the names of the known states") - res.append(" wait
sleep for
seconds") - res.append(" x = y assign the value of y to x") - res.append(" x = [v1 v2...] assign a vector of values to x") - res.append(" x[idx] = val assign a value to dimension idx of x") - return "\n".join(res) - - def get_keywords(self): - known_vars = [] - known_constr = [] - if self.get_active_group() != None: - known_vars = self.get_active_group().get_remembered_joint_values().keys() - known_constr = self.get_active_group().get_known_constraints() - groups = self._robot.get_group_names() - return { - "go": ["up", "down", "left", "right", "backward", "forward", "random"] - + list(known_vars), - "help": [], - "record": known_vars, - "show": known_vars, - "wait": [], - "delete": known_vars, - "database": [], - "current": [], - "use": groups, - "load": [], - "save": [], - "pick": [], - "place": [], - "plan": known_vars, - "allow": ["replanning", "looking"], - "constrain": known_constr, - "vars": [], - "joints": [], - "tolerance": [], - "time": [], - "eef": [], - "execute": [], - "ground": [], - "id": [], - } diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py deleted file mode 100644 index 44c171ca49..0000000000 --- a/moveit_commander/src/moveit_commander/move_group.py +++ /dev/null @@ -1,783 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Ioan Sucan, William Baker - -from geometry_msgs.msg import Pose, PoseStamped -from moveit_msgs.msg import ( - RobotTrajectory, - Grasp, - PlaceLocation, - Constraints, - RobotState, -) -from moveit_msgs.msg import ( - MoveItErrorCodes, - TrajectoryConstraints, - PlannerInterfaceDescription, - MotionPlanRequest, -) -from sensor_msgs.msg import JointState -import rospy -import tf -from moveit_ros_planning_interface import _moveit_move_group_interface -from .exception import MoveItCommanderException -import moveit_commander.conversions as conversions - - -class MoveGroupCommander(object): - """ - Execution of simple commands for a particular group - """ - - def __init__( - self, name, robot_description="robot_description", ns="", wait_for_servers=5.0 - ): - """Specify the group name for which to construct this commander instance. Throws an exception if there is an initialization error.""" - self._g = _moveit_move_group_interface.MoveGroupInterface( - name, robot_description, ns, wait_for_servers - ) - - def get_name(self): - """Get the name of the group this instance was initialized for""" - return self._g.get_name() - - def stop(self): - """Stop the current execution, if any""" - self._g.stop() - - def get_active_joints(self): - """Get the active joints of this group""" - return self._g.get_active_joints() - - def get_joints(self): - """Get the joints of this group""" - return self._g.get_joints() - - def get_variable_count(self): - """Return the number of variables used to parameterize a state in this group (larger or equal to number of DOF)""" - return self._g.get_variable_count() - - def has_end_effector_link(self): - """Check if this group has a link that is considered to be an end effector""" - return len(self._g.get_end_effector_link()) > 0 - - def get_end_effector_link(self): - """Get the name of the link that is considered to be an end-effector. Return an empty string if there is no end-effector.""" - return self._g.get_end_effector_link() - - def set_end_effector_link(self, link_name): - """Set the name of the link to be considered as an end effector""" - if not self._g.set_end_effector_link(link_name): - raise MoveItCommanderException("Unable to set end effector link") - - def get_interface_description(self): - """Get the description of the planner interface (list of planner ids)""" - desc = PlannerInterfaceDescription() - conversions.msg_from_string(desc, self._g.get_interface_description()) - return desc - - def get_pose_reference_frame(self): - """Get the reference frame assumed for poses of end-effectors""" - return self._g.get_pose_reference_frame() - - def set_pose_reference_frame(self, reference_frame): - """Set the reference frame to assume for poses of end-effectors""" - self._g.set_pose_reference_frame(reference_frame) - - def get_planning_frame(self): - """Get the name of the frame where all planning is performed""" - return self._g.get_planning_frame() - - def get_current_joint_values(self): - """Get the current configuration of the group as a list (these are values published on /joint_states)""" - return self._g.get_current_joint_values() - - def get_current_pose(self, end_effector_link=""): - """Get the current pose of the end-effector of the group. Throws an exception if there is not end-effector.""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - return conversions.list_to_pose_stamped( - self._g.get_current_pose(end_effector_link), self.get_planning_frame() - ) - else: - raise MoveItCommanderException( - "There is no end effector to get the pose of" - ) - - def get_current_rpy(self, end_effector_link=""): - """Get a list of 3 elements defining the [roll, pitch, yaw] of the end-effector. Throws an exception if there is not end-effector.""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - return self._g.get_current_rpy(end_effector_link) - else: - raise MoveItCommanderException("There is no end effector to get the rpy of") - - def get_random_joint_values(self): - return self._g.get_random_joint_values() - - def get_random_pose(self, end_effector_link=""): - if len(end_effector_link) > 0 or self.has_end_effector_link(): - return conversions.list_to_pose_stamped( - self._g.get_random_pose(end_effector_link), self.get_planning_frame() - ) - else: - raise MoveItCommanderException( - "There is no end effector to get the pose of" - ) - - def set_start_state_to_current_state(self): - self._g.set_start_state_to_current_state() - - def set_start_state(self, msg): - """ - Specify a start state for the group. - - Parameters - ---------- - msg : moveit_msgs/RobotState - - Examples - -------- - >>> from moveit_msgs.msg import RobotState - >>> from sensor_msgs.msg import JointState - >>> joint_state = JointState() - >>> joint_state.header = Header() - >>> joint_state.header.stamp = rospy.Time.now() - >>> joint_state.name = ['joint_a', 'joint_b'] - >>> joint_state.position = [0.17, 0.34] - >>> moveit_robot_state = RobotState() - >>> moveit_robot_state.joint_state = joint_state - >>> group.set_start_state(moveit_robot_state) - """ - self._g.set_start_state(conversions.msg_to_string(msg)) - - def get_current_state_bounded(self): - """Get the current state of the robot bounded.""" - s = RobotState() - c_str = self._g.get_current_state_bounded() - conversions.msg_from_string(s, c_str) - return s - - def get_current_state(self): - """Get the current state of the robot.""" - s = RobotState() - c_str = self._g.get_current_state() - conversions.msg_from_string(s, c_str) - return s - - def get_joint_value_target(self): - return self._g.get_joint_value_target() - - def set_joint_value_target(self, arg1, arg2=None, arg3=None): - """ - Specify a target joint configuration for the group. - - if the type of arg1 is one of the following: dict, list, JointState message, then no other arguments should be provided. - The dict should specify pairs of joint variable names and their target values, the list should specify all the variable values - for the group. The JointState message specifies the positions of some single-dof joints. - - if the type of arg1 is string, then arg2 is expected to be defined and be either a real value or a list of real values. This is - interpreted as setting a particular joint to a particular value. - - if the type of arg1 is Pose or PoseStamped, both arg2 and arg3 could be defined. If arg2 or arg3 are defined, their types must - be either string or bool. The string type argument is interpreted as the end-effector the pose is specified for (default is to use - the default end-effector), and the bool is used to decide whether the pose specified is approximate (default is false). This situation - allows setting the joint target of the group by calling IK. This does not send a pose to the planner and the planner will do no IK. - Instead, one IK solution will be computed first, and that will be sent to the planner. - """ - if isinstance(arg1, JointState): - if arg2 is not None or arg3 is not None: - raise MoveItCommanderException("Too many arguments specified") - if not self._g.set_joint_value_target_from_joint_state_message( - conversions.msg_to_string(arg1) - ): - raise MoveItCommanderException( - "Error setting joint target. Is the target within bounds?" - ) - - elif isinstance(arg1, str): - if arg2 is None: - raise MoveItCommanderException( - "Joint value expected when joint name specified" - ) - if arg3 is not None: - raise MoveItCommanderException("Too many arguments specified") - if not self._g.set_joint_value_target(arg1, arg2): - raise MoveItCommanderException( - "Error setting joint target. Is the target within bounds?" - ) - - elif isinstance(arg1, (Pose, PoseStamped)): - approx = False - eef = "" - if arg2 is not None: - if type(arg2) is str: - eef = arg2 - else: - if type(arg2) is bool: - approx = arg2 - else: - raise MoveItCommanderException("Unexpected type") - if arg3 is not None: - if type(arg3) is str: - eef = arg3 - else: - if type(arg3) is bool: - approx = arg3 - else: - raise MoveItCommanderException("Unexpected type") - r = False - if type(arg1) is PoseStamped: - r = self._g.set_joint_value_target_from_pose_stamped( - conversions.msg_to_string(arg1), eef, approx - ) - else: - r = self._g.set_joint_value_target_from_pose( - conversions.msg_to_string(arg1), eef, approx - ) - if not r: - if approx: - raise MoveItCommanderException( - "Error setting joint target. Does your IK solver support approximate IK?" - ) - else: - raise MoveItCommanderException( - "Error setting joint target. Is the IK solver functional?" - ) - - elif hasattr(arg1, "__iter__"): - if arg2 is not None or arg3 is not None: - raise MoveItCommanderException("Too many arguments specified") - if not self._g.set_joint_value_target(arg1): - raise MoveItCommanderException( - "Error setting joint target. Is the target within bounds?" - ) - - else: - raise MoveItCommanderException( - "Unsupported argument of type %s" % type(arg1) - ) - - def set_rpy_target(self, rpy, end_effector_link=""): - """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - if len(rpy) == 3: - if not self._g.set_rpy_target( - rpy[0], rpy[1], rpy[2], end_effector_link - ): - raise MoveItCommanderException("Unable to set orientation target") - else: - raise MoveItCommanderException("Expected [roll, pitch, yaw]") - else: - raise MoveItCommanderException( - "There is no end effector to set the pose for" - ) - - def set_orientation_target(self, q, end_effector_link=""): - """Specify a target orientation for the end-effector. Any position of the end-effector is acceptable.""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - if len(q) == 4: - if not self._g.set_orientation_target( - q[0], q[1], q[2], q[3], end_effector_link - ): - raise MoveItCommanderException("Unable to set orientation target") - else: - raise MoveItCommanderException("Expected [qx, qy, qz, qw]") - else: - raise MoveItCommanderException( - "There is no end effector to set the pose for" - ) - - def set_position_target(self, xyz, end_effector_link=""): - """Specify a target position for the end-effector. Any orientation of the end-effector is acceptable.""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - if not self._g.set_position_target( - xyz[0], xyz[1], xyz[2], end_effector_link - ): - raise MoveItCommanderException("Unable to set position target") - else: - raise MoveItCommanderException( - "There is no end effector to set the pose for" - ) - - def set_pose_target(self, pose, end_effector_link=""): - """Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" - """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ - if len(end_effector_link) > 0 or self.has_end_effector_link(): - ok = False - if type(pose) is PoseStamped: - old = self.get_pose_reference_frame() - self.set_pose_reference_frame(pose.header.frame_id) - ok = self._g.set_pose_target( - conversions.pose_to_list(pose.pose), end_effector_link - ) - self.set_pose_reference_frame(old) - elif type(pose) is Pose: - ok = self._g.set_pose_target( - conversions.pose_to_list(pose), end_effector_link - ) - else: - ok = self._g.set_pose_target(pose, end_effector_link) - if not ok: - raise MoveItCommanderException("Unable to set target pose") - else: - raise MoveItCommanderException( - "There is no end effector to set the pose for" - ) - - def set_pose_targets(self, poses, end_effector_link=""): - """Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw]""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - if not self._g.set_pose_targets( - [conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], - end_effector_link, - ): - raise MoveItCommanderException("Unable to set target poses") - else: - raise MoveItCommanderException("There is no end effector to set poses for") - - def shift_pose_target(self, axis, value, end_effector_link=""): - """Get the current pose of the end effector, add value to the corresponding axis (0..5: X, Y, Z, R, P, Y) and set the new pose as the pose target""" - if len(end_effector_link) > 0 or self.has_end_effector_link(): - pose = self._g.get_current_pose(end_effector_link) - # by default we get orientation as a quaternion list - # if we are updating a rotation axis however, we convert the orientation to RPY - if axis > 2: - (r, p, y) = tf.transformations.euler_from_quaternion(pose[3:]) - pose = [pose[0], pose[1], pose[2], r, p, y] - if axis >= 0 and axis < 6: - pose[axis] = pose[axis] + value - self.set_pose_target(pose, end_effector_link) - else: - raise MoveItCommanderException("An axis value between 0 and 5 expected") - else: - raise MoveItCommanderException("There is no end effector to set poses for") - - def clear_pose_target(self, end_effector_link): - """Clear the pose target for a particular end-effector""" - self._g.clear_pose_target(end_effector_link) - - def clear_pose_targets(self): - """Clear all known pose targets""" - self._g.clear_pose_targets() - - def set_random_target(self): - """Set a random joint configuration target""" - self._g.set_random_target() - - def get_named_targets(self): - """Get a list of all the names of joint configurations.""" - return self._g.get_named_targets() - - def set_named_target(self, name): - """Set a joint configuration by name. The name can be a name previlusy remembered with remember_joint_values() or a configuration specified in the SRDF.""" - if not self._g.set_named_target(name): - raise MoveItCommanderException( - "Unable to set target %s. Is the target within bounds?" % name - ) - - def get_named_target_values(self, target): - """Get a dictionary of joint values of a named target""" - return self._g.get_named_target_values(target) - - def remember_joint_values(self, name, values=None): - """Record the specified joint configuration of the group under the specified name. If no values are specified, the current state of the group is recorded.""" - if values is None: - values = self.get_current_joint_values() - self._g.remember_joint_values(name, values) - - def get_remembered_joint_values(self): - """Get a dictionary that maps names to joint configurations for the group""" - return self._g.get_remembered_joint_values() - - def forget_joint_values(self, name): - """Forget a stored joint configuration""" - self._g.forget_joint_values(name) - - def get_goal_tolerance(self): - """Return a tuple of goal tolerances: joint, position and orientation.""" - return ( - self.get_goal_joint_tolerance(), - self.get_goal_position_tolerance(), - self.get_goal_orientation_tolerance(), - ) - - def get_goal_joint_tolerance(self): - """Get the tolerance for achieving a joint goal (distance for each joint variable)""" - return self._g.get_goal_joint_tolerance() - - def get_goal_position_tolerance(self): - """When moving to a position goal or to a pose goal, the tolerance for the goal position is specified as the radius a sphere around the target origin of the end-effector""" - return self._g.get_goal_position_tolerance() - - def get_goal_orientation_tolerance(self): - """When moving to an orientation goal or to a pose goal, the tolerance for the goal orientation is specified as the distance (roll, pitch, yaw) to the target origin of the end-effector""" - return self._g.get_goal_orientation_tolerance() - - def set_goal_tolerance(self, value): - """Set the joint, position and orientation goal tolerances simultaneously""" - self._g.set_goal_tolerance(value) - - def set_goal_joint_tolerance(self, value): - """Set the tolerance for a target joint configuration""" - self._g.set_goal_joint_tolerance(value) - - def set_goal_position_tolerance(self, value): - """Set the tolerance for a target end-effector position""" - self._g.set_goal_position_tolerance(value) - - def set_goal_orientation_tolerance(self, value): - """Set the tolerance for a target end-effector orientation""" - self._g.set_goal_orientation_tolerance(value) - - def allow_looking(self, value): - """Enable/disable looking around for motion planning""" - self._g.allow_looking(value) - - def allow_replanning(self, value): - """Enable/disable replanning""" - self._g.allow_replanning(value) - - def get_known_constraints(self): - """Get a list of names for the constraints specific for this group, as read from the warehouse""" - return self._g.get_known_constraints() - - def get_path_constraints(self): - """Get the actual path constraints in form of a moveit_msgs.msgs.Constraints""" - c = Constraints() - c_str = self._g.get_path_constraints() - conversions.msg_from_string(c, c_str) - return c - - def set_path_constraints(self, value): - """Specify the path constraints to be used (as read from the database)""" - if value is None: - self.clear_path_constraints() - else: - if type(value) is Constraints: - self._g.set_path_constraints_from_msg(conversions.msg_to_string(value)) - elif not self._g.set_path_constraints(value): - raise MoveItCommanderException( - "Unable to set path constraints " + value - ) - - def clear_path_constraints(self): - """Specify that no path constraints are to be used during motion planning""" - self._g.clear_path_constraints() - - def get_trajectory_constraints(self): - """Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints""" - c = TrajectoryConstraints() - c_str = self._g.get_trajectory_constraints() - conversions.msg_from_string(c, c_str) - return c - - def set_trajectory_constraints(self, value): - """Specify the trajectory constraints to be used (setting from database is not implemented yet)""" - if value is None: - self.clear_trajectory_constraints() - else: - if type(value) is TrajectoryConstraints: - self._g.set_trajectory_constraints_from_msg( - conversions.msg_to_string(value) - ) - else: - raise MoveItCommanderException( - "Unable to set trajectory constraints " + value - ) - - def clear_trajectory_constraints(self): - """Specify that no trajectory constraints are to be used during motion planning""" - self._g.clear_trajectory_constraints() - - def set_constraints_database(self, host, port): - """Specify which database to connect to for loading possible path constraints""" - self._g.set_constraints_database(host, port) - - def set_planning_time(self, seconds): - """Specify the amount of time to be used for motion planning.""" - self._g.set_planning_time(seconds) - - def get_planning_time(self): - """Specify the amount of time to be used for motion planning.""" - return self._g.get_planning_time() - - def set_planning_pipeline_id(self, planning_pipeline): - """Specify which planning pipeline to use when motion planning (e.g. ompl, pilz_industrial_motion_planner)""" - self._g.set_planning_pipeline_id(planning_pipeline) - - def get_planning_pipeline_id(self): - """Get the current planning_pipeline_id (e.g. ompl, pilz_industrial_motion_planner)""" - return self._g.get_planning_pipeline_id() - - def set_planner_id(self, planner_id): - """Specify which planner of the currently selected pipeline to use when motion planning (e.g. RRTConnect, LIN)""" - self._g.set_planner_id(planner_id) - - def get_planner_id(self): - """Get the current planner_id (e.g. RRTConnect, LIN) of the currently selected pipeline""" - return self._g.get_planner_id() - - def set_num_planning_attempts(self, num_planning_attempts): - """Set the number of times the motion plan is to be computed from scratch before the shortest solution is returned. The default value is 1.""" - self._g.set_num_planning_attempts(num_planning_attempts) - - def set_workspace(self, ws): - """Set the workspace for the robot as either [], [minX, minY, maxX, maxY] or [minX, minY, minZ, maxX, maxY, maxZ]""" - if len(ws) == 0: - self._g.set_workspace(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) - else: - if len(ws) == 4: - self._g.set_workspace(ws[0], ws[1], 0.0, ws[2], ws[3], 0.0) - else: - if len(ws) == 6: - self._g.set_workspace(ws[0], ws[1], ws[2], ws[3], ws[4], ws[5]) - else: - raise MoveItCommanderException( - "Expected 0, 4 or 6 values in list specifying workspace" - ) - - def set_max_velocity_scaling_factor(self, value): - """Set a scaling factor to reduce the maximum joint velocities. Allowed values are in (0,1]. - The default value is set in the joint_limits.yaml of the moveit_config package.""" - if value > 0 and value <= 1: - self._g.set_max_velocity_scaling_factor(value) - else: - raise MoveItCommanderException( - "Expected value in the range from 0 to 1 for scaling factor" - ) - - def set_max_acceleration_scaling_factor(self, value): - """Set a scaling factor to reduce the maximum joint accelerations. Allowed values are in (0,1]. - The default value is set in the joint_limits.yaml of the moveit_config package.""" - if value > 0 and value <= 1: - self._g.set_max_acceleration_scaling_factor(value) - else: - raise MoveItCommanderException( - "Expected value in the range from 0 to 1 for scaling factor" - ) - - def go(self, joints=None, wait=True): - """Set the target of the group and then move the group to the specified target""" - if type(joints) is bool: - wait = joints - joints = None - - elif type(joints) is JointState: - self.set_joint_value_target(joints) - - elif type(joints) is Pose: - self.set_pose_target(joints) - - elif joints is not None: - try: - self.set_joint_value_target(self.get_remembered_joint_values()[joints]) - except (KeyError, TypeError): - self.set_joint_value_target(joints) - if wait: - return self._g.move() - else: - return self._g.async_move() - - def plan(self, joints=None): - """Return a tuple of the motion planning results such as - (success flag : boolean, trajectory message : RobotTrajectory, - planning time : float, error code : MoveitErrorCodes)""" - if type(joints) is JointState: - self.set_joint_value_target(joints) - - elif type(joints) is Pose: - self.set_pose_target(joints) - - elif joints is not None: - try: - self.set_joint_value_target(self.get_remembered_joint_values()[joints]) - except MoveItCommanderException: - self.set_joint_value_target(joints) - - (error_code_msg, trajectory_msg, planning_time) = self._g.plan() - - error_code = MoveItErrorCodes() - error_code.deserialize(error_code_msg) - plan = RobotTrajectory() - return ( - error_code.val == MoveItErrorCodes.SUCCESS, - plan.deserialize(trajectory_msg), - planning_time, - error_code, - ) - - def construct_motion_plan_request(self): - """Returns a MotionPlanRequest filled with the current goals of the move_group_interface""" - mpr = MotionPlanRequest() - return mpr.deserialize(self._g.construct_motion_plan_request()) - - def compute_cartesian_path( - self, - waypoints, - eef_step, - jump_threshold, - avoid_collisions=True, - path_constraints=None, - ): - """Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory.""" - if path_constraints: - if type(path_constraints) is Constraints: - constraints_str = conversions.msg_to_string(path_constraints) - else: - raise MoveItCommanderException( - "Unable to set path constraints, unknown constraint type " - + type(path_constraints) - ) - (ser_path, fraction) = self._g.compute_cartesian_path( - [conversions.pose_to_list(p) for p in waypoints], - eef_step, - jump_threshold, - avoid_collisions, - constraints_str, - ) - else: - (ser_path, fraction) = self._g.compute_cartesian_path( - [conversions.pose_to_list(p) for p in waypoints], - eef_step, - jump_threshold, - avoid_collisions, - ) - - path = RobotTrajectory() - path.deserialize(ser_path) - return (path, fraction) - - def execute(self, plan_msg, wait=True): - """Execute a previously planned path""" - if wait: - return self._g.execute(conversions.msg_to_string(plan_msg)) - else: - return self._g.async_execute(conversions.msg_to_string(plan_msg)) - - def attach_object(self, object_name, link_name="", touch_links=[]): - """Given the name of an object existing in the planning scene, attach it to a link. The link used is specified by the second argument. If left unspecified, the end-effector link is used, if one is known. If there is no end-effector link, the first link in the group is used. If no link is identified, failure is reported. True is returned if an attach request was successfully sent to the move_group node. This does not verify that the attach request also was successfully applied by move_group.""" - return self._g.attach_object(object_name, link_name, touch_links) - - def detach_object(self, name=""): - """Given the name of a link, detach the object(s) from that link. If no such link exists, the name is interpreted as an object name. If there is no name specified, an attempt is made to detach all objects attached to any link in the group.""" - return self._g.detach_object(name) - - def pick(self, object_name, grasp=[], plan_only=False): - """Pick the named object. A grasp message, or a list of Grasp messages can also be specified as argument.""" - if type(grasp) is Grasp: - return self._g.pick( - object_name, conversions.msg_to_string(grasp), plan_only - ) - else: - return self._g.pick( - object_name, [conversions.msg_to_string(x) for x in grasp], plan_only - ) - - def place(self, object_name, location=None, plan_only=False): - """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" - result = False - if not location: - result = self._g.place(object_name, plan_only) - elif type(location) is PoseStamped: - old = self.get_pose_reference_frame() - self.set_pose_reference_frame(location.header.frame_id) - result = self._g.place( - object_name, conversions.pose_to_list(location.pose), plan_only - ) - self.set_pose_reference_frame(old) - elif type(location) is Pose: - result = self._g.place( - object_name, conversions.pose_to_list(location), plan_only - ) - elif type(location) is PlaceLocation: - result = self._g.place( - object_name, conversions.msg_to_string(location), plan_only - ) - elif type(location) is list: - if location: - if type(location[0]) is PlaceLocation: - result = self._g.place_locations_list( - object_name, - [conversions.msg_to_string(x) for x in location], - plan_only, - ) - elif type(location[0]) is PoseStamped: - result = self._g.place_poses_list( - object_name, - [conversions.msg_to_string(x) for x in location], - plan_only, - ) - else: - raise MoveItCommanderException( - "Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object" - ) - else: - raise MoveItCommanderException( - "Parameter location must be a Pose, PoseStamped, PlaceLocation, list of PoseStamped or list of PlaceLocation object" - ) - return result - - def set_support_surface_name(self, value): - """Set the support surface name for a place operation""" - self._g.set_support_surface_name(value) - - def retime_trajectory( - self, - ref_state_in, - traj_in, - velocity_scaling_factor=1.0, - acceleration_scaling_factor=1.0, - algorithm="iterative_time_parameterization", - ): - ser_ref_state_in = conversions.msg_to_string(ref_state_in) - ser_traj_in = conversions.msg_to_string(traj_in) - ser_traj_out = self._g.retime_trajectory( - ser_ref_state_in, - ser_traj_in, - velocity_scaling_factor, - acceleration_scaling_factor, - algorithm, - ) - traj_out = RobotTrajectory() - traj_out.deserialize(ser_traj_out) - return traj_out - - def get_jacobian_matrix(self, joint_values, reference_point=None): - """Get the jacobian matrix of the group as a list""" - return self._g.get_jacobian_matrix( - joint_values, - [0.0, 0.0, 0.0] if reference_point is None else reference_point, - ) - - def enforce_bounds(self, robot_state_msg): - """Takes a moveit_msgs RobotState and enforces the state bounds, based on the C++ RobotState enforceBounds()""" - s = RobotState() - c_str = self._g.enforce_bounds(conversions.msg_to_string(robot_state_msg)) - conversions.msg_from_string(s, c_str) - return s diff --git a/moveit_commander/src/moveit_commander/planning_scene_interface.py b/moveit_commander/src/moveit_commander/planning_scene_interface.py deleted file mode 100644 index dd58e6529f..0000000000 --- a/moveit_commander/src/moveit_commander/planning_scene_interface.py +++ /dev/null @@ -1,364 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# 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: Ioan Sucan, Felix Messmer - -import rospy -from rosgraph.names import ns_join -from . import conversions - -from moveit_msgs.msg import PlanningScene, CollisionObject, AttachedCollisionObject -from moveit_ros_planning_interface import _moveit_planning_scene_interface -from geometry_msgs.msg import Pose, Point -from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle -from .exception import MoveItCommanderException -from moveit_msgs.srv import ApplyPlanningScene, ApplyPlanningSceneRequest - -try: - from pyassimp import pyassimp -except: - # support pyassimp > 3.0 - try: - import pyassimp - except: - pyassimp = False - print( - "Failed to import pyassimp, see https://github.com/ros-planning/moveit/issues/86 for more info" - ) - - -class PlanningSceneInterface(object): - """ - Python interface for a C++ PlanningSceneInterface. - Uses both C++ wrapped methods and scene manipulation topics - to manipulate the PlanningScene managed by the PlanningSceneMonitor. - See wrap_python_planning_scene_interface.cpp for the wrapped methods. - """ - - def __init__(self, ns="", synchronous=False, service_timeout=5.0): - """Create a planning scene interface; it uses both C++ wrapped methods and scene manipulation topics.""" - self._psi = _moveit_planning_scene_interface.PlanningSceneInterface(ns) - - self._pub_co = rospy.Publisher( - ns_join(ns, "collision_object"), CollisionObject, queue_size=100 - ) - self._pub_aco = rospy.Publisher( - ns_join(ns, "attached_collision_object"), - AttachedCollisionObject, - queue_size=100, - ) - self.__synchronous = synchronous - if self.__synchronous: - self._apply_planning_scene_diff = rospy.ServiceProxy( - ns_join(ns, "apply_planning_scene"), ApplyPlanningScene - ) - self._apply_planning_scene_diff.wait_for_service(service_timeout) - - def __submit(self, collision_object, attach=False): - if self.__synchronous: - diff_req = self.__make_planning_scene_diff_req(collision_object, attach) - self._apply_planning_scene_diff.call(diff_req) - else: - if attach: - self._pub_aco.publish(collision_object) - else: - self._pub_co.publish(collision_object) - - def add_object(self, collision_object): - """Add an object to the planning scene""" - self.__submit(collision_object, attach=False) - - def add_sphere(self, name, pose, radius=1): - """Add a sphere to the planning scene""" - co = self.__make_sphere(name, pose, radius) - self.__submit(co, attach=False) - - def add_cylinder(self, name, pose, height, radius): - """Add a cylinder to the planning scene""" - co = self.__make_cylinder(name, pose, height, radius) - self.__submit(co, attach=False) - - def add_mesh(self, name, pose, filename, size=(1, 1, 1)): - """Add a mesh to the planning scene""" - co = self.__make_mesh(name, pose, filename, size) - self.__submit(co, attach=False) - - def add_box(self, name, pose, size=(1, 1, 1)): - """Add a box to the planning scene""" - co = self.__make_box(name, pose, size) - self.__submit(co, attach=False) - - def add_plane(self, name, pose, normal=(0, 0, 1), offset=0): - """Add a plane to the planning scene""" - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - p = Plane() - p.coef = list(normal) - p.coef.append(offset) - co.planes = [p] - co.plane_poses = [pose.pose] - self.__submit(co, attach=False) - - def attach_object(self, attached_collision_object): - """Attach an object in the planning scene""" - self.__submit(attached_collision_object, attach=True) - - def attach_mesh( - self, link, name, pose=None, filename="", size=(1, 1, 1), touch_links=[] - ): - aco = AttachedCollisionObject() - if (pose is not None) and filename: - aco.object = self.__make_mesh(name, pose, filename, size) - else: - aco.object = self.__make_existing(name) - aco.link_name = link - aco.touch_links = [link] - if len(touch_links) > 0: - aco.touch_links = touch_links - self.__submit(aco, attach=True) - - def attach_box(self, link, name, pose=None, size=(1, 1, 1), touch_links=[]): - aco = AttachedCollisionObject() - if pose is not None: - aco.object = self.__make_box(name, pose, size) - else: - aco.object = self.__make_existing(name) - aco.link_name = link - if len(touch_links) > 0: - aco.touch_links = touch_links - else: - aco.touch_links = [link] - self.__submit(aco, attach=True) - - def clear(self): - """Remove all objects from the planning scene""" - self.remove_attached_object() - self.remove_world_object() - - def remove_world_object(self, name=None): - """ - Remove an object from planning scene, or all if no name is provided - """ - co = CollisionObject() - co.operation = CollisionObject.REMOVE - if name is not None: - co.id = name - self.__submit(co, attach=False) - - def remove_attached_object(self, link=None, name=None): - """ - Remove an attached object from the robot, or all objects attached to the link if no name is provided, - or all attached objects in the scene if neither link nor name are provided. - - Removed attached objects remain in the scene as world objects. - Call remove_world_object afterwards to remove them from the scene. - """ - aco = AttachedCollisionObject() - aco.object.operation = CollisionObject.REMOVE - if link is not None: - aco.link_name = link - if name is not None: - aco.object.id = name - self.__submit(aco, attach=True) - - def get_known_object_names(self, with_type=False): - """ - Get the names of all known objects in the world. If with_type is set to true, only return objects that have a known type. - """ - return self._psi.get_known_object_names(with_type) - - def get_known_object_names_in_roi( - self, minx, miny, minz, maxx, maxy, maxz, with_type=False - ): - """ - Get the names of known objects in the world that are located within a bounding region (specified in the frame reported by - get_planning_frame()). If with_type is set to true, only return objects that have a known type. - """ - return self._psi.get_known_object_names_in_roi( - minx, miny, minz, maxx, maxy, maxz, with_type - ) - - def get_object_poses(self, object_ids): - """ - Get the poses from the objects identified by the given object ids list. - """ - ser_ops = self._psi.get_object_poses(object_ids) - ops = dict() - for key in ser_ops: - msg = Pose() - conversions.msg_from_string(msg, ser_ops[key]) - ops[key] = msg - return ops - - def get_objects(self, object_ids=[]): - """ - Get the objects identified by the given object ids list. If no ids are provided, return all the known objects. - """ - ser_objs = self._psi.get_objects(object_ids) - objs = dict() - for key in ser_objs: - msg = CollisionObject() - conversions.msg_from_string(msg, ser_objs[key]) - objs[key] = msg - return objs - - def get_attached_objects(self, object_ids=[]): - """ - Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects. - """ - ser_aobjs = self._psi.get_attached_objects(object_ids) - aobjs = dict() - for key in ser_aobjs: - msg = AttachedCollisionObject() - conversions.msg_from_string(msg, ser_aobjs[key]) - aobjs[key] = msg - return aobjs - - def apply_planning_scene(self, planning_scene_message): - """ - Applies the planning scene message. - """ - return self._psi.apply_planning_scene( - conversions.msg_to_string(planning_scene_message) - ) - - @staticmethod - def __make_existing(name): - """ - Create an empty Collision Object. Used when the object already exists - """ - co = CollisionObject() - co.id = name - return co - - @staticmethod - def __make_box(name, pose, size): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - co.pose = pose.pose - box = SolidPrimitive() - box.type = SolidPrimitive.BOX - box.dimensions = list(size) - co.primitives = [box] - return co - - @staticmethod - def __make_mesh(name, pose, filename, scale=(1, 1, 1)): - co = CollisionObject() - if pyassimp is False: - raise MoveItCommanderException( - "Pyassimp needs patch https://launchpadlibrarian.net/319496602/patchPyassim.txt" - ) - scene = pyassimp.load(filename) - if not scene.meshes or len(scene.meshes) == 0: - raise MoveItCommanderException("There are no meshes in the file") - if len(scene.meshes[0].faces) == 0: - raise MoveItCommanderException("There are no faces in the mesh") - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - co.pose = pose.pose - - mesh = Mesh() - first_face = scene.meshes[0].faces[0] - if hasattr(first_face, "__len__"): - for face in scene.meshes[0].faces: - if len(face) == 3: - triangle = MeshTriangle() - triangle.vertex_indices = [face[0], face[1], face[2]] - mesh.triangles.append(triangle) - elif hasattr(first_face, "indices"): - for face in scene.meshes[0].faces: - if len(face.indices) == 3: - triangle = MeshTriangle() - triangle.vertex_indices = [ - face.indices[0], - face.indices[1], - face.indices[2], - ] - mesh.triangles.append(triangle) - else: - raise MoveItCommanderException( - "Unable to build triangles from mesh due to mesh object structure" - ) - for vertex in scene.meshes[0].vertices: - point = Point() - point.x = vertex[0] * scale[0] - point.y = vertex[1] * scale[1] - point.z = vertex[2] * scale[2] - mesh.vertices.append(point) - co.meshes = [mesh] - pyassimp.release(scene) - return co - - @staticmethod - def __make_sphere(name, pose, radius): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - co.pose = pose.pose - sphere = SolidPrimitive() - sphere.type = SolidPrimitive.SPHERE - sphere.dimensions = [radius] - co.primitives = [sphere] - return co - - @staticmethod - def __make_cylinder(name, pose, height, radius): - co = CollisionObject() - co.operation = CollisionObject.ADD - co.id = name - co.header = pose.header - co.pose = pose.pose - cylinder = SolidPrimitive() - cylinder.type = SolidPrimitive.CYLINDER - cylinder.dimensions = [height, radius] - co.primitives = [cylinder] - return co - - @staticmethod - def __make_planning_scene_diff_req(collision_object, attach=False): - scene = PlanningScene() - scene.is_diff = True - scene.robot_state.is_diff = True - if attach: - scene.robot_state.attached_collision_objects = [collision_object] - else: - scene.world.collision_objects = [collision_object] - planning_scene_diff_req = ApplyPlanningSceneRequest() - planning_scene_diff_req.scene = scene - return planning_scene_diff_req diff --git a/moveit_commander/src/moveit_commander/robot.py b/moveit_commander/src/moveit_commander/robot.py deleted file mode 100644 index e86e44d723..0000000000 --- a/moveit_commander/src/moveit_commander/robot.py +++ /dev/null @@ -1,329 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Willow Garage, Inc. -# 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: Ioan Sucan - -from moveit_commander import MoveGroupCommander -from .exception import MoveItCommanderException -from moveit_ros_planning_interface import _moveit_robot_interface -from moveit_msgs.msg import RobotState -from visualization_msgs.msg import MarkerArray -import moveit_commander.conversions as conversions - - -class RobotCommander(object): - class Joint(object): - def __init__(self, robot, name): - self._robot = robot - self._name = name - - def name(self): - return self._name - - def variable_count(self): - """ - @return number of the list that _Joint__get_joint_limits - methods returns. - @see: http://docs.ros.org/en/latest/api/moveit_core/html/cpp/classmoveit_1_1core_1_1JointModel.html#details - for more about variable. - """ - return len(self.__get_joint_limits()) - - def bounds(self): - """ - @return: Either a single list of min and max joint limits, or - a set of those lists, depending on the number of variables - available in this joint. - """ - l = self.__get_joint_limits() - if len(l) == 1: - return l[0] - else: - return l - - def min_bound(self): - """ - @return: Either a single min joint limit value, or - a set of min values, depending on the number of variables - available in this joint. - """ - limits = self.__get_joint_limits() - if len(limits) == 1: - return limits[0][0] - else: - return [l[0] for l in limits] - - def max_bound(self): - """ - @return: Either a single max joint limit value, or - a set of max values, depending on the number of variables - available in this joint. - """ - limits = self.__get_joint_limits() - if len(limits) == 1: - return limits[0][1] - else: - return [l[1] for l in limits] - - def value(self): - """ - @rtype float - - (Editor's comment by @130s) I doubt there's a case where this method goes into - "else" block, because get_current_joint_values always return a single list. - - cf. getCurrentJointValues https://github.com/ros-planning/moveit_ros/blob/8e819dda2b19462b8d0c5aacc69706c8a9d8d883/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp#L176 - """ - vals = self._robot._r.get_current_joint_values(self._name) - if len(vals) == 1: - return vals[0] - else: - return vals - - def move(self, position, wait=True): - """ - @param position [float]: List of joint angles to achieve. - @param wait bool: If false, the commands gets operated asynchronously. - """ - group = self._robot.get_default_owner_group(self.name()) - if group is None: - raise MoveItCommanderException( - "There is no known group containing joint %s. Cannot move." - % self._name - ) - gc = self._robot.get_group(group) - if gc is not None: - gc.set_joint_value_target(gc.get_current_joint_values()) - gc.set_joint_value_target(self._name, position) - return gc.go(wait) - return False - - def __get_joint_limits(self): - """ - @return: A list of length of 2 that contains max and min positional - limits of the specified joint. - """ - return self._robot._r.get_joint_limits(self._name) - - class Link(object): - def __init__(self, robot, name): - self._robot = robot - self._name = name - - def name(self): - return self._name - - def pose(self): - """ - @rtype: geometry_msgs.Pose - """ - return conversions.list_to_pose_stamped( - self._robot._r.get_link_pose(self._name), - self._robot.get_planning_frame(), - ) - - def __init__(self, robot_description="robot_description", ns=""): - self._robot_description = robot_description - self._ns = ns - self._r = _moveit_robot_interface.RobotInterface(robot_description, ns) - self._groups = {} - self._joint_owner_groups = {} - - def get_planning_frame(self): - """ - Get the frame of reference in which planning is done (and environment - is maintained) - """ - return self._r.get_planning_frame() - - def get_robot_markers(self, *args): - """Get a MarkerArray of the markers that make up this robot - - Usage: - (): gets all markers for current state - state (RobotState): gets markers for a particular state - values (dict): get markers with given values - values, links (dict, list): get markers with given values and these links - group (string): get all markers for a group - group, values (string, dict): get all markers for a group with desired values - """ - mrkr = MarkerArray() - if not args: - conversions.msg_from_string(mrkr, self._r.get_robot_markers()) - else: - if isinstance(args[0], RobotState): - msg_str = conversions.msg_to_string(args[0]) - conversions.msg_from_string(mrkr, self._r.get_robot_markers(msg_str)) - elif isinstance(args[0], dict): - conversions.msg_from_string(mrkr, self._r.get_robot_markers(*args)) - elif isinstance(args[0], str): - conversions.msg_from_string(mrkr, self._r.get_group_markers(*args)) - else: - raise MoveItCommanderException("Unexpected type") - return mrkr - - def get_root_link(self): - """Get the name of the root link of the robot model""" - return self._r.get_robot_root_link() - - def get_active_joint_names(self, group=None): - """ - Get the names of all the movable joints that make up a group. - If no group name is specified, all joints in the robot model are returned. - Excludes fixed and mimic joints. - """ - if group is not None: - if self.has_group(group): - return self._r.get_group_active_joint_names(group) - else: - raise MoveItCommanderException("There is no group named %s" % group) - else: - return self._r.get_active_joint_names() - - def get_joint_names(self, group=None): - """ - Get the names of all the movable joints that make up a group. - If no group name is specified, all joints in the robot model are returned. - Includes fixed and mimic joints. - """ - if group is not None: - if self.has_group(group): - return self._r.get_group_joint_names(group) - else: - raise MoveItCommanderException("There is no group named %s" % group) - else: - return self._r.get_joint_names() - - def get_link_names(self, group=None): - """ - Get the links that make up a group. If no group name is specified, - all the links in the robot model are returned. - """ - if group is not None: - if self.has_group(group): - return self._r.get_group_link_names(group) - else: - raise MoveItCommanderException("There is no group named %s" % group) - else: - return self._r.get_link_names() - - def get_group_names(self): - """Get the names of the groups defined for the robot""" - return self._r.get_group_names() - - def get_current_state(self): - """Get a RobotState message describing the current state of the robot""" - s = RobotState() - s.deserialize(self._r.get_current_state()) - return s - - def get_current_variable_values(self): - """ - Get a dictionary mapping variable names to values. - Note that a joint may consist of one or more variables. - """ - return self._r.get_current_variable_values() - - def get_joint(self, name): - """ - @param name str: Name of movegroup - @rtype: moveit_commander.robot.Joint - @raise exception: MoveItCommanderException - """ - if name in self.get_joint_names(): - return self.Joint(self, name) - else: - raise MoveItCommanderException("There is no joint named %s" % name) - - def get_link(self, name): - """ - @param name str: Name of movegroup - @rtype: moveit_commander.robot.Link - @raise exception: MoveItCommanderException - """ - if name in self.get_link_names(): - return self.Link(self, name) - else: - raise MoveItCommanderException("There is no link named %s" % name) - - def get_group(self, name): - """ - @param name str: Name of movegroup - @rtype: moveit_commander.MoveGroupCommander - """ - if not name in self._groups: - if not self.has_group(name): - raise MoveItCommanderException("There is no group named %s" % name) - self._groups[name] = MoveGroupCommander( - name, self._robot_description, self._ns - ) - return self._groups[name] - - def has_group(self, name): - """ - @param name str: Name of movegroup - @rtype: bool - """ - return self._r.has_group(name) - - def get_default_owner_group(self, joint_name): - """ - Get the name of the smallest group (fewest joints) that includes - the joint name specified as argument. - """ - if not joint_name in self._joint_owner_groups: - group = None - for g in self.get_group_names(): - if joint_name in self.get_joint_names(g): - if group is None: - group = g - else: - if len(self.get_link_names(g)) < len( - self.get_link_names(group) - ): - group = g - self._joint_owner_groups[joint_name] = group - return self._joint_owner_groups[joint_name] - - def __getattr__(self, name): - """ - We catch the names of groups, joints and links to allow easy access - to their properties. - """ - if name in self.get_group_names(): - return self.get_group(name) - elif name in self.get_joint_names(): - return self.Joint(self, name) - elif name in self.get_link_names(): - return self.Link(self, name) - else: - return object.__getattribute__(self, name) diff --git a/moveit_commander/src/moveit_commander/roscpp_initializer.py b/moveit_commander/src/moveit_commander/roscpp_initializer.py deleted file mode 100644 index 8f7f5145b3..0000000000 --- a/moveit_commander/src/moveit_commander/roscpp_initializer.py +++ /dev/null @@ -1,45 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Ioan Sucan - -from moveit_ros_planning_interface import _moveit_roscpp_initializer - - -def roscpp_initialize(args): - # remove __name:= argument - args2 = [a for a in args if not a.startswith("__name:=")] - _moveit_roscpp_initializer.roscpp_init("move_group_commander_wrappers", args2) - - -def roscpp_shutdown(): - _moveit_roscpp_initializer.roscpp_shutdown() diff --git a/moveit_commander/test/CMakeLists.txt b/moveit_commander/test/CMakeLists.txt deleted file mode 100644 index 191e89a9a5..0000000000 --- a/moveit_commander/test/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - - add_rostest(python_moveit_commander.test) - add_rostest(python_moveit_commander_ns.test) - add_rostest(python_time_parameterization.test) -endif() diff --git a/moveit_commander/test/python_moveit_commander.py b/moveit_commander/test/python_moveit_commander.py deleted file mode 100755 index 75054d8a90..0000000000 --- a/moveit_commander/test/python_moveit_commander.py +++ /dev/null @@ -1,169 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: William Baker - -import unittest - -import genpy -import numpy as np -import rospy -import rostest -import os - -from moveit_msgs.msg import RobotState -from sensor_msgs.msg import JointState - -from moveit_commander import RobotCommander, PlanningSceneInterface - - -class PythonMoveitCommanderTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - JOINT_NAMES = [ - "joint_1", - "joint_2", - "joint_3", - "joint_4", - "joint_5", - "joint_6", - ] - - @classmethod - def setUpClass(self): - self.commander = RobotCommander("robot_description") - self.group = self.commander.get_group(self.PLANNING_GROUP) - - @classmethod - def tearDown(self): - pass - - def test_enforce_bounds_empty_state(self): - in_msg = RobotState() - with self.assertRaises(genpy.DeserializationError): - self.group.enforce_bounds(in_msg) - - def test_enforce_bounds(self): - in_msg = RobotState() - in_msg.joint_state.header.frame_id = "base_link" - in_msg.joint_state.name = self.JOINT_NAMES - in_msg.joint_state.position = [0] * 6 - in_msg.joint_state.position[0] = 1000 - - out_msg = self.group.enforce_bounds(in_msg) - - self.assertEqual(in_msg.joint_state.position[0], 1000) - self.assertLess(out_msg.joint_state.position[0], 1000) - - def test_get_current_state(self): - expected_state = RobotState() - expected_state.joint_state.header.frame_id = "base_link" - expected_state.multi_dof_joint_state.header.frame_id = "base_link" - expected_state.joint_state.name = self.JOINT_NAMES - expected_state.joint_state.position = [0] * 6 - self.assertEqual(self.group.get_current_state(), expected_state) - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - success1, plan1, time1, err1 = self.plan(current + 0.2) - success2, plan2, time2, err2 = self.plan(current + 0.2) - self.assertTrue(success1) - self.assertTrue(success2) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - success3, plan3, time3, err3 = self.plan(current) - self.assertTrue(success3) - self.assertTrue(self.group.execute(plan3)) - - def test_gogogo(self): - current_joints = np.asarray(self.group.get_current_joint_values()) - - self.group.set_joint_value_target(current_joints) - self.assertTrue(self.group.go(True)) - - self.assertTrue(self.group.go(current_joints)) - self.assertTrue(self.group.go(list(current_joints))) - self.assertTrue(self.group.go(tuple(current_joints))) - self.assertTrue( - self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints)) - ) - - self.group.remember_joint_values("current") - self.assertTrue(self.group.go("current")) - - current_pose = self.group.get_current_pose() - self.assertTrue(self.group.go(current_pose)) - - def test_planning_scene_interface(self): - planning_scene = PlanningSceneInterface() - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_moveit_commander" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderTest) - - # suppress cleanup segfault - os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander.test b/moveit_commander/test/python_moveit_commander.test deleted file mode 100644 index eb99c5935d..0000000000 --- a/moveit_commander/test/python_moveit_commander.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_commander/test/python_moveit_commander_ns.py b/moveit_commander/test/python_moveit_commander_ns.py deleted file mode 100755 index 6b159bfe96..0000000000 --- a/moveit_commander/test/python_moveit_commander_ns.py +++ /dev/null @@ -1,115 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: William Baker -# -# This test is used to ensure planning with a RobotCommander is -# possbile if the robot's move_group node is in a different namespace - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_commander import RobotCommander - - -class PythonMoveitCommanderNsTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - PLANNING_NS = "test_ns/" - - @classmethod - def setUpClass(self): - self.commander = RobotCommander( - "%srobot_description" % self.PLANNING_NS, self.PLANNING_NS - ) - self.group = self.commander.get_group(self.PLANNING_GROUP) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - success1, plan1, time1, err1 = self.plan(current + 0.2) - success2, plan2, time2, err2 = self.plan(current + 0.2) - self.assertTrue(success1) - self.assertTrue(success2) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - success3, plan3, time3, err3 = self.plan(current) - self.assertTrue(success3) - self.assertTrue(self.group.execute(plan3)) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_moveit_commander_ns" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderNsTest) - - # suppress cleanup segfault - os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander_ns.test b/moveit_commander/test/python_moveit_commander_ns.test deleted file mode 100644 index eefed9aec6..0000000000 --- a/moveit_commander/test/python_moveit_commander_ns.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.py b/moveit_commander/test/python_moveit_commander_ros_namespace.py deleted file mode 100755 index b7de1f059e..0000000000 --- a/moveit_commander/test/python_moveit_commander_ros_namespace.py +++ /dev/null @@ -1,72 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2020, Open Source Robotics Foundation -# 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 Open Source Robotics Foundation. 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 Mitrano -# -# This test is used to ensure planning with a RobotCommander is -# possible if the robot's move_group node is in a different namespace - -import unittest -import rospy -import rostest -import os - -from moveit_commander import PlanningSceneInterface - - -class PythonMoveitCommanderRosNamespaceTest(unittest.TestCase): - def test_namespace(self): - self.scene = PlanningSceneInterface() - expected_resolved_co_name = "/test_ros_namespace/collision_object" - expected_resolved_aco_name = "/test_ros_namespace/attached_collision_object" - self.assertEqual(self.scene._pub_co.resolved_name, expected_resolved_co_name) - self.assertEqual(self.scene._pub_aco.resolved_name, expected_resolved_aco_name) - - def test_namespace_synchronous(self): - self.scene = PlanningSceneInterface(synchronous=True) - expected_resolved_apply_diff_name = "/test_ros_namespace/apply_planning_scene" - self.assertEqual( - self.scene._apply_planning_scene_diff.resolved_name, - expected_resolved_apply_diff_name, - ) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_moveit_commander_ros_namespace" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveitCommanderRosNamespaceTest) - - # suppress cleanup segfault - os._exit(0) diff --git a/moveit_commander/test/python_moveit_commander_ros_namespace.test b/moveit_commander/test/python_moveit_commander_ros_namespace.test deleted file mode 100644 index a2c3800c57..0000000000 --- a/moveit_commander/test/python_moveit_commander_ros_namespace.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - diff --git a/moveit_commander/test/python_time_parameterization.py b/moveit_commander/test/python_time_parameterization.py deleted file mode 100755 index 1cf587d6af..0000000000 --- a/moveit_commander/test/python_time_parameterization.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# 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: Masaki Murooka (JSK Robotics Lab, The University of Tokyo) - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_commander import RobotCommander - - -class PythonTimeParameterizationTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.commander = RobotCommander("robot_description") - self.group = self.commander.get_group(self.PLANNING_GROUP) - - @classmethod - def tearDown(self): - pass - - def plan(self): - start_pose = self.group.get_current_pose().pose - goal_pose = self.group.get_current_pose().pose - goal_pose.position.z -= 0.1 - (plan, fraction) = self.group.compute_cartesian_path( - [start_pose, goal_pose], 0.005, 0.0 - ) - self.assertEqual(fraction, 1.0, "Cartesian path plan failed") - return plan - - def time_parameterization(self, plan, algorithm): - ref_state = self.commander.get_current_state() - retimed_plan = self.group.retime_trajectory( - ref_state, - plan, - velocity_scaling_factor=0.1, - acceleration_scaling_factor=0.1, - algorithm=algorithm, - ) - return retimed_plan - - def test_plan_and_time_parameterization(self): - plan = self.plan() - retimed_plan = self.time_parameterization( - plan, "iterative_time_parameterization" - ) - self.assertTrue( - len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" - ) - retimed_plan = self.time_parameterization( - plan, "iterative_spline_parameterization" - ) - self.assertTrue( - len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" - ) - retimed_plan = self.time_parameterization( - plan, "time_optimal_trajectory_generation" - ) - self.assertTrue( - len(retimed_plan.joint_trajectory.points) > 0, "Retimed plan is invalid" - ) - retimed_plan = self.time_parameterization(plan, "") - self.assertTrue( - len(retimed_plan.joint_trajectory.points) == 0, "Invalid retime algorithm" - ) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_time_parameterization" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonTimeParameterizationTest) - - # suppress cleanup segfault - os._exit(0) diff --git a/moveit_commander/test/python_time_parameterization.test b/moveit_commander/test/python_time_parameterization.test deleted file mode 100644 index 3a83385699..0000000000 --- a/moveit_commander/test/python_time_parameterization.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 7a27cda45a..fed1b4480e 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -36,8 +36,6 @@ find_package(urdf REQUIRED) find_package(urdfdom REQUIRED) find_package(urdfdom_headers REQUIRED) find_package(visualization_msgs REQUIRED) -# TODO: Port python bindings -# find_package(pybind11 REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) @@ -118,34 +116,6 @@ add_subdirectory(collision_detection) add_subdirectory(collision_detection_bullet) add_subdirectory(collision_detection_fcl) -# TODO: Port python bindings -# add_subdirectory(python) -# set(pymoveit_libs -# moveit_collision_detection -# moveit_kinematic_constraints -# moveit_planning_scene -# moveit_python_tools -# moveit_robot_model -# moveit_robot_state -# moveit_transforms -# ) -# -# pybind_add_module(pymoveit_core -# python/pymoveit_core.cpp -# collision_detection/src/pycollision_detection.cpp -# robot_model/src/pyrobot_model.cpp -# robot_state/src/pyrobot_state.cpp -# transforms/src/pytransforms.cpp -# planning_scene/src/pyplanning_scene.cpp -# kinematic_constraints/src/pykinematic_constraint.cpp -# ) -# target_include_directories(pymoveit_core SYSTEM PRIVATE ${catkin_INCLUDE_DIRS}) -# target_link_libraries(pymoveit_core PRIVATE ${pymoveit_libs} ${catkin_LIBRARIES}) -# -# #catkin_lint: ignore_once undefined_target (pymoveit_core is defined by pybind_add_module) -# install(TARGETS pymoveit_core LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION}) - - install( TARGETS collision_detector_bullet_plugin @@ -165,8 +135,6 @@ install( moveit_planning_interface moveit_planning_scene moveit_planning_request_adapter - # TODO: Port python bindings - # moveit_python_tools moveit_robot_model moveit_robot_state moveit_robot_trajectory diff --git a/moveit_core/doc/_templates/custom-class-template.rst b/moveit_core/doc/_templates/custom-class-template.rst deleted file mode 100644 index f73eda50ec..0000000000 --- a/moveit_core/doc/_templates/custom-class-template.rst +++ /dev/null @@ -1,34 +0,0 @@ -{{ fullname | escape | underline}} - -.. currentmodule:: {{ module }} - -.. autoclass:: {{ objname }} - :members: - :show-inheritance: - :inherited-members: - :special-members: __call__, __add__, __mul__ - - {% block methods %} - {% if methods %} - .. rubric:: {{ _('Methods') }} - - .. autosummary:: - :nosignatures: - {% for item in methods %} - {%- if not item.startswith('_') %} - ~{{ name }}.{{ item }} - {%- endif -%} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block attributes %} - {% if attributes %} - .. rubric:: {{ _('Attributes') }} - - .. autosummary:: - {% for item in attributes %} - ~{{ name }}.{{ item }} - {%- endfor %} - {% endif %} - {% endblock %} diff --git a/moveit_core/doc/_templates/custom-module-template.rst b/moveit_core/doc/_templates/custom-module-template.rst deleted file mode 100644 index d066d0e4dc..0000000000 --- a/moveit_core/doc/_templates/custom-module-template.rst +++ /dev/null @@ -1,66 +0,0 @@ -{{ fullname | escape | underline}} - -.. automodule:: {{ fullname }} - - {% block attributes %} - {% if attributes %} - .. rubric:: Module attributes - - .. autosummary:: - :toctree: - {% for item in attributes %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block functions %} - {% if functions %} - .. rubric:: {{ _('Functions') }} - - .. autosummary:: - :toctree: - :nosignatures: - {% for item in functions %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block classes %} - {% if classes %} - .. rubric:: {{ _('Classes') }} - - .. autosummary:: - :toctree: - :template: custom-class-template.rst - :nosignatures: - {% for item in classes %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block exceptions %} - {% if exceptions %} - .. rubric:: {{ _('Exceptions') }} - - .. autosummary:: - :toctree: - {% for item in exceptions %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - -{% block modules %} -{% if modules %} -.. autosummary:: - :toctree: - :template: custom-module-template.rst - :recursive: -{% for item in modules %} - {{ item }} -{%- endfor %} -{% endif %} -{% endblock %} diff --git a/moveit_core/doc/api.rst b/moveit_core/doc/api.rst deleted file mode 100644 index 58c6bd1760..0000000000 --- a/moveit_core/doc/api.rst +++ /dev/null @@ -1,6 +0,0 @@ -.. autosummary:: - :toctree: _autosummary - :template: custom-module-template.rst - :recursive: - - moveit.core diff --git a/moveit_core/doc/conf.py b/moveit_core/doc/conf.py deleted file mode 100644 index 0ae380d607..0000000000 --- a/moveit_core/doc/conf.py +++ /dev/null @@ -1,200 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Configuration file for the Sphinx documentation builder. -# -# This file does only contain a selection of the most common options. For a -# full list see the documentation: -# http://www.sphinx-doc.org/en/master/config - -# -- 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 -import sphinx_rtd_theme - - -# -- Project information ----------------------------------------------------- - -project = "moveit.core" -copyright = "2021, MoveIt maintainer team" -author = "MoveIt maintainer team" - -# The short X.Y version -version = "" -# The full version, including alpha/beta/rc tags -release = "" - - -# -- General configuration --------------------------------------------------- - -# If your documentation needs a minimal Sphinx version, state it here. -# -# needs_sphinx = '1.0' - -# 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.autodoc", - "sphinx.ext.autosummary", - "sphinx.ext.intersphinx", - "sphinx_rtd_theme", -] - -# NOTE: Important variables that make auto-generation work, -# see https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html -autosummary_generate = True -autosummary_imported_members = True -autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries - -# Customization, not as important -html_show_sourcelink = ( - True # Remove 'view source code' from top of page (for html, not python) -) -autodoc_inherit_docstrings = True # If no docstring, inherit from base class -set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints -add_module_names = False - - -# Add any paths that contain templates here, relative to this directory. -templates_path = ["_templates"] - -# The suffix(es) of source filenames. -# You can specify multiple suffix as a list of string: -# -# source_suffix = ['.rst', '.md'] -source_suffix = ".rst" - -# The master toctree document. -master_doc = "index" - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -# -# This is also used if you do content translation via gettext catalogs. -# Usually you set "language" from the command line for these cases. -language = None - -# 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 = ["_build", "Thumbs.db", ".DS_Store", "_templates"] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = None - - -# -- 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" - - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -# 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"] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# -# The default sidebars (for documents that don't match any pattern) are -# defined by theme itself. Builtin themes are using these templates by -# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', -# 'searchbox.html']``. -# -# html_sidebars = {} - - -# -- Options for HTMLHelp output --------------------------------------------- - -# Output file base name for HTML help builder. -htmlhelp_basename = "moveit_coredoc" - - -# -- Options for LaTeX output ------------------------------------------------ - -latex_elements = { - # The paper size ('letterpaper' or 'a4paper'). - # - # 'papersize': 'letterpaper', - # The font size ('10pt', '11pt' or '12pt'). - # - # 'pointsize': '10pt', - # Additional stuff for the LaTeX preamble. - # - # 'preamble': '', - # Latex figure (float) alignment - # - # 'figure_align': 'htbp', -} - -# Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, -# author, documentclass [howto, manual, or own class]). -latex_documents = [ - (master_doc, "moveit_core.tex", "moveit\\_core Documentation", "peter", "manual"), -] - - -# -- Options for manual page output ------------------------------------------ - -# One entry per manual page. List of tuples -# (source start file, name, description, authors, manual section). -man_pages = [(master_doc, "moveit_core", "moveit_core Documentation", [author], 1)] - - -# -- Options for Texinfo output ---------------------------------------------- - -# Grouping the document tree into Texinfo files. List of tuples -# (source start file, target name, title, author, -# dir menu entry, description, category) -texinfo_documents = [ - ( - master_doc, - "moveit_core", - "moveit_core Documentation", - author, - "moveit_core", - "One line description of project.", - "Miscellaneous", - ), -] - - -# -- Options for Epub output ------------------------------------------------- - -# Bibliographic Dublin Core info. -epub_title = project - -# The unique identifier of the text. This can be a ISBN number -# or the project homepage. -# -# epub_identifier = '' - -# A unique identification for the text. -# -# epub_uid = '' - -# A list of files that should not be packed into the epub file. -epub_exclude_files = ["search.html"] - - -# -- Extension configuration ------------------------------------------------- - -# -- Options for intersphinx extension --------------------------------------- - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {"https://docs.python.org/3": None} diff --git a/moveit_core/doc/index.rst b/moveit_core/doc/index.rst deleted file mode 100644 index 9c719062c5..0000000000 --- a/moveit_core/doc/index.rst +++ /dev/null @@ -1,13 +0,0 @@ -Python API Reference: moveit_core -======================================= - -.. note:: - These docs are autogenerated. You may see reference to module names like pymoveit_core, - but you should not use these in your code. Instead, use the modules under the `moveit` namespace. - For example, you should write ``import moveit.core`` instead of ``import pymoveit_core``. - The latter will still work, but will not contain the full API that ``import moveit.core`` does. - -.. toctree:: - - Home page - API reference diff --git a/moveit_core/package.xml b/moveit_core/package.xml index fa4f67e31d..5805c0c249 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -42,7 +42,6 @@ octomap_msgs octomap pluginlib - pybind11_vendor random_numbers rclcpp ruckig 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 f78d8025d7..0000000000 --- a/moveit_core/python/tools/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -set(MOVEIT_LIB_NAME moveit_python_tools) - -add_library(${MOVEIT_LIB_NAME} - src/pybind_rosmsg_typecasters.cpp -) -#add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) # TODO: Fix -set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - -install( - TARGETS - ${MOVEIT_LIB_NAME} - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) - -install(DIRECTORY include/ DESTINATION include) 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/python/tools/src/pybind_rosmsg_typecasters.cpp b/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp deleted file mode 100644 index c94c57f85b..0000000000 --- a/moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp +++ /dev/null @@ -1,69 +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 */ - -#include - -namespace py = pybind11; -namespace moveit -{ -namespace python -{ -py::object createMessage(const std::string& ros_msg_name) -{ - // find delimiting '/' in ros msg name - std::size_t pos = ros_msg_name.find('/'); - // import module - py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str()); - // retrieve type instance - py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str()); - // create message instance - return cls(); -} - -bool convertible(const pybind11::handle& h, const char* ros_msg_name) -{ - try - { - PyObject* o = h.attr("_type").ptr(); - return py::cast(o) == ros_msg_name; - } - catch (const std::exception& e) - { - return false; - } -} -} // namespace python -} // namespace moveit From c527cb84e8f5cef9e25fe50e85f3ddab1bd125d1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 30 May 2025 09:49:41 +0200 Subject: [PATCH 22/28] Delete redundant file --- moveit_py/moveit/__init__.pyi | 3 --- 1 file changed, 3 deletions(-) delete mode 100644 moveit_py/moveit/__init__.pyi diff --git a/moveit_py/moveit/__init__.pyi b/moveit_py/moveit/__init__.pyi deleted file mode 100644 index 713f9aff74..0000000000 --- a/moveit_py/moveit/__init__.pyi +++ /dev/null @@ -1,3 +0,0 @@ -from moveit.core import * -from moveit.planning import * -from moveit.utils import get_launch_params_filepaths as get_launch_params_filepaths From 595d00b4ec48c889bd829a93052f9d7ae54036ef Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Fri, 30 May 2025 13:50:17 +0200 Subject: [PATCH 23/28] Fix tests --- moveit_py/test/unit/test_robot_model.py | 2 +- moveit_py/test/unit/test_robot_state.py | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_py/test/unit/test_robot_model.py b/moveit_py/test/unit/test_robot_model.py index 9584ec624f..a616ba7a51 100644 --- a/moveit_py/test/unit/test_robot_model.py +++ b/moveit_py/test/unit/test_robot_model.py @@ -1,5 +1,5 @@ import unittest -from moveit.core.robot_model import JointModelGroup, RobotModel +from test_moveit.core.robot_model 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) diff --git a/moveit_py/test/unit/test_robot_state.py b/moveit_py/test/unit/test_robot_state.py index 2a1e8db516..f5f6d8ed4a 100644 --- a/moveit_py/test/unit/test_robot_state.py +++ b/moveit_py/test/unit/test_robot_state.py @@ -3,8 +3,8 @@ from geometry_msgs.msg import Pose -from moveit.core.robot_state import RobotState -from moveit.core.robot_model import RobotModel +from test_moveit.core.robot_state import RobotState +from test_moveit.core.robot_model import RobotModel # TODO (peterdavidfagan): depend on moveit_resources package directly. From 55458893546ca67fa6e79bdab3d2b3eb5a8c31e0 Mon Sep 17 00:00:00 2001 From: SamueleSandrini Date: Fri, 30 May 2025 18:36:04 +0200 Subject: [PATCH 24/28] Fix names and clang-tidy issue --- moveit_py/package.xml | 2 -- .../moveit_core/collision_detection/collision_common.hpp | 3 +-- .../moveit_core/collision_detection/collision_matrix.hpp | 3 +-- moveit_py/src/moveit/moveit_core/collision_detection/world.hpp | 3 +-- .../moveit_core/controller_manager/controller_manager.hpp | 3 +-- .../src/moveit/moveit_core/kinematic_constraints/utils.cpp | 3 +-- .../src/moveit/moveit_core/kinematic_constraints/utils.hpp | 3 +-- .../moveit_core/planning_interface/planning_response.cpp | 3 +-- .../moveit_core/planning_interface/planning_response.hpp | 3 +-- .../src/moveit/moveit_core/planning_scene/planning_scene.cpp | 3 +-- .../src/moveit/moveit_core/planning_scene/planning_scene.hpp | 3 +-- moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp | 3 +-- .../src/moveit/moveit_core/robot_model/joint_model_group.hpp | 3 +-- moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp | 3 +-- moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp | 3 +-- moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp | 3 +-- .../moveit/moveit_core/robot_trajectory/robot_trajectory.cpp | 3 +-- .../moveit/moveit_core/robot_trajectory/robot_trajectory.hpp | 3 +-- moveit_py/src/moveit/moveit_core/transforms/transforms.hpp | 3 +-- .../moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp | 2 +- moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp | 3 +-- moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp | 3 +-- .../src/moveit/moveit_ros/moveit_cpp/planning_component.cpp | 3 +-- .../src/moveit/moveit_ros/moveit_cpp/planning_component.hpp | 3 +-- .../planning_scene_monitor/planning_scene_monitor.cpp | 3 +-- .../planning_scene_monitor/planning_scene_monitor.hpp | 3 +-- .../trajectory_execution_manager.cpp | 3 +-- .../trajectory_execution_manager.hpp | 3 +-- moveit_py/src/moveit/planning.cpp | 3 +-- 29 files changed, 28 insertions(+), 57 deletions(-) diff --git a/moveit_py/package.xml b/moveit_py/package.xml index a38246a9ef..2c63feb1fb 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -6,12 +6,10 @@ Python binding for MoveIt 2 Peter David Fagan - Samuele Sandrini BSD Peter David Fagan - Samuele Sandrini ament_cmake diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp index e797f654c8..f195e42ea3 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp index 50415739d2..35b1dcef9c 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp index 44fa728f28..b4069670b7 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/world.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Jafar Uruç - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Jafar Uruç, Samuele Sandrini */ +/* Author: Jafar Uruç */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp index dd4e690e5f..7431378376 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp index 3c858b4b79..ff181e20b7 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "utils.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp index 5e8b21a0b6..d6cc8a6b93 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 index 1a4a407b1f..fb983ae18e 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "planning_response.hpp" diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp index 6d36eef127..5f5648c9fe 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 index 2bd3c8232c..89c56817da 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan, Samuele Sandrini */ +/* Author: Peter David Fagan */ #include "planning_scene.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp index 931a6803f0..8f18b905d8 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan, Samuele Sandrini */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp index e602da75af..85ee7b3739 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Jafar Uruç - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Jafar Uruç, Samuele Sandrini */ +/* Authors: Jafar Uruç */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp index c3ef4d5e19..c6eda98a2b 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 index 42fd63b970..9deab30541 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "robot_model.hpp" #include 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 index 017fbf03ee..e2051de323 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "robot_state.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp index 3b557077ff..7eb2d9b4fa 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan, Samuele Sandrini */ +/* Author: Peter David Fagan */ #pragma once 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 index 1fc6d0ebff..be9067a7bb 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "robot_trajectory.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp index 06e09330d9..c08e649d9a 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp index ce903167fb..ca517fbf8b 100644 --- a/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp index bffb9412ab..e496b51f62 100644 --- a/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp +++ b/moveit_py/src/moveit/moveit_py_utils/include/moveit_py/moveit_py_utils/rclpy_time_typecaster.hpp @@ -75,7 +75,7 @@ struct type_caster static py::handle cast(const rclcpp::Time& src, return_value_policy /* policy */, py::handle /* parent */) { py::module rclpy_time = py::module::import("rclpy.time"); - py::object Time = rclpy_time.attr("Time"); + py::object Time = rclpy_time.attr("Time"); // NOLINT(readability-identifier-naming) int64_t nanoseconds = src.nanoseconds(); int clock_type = static_cast(src.get_clock_type()); 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 index de4ea77c64..dfd34d3e7e 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "moveit_cpp.hpp" #include diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp index 501bc12e11..29d7123039 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 index 0d19020cf6..dcd2928d29 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "planning_component.hpp" #include diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index 22d0a0908a..c0ce8730e1 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 index ea177d8791..b325cb6090 100644 --- 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 @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #include "planning_scene_monitor.hpp" diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp index ee4fe8d07f..831a6510b6 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan, Samuele Sandrini */ +/* Authors: Peter David Fagan */ #pragma once 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 5611a12c44..f640e36ee9 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 @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Matthijs van der Burgh - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Matthijs van der Burgh, Samuele Sandrini */ +/* Author: Matthijs van der Burgh */ #include "trajectory_execution_manager.hpp" diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp index dfd031f619..81c745301e 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2023, Matthijs van der Burgh - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Matthijs van der Burgh, Samuele Sandrini */ +/* Authors: Matthijs van der Burgh */ #pragma once diff --git a/moveit_py/src/moveit/planning.cpp b/moveit_py/src/moveit/planning.cpp index 1d26669706..a3826054bf 100644 --- a/moveit_py/src/moveit/planning.cpp +++ b/moveit_py/src/moveit/planning.cpp @@ -2,7 +2,6 @@ * Software License Agreement (BSD License) * * Copyright (c) 2022, Peter David Fagan - * Copyright (c) 2025, Samuele Sandrini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Peter David Fagan, Samuele Sandrini */ +/* Author: Peter David Fagan */ #include "moveit_ros/moveit_cpp/moveit_cpp.hpp" #include "moveit_ros/moveit_cpp/planning_component.hpp" From 338f24a6f53e22f221dd38df5cb28ae49b1195e9 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 1 Jun 2025 11:48:52 +0200 Subject: [PATCH 25/28] Further reduce differences to main --- moveit_py/docs/{script => source}/conf.py | 0 moveit_py/docs/{script => source}/index.rst | 0 .../collision_detection/collision_common.hpp | 2 +- .../collision_detection/collision_matrix.hpp | 2 +- .../controller_manager/controller_manager.hpp | 2 +- .../kinematic_constraints/utils.cpp | 2 +- .../kinematic_constraints/utils.hpp | 2 +- .../planning_interface/planning_response.cpp | 2 +- .../planning_interface/planning_response.hpp | 2 +- .../planning_scene/planning_scene.cpp | 20 +++++++++++++++++++ .../moveit_core/robot_model/joint_model.hpp | 2 +- .../robot_model/joint_model_group.hpp | 2 +- .../moveit_core/robot_model/robot_model.cpp | 2 +- .../moveit_core/robot_state/robot_state.cpp | 2 +- .../robot_trajectory/robot_trajectory.cpp | 2 +- .../robot_trajectory/robot_trajectory.hpp | 2 +- .../moveit_core/transforms/transforms.hpp | 2 +- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 2 +- .../moveit_ros/moveit_cpp/moveit_cpp.hpp | 2 +- .../moveit_cpp/planning_component.cpp | 2 +- .../moveit_cpp/planning_component.hpp | 2 +- .../planning_scene_monitor.cpp | 2 +- .../planning_scene_monitor.hpp | 2 +- .../trajectory_execution_manager.hpp | 2 +- 24 files changed, 41 insertions(+), 21 deletions(-) rename moveit_py/docs/{script => source}/conf.py (100%) rename moveit_py/docs/{script => source}/index.rst (100%) diff --git a/moveit_py/docs/script/conf.py b/moveit_py/docs/source/conf.py similarity index 100% rename from moveit_py/docs/script/conf.py rename to moveit_py/docs/source/conf.py diff --git a/moveit_py/docs/script/index.rst b/moveit_py/docs/source/index.rst similarity index 100% rename from moveit_py/docs/script/index.rst rename to moveit_py/docs/source/index.rst diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp index f195e42ea3..a1dfcb44ff 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_common.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp index 35b1dcef9c..a111c176f1 100644 --- a/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp +++ b/moveit_py/src/moveit/moveit_core/collision_detection/collision_matrix.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp index 7431378376..2fa5390292 100644 --- a/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp +++ b/moveit_py/src/moveit/moveit_core/controller_manager/controller_manager.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp index ff181e20b7..89ee466916 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "utils.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp index d6cc8a6b93..c2b47f9469 100644 --- a/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp +++ b/moveit_py/src/moveit/moveit_core/kinematic_constraints/utils.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index fb983ae18e..1995adff31 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "planning_response.hpp" diff --git a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp index 5f5648c9fe..017624487e 100644 --- a/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp +++ b/moveit_py/src/moveit/moveit_core/planning_interface/planning_response.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index 89c56817da..88b17af11d 100644 --- a/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp +++ b/moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp @@ -247,6 +247,26 @@ void initPlanningScene(py::module& m) This method will remove all collision object from the scene except for attached collision objects. )") + .def("set_current_state", + py::overload_cast(&planning_scene::PlanningScene::setCurrentState), + py::arg("robot_state"), + R"( + Set the current state using a moveit_msgs::msg::RobotState message. + + Args: + robot_state (moveit_msgs.msg.RobotState): The robot state message to set as current state. + )") + + .def("set_current_state", + py::overload_cast(&planning_scene::PlanningScene::setCurrentState), + py::arg("robot_state"), + R"( + Set the current state using a moveit::core::RobotState object. + + Args: + robot_state (moveit_py.core.RobotState): The robot state object to set as current state. + )") + // checking state validity .def("is_state_valid", py::overload_cast( diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp index 85ee7b3739..bcd72d0171 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Jafar Uruç */ +/* Author: Jafar Uruç */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp index c6eda98a2b..be54b09e6a 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/joint_model_group.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index 9deab30541..f9cb943c6a 100644 --- a/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_model/robot_model.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "robot_model.hpp" #include 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 index e2051de323..e34e1b2d04 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "robot_state.hpp" #include 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 index be9067a7bb..d8d633747f 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "robot_trajectory.hpp" #include diff --git a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp index c08e649d9a..7e1cca5ace 100644 --- a/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp +++ b/moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp index ca517fbf8b..c8a72ccdb9 100644 --- a/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp +++ b/moveit_py/src/moveit/moveit_core/transforms/transforms.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index dfd34d3e7e..bc8e9c5812 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "moveit_cpp.hpp" #include diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp index 29d7123039..970e074e04 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index dcd2928d29..9033552d5e 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "planning_component.hpp" #include diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp index c0ce8730e1..a142beb7af 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once 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 index b325cb6090..44deab079f 100644 --- 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 @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #include "planning_scene_monitor.hpp" diff --git a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp index 831a6510b6..bad998e909 100644 --- a/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp +++ b/moveit_py/src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Peter David Fagan */ +/* Author: Peter David Fagan */ #pragma once diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp index 81c745301e..c74caba117 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.hpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Matthijs van der Burgh */ +/* Author: Matthijs van der Burgh */ #pragma once From 8f8140eb728c5d67dc48055da9e458068a6513b7 Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Mon, 2 Jun 2025 13:14:10 +0200 Subject: [PATCH 26/28] Reduce stub file differences with main --- moveit_py/moveit/core/__init__.pyi | 11 -- moveit_py/moveit/core/collision_detection.pyi | 36 ++--- moveit_py/moveit/core/controller_manager.pyi | 6 +- .../moveit/core/kinematic_constraints.pyi | 8 +- moveit_py/moveit/core/planning_interface.pyi | 10 +- moveit_py/moveit/core/planning_scene.pyi | 54 +++---- moveit_py/moveit/core/robot_model.pyi | 58 ++++---- moveit_py/moveit/core/robot_state.pyi | 57 ++++---- moveit_py/moveit/core/robot_trajectory.pyi | 26 ++-- moveit_py/moveit/planning.pyi | 137 ++++++++---------- moveit_py/moveit/utils.pyi | 4 +- 11 files changed, 197 insertions(+), 210 deletions(-) diff --git a/moveit_py/moveit/core/__init__.pyi b/moveit_py/moveit/core/__init__.pyi index faa01f8153..2a0de17e04 100644 --- a/moveit_py/moveit/core/__init__.pyi +++ b/moveit_py/moveit/core/__init__.pyi @@ -1,13 +1,2 @@ -from . import ( - collision_detection as collision_detection, - controller_manager as controller_manager, - kinematic_constraints as kinematic_constraints, - planning_interface as planning_interface, - planning_scene as planning_scene, - robot_model as robot_model, - robot_state as robot_state, - robot_trajectory as robot_trajectory, -) - 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 index f33209a10f..9abf7f45b6 100644 --- a/moveit_py/moveit/core/collision_detection.pyi +++ b/moveit_py/moveit/core/collision_detection.pyi @@ -1,27 +1,27 @@ -from _typeshed import Incomplete +from typing import Any class AllowedCollisionMatrix: def __init__(self, *args, **kwargs) -> None: ... - def clear(self, *args, **kwargs): ... - def get_entry(self, *args, **kwargs): ... - def remove_entry(self, *args, **kwargs): ... - def set_entry(self, *args, **kwargs): ... + def clear(self, *args, **kwargs) -> Any: ... + def get_entry(self, *args, **kwargs) -> Any: ... + def remove_entry(self, *args, **kwargs) -> Any: ... + def set_entry(self, *args, **kwargs) -> Any: ... class CollisionRequest: - contacts: Incomplete - cost: Incomplete - distance: Incomplete - joint_model_group_name: Incomplete - max_contacts: Incomplete - max_contacts_per_pair: Incomplete - max_cost_sources: Incomplete - verbose: Incomplete + 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: Incomplete - contact_count: Incomplete - contacts: Incomplete - cost_sources: Incomplete - distance: Incomplete + 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 index 5856147eca..fc5df2530b 100644 --- a/moveit_py/moveit/core/controller_manager.pyi +++ b/moveit_py/moveit/core/controller_manager.pyi @@ -1,5 +1,7 @@ +from typing import Any + class ExecutionStatus: def __init__(self, *args, **kwargs) -> None: ... - def __bool__(self) -> bool: ... @property - def status(self): ... + def status(self) -> Any: ... + def __bool__(self) -> Any: ... diff --git a/moveit_py/moveit/core/kinematic_constraints.pyi b/moveit_py/moveit/core/kinematic_constraints.pyi index 4755843c53..f425d39e62 100644 --- a/moveit_py/moveit/core/kinematic_constraints.pyi +++ b/moveit_py/moveit/core/kinematic_constraints.pyi @@ -1,3 +1,5 @@ -def construct_constraints_from_node(*args, **kwargs): ... -def construct_joint_constraint(*args, **kwargs): ... -def construct_link_constraint(*args, **kwargs): ... +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 index 2b0e962c0e..f190ca47c3 100644 --- a/moveit_py/moveit/core/planning_interface.pyi +++ b/moveit_py/moveit/core/planning_interface.pyi @@ -1,9 +1,11 @@ +from typing import Any + class MotionPlanResponse: def __init__(self, *args, **kwargs) -> None: ... - def __bool__(self) -> bool: ... + def __bool__(self) -> Any: ... @property - def error_code(self): ... + def error_code(self) -> Any: ... @property - def planning_time(self): ... + def planning_time(self) -> Any: ... @property - def trajectory(self): ... + def trajectory(self) -> Any: ... diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi index a0aa6dda50..9cc633ddcb 100644 --- a/moveit_py/moveit/core/planning_scene.pyi +++ b/moveit_py/moveit/core/planning_scene.pyi @@ -1,35 +1,35 @@ -from _typeshed import Incomplete +from typing import Any class PlanningScene: - current_state: Incomplete - name: Incomplete + current_state: Any + name: Any def __init__(self, *args, **kwargs) -> None: ... - def apply_collision_object(self, *args, **kwargs): ... - def check_collision(self, *args, **kwargs): ... - def check_collision_unpadded(self, *args, **kwargs): ... - def check_self_collision(self, *args, **kwargs): ... - def get_frame_transform(self, *args, **kwargs): ... - def is_path_valid(self, *args, **kwargs): ... - def is_state_colliding(self, *args, **kwargs): ... - def is_state_constrained(self, *args, **kwargs): ... - def is_state_valid(self, *args, **kwargs): ... - def knows_frame_transform(self, *args, **kwargs): ... - def load_geometry_from_file(self, *args, **kwargs): ... - def process_attached_collision_object(self, *args, **kwargs): ... - def process_octomap(self, *args, **kwargs): ... - def process_planning_scene_world(self, *args, **kwargs): ... - def remove_all_collision_objects(self, *args, **kwargs): ... - def save_geometry_to_file(self, *args, **kwargs): ... - def set_object_color(self, *args, **kwargs): ... - def __copy__(self): ... - def __deepcopy__(self): ... + 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 save_geometry_to_file(self, file_name_and_path) -> Any: ... + def load_geometry_from_file(self, file_name_and_path) -> Any: ... + def __copy__(self) -> Any: ... + def __deepcopy__(self) -> Any: ... @property - def allowed_collision_matrix(self): ... + def planning_frame(self) -> Any: ... @property - def planning_frame(self): ... + def planning_scene_message(self) -> Any: ... @property - def planning_scene_message(self): ... + def robot_model(self) -> Any: ... @property - def robot_model(self): ... + def transforms(self) -> Any: ... @property - def transforms(self): ... + def allowed_collision_matrix(self) -> Any: ... diff --git a/moveit_py/moveit/core/robot_model.pyi b/moveit_py/moveit/core/robot_model.pyi index 4941562826..ac7520e5bd 100644 --- a/moveit_py/moveit/core/robot_model.pyi +++ b/moveit_py/moveit/core/robot_model.pyi @@ -1,60 +1,62 @@ +from typing import Any + class JointModelGroup: def __init__(self, *args, **kwargs) -> None: ... - def satisfies_position_bounds(self, *args, **kwargs): ... + def satisfies_position_bounds(self, *args, **kwargs) -> Any: ... @property - def active_joint_model_bounds(self): ... + def active_joint_model_bounds(self) -> Any: ... @property - def active_joint_model_names(self): ... + def active_joint_model_names(self) -> Any: ... @property - def eef_name(self): ... + def eef_name(self) -> Any: ... @property - def joint_model_names(self): ... + def joint_model_names(self) -> Any: ... @property - def link_model_names(self): ... + def link_model_names(self) -> Any: ... @property - def name(self): ... + def name(self) -> Any: ... class RobotModel: def __init__(self, *args, **kwargs) -> None: ... - def get_joint_model_group(self, *args, **kwargs): ... - def get_model_info(self, *args, **kwargs): ... - def has_joint_model_group(self, *args, **kwargs): ... + 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): ... + def end_effectors(self) -> Any: ... @property - def joint_model_group_names(self): ... + def joint_model_group_names(self) -> Any: ... @property - def joint_model_groups(self): ... + def joint_model_groups(self) -> Any: ... @property - def model_frame(self): ... + def model_frame(self) -> Any: ... @property - def name(self): ... + def name(self) -> Any: ... @property - def root_joint_name(self): ... + def root_joint_name(self) -> Any: ... class VariableBounds: def __init__(self, *args, **kwargs) -> None: ... @property - def acceleration_bounded(self): ... + def acceleration_bounded(self) -> Any: ... @property - def jerk_bounded(self): ... + def jerk_bounded(self) -> Any: ... @property - def max_acceleration(self): ... + def max_acceleration(self) -> Any: ... @property - def max_jerk(self): ... + def max_jerk(self) -> Any: ... @property - def max_position(self): ... + def max_position(self) -> Any: ... @property - def max_velocity(self): ... + def max_velocity(self) -> Any: ... @property - def min_acceleration(self): ... + def min_acceleration(self) -> Any: ... @property - def min_jerk(self): ... + def min_jerk(self) -> Any: ... @property - def min_position(self): ... + def min_position(self) -> Any: ... @property - def min_velocity(self): ... + def min_velocity(self) -> Any: ... @property - def position_bounded(self): ... + def position_bounded(self) -> Any: ... @property - def velocity_bounded(self): ... + def velocity_bounded(self) -> Any: ... diff --git a/moveit_py/moveit/core/robot_state.pyi b/moveit_py/moveit/core/robot_state.pyi index 40ebd614f8..8ba90828b3 100644 --- a/moveit_py/moveit/core/robot_state.pyi +++ b/moveit_py/moveit/core/robot_state.pyi @@ -1,36 +1,35 @@ -from _typeshed import Incomplete +from typing import Any, ClassVar class RobotState: - joint_accelerations: Incomplete - joint_efforts: Incomplete - joint_positions: Incomplete - joint_velocities: Incomplete + 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): ... - def get_frame_transform(self, *args, **kwargs): ... - def get_global_link_transform(self, *args, **kwargs): ... - def get_jacobian(self, *args, **kwargs): ... - def get_joint_group_accelerations(self, *args, **kwargs): ... - def get_joint_group_positions(self, *args, **kwargs): ... - def get_joint_group_velocities(self, *args, **kwargs): ... - def get_pose(self, *args, **kwargs): ... - def set_from_ik(self, *args, **kwargs): ... - def set_joint_group_accelerations(self, *args, **kwargs): ... - def set_joint_group_active_positions(self, *args, **kwargs): ... - def set_joint_group_positions(self, *args, **kwargs): ... - def set_joint_group_velocities(self, *args, **kwargs): ... - def set_to_default_values(self, *args, **kwargs): ... - def set_to_random_positions(self, *args, **kwargs): ... - def update(self, *args, **kwargs): ... - def __copy__(self): ... - def __deepcopy__(self): ... + 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): ... + def dirty(self) -> Any: ... @property - def robot_model(self): ... + def robot_model(self) -> Any: ... @property - def state_info(self): ... - @property - def state_tree(self): ... + def state_tree(self) -> Any: ... -def robotStateToRobotStateMsg(*args, **kwargs): ... +def robotStateToRobotStateMsg(*args, **kwargs) -> Any: ... diff --git a/moveit_py/moveit/core/robot_trajectory.pyi b/moveit_py/moveit/core/robot_trajectory.pyi index 48bfc9ffdc..a0bccd5c48 100644 --- a/moveit_py/moveit/core/robot_trajectory.pyi +++ b/moveit_py/moveit/core/robot_trajectory.pyi @@ -1,19 +1,19 @@ -from _typeshed import Incomplete +from typing import Any class RobotTrajectory: - joint_model_group_name: Incomplete + joint_model_group_name: Any def __init__(self, *args, **kwargs) -> None: ... - def get_robot_trajectory_msg(self, *args, **kwargs): ... - def get_waypoint_durations(self, *args, **kwargs): ... - def set_robot_trajectory_msg(self, *args, **kwargs): ... - def unwind(self, *args, **kwargs): ... - def __getitem__(self, index): ... - def __iter__(self): ... - def __len__(self) -> int: ... - def __reverse__(self, *args, **kwargs): ... + def get_robot_trajectory_msg(self, *args, **kwargs) -> Any: ... + def get_waypoint_durations(self, *args, **kwargs) -> Any: ... + def set_robot_trajectory_msg(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): ... + def average_segment_duration(self) -> Any: ... @property - def duration(self): ... + def duration(self) -> Any: ... @property - def robot_model(self): ... + def robot_model(self) -> Any: ... diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index 737f7677c2..972970998a 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -1,105 +1,94 @@ -import types -from _typeshed import Incomplete +from typing import Any class LockedPlanningSceneContextManagerRO: def __init__(self, *args, **kwargs) -> None: ... - def __enter__(self): ... - def __exit__( - self, - type: type[BaseException] | None, - value: BaseException | None, - traceback: types.TracebackType | None, - ): ... + def __enter__(self) -> Any: ... + def __exit__(self, type, value, traceback) -> Any: ... class LockedPlanningSceneContextManagerRW: def __init__(self, *args, **kwargs) -> None: ... - def __enter__(self): ... - def __exit__( - self, - type: type[BaseException] | None, - value: BaseException | None, - traceback: types.TracebackType | 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): ... - def get_planning_component(self, *args, **kwargs): ... - def get_planning_scene_monitor(self, *args, **kwargs): ... - def get_robot_model(self, *args, **kwargs): ... - def get_trajectory_execution_manager(self, *args, **kwargs): ... - def shutdown(self, *args, **kwargs): ... + 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 get_trajectory_execution_manager(self, *args, **kwargs) -> Any: ... + def shutdown(self, *args, **kwargs) -> Any: ... class PlanRequestParameters: - max_acceleration_scaling_factor: Incomplete - max_velocity_scaling_factor: Incomplete - planner_id: Incomplete - planning_attempts: Incomplete - planning_pipeline: Incomplete - planning_time: Incomplete + 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 PlanSolution: def __init__(self, *args, **kwargs) -> None: ... def __bool__(self) -> bool: ... @property - def error_code(self): ... + def error_code(self) -> Any: ... @property - def start_state(self): ... + def start_state(self) -> Any: ... @property - def trajectory(self): ... + def trajectory(self) -> Any: ... class PlanningComponent: def __init__(self, *args, **kwargs) -> None: ... - def get_named_target_state_values(self, *args, **kwargs): ... - def get_start_state(self, *args, **kwargs): ... - def plan(self, *args, **kwargs): ... - def set_goal_state(self, *args, **kwargs): ... - def set_path_constraints(self, *args, **kwargs): ... - def set_start_state(self, *args, **kwargs): ... - def set_start_state_to_current_state(self, *args, **kwargs): ... - def set_workspace(self, *args, **kwargs): ... - def unset_workspace(self, *args, **kwargs): ... + def get_named_target_state_values(self, *args, **kwargs) -> Any: ... + 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_states(self): ... + def named_target_states(self) -> Any: ... @property - def planning_group_name(self): ... + def planning_group_name(self) -> Any: ... class PlanningSceneMonitor: def __init__(self, *args, **kwargs) -> None: ... - def clear_octomap(self, *args, **kwargs): ... - def new_planning_scene_message(self, *args, **kwargs): ... - def read_only(self, *args, **kwargs): ... - def read_write(self, *args, **kwargs): ... - def request_planning_scene_state(self, *args, **kwargs): ... - def start_scene_monitor(self, *args, **kwargs): ... - def start_state_monitor(self, *args, **kwargs): ... - def stop_scene_monitor(self, *args, **kwargs): ... - def stop_state_monitor(self, *args, **kwargs): ... - def update_frame_transforms(self, *args, **kwargs): ... - def wait_for_current_robot_state(self, *args, **kwargs): ... + def clear_octomap(self, *args, **kwargs) -> Any: ... + def new_planning_scene_message(self, *args, **kwargs) -> Any: ... + def read_only(self, *args, **kwargs) -> Any: ... + def read_write(self, *args, **kwargs) -> Any: ... + def request_planning_scene_state(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 update_frame_transforms(self, *args, **kwargs) -> Any: ... + def wait_for_current_robot_state(self, *args, **kwargs) -> Any: ... @property - def name(self): ... + def name(self) -> Any: ... class TrajectoryExecutionManager: def __init__(self, *args, **kwargs) -> None: ... - def are_controllers_active(self, *args, **kwargs): ... - def enable_execution_duration_monitoring(self, *args, **kwargs): ... - def ensure_active_controller(self, *args, **kwargs): ... - def ensure_active_controllers(self, *args, **kwargs): ... - def ensure_active_controllers_for_group(self, *args, **kwargs): ... - def ensure_active_controllers_for_joints(self, *args, **kwargs): ... - def execute(self, *args, **kwargs): ... - def execute_and_wait(self, *args, **kwargs): ... - def get_last_execution_status(self, *args, **kwargs): ... - def is_controller_active(self, *args, **kwargs): ... - def is_managing_controllers(self, *args, **kwargs): ... - def process_event(self, *args, **kwargs): ... - def push(self, *args, **kwargs): ... - def set_allowed_execution_duration_scaling(self, *args, **kwargs): ... - def set_allowed_goal_duration_margin(self, *args, **kwargs): ... - def set_allowed_start_tolerance(self, *args, **kwargs): ... - def set_execution_velocity_scaling(self, *args, **kwargs): ... - def set_wait_for_trajectory_completion(self, *args, **kwargs): ... - def stop_execution(self, *args, **kwargs): ... - def wait_for_execution(self, *args, **kwargs): ... + def are_controllers_active(self, *args, **kwargs) -> Any: ... + def enable_execution_duration_monitoring(self, *args, **kwargs) -> Any: ... + def ensure_active_controller(self, *args, **kwargs) -> Any: ... + def ensure_active_controllers(self, *args, **kwargs) -> Any: ... + def ensure_active_controllers_for_group(self, *args, **kwargs) -> Any: ... + def ensure_active_controllers_for_joints(self, *args, **kwargs) -> Any: ... + def execute(self, *args, **kwargs) -> Any: ... + def execute_and_wait(self, *args, **kwargs) -> Any: ... + def get_last_execution_status(self, *args, **kwargs) -> Any: ... + def is_controller_active(self, *args, **kwargs) -> Any: ... + def is_managing_controllers(self, *args, **kwargs) -> Any: ... + def process_event(self, *args, **kwargs) -> Any: ... + def push(self, *args, **kwargs) -> Any: ... + def set_allowed_execution_duration_scaling(self, *args, **kwargs) -> Any: ... + def set_allowed_goal_duration_margin(self, *args, **kwargs) -> Any: ... + def set_allowed_start_tolerance(self, *args, **kwargs) -> Any: ... + def set_execution_velocity_scaling(self, *args, **kwargs) -> Any: ... + def set_wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ... + def stop_execution(self, *args, **kwargs) -> Any: ... + def wait_for_execution(self, *args, **kwargs) -> Any: ... diff --git a/moveit_py/moveit/utils.pyi b/moveit_py/moveit/utils.pyi index ce88a9f080..d840a3ffed 100644 --- a/moveit_py/moveit/utils.pyi +++ b/moveit_py/moveit/utils.pyi @@ -1,2 +1,4 @@ +from typing import List, Optional + def create_params_file_from_dict(params, node_name): ... -def get_launch_params_filepaths(cli_args: list[str] | None = None) -> list[str]: ... +def get_launch_params_filepaths(cli_args: Optional[List[str]] = ...) -> List[str]: ... From c050643f26a6ac08681d3682e75b52d5f5c53141 Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Mon, 2 Jun 2025 14:43:38 +0200 Subject: [PATCH 27/28] Fix authors --- moveit_py/docs/source/conf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_py/docs/source/conf.py b/moveit_py/docs/source/conf.py index ec11adfe85..4fbfbaa2cd 100644 --- a/moveit_py/docs/source/conf.py +++ b/moveit_py/docs/source/conf.py @@ -18,8 +18,8 @@ # -- Project information ----------------------------------------------------- project = "moveit_py" -copyright = "2022, Peter David Fagan; 2025, Samuele Sandrini" -author = "Peter David Fagan, Samuele Sandrini" +copyright = "2022, Peter David Fagan; 2025" +author = "Peter David Fagan" # -- General configuration --------------------------------------------------- From 2933784ce10892537a5a9366d4064630215262cf Mon Sep 17 00:00:00 2001 From: Samuele Sandrini Date: Mon, 2 Jun 2025 18:31:15 +0200 Subject: [PATCH 28/28] Fix arg name in PlanningScene stub file --- moveit_py/moveit/core/planning_scene.pyi | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/moveit_py/moveit/core/planning_scene.pyi b/moveit_py/moveit/core/planning_scene.pyi index 9cc633ddcb..3923ecf28e 100644 --- a/moveit_py/moveit/core/planning_scene.pyi +++ b/moveit_py/moveit/core/planning_scene.pyi @@ -19,8 +19,8 @@ class PlanningScene: 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 save_geometry_to_file(self, file_name_and_path) -> Any: ... - def load_geometry_from_file(self, file_name_and_path) -> Any: ... + def save_geometry_to_file(self, file_path_and_name) -> Any: ... + def load_geometry_from_file(self, file_path_and_name) -> Any: ... def __copy__(self) -> Any: ... def __deepcopy__(self) -> Any: ... @property