From 12887f043941a7998fd01641487f932f6d9f18ce Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Wed, 4 Dec 2024 20:40:20 +0900 Subject: [PATCH 01/20] Updated OM-X package for MoveIt 2 support in ROS 2 --- CONTRIBUTING.md | 18 + README.md | 42 +- open_manipulator/CHANGELOG.rst | 16 +- open_manipulator/CMakeLists.txt | 6 +- open_manipulator/package.xml | 34 +- open_manipulator_control_gui/CHANGELOG.rst | 22 - open_manipulator_control_gui/CMakeLists.txt | 116 -- .../open_manipulator_control_gui/qnode.hpp | 128 -- .../open_manipulator_control_gui.launch | 8 - open_manipulator_control_gui/mainpage.dox | 26 - open_manipulator_control_gui/package.xml | 29 - .../resources/images.qrc | 5 - .../resources/images/icon.png | Bin 7695 -> 0 bytes open_manipulator_control_gui/src/main.cpp | 43 - .../src/main_window.cpp | 355 ---- open_manipulator_control_gui/src/qnode.cpp | 233 --- .../ui/main_window.ui | 1625 ----------------- open_manipulator_controller/CHANGELOG.rst | 60 - open_manipulator_controller/CMakeLists.txt | 95 - .../open_manipulator_controller.h | 174 -- .../launch/open_manipulator_controller.launch | 12 - open_manipulator_controller/package.xml | 29 - .../scripts/create_udev_rules | 14 - .../src/open_manipulator_controller.cpp | 661 ------- open_manipulator_description/CHANGELOG.rst | 55 - open_manipulator_description/CMakeLists.txt | 45 - .../launch/open_manipulator_rviz.launch | 14 - .../launch/open_manipulator_upload.launch | 4 - .../urdf/open_manipulator.gazebo.xacro | 106 -- .../urdf/open_manipulator.transmission.xacro | 48 - .../urdf/open_manipulator.urdf.xacro | 278 --- .../urdf/open_manipulator_robot.urdf.xacro | 27 - open_manipulator_libs/CHANGELOG.rst | 43 - open_manipulator_libs/CMakeLists.txt | 76 - .../open_manipulator_libs/custom_trajectory.h | 159 -- .../include/open_manipulator_libs/dynamixel.h | 189 -- .../open_manipulator_libs/kinematics.h | 118 -- .../open_manipulator_libs/open_manipulator.h | 56 - open_manipulator_libs/package.xml | 25 - .../src/custom_trajectory.cpp | 346 ---- open_manipulator_libs/src/dynamixel.cpp | 973 ---------- open_manipulator_libs/src/kinematics.cpp | 1041 ----------- .../src/open_manipulator.cpp | 217 --- open_manipulator_teleop/CHANGELOG.rst | 19 - open_manipulator_teleop/CMakeLists.txt | 72 - .../open_manipulator_teleop_joystick.h | 85 - .../open_manipulator_teleop_keyboard.h | 97 - .../open_manipulator_teleop_joystick.launch | 9 - .../open_manipulator_teleop_keyboard.launch | 9 - open_manipulator_teleop/package.xml | 23 - .../src/open_manipulator_teleop_joystick.cpp | 228 --- .../src/open_manipulator_teleop_keyboard.cpp | 391 ---- ...ipulator.repos => open_manipulator_x.repos | 20 +- .../99-open-manipulator-cdc.rules | 2 +- open_manipulator_x_bringup/CMakeLists.txt | 32 + .../config/gazebo_controller_manager.yaml | 59 + .../config/hardware_controller_manager.yaml | 49 + .../open_manipulator_x_bringup.dsv.in | 1 + .../launch/base.launch.py | 215 +++ .../launch/fake.launch.py | 79 + .../launch/gazebo.launch.py | 158 ++ .../launch/hardware.launch.py | 75 + open_manipulator_x_bringup/package.xml | 22 + .../rviz/open_manipulator_x.rviz | 145 +- .../scripts/create_udev_rules | 14 + .../worlds/empty_world.model | 46 + .../worlds/turtlebot3_world.model | 53 + .../turtlebot3_world/meshes/hexagon.dae | 76 + .../worlds/turtlebot3_world/meshes/wall.dae | 76 + .../worlds/turtlebot3_world/model-1_4.sdf | 549 ++++++ .../worlds/turtlebot3_world/model.config | 17 + .../worlds/turtlebot3_world/model.sdf | 549 ++++++ open_manipulator_x_description/CHANGELOG.rst | 105 ++ open_manipulator_x_description/CMakeLists.txt | 31 + .../open_manipulator_x_description.dsv.in | 1 + .../gazebo/materials.xacro | 48 + .../gazebo/open_manipulator_x.gazebo.xacro | 57 + .../launch/.DS_Store | Bin 0 -> 6148 bytes .../__pycache__/model.launch.cpython-310.pyc | Bin 0 -> 1782 bytes .../launch/model.launch.py | 105 ++ .../meshes/Plate.stl | Bin 0 -> 354484 bytes .../meshes/gripper_left_palm.stl | Bin .../meshes/gripper_right_palm.stl | Bin .../meshes/link1.stl | Bin .../meshes/link2.stl | Bin .../meshes/link3.stl | Bin .../meshes/link4.stl | Bin .../meshes/link5.stl | Bin .../package.xml | 25 +- ...en_manipulator_x_system.ros2_control.xacro | 188 ++ .../rviz/open_manipulator_x.rviz | 256 +++ .../urdf/materials.xacro | 8 - .../urdf/open_manipulator_x.urdf.xacro | 266 +++ .../urdf/open_manipulator_x_robot.urdf.xacro | 47 + open_manipulator_x_gui/CMakeLists.txt | 113 ++ .../open_manipulator_x_gui}/main_window.hpp | 48 +- .../include/open_manipulator_x_gui/qnode.hpp | 84 + .../launch/open_manipulator_x_gui.launch.py | 24 + open_manipulator_x_gui/package.xml | 32 + open_manipulator_x_gui/src/main.cpp | 14 + open_manipulator_x_gui/src/main_window.cpp | 571 ++++++ open_manipulator_x_gui/src/qnode.cpp | 302 +++ open_manipulator_x_gui/ui/main_window.ui | 1305 +++++++++++++ .../.setup_assistant | 25 + .../CMakeLists.txt | 11 + .../config/initial_positions.yaml | 8 + .../config/joint_limits.yaml | 42 + .../config/kinematics.yaml | 6 + .../config/moveit.rviz | 51 + .../config/moveit_controllers.yaml | 21 + .../config/moveit_servo.yaml | 69 + .../config/moveit_teleop.rviz | 236 +++ .../config/ompl_planning.yaml | 203 ++ .../open_manipulator_x.ros2_control.xacro | 49 + .../config/open_manipulator_x.srdf | 60 + .../config/open_manipulator_x.urdf.xacro | 13 + .../config/pilz_cartesian_limits.yaml | 6 + .../launch/demo.launch.py | 7 + .../launch/move_group.launch.py | 143 ++ .../launch/moveit_core.launch.py | 47 + .../launch/moveit_gazebo.launch.py | 84 + .../launch/moveit_rviz.launch.py | 118 ++ .../launch/rsp.launch.py | 56 + .../launch/servo.launch.py | 101 + .../launch/setup_assistant.launch.py | 7 + .../launch/spawn_controllers.launch.py | 42 + .../launch/static_virtual_joint_tfs.launch.py | 37 + .../launch/warehouse_db.launch.py | 56 + open_manipulator_x_moveit_config/package.xml | 52 + open_manipulator_x_playground/CMakeLists.txt | 45 + open_manipulator_x_playground/package.xml | 20 + .../src/hello_moveit.cpp | 57 + open_manipulator_x_teleop/CMakeLists.txt | 55 + .../open_manipulator_x_teleop.hpp | 129 ++ open_manipulator_x_teleop/package.xml | 27 + .../src/open_manipulator_x_teleop.cpp | 272 +++ 136 files changed, 7989 insertions(+), 8537 deletions(-) create mode 100644 CONTRIBUTING.md delete mode 100644 open_manipulator_control_gui/CHANGELOG.rst delete mode 100644 open_manipulator_control_gui/CMakeLists.txt delete mode 100644 open_manipulator_control_gui/include/open_manipulator_control_gui/qnode.hpp delete mode 100644 open_manipulator_control_gui/launch/open_manipulator_control_gui.launch delete mode 100644 open_manipulator_control_gui/mainpage.dox delete mode 100644 open_manipulator_control_gui/package.xml delete mode 100644 open_manipulator_control_gui/resources/images.qrc delete mode 100644 open_manipulator_control_gui/resources/images/icon.png delete mode 100644 open_manipulator_control_gui/src/main.cpp delete mode 100644 open_manipulator_control_gui/src/main_window.cpp delete mode 100644 open_manipulator_control_gui/src/qnode.cpp delete mode 100644 open_manipulator_control_gui/ui/main_window.ui delete mode 100644 open_manipulator_controller/CHANGELOG.rst delete mode 100644 open_manipulator_controller/CMakeLists.txt delete mode 100644 open_manipulator_controller/include/open_manipulator_controller/open_manipulator_controller.h delete mode 100644 open_manipulator_controller/launch/open_manipulator_controller.launch delete mode 100644 open_manipulator_controller/package.xml delete mode 100755 open_manipulator_controller/scripts/create_udev_rules delete mode 100644 open_manipulator_controller/src/open_manipulator_controller.cpp delete mode 100644 open_manipulator_description/CHANGELOG.rst delete mode 100644 open_manipulator_description/CMakeLists.txt delete mode 100644 open_manipulator_description/launch/open_manipulator_rviz.launch delete mode 100644 open_manipulator_description/launch/open_manipulator_upload.launch delete mode 100644 open_manipulator_description/urdf/open_manipulator.gazebo.xacro delete mode 100644 open_manipulator_description/urdf/open_manipulator.transmission.xacro delete mode 100644 open_manipulator_description/urdf/open_manipulator.urdf.xacro delete mode 100644 open_manipulator_description/urdf/open_manipulator_robot.urdf.xacro delete mode 100644 open_manipulator_libs/CHANGELOG.rst delete mode 100644 open_manipulator_libs/CMakeLists.txt delete mode 100644 open_manipulator_libs/include/open_manipulator_libs/custom_trajectory.h delete mode 100644 open_manipulator_libs/include/open_manipulator_libs/dynamixel.h delete mode 100644 open_manipulator_libs/include/open_manipulator_libs/kinematics.h delete mode 100644 open_manipulator_libs/include/open_manipulator_libs/open_manipulator.h delete mode 100644 open_manipulator_libs/package.xml delete mode 100644 open_manipulator_libs/src/custom_trajectory.cpp delete mode 100644 open_manipulator_libs/src/dynamixel.cpp delete mode 100644 open_manipulator_libs/src/kinematics.cpp delete mode 100644 open_manipulator_libs/src/open_manipulator.cpp delete mode 100644 open_manipulator_teleop/CHANGELOG.rst delete mode 100644 open_manipulator_teleop/CMakeLists.txt delete mode 100644 open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_joystick.h delete mode 100644 open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_keyboard.h delete mode 100644 open_manipulator_teleop/launch/open_manipulator_teleop_joystick.launch delete mode 100644 open_manipulator_teleop/launch/open_manipulator_teleop_keyboard.launch delete mode 100644 open_manipulator_teleop/package.xml delete mode 100644 open_manipulator_teleop/src/open_manipulator_teleop_joystick.cpp delete mode 100644 open_manipulator_teleop/src/open_manipulator_teleop_keyboard.cpp rename openmanipulator.repos => open_manipulator_x.repos (51%) rename {open_manipulator_controller => open_manipulator_x_bringup}/99-open-manipulator-cdc.rules (50%) create mode 100644 open_manipulator_x_bringup/CMakeLists.txt create mode 100644 open_manipulator_x_bringup/config/gazebo_controller_manager.yaml create mode 100644 open_manipulator_x_bringup/config/hardware_controller_manager.yaml create mode 100644 open_manipulator_x_bringup/env-hooks/open_manipulator_x_bringup.dsv.in create mode 100644 open_manipulator_x_bringup/launch/base.launch.py create mode 100644 open_manipulator_x_bringup/launch/fake.launch.py create mode 100644 open_manipulator_x_bringup/launch/gazebo.launch.py create mode 100644 open_manipulator_x_bringup/launch/hardware.launch.py create mode 100644 open_manipulator_x_bringup/package.xml rename open_manipulator_description/rviz/open_manipulator.rviz => open_manipulator_x_bringup/rviz/open_manipulator_x.rviz (54%) create mode 100644 open_manipulator_x_bringup/scripts/create_udev_rules create mode 100644 open_manipulator_x_bringup/worlds/empty_world.model create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world.model create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model.config create mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf create mode 100644 open_manipulator_x_description/CHANGELOG.rst create mode 100644 open_manipulator_x_description/CMakeLists.txt create mode 100644 open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in create mode 100644 open_manipulator_x_description/gazebo/materials.xacro create mode 100644 open_manipulator_x_description/gazebo/open_manipulator_x.gazebo.xacro create mode 100644 open_manipulator_x_description/launch/.DS_Store create mode 100644 open_manipulator_x_description/launch/__pycache__/model.launch.cpython-310.pyc create mode 100644 open_manipulator_x_description/launch/model.launch.py create mode 100644 open_manipulator_x_description/meshes/Plate.stl rename open_manipulator_description/meshes/chain_link_grip_l.stl => open_manipulator_x_description/meshes/gripper_left_palm.stl (100%) rename open_manipulator_description/meshes/chain_link_grip_r.stl => open_manipulator_x_description/meshes/gripper_right_palm.stl (100%) rename open_manipulator_description/meshes/chain_link1.stl => open_manipulator_x_description/meshes/link1.stl (100%) rename open_manipulator_description/meshes/chain_link2.stl => open_manipulator_x_description/meshes/link2.stl (100%) rename open_manipulator_description/meshes/chain_link3.stl => open_manipulator_x_description/meshes/link3.stl (100%) rename open_manipulator_description/meshes/chain_link4.stl => open_manipulator_x_description/meshes/link4.stl (100%) rename open_manipulator_description/meshes/chain_link5.stl => open_manipulator_x_description/meshes/link5.stl (100%) rename {open_manipulator_description => open_manipulator_x_description}/package.xml (53%) create mode 100644 open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro create mode 100644 open_manipulator_x_description/rviz/open_manipulator_x.rviz rename {open_manipulator_description => open_manipulator_x_description}/urdf/materials.xacro (71%) create mode 100644 open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro create mode 100644 open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro create mode 100644 open_manipulator_x_gui/CMakeLists.txt rename {open_manipulator_control_gui/include/open_manipulator_control_gui => open_manipulator_x_gui/include/open_manipulator_x_gui}/main_window.hpp (55%) create mode 100644 open_manipulator_x_gui/include/open_manipulator_x_gui/qnode.hpp create mode 100644 open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py create mode 100644 open_manipulator_x_gui/package.xml create mode 100644 open_manipulator_x_gui/src/main.cpp create mode 100644 open_manipulator_x_gui/src/main_window.cpp create mode 100644 open_manipulator_x_gui/src/qnode.cpp create mode 100644 open_manipulator_x_gui/ui/main_window.ui create mode 100644 open_manipulator_x_moveit_config/.setup_assistant create mode 100644 open_manipulator_x_moveit_config/CMakeLists.txt create mode 100644 open_manipulator_x_moveit_config/config/initial_positions.yaml create mode 100644 open_manipulator_x_moveit_config/config/joint_limits.yaml create mode 100644 open_manipulator_x_moveit_config/config/kinematics.yaml create mode 100644 open_manipulator_x_moveit_config/config/moveit.rviz create mode 100644 open_manipulator_x_moveit_config/config/moveit_controllers.yaml create mode 100644 open_manipulator_x_moveit_config/config/moveit_servo.yaml create mode 100644 open_manipulator_x_moveit_config/config/moveit_teleop.rviz create mode 100644 open_manipulator_x_moveit_config/config/ompl_planning.yaml create mode 100644 open_manipulator_x_moveit_config/config/open_manipulator_x.ros2_control.xacro create mode 100644 open_manipulator_x_moveit_config/config/open_manipulator_x.srdf create mode 100644 open_manipulator_x_moveit_config/config/open_manipulator_x.urdf.xacro create mode 100644 open_manipulator_x_moveit_config/config/pilz_cartesian_limits.yaml create mode 100644 open_manipulator_x_moveit_config/launch/demo.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/move_group.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/moveit_core.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/rsp.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/servo.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/setup_assistant.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py create mode 100644 open_manipulator_x_moveit_config/launch/warehouse_db.launch.py create mode 100644 open_manipulator_x_moveit_config/package.xml create mode 100644 open_manipulator_x_playground/CMakeLists.txt create mode 100644 open_manipulator_x_playground/package.xml create mode 100644 open_manipulator_x_playground/src/hello_moveit.cpp create mode 100644 open_manipulator_x_teleop/CMakeLists.txt create mode 100644 open_manipulator_x_teleop/include/open_manipulator_x_teleop/open_manipulator_x_teleop.hpp create mode 100644 open_manipulator_x_teleop/package.xml create mode 100644 open_manipulator_x_teleop/src/open_manipulator_x_teleop.cpp diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 00000000..cfba094d --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,18 @@ +Any contribution that you make to this repository will +be under the Apache 2 License, as dictated by that +[license](http://www.apache.org/licenses/LICENSE-2.0.html): + +~~~ +5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. +~~~ + +Contributors must sign-off each commit by adding a `Signed-off-by: ...` +line to commit messages to certify that they have the right to submit +the code they are contributing to the project according to the +[Developer Certificate of Origin (DCO)](https://developercertificate.org/). diff --git a/README.md b/README.md index 899cf5fb..d4cea515 100644 --- a/README.md +++ b/README.md @@ -1,17 +1,18 @@ -# OpenManipulator - - - [![kinetic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/kinetic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/kinetic-devel) - [![melodic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/melodic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/melodic-devel) - [![noetic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/noetic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/noetic-devel) +[![dashing-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/dashing-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/dashing-devel) +[![foxy-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/foxy-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/foxy-devel) + +# OpenMANIPULATOR-X + + + -## ROBOTIS e-Manual for OpenManipulator -- [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) +# ROBOTIS e-Manual for OpenMANIPULATOR-X +- [http://emanual.robotis.com/docs/en/platform/openmanipulator/](http://emanual.robotis.com/docs/en/platform/openmanipulator/) -## Wiki for open_manipulator Packages +# Wiki for open_manipulator Packages - http://wiki.ros.org/open_manipulator (metapackage) - http://wiki.ros.org/open_manipulator_control_gui - http://wiki.ros.org/open_manipulator_controller @@ -20,7 +21,7 @@ - http://wiki.ros.org/open_manipulator_moveit - http://wiki.ros.org/open_manipulator_teleop -## Open Source related to OpenManipulator +# Open Source related to OpenMANIPULATOR-X - [robotis_manipulator](https://github.com/ROBOTIS-GIT/robotis_manipulator) - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) - [open_manipulator_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_msgs) @@ -31,7 +32,7 @@ - [open_manipulator_with_tb3](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3) - [open_manipulator_with_tb3_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_msgs) - [open_manipulator_with_tb3_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_simulations) -- [turtlebot3](https://github.com/ROBOTIS-GIT/turtlebot3) +- [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) - [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) - [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) - [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) @@ -45,12 +46,13 @@ - [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) - [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) -## Documents and Videos related to OpenManipulator -- [ROBOTIS e-Manual for OpenManipulator](http://emanual.robotis.com/docs/en/platform/openmanipulator/) -- [ROBOTIS e-Manual for TurtleBot3](http://turtlebot3.robotis.com/) -- [ROBOTIS e-Manual for ROBOTIS MANIPULATOR-H](http://emanual.robotis.com/docs/en/platform/manipulator_h/introduction/) -- [ROBOTIS e-Manual for Dynamixel SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) -- [ROBOTIS e-Manual for Dynamixel Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) -- [e-Book for TurtleBot3 and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) -- [Videos for OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb) -- [Videos for TurtleBot3 and OpenManipulator](https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU) +# Documents and Videos related to OpenMANIPULATOR-X +- [ROBOTIS e-Manual for OpenMANIPULATOR-X](http://emanual.robotis.com/docs/en/platform/openmanipulator/) +- [ROBOTIS e-Manual for OpenMANIPULATOR-P](https://emanual.robotis.com/docs/en/platform/openmanipulator_p/overview/) +- [ROBOTIS e-Manual for DYNAMIXEL SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) +- [ROBOTIS e-Manual for DYNAMIXEL Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) +- [e-Book for open_manipulator and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) + +https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb + +https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU diff --git a/open_manipulator/CHANGELOG.rst b/open_manipulator/CHANGELOG.rst index 43b585d9..1ea5bcd1 100644 --- a/open_manipulator/CHANGELOG.rst +++ b/open_manipulator/CHANGELOG.rst @@ -2,12 +2,22 @@ Changelog for package open_manipulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -2.0.2 (2021-06-21) +2.3.0 (2021-10-06) ------------------ -* Noetic support -* Remove use_gui param from joint_state_publisher package +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python * Contributors: Will Son +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + 2.0.1 (2019-02-18) ------------------ * added dependency option for open_manipulator_control_gui package diff --git a/open_manipulator/CMakeLists.txt b/open_manipulator/CMakeLists.txt index f2ff54f1..518810cc 100644 --- a/open_manipulator/CMakeLists.txt +++ b/open_manipulator/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.5) project(open_manipulator) -find_package(catkin REQUIRED) -catkin_metapackage() +find_package(ament_cmake REQUIRED) +ament_package() diff --git a/open_manipulator/package.xml b/open_manipulator/package.xml index 86d21e6b..8010d294 100644 --- a/open_manipulator/package.xml +++ b/open_manipulator/package.xml @@ -1,27 +1,27 @@ - + + open_manipulator - 2.0.2 + 2.3.0 - ROS-enabled OpenManipulator is a full open robot platform consisting of OpenSoftware​, OpenHardware and OpenCR(Embedded board)​. - The OpenManipulator is allowed users to control it more easily by linking with the MoveIt! package. Moreover it has full hardware compatibility with TurtleBot3​. - Even if you do not have a real robot, you can control the robot in the Gazebo simulator​. + OpenMANIPULATOR-X meta ROS 2 package. + Will Son Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues Darby Lim Hye-Jong KIM Ryan Shim Yong-Ho Na - Will Son - http://wiki.ros.org/open_manipulator - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues - catkin - open_manipulator_control_gui - open_manipulator_controller - open_manipulator_description - open_manipulator_libs - open_manipulator_teleop - + Will Son + ament_cmake + open_manipulator_x_controller + open_manipulator_x_description + open_manipulator_x_libs + open_manipulator_x_teleop + + ament_cmake + diff --git a/open_manipulator_control_gui/CHANGELOG.rst b/open_manipulator_control_gui/CHANGELOG.rst deleted file mode 100644 index e0455e1e..00000000 --- a/open_manipulator_control_gui/CHANGELOG.rst +++ /dev/null @@ -1,22 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_control_gui -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.2 (2021-06-21) ------------------- -* Noetic support -* Contributors: Will Son - -2.0.1 (2019-02-18) ------------------- -* added dependency option for open_manipulator_control_gui package -* Contributors: Pyo - -2.0.0 (2019-02-08) ------------------- -* updated the CHANGELOG and version to release binary packages (first relese) -* updated function name, UI -* added group names and gripper args -* added position only client -* modified topic names, end-effector name -* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Guilherme de Campos Affonso, Pyo diff --git a/open_manipulator_control_gui/CMakeLists.txt b/open_manipulator_control_gui/CMakeLists.txt deleted file mode 100644 index cb740e98..00000000 --- a/open_manipulator_control_gui/CMakeLists.txt +++ /dev/null @@ -1,116 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(open_manipulator_control_gui) - -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - sensor_msgs - open_manipulator_msgs - cmake_modules -) -find_package(Eigen3 REQUIRED) -find_package(Qt5Widgets REQUIRED) - -set(${PROJECT_NAME}_SRCS - src/main.cpp - src/main_window.cpp - src/qnode.cpp -) - -set(${PROJECT_NAME}_HDRS - include/${PROJECT_NAME}/main_window.hpp - include/${PROJECT_NAME}/qnode.hpp -) - -set(${PROJECT_NAME}_UIS - ui/main_window.ui -) - -set(${PROJECT_NAME}_RESOURCES - resources/images.qrc -) - -set(${PROJECT_NAME}_INCLUDE_DIRECTORIES - "${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION}/${PROJECT_NAME}" -) - -if(NOT EXISTS ${${PROJECT_NAME}_INCLUDE_DIRECTORIES}) - file(MAKE_DIRECTORY ${${PROJECT_NAME}_INCLUDE_DIRECTORIES}) -endif() - - - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp std_msgs sensor_msgs open_manipulator_msgs - DEPENDS EIGEN3 -) - -qt5_wrap_cpp(${PROJECT_NAME}_MOCS ${${PROJECT_NAME}_HDRS}) -qt5_add_resources(${PROJECT_NAME}_RCC ${${PROJECT_NAME}_RESOURCES}) - -# ensure generated header files are being created in the devel space -set(_cmake_current_binary_dir "${CMAKE_CURRENT_BINARY_DIR}") -set(CMAKE_CURRENT_BINARY_DIR ${open_manipulator_control_gui_INCLUDE_DIRECTORIES}) - -qt5_wrap_ui(${PROJECT_NAME}_UIS_H ${${PROJECT_NAME}_UIS}) - -set(CMAKE_CURRENT_BINARY_DIR "${_cmake_current_binary_dir}") - - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${open_manipulator_control_gui_INCLUDE_DIRECTORIES} - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -add_executable(open_manipulator_control_gui ${open_manipulator_control_gui_SRCS} ${open_manipulator_control_gui_MOCS} ${open_manipulator_control_gui_UIS_H} ${open_manipulator_control_gui_RCC}) -add_dependencies(open_manipulator_control_gui ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open_manipulator_control_gui Qt5::Widgets ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) - -################################################################################# -## Install -################################################################################# -install(TARGETS open_manipulator_control_gui - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY resources ui launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/open_manipulator_control_gui/include/open_manipulator_control_gui/qnode.hpp b/open_manipulator_control_gui/include/open_manipulator_control_gui/qnode.hpp deleted file mode 100644 index 898b083a..00000000 --- a/open_manipulator_control_gui/include/open_manipulator_control_gui/qnode.hpp +++ /dev/null @@ -1,128 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -/***************************************************************************** -** Ifdefs -*****************************************************************************/ - -#ifndef open_manipulator_control_gui_QNODE_HPP_ -#define open_manipulator_control_gui_QNODE_HPP_ - -/***************************************************************************** -** Includes -*****************************************************************************/ - -// To workaround boost/qt4 problems that won't be bugfixed. Refer to -// https://bugreports.qt.io/browse/QTBUG-22829 -#ifndef Q_MOC_RUN -#include -#endif -#include -#include -#include - -#include -#include - -#include "open_manipulator_msgs/OpenManipulatorState.h" -#include "open_manipulator_msgs/SetJointPosition.h" -#include "open_manipulator_msgs/SetKinematicsPose.h" -#include "open_manipulator_msgs/SetDrawingTrajectory.h" -#include "open_manipulator_msgs/SetActuatorState.h" - -#define NUM_OF_JOINT_AND_TOOL 5 - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace open_manipulator_control_gui { - -/***************************************************************************** -** Class -*****************************************************************************/ - -class QNode : public QThread { - Q_OBJECT -public: - QNode(int argc, char** argv ); - virtual ~QNode(); - bool init(); - void run(); - - /********************* - ** Logging - **********************/ - enum LogLevel { - Debug, - Info, - Warn, - Error, - Fatal - }; - - QStringListModel* loggingModel() { return &logging_model; } - void log( const LogLevel &level, const std::string &msg); - - void manipulatorStatesCallback(const open_manipulator_msgs::OpenManipulatorState::ConstPtr &msg); - void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg); - - std::vector getPresentJointAngle(); - std::vector getPresentKinematicsPose(); - bool getOpenManipulatorMovingState(); - bool getOpenManipulatorActuatorState(); - - void setOption(std::string opt); - bool setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time); - bool setTaskSpacePath(std::vector kinematics_pose, double path_time); - bool setDrawingTrajectory(std::string name, std::vector arg, double path_time); - bool setToolControl(std::vector joint_angle); - bool setActuatorState(bool actuator_state); - -Q_SIGNALS: - void rosShutdown(); - -private: - int init_argc; - char** init_argv; - QStringListModel logging_model; - - ros::Publisher open_manipulator_option_pub_; - - ros::Subscriber open_manipulator_states_sub_; - ros::Subscriber open_manipulator_joint_states_sub_; - ros::Subscriber open_manipulator_kinematics_pose_sub_; - - ros::ServiceClient goal_joint_space_path_client_; - ros::ServiceClient goal_task_space_path_position_only_client_; - ros::ServiceClient goal_tool_control_client_; - ros::ServiceClient set_actuator_state_client_; - ros::ServiceClient goal_drawing_trajectory_client_; - - std::vector present_joint_angle_; - std::vector present_kinematic_position_; - open_manipulator_msgs::KinematicsPose kinematics_pose_; - - bool open_manipulator_is_moving_; - bool open_manipulator_actuator_enabled_; -}; - -} // namespace open_manipulator_control_gui - -#endif /* open_manipulator_control_gui_QNODE_HPP_ */ diff --git a/open_manipulator_control_gui/launch/open_manipulator_control_gui.launch b/open_manipulator_control_gui/launch/open_manipulator_control_gui.launch deleted file mode 100644 index cd360f5b..00000000 --- a/open_manipulator_control_gui/launch/open_manipulator_control_gui.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/open_manipulator_control_gui/mainpage.dox b/open_manipulator_control_gui/mainpage.dox deleted file mode 100644 index ddc35886..00000000 --- a/open_manipulator_control_gui/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b open_manipulator_control_gui is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/open_manipulator_control_gui/package.xml b/open_manipulator_control_gui/package.xml deleted file mode 100644 index 1ed0fdba..00000000 --- a/open_manipulator_control_gui/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - open_manipulator_control_gui - 2.0.2 - - OpenManipulator GUI control package based on QT - - Apache 2.0 - Darby Lim - Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son - http://wiki.ros.org/open_manipulator_control_gui - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues - catkin - roscpp - std_msgs - sensor_msgs - open_manipulator_msgs - qtbase5-dev - qt5-qmake - libqt5-core - libqt5-gui - cmake_modules - eigen - diff --git a/open_manipulator_control_gui/resources/images.qrc b/open_manipulator_control_gui/resources/images.qrc deleted file mode 100644 index e91e9dc2..00000000 --- a/open_manipulator_control_gui/resources/images.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - images/icon.png - - diff --git a/open_manipulator_control_gui/resources/images/icon.png b/open_manipulator_control_gui/resources/images/icon.png deleted file mode 100644 index 541934e609ed7a8592cf1ffa6b06059476be06a9..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 7695 zcmV+q9`NCbP)Px#1ZP1_K>z@;j|==^1poj532;bRa{vGi!vFvd!vV){sAK>D02p*dSaefwW^{L9 za%BK;VQFr3E^cLXAT%y8E;VI^GGzb&9e_zhK~#8N?VSghRK>E#QQv#_zW46;ys$Gn zvkGiNk|Y5^C7OszMo<*Yf*CU)hyns42neErs0b*aU_ia*grZ_bKm;TyK>@48-{C{7=PEU8AK2_b-Rn^r!rHnCurNfw9Gyh75F1cp@l@48U&pg7B zSGsihyij0PZYVGzKQnN7Zf0invSrKuxAXO%21@H#;%nv+j=WItj{I=YM5P1yp}>~B zaK?<>%#3aov;Gl~Vn;+i=8cx|_K^Xb~ZFWAVmHG1X&&?U9)GNe*LMas3DYIg7Zm3K>sd`MLg(Cw> z>saDzW)jHGF7@}E%#1d<;f%TRvOA(*zQS=K)1pa38L8#;voK{#UNG3eshTqKLKzcj z@1o8fq`Ss!-@e`4bWJZ)AuH2VESqI|UDCsJ@7z&lN7g|`h?tTYxj+U;hbE;;{fE=k zVTIC~e`1p#1HTuC3Hjl`TeNd~?;D)f58|sYJ~yY-I8mQ-+YQ&7y?ghXJ$v?;H(&dg>Da18 zL4UO$p}?Y?VCm-4Du;OeGVdV_wL=! zH^K-h`{DcVnbv1CvHfrs>oR@olq&U~&httf(v|oO2!%rbE7N_lbnpP3M7|Cmbhp{I zb*mIh9bbv&QtN~~eQ}vpE16x+BMU(Z9u72-J`9&~pGewXKrl(nz=pMJX=>4=k-4CK zTXXx3H<-zfOfU;R{@DDyaf7+^g05QjYFRJ#iYST(;%VBGkDHULRxW6hg*rF$pVgf% zlqjSNaT##5bmmptvCikVGAmcCkgwB&_uhWXR4#wKwheKkE!hg;a8UC4OImNAZ_^2) zS)C{YfmztJUR`r~{gXAZzEsQ0)&ts?EyZ<{ylo`KHO3**-pbmpn`bsGCK`SMlzG3d^I(J!ovE2(;3j=4j$92RbflM1M}X7wsv zuNv2_t?f2BxwhH4bEi|HC^oELuWQD^J_WzX3I8)&iIQf>HA0OCMK zp-^muOyF zvqKq8l{jgFJi;FXuDSeD9SxV9-$lMI8NBl13;K*RPOYahzN7&0A?ltewKN_+ng5Zg zc~E2fJvS@RNC}h%Nb~*}Fhwlf_AQzzftH9KUs4#{|4x0;^UiH;e*0~klbc@PzTg>% zKEGJ}?Lt|cBdRK@a9$|m3?))AAx(QTKv0MQnDYGHyH`&sQZhjHUUS(c`kaofTbND1 z{Nj9*9_$x`?ab5atGG*6t6REc@j)htQJNhHoaE3cWk|De8E~{bXfY2Qd;bWvBvA)3 z#x`!)AjL`sh=%L0x#@*{1={k z)>O$ZD+4Ioe7AV9^G))=!nO-DDr#0%*8e#4N*dB!aty$~f(rqMS~4;;Rebcp`=&{~ zI{LDR4H&Ux(IVgKFpR|Kb5ZAxI`V7e=a~23aW}-0!Sm0~()Q?AerCqgLaUU#snS$( z3;_2I9H0+=ZpJfCPRRmUJ@KJ&rol-yBQGoo7X+5^Q;$zJ@4oq_`R1!H&BCw0G7G=@ zN(nw=+Eml$=HBL%8r8HsD)faN+nZ(IFLhciKCly<{u~f1{wjxNNkN)P$bheT$cTFf zOLhd`f44+U`t#bHQzC|$7g&RE74*8ahpL4IBhVx9eeFsW%q`blqk9DD?kfrd#wpK2 zGfKssEzQJbKq+bXBOY*1zrKdcat|#KElU=CYhIi)TX(+h9e9_{ zOwzbfBh9R*r|XP>NF|9M*RD~gpfWuaT#)Pzn>3L;0{}9R{9*Up?d1Ib#mg@|UoabU zLuL9nr1J%-o*Dyo{_%&30pv2H@y~)kq~Ex~A09i#%$YS)KJRe8Nga4)uXb#g{7~Qz zwrm~pC4^Lu%YdU~T75>tg9qFx*@bc24c95>Kl$iGC+Elnp3mjqe{aT*e!$eLR>jn* zQqf%4se=rj8m9Ysozk*4u<;Wmie3@g`PxU2s@@DxZ>X~k`PAc&Np@l2SLeHax7_69 z93HU6IegGP=5jHvy0&X$uIh1-CXB7tXEaso6{VqF^RvvX>C?;~J0hcg?V8n^zkXeM zyskuKCIww&}iO$9A;}8`Y_G*u)S}dboHuLPEPTFme#UqV`YL2 z+{J!kkEEJ61Ei6zG{!cc*XH|8SNGHhqH!D!j1nUE=+fEwlpNlE<8>8B=^Q-ozw@@8 z1uV#yDk0-Qs>GQA;lMB&YS!>H$#Tc5FTJP_gAj)UjBb>H70Z@6IsVaCMCY|vT;_aA z9}t7kgjpbDj5`aYcpL^~MuMMi=y|1NxdXpF)3|-hW=_tb1q&!4iH+)oS0M2Y~GY!cQ-(!5hbXzARPEuXzA%ElH%?R&?q=30*-xv zco=$j#3%CU{EwWRLl0gWmqWd3Rn;$)2yD``=~Gg8#SzVt_Gbr!wH$gEffRLT02|Ow z1Lmh6f0Qh5y!Oh=S`HV$;RH=LKjZ)IP@+8De5r;!of~duAZNhaOVhr5Z*>284fof z6h`Aoq8{VQb^{U9=>LBFSj3c~%AS78$#z7$CbX&h?;R`#*+!?P(a(|Z1M9uQ9i^B% z1M-Kbf7vi%T|zC41-pBMF(!I1Ec`}wPf{Xs z5{J-60?8IF{KjnDuwFGU0tuLPHsH9X&|&8EsY(Y7M9xQ$pPO0cY=`EBAdm5w0ilf1 zRK)A)2VL5nquS7+*rf3T2!M!Tp}pn0YjrmKuzb0B_L--Zm^je9KwU%&-rmacPIvXW zU0n_Fuyf}hX5#p9GV>~^1~lOI+tlyW)Y(kR%M4C(XkG|X)tv!(;owXvUMt##AKcxy zk3MMO*Izq1M+WFm&r5pfP8h^weP9zO-c03th=|{4L6XscxHnpz9|w zQ$AIzm3mn7iS`Ew0AMB)Egf&)81cn^1bgJ8M+}#Kj?v8`)Y`3Ee|4CUsfHeOkpaEe z%}Xjxxidf(j)hc=-inL|%zDa?9XZ0uNe;wg;=4JuX7vIiCN`0eW`PVZiKvVnJ9g+% zCT7Zq@4GMH%$!f>&sQzVt)E0=54inS)tFeAZL~E>w4oxd32D-61_ahn2@Svx7`2p7 z)-y`HCIrRbM14dAV(;;ZkEpkmg^-1G@*@vxypRnEK|6bB%mYcs_Qb=qCmz?fSya97 z$hZepgQ8r##Ns_%6oKeG4i-9eFMu@Z$pE#2S+x9s_a>!4fkz)vtb{F8TxnpeZp!2new8t|X!9 zrw-iB8M7B$Nc(3h*rDE6y0ZTjWs0^OS3q~DS}>IB2D=zgKrKdZqh`nfp3 zaHB0|nHz}Et93$T_QfWAN-E>d{a2Q*pEO-pr0JrE{+QP*uxXF z9fG&qz!s*ZNo*SHu}*RfD|VfTJ?tUH(!&5`G5f=8&LqN!A%nE-8+%?A*BJWc=Z)%# zjX7*xkdU*u!|i_T$f2pb#PO)IV0wukD=OU*3Wyts$M1pdX+< z635B;+;~{MYNZmCLv`^^SYmD3psGX(Cmfb2Cu-KGc&OvNHmy}&6Zz)`tZKJslUSJD zfEpV+19Gy0%^cb*q?jiIl3Ik(uNjdIYu9vg+|mEGTeKryQyu*B^Ut2d=Y=ll(9X=B z`Hbq__uSc6t=+R5oT}e@of}Ss_AQ(1raG#q8@y&Asj<9w>>9+(?%nIEB4vfHT^-sh zq^LUsL}%4=xM&=H;AaikwR5MAR5YG=@|x4_u^qRGGh?x03pWG0JKxu@RyCzytZp6K z=_vngoBNrBM9#S8jT`8ISpNM|JwFJG?fpQKFy)MLYj4FKz?RMJ1TI|5)Y0CMh6jSmCGF; z_jDs2qD`DxUO>CCFX5JWu^|KctBS{t7T(v>YE>^(=7IZ$Ijtp!ciwnI2}RhV*Pug7 z>UYb{MfCgVgo^$8D=(`%n$I_1d-cKZW3FwJCOu|ABr0w{mDux)2i&p{2K6u=o_=bI zb^>|p1x~g5k+l#iww(R^>@(Ay){+B6Csvd;9`(@eEw~PWkoeB$w$_js4*p%)<05@G z{8_kg=p(mIFvs^|J8tuta_%qUUJu*6Xiz za*Bd$WQcUr4kwX#K16^C9AQcoxW#ewq4&?3HN(kG4tQ^?ifw{nhq!%U_)zVmEWk^I z=7mC3q_{f+Kwf6xMJiq-Q_>5s&V9+`%gb{)QX>6(*RGxVa@Y&_gS@b8>33>eMH4tt zVw&4n<(jYfZ5tWta{pH z10VG``ogDZFz`5Y(80M5jg1(4BMWtG*m`a_aFY@?3Q{D_49F^7UMlK3zyUY51-UT= zJs#f)`J9-X_uhL_#UD7*1JekgO5{TRLe$lkO&jTR+!C8O5d5dLYxO1Btns?9E){H+ zwopCoKwyS(${uxk!q_op^nJrs{My6Yh%MHE=r}5Cd`jByb~Bh1@n(SFD48~TLdGp& zBd{1Hi*XN()cRL;zc8VS%e2NL%LN`VofuT&j~Ma9;IP+g*9`JX0>qqhJQnRH`u%M} zta4(L>kMLD>)b9z8E#u~iw%+@aTy>MaqtrRp#HbrY91VQzwTTS;*l!w1K9{-B9y(R zb|Tg|Nbb4O^Z8}`Y7!yH6PAaXwb=tHx1%^K&MqTs#Qh#EkdEL+JSSX^*5AfTg_ENuHi?hw|9&Y5riRiNeIiWK39g-G; z6pPOQkQWXNpb;#(bRKku37l-9ZR9s{HPPi9L_<6{@_vm#L-k}^vZQWRNd;Uoh2GD( z1^aIK;lMXSM%OcAQYA41${%}dY4$T|tecEFHdS!GMayr9Nt6QWAac*|#t1SK2bjIk z6}K`noppkvu)GK_ggS_YYW?!4vP3cXag!V>Sp7dj^n)ok6qqM#=^XC6j=lO!swHLs zAf_d{QFP=RdtId}7&}||jH16ktyYbsu9`v3eRTXd6-%)eu1(~hbLQ!KC1t$bw8H`H z8oaegiAfV4(nEr7@1O%Y${it9eKFvu3Z+W@O}My>X^*PKi9;G>^rDvXHybGgty-VY zU3c6jO(hAL5nDEI){#F$%=$;iKd3eXql^4Q0T%aohN2s6WFT>*AWHqzdrJwO88c#d zBrlXP$f0Y5*8CHjEOG*Y#$rHxBgFq%D(}NML2S?uV5A70!NnGax4zY(_z}8nED# zkF{d-o-T3lwn1V%EqEa71q=0|0%7R9BqI$MxgQWt^U|EzDnfV%W^OX_|H(-cwI1R5 z7OY?75wQW#)NGU-)<1g%T^wc$ycvKICS2Ol_~hDZ)zPs!RVt}$x1LX;)~v6Db5j;9 zWT=+-$`6Hw3W!8Q=JKLUf8;o!?VN~2YobpiWHNUi@m;e6Szg6V1k{Ucoh;d0EifxKtX$+_PQ%B)5drK3;ZCIiMCs3{nc00N{)`@VUXZ7BNbf4Dha|fOIQ7noKFkO+eXX5L^~iEY&?U($KRYs zCRB&M*y|oRG!hxK0k{^Q0oma)Rq{mp!QT(}=DsjTA3AFI!HsXMV@6!tW@kD%5!i6= zsq+8f03j>{mJtkw5yznkW=N-VTIwb~YHlj3Ez*D}aNZ5s&;MMR&l-e(h|h>Yz!k~S zpI0njrta#zP;gIPunZe5@fiR_3_VXH__&#N$XUX#qd~wGaiIqe9Gt={MFXN9KN}M7 zvg|(-7}3P5@?J0%*Ms$G5zl19(OC!BABWiFu|VTJuxm|DX5a>BD@`V5Kz3%CQ>4;< z9>^>>zkS4$sXx%;phK(Xs-jwXPdV;grW^^)!d?^eX9eqnwRyPy*e=#wUd0h58}W@; zBV64cQzb}X8tA zMI+#<0P;{j*_K0PMJ&~P{u|;AQ4~$2RDd0 zvKvczp^TA2+k$;zspifADR>(dViPyoTA2@#(JE$({*QMux*Q{+y;ziN?cx6?Vvc+k zuETYwM50~lsRuEZ6C<|KCgQG^8l`EDPnk`2HcJJP$`w$TV22Hn)?R~o^0CoZftk7_Gj>m2o zkfQDkaBxg+xJ)N$M6WMK%|Zt9H;hR2GVOi)_No}79TE&~-l&1PYjNWyiZ3cD@LU3i z2lcWus7;OJxyRYKcbT*ECWg z76&RIIyy4{N4H7Bk%ggyYv7{~K2S5iB$&d#wPjBBJLT_E8)%xar@^Z6xKyQ5`2ytq^N6(-8#hNo>e@B?>92fPP4aYT=WRPoOM3w~XX z-}({`uV)1NJxR$(CRml1;5Kv%tD@rOM$RIJIQRx3xNRibIl#xMMj`|x#(24 zN+oqea|shO(}p41pVq>?ZGrop_*3!;28b2GFvZ#dd6^maI^>EAsg^tgEZ{JU3>pz? zYCNF6o!u~UkZyp;(&)vT6Ol0Gx+{vFuHmKdUNYLJK0aAhI~aqf^R$y{=q?lnPvvsQ z>xMo9g2dk>)MQ3F2ME!}v12&EfpZZ9$?J!F>|w%KwUfv@>j@yb50%?m|8Kqan!bk^ zub=z9#9OMR&H%vA{YhsIv^o0>PgesY3xkBtVaiAQivXJ%9I&8`WWmd$ad7Pc3HyU{ z1NJj#*OJxLfQ@K&`|&!~BeXXdPnlLt8|y)D-cbhRu6TFE#X_1Z9s>kN<@4LZ(h)p@ z(JzZU96X@EiUY(58|-exKu%XFUshx7j}+KIgJ^e4EMPP`jYxtWRZIE*6%DE8&Vawtp-Zlrf2Bj0Tr>Ylhboy;{|4V4abKhFS8xCT002ov JPDHLkV1mif$sGUy diff --git a/open_manipulator_control_gui/src/main.cpp b/open_manipulator_control_gui/src/main.cpp deleted file mode 100644 index f6ddebff..00000000 --- a/open_manipulator_control_gui/src/main.cpp +++ /dev/null @@ -1,43 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include "../include/open_manipulator_control_gui/main_window.hpp" - -/***************************************************************************** -** Main -*****************************************************************************/ - -int main(int argc, char **argv) { - - /********************* - ** Qt - **********************/ - QApplication app(argc, argv); - open_manipulator_control_gui::MainWindow w(argc,argv); - w.show(); - app.connect(&app, SIGNAL(lastWindowClosed()), &app, SLOT(quit())); - int result = app.exec(); - - return result; -} diff --git a/open_manipulator_control_gui/src/main_window.cpp b/open_manipulator_control_gui/src/main_window.cpp deleted file mode 100644 index 8e169d49..00000000 --- a/open_manipulator_control_gui/src/main_window.cpp +++ /dev/null @@ -1,355 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include -#include "../include/open_manipulator_control_gui/main_window.hpp" - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace open_manipulator_control_gui { - -using namespace Qt; - -/***************************************************************************** -** Implementation [MainWindow] -*****************************************************************************/ - -MainWindow::MainWindow(int argc, char** argv, QWidget *parent) - : QMainWindow(parent) - , qnode(argc,argv) -{ - ui.setupUi(this); // Calling this incidentally connects all ui's triggers to on_...() callbacks in this class. - QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); // qApp is a global variable for the application - connect(ui.tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabSelected())); - QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close())); - - qnode.init(); - -} - -MainWindow::~MainWindow() {} - -void MainWindow::timerCallback() -{ - std::vector joint_angle = qnode.getPresentJointAngle(); - if(joint_angle.size() != 5) - return; - - ui.txt_j1->setText(QString::number(joint_angle.at(0),'f', 3)); - ui.txt_j2->setText(QString::number(joint_angle.at(1),'f', 3)); - ui.txt_j3->setText(QString::number(joint_angle.at(2),'f', 3)); - ui.txt_j4->setText(QString::number(joint_angle.at(3),'f', 3)); - ui.txt_grip->setText(QString::number(joint_angle.at(4),'f', 3)); - - std::vector position = qnode.getPresentKinematicsPose(); - if(position.size() != 3) - return; - - ui.txt_x->setText(QString::number(position.at(0),'f', 3)); - ui.txt_y->setText(QString::number(position.at(1),'f', 3)); - ui.txt_z->setText(QString::number(position.at(2),'f', 3)); - - if(qnode.getOpenManipulatorActuatorState() == true) - ui.txt_actuactor_state->setText("Actuator enabled"); - else - ui.txt_actuactor_state->setText("Actuator disabled"); - if(qnode.getOpenManipulatorMovingState() == true) - ui.txt_moving_state->setText("Robot is moving"); - else - ui.txt_moving_state->setText("Robot is stopped"); - -} -void MainWindow::tabSelected() -{ - if(ui.tabWidget->currentIndex()==0) - on_btn_read_joint_angle_clicked(); - if(ui.tabWidget->currentIndex()==1) - on_btn_read_kinematic_pose_clicked(); -} - -void MainWindow::writeLog(QString str) -{ - ui.plainTextEdit_log->moveCursor (QTextCursor::End); - ui.plainTextEdit_log->appendPlainText(str); -} - -void MainWindow::on_btn_timer_start_clicked(void) -{ - timer = new QTimer(this); - connect(timer, SIGNAL(timeout()), this, SLOT(timerCallback())); - timer->start(100); - - writeLog("QTimer start : 100ms"); - ui.btn_timer_start->setEnabled(false); - ui.btn_actuator_disable->setEnabled(true); - ui.btn_actuator_enable->setEnabled(true); - ui.btn_gripper_close->setEnabled(true); - ui.btn_gripper_open->setEnabled(true); - ui.btn_home_pose->setEnabled(true); - ui.btn_init_pose->setEnabled(true); - ui.btn_read_joint_angle->setEnabled(true); - ui.btn_read_kinematic_pose->setEnabled(true); - ui.btn_send_joint_angle->setEnabled(true); - ui.btn_send_kinematic_pose->setEnabled(true); - ui.btn_send_drawing_trajectory->setEnabled(true); - ui.btn_set_gripper->setEnabled(true); -} - -void MainWindow::on_btn_actuator_enable_clicked(void) -{ - if(!qnode.setActuatorState(true)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send actuator state to enable"); -} - -void MainWindow::on_btn_actuator_disable_clicked(void) -{ - if(!qnode.setActuatorState(false)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send actuator state to disable"); -} - -void MainWindow::on_btn_init_pose_clicked(void) -{ - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(0.0); - joint_name.push_back("joint3"); joint_angle.push_back(0.0); - joint_name.push_back("joint4"); joint_angle.push_back(0.0); - - if(!qnode.setJointSpacePath(joint_name, joint_angle, path_time)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send joint angle to initial pose"); -} - -void MainWindow::on_btn_home_pose_clicked(void) -{ - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(-1.05); - joint_name.push_back("joint3"); joint_angle.push_back(0.35); - joint_name.push_back("joint4"); joint_angle.push_back(0.70); - if(!qnode.setJointSpacePath(joint_name, joint_angle, path_time)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - writeLog("Send joint angle to home pose"); -} - -void MainWindow::on_btn_gripper_open_clicked(void) -{ - std::vector joint_angle; - joint_angle.push_back(0.01); - - if(!qnode.setToolControl(joint_angle)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send gripper open"); -} - -void MainWindow::on_btn_gripper_close_clicked(void) -{ - std::vector joint_angle; - joint_angle.push_back(-0.01); - if(!qnode.setToolControl(joint_angle)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send gripper close"); -} - - -void MainWindow::on_btn_read_joint_angle_clicked(void) -{ - std::vector joint_angle = qnode.getPresentJointAngle(); - ui.doubleSpinBox_j1->setValue(joint_angle.at(0)); - ui.doubleSpinBox_j2->setValue(joint_angle.at(1)); - ui.doubleSpinBox_j3->setValue(joint_angle.at(2)); - ui.doubleSpinBox_j4->setValue(joint_angle.at(3)); - ui.doubleSpinBox_gripper->setValue(joint_angle.at(4)); - - writeLog("Read joint angle"); -} -void MainWindow::on_btn_send_joint_angle_clicked(void) -{ - std::vector joint_name; - std::vector joint_angle; - double path_time = ui.doubleSpinBox_time_js->value(); - - joint_name.push_back("joint1"); joint_angle.push_back(ui.doubleSpinBox_j1->value()); - joint_name.push_back("joint2"); joint_angle.push_back(ui.doubleSpinBox_j2->value()); - joint_name.push_back("joint3"); joint_angle.push_back(ui.doubleSpinBox_j3->value()); - joint_name.push_back("joint4"); joint_angle.push_back(ui.doubleSpinBox_j4->value()); - - if(!qnode.setJointSpacePath(joint_name, joint_angle, path_time)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send joint angle"); -} -void MainWindow::on_btn_read_kinematic_pose_clicked(void) -{ - std::vector position = qnode.getPresentKinematicsPose(); - ui.doubleSpinBox_x->setValue(position.at(0)); - ui.doubleSpinBox_y->setValue(position.at(1)); - ui.doubleSpinBox_z->setValue(position.at(2)); - - writeLog("Read task pose"); -} -void MainWindow::on_btn_send_kinematic_pose_clicked(void) -{ - std::vector kinematics_pose; - double path_time = ui.doubleSpinBox_time_cs->value(); - - kinematics_pose.push_back(ui.doubleSpinBox_x->value()); - kinematics_pose.push_back(ui.doubleSpinBox_y->value()); - kinematics_pose.push_back(ui.doubleSpinBox_z->value()); - - if(!qnode.setTaskSpacePath(kinematics_pose, path_time)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - - writeLog("Send task pose"); -} -void MainWindow::on_btn_set_gripper_clicked(void) -{ - std::vector joint_angle; - joint_angle.push_back(ui.doubleSpinBox_gripper->value()); - if(!qnode.setToolControl(joint_angle)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - writeLog("Send gripper value"); -} - -void MainWindow::on_btn_get_manipulator_setting_clicked(void) -{ - qnode.setOption("print_open_manipulator_setting"); - writeLog("Check the terminal of open_manipulator_controller package"); -} - - -void MainWindow::on_radio_drawing_line_clicked(void) -{ - ui.txt_drawing_arg_1->setText("Transpose X"); - ui.txt_drawing_arg_2->setText("Transpose Y"); - ui.txt_drawing_arg_3->setText("Transpose Z"); - ui.txt_drawing_arg_unit_1->setText("m"); - ui.txt_drawing_arg_unit_2->setText("m"); - ui.txt_drawing_arg_unit_3->setText("m"); - ui.doubleSpinBox_drawing_arg_1->setValue(0.0); - ui.doubleSpinBox_drawing_arg_2->setValue(0.0); - ui.doubleSpinBox_drawing_arg_3->setValue(0.0); -} -void MainWindow::on_radio_drawing_circle_clicked(void) -{ - ui.txt_drawing_arg_1->setText("Radius"); - ui.txt_drawing_arg_2->setText("Revolution"); - ui.txt_drawing_arg_3->setText("Start angle"); - ui.txt_drawing_arg_unit_1->setText("m"); - ui.txt_drawing_arg_unit_2->setText("rev"); - ui.txt_drawing_arg_unit_3->setText("rad"); - ui.doubleSpinBox_drawing_arg_1->setValue(0.0); - ui.doubleSpinBox_drawing_arg_2->setValue(0.0); - ui.doubleSpinBox_drawing_arg_3->setValue(0.0); -} -void MainWindow::on_radio_drawing_rhombus_clicked(void) -{ - ui.txt_drawing_arg_1->setText("Radius"); - ui.txt_drawing_arg_2->setText("Revolution"); - ui.txt_drawing_arg_3->setText("Start angle"); - ui.txt_drawing_arg_unit_1->setText("m"); - ui.txt_drawing_arg_unit_2->setText("rev"); - ui.txt_drawing_arg_unit_3->setText("rad"); - ui.doubleSpinBox_drawing_arg_1->setValue(0.0); - ui.doubleSpinBox_drawing_arg_2->setValue(0.0); - ui.doubleSpinBox_drawing_arg_3->setValue(0.0); -} -void MainWindow::on_radio_drawing_heart_clicked(void) -{ - ui.txt_drawing_arg_1->setText("Radius"); - ui.txt_drawing_arg_2->setText("Revolution"); - ui.txt_drawing_arg_3->setText("Start angle"); - ui.txt_drawing_arg_unit_1->setText("m"); - ui.txt_drawing_arg_unit_2->setText("rev"); - ui.txt_drawing_arg_unit_3->setText("rad"); - ui.doubleSpinBox_drawing_arg_1->setValue(0.0); - ui.doubleSpinBox_drawing_arg_2->setValue(0.0); - ui.doubleSpinBox_drawing_arg_3->setValue(0.0); -} -void MainWindow::on_btn_send_drawing_trajectory_clicked(void) -{ - std::string name; - if(ui.radio_drawing_line->isChecked()) name = "line"; - else if(ui.radio_drawing_circle->isChecked()) name = "circle"; - else if(ui.radio_drawing_rhombus->isChecked()) name = "rhombus"; - else if(ui.radio_drawing_heart->isChecked()) name = "heart"; - - std::vector arg; - arg.push_back(ui.doubleSpinBox_drawing_arg_1->value()); - arg.push_back(ui.doubleSpinBox_drawing_arg_2->value()); - arg.push_back(ui.doubleSpinBox_drawing_arg_3->value()); - - double path_time = ui.doubleSpinBox_time_drawing->value(); - - if(!qnode.setDrawingTrajectory(name, arg, path_time)) - { - writeLog("[ERR!!] Failed to send service"); - return; - } - writeLog("Send drawing trajectory"); -} - -} // namespace open_manipulator_control_gui - diff --git a/open_manipulator_control_gui/src/qnode.cpp b/open_manipulator_control_gui/src/qnode.cpp deleted file mode 100644 index 69e4f1eb..00000000 --- a/open_manipulator_control_gui/src/qnode.cpp +++ /dev/null @@ -1,233 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -/***************************************************************************** -** Includes -*****************************************************************************/ - -#include -#include -#include -#include -#include -#include "../include/open_manipulator_control_gui/qnode.hpp" - -/***************************************************************************** -** Namespaces -*****************************************************************************/ - -namespace open_manipulator_control_gui { - -/***************************************************************************** -** Implementation -*****************************************************************************/ - -QNode::QNode(int argc, char** argv ) : - init_argc(argc), - init_argv(argv), - open_manipulator_actuator_enabled_(false), - open_manipulator_is_moving_(false) - {} - -QNode::~QNode() { - if(ros::isStarted()) { - ros::shutdown(); // explicitly needed since we use ros::start(); - ros::waitForShutdown(); - } - wait(); -} - -bool QNode::init() { - ros::init(init_argc,init_argv,"open_manipulator_control_gui"); - if ( ! ros::master::check() ) { - return false; - } - ros::start(); // explicitly needed since our nodehandle is going out of scope. - ros::NodeHandle n(""); - - // msg publisher - open_manipulator_option_pub_ = n.advertise("option", 10); - // msg subscriber - open_manipulator_states_sub_ = n.subscribe("states", 10, &QNode::manipulatorStatesCallback, this); - open_manipulator_joint_states_sub_ = n.subscribe("joint_states", 10, &QNode::jointStatesCallback, this); - open_manipulator_kinematics_pose_sub_ = n.subscribe("kinematics_pose", 10, &QNode::kinematicsPoseCallback, this); - // service client - goal_joint_space_path_client_ = n.serviceClient("goal_joint_space_path"); - goal_task_space_path_position_only_client_ = n.serviceClient("goal_task_space_path_position_only"); - goal_tool_control_client_ = n.serviceClient("goal_tool_control"); - set_actuator_state_client_ = n.serviceClient("set_actuator_state"); - goal_drawing_trajectory_client_ = n.serviceClient("goal_drawing_trajectory"); - - start(); - return true; -} - -void QNode::run() { - ros::Rate loop_rate(10); - while ( ros::ok() ) { - ros::spinOnce(); - loop_rate.sleep(); - } - std::cout << "Ros shutdown, proceeding to close the gui." << std::endl; - Q_EMIT rosShutdown(); -} - -void QNode::manipulatorStatesCallback(const open_manipulator_msgs::OpenManipulatorState::ConstPtr &msg) -{ - if(msg->open_manipulator_moving_state == msg->IS_MOVING) - open_manipulator_is_moving_ = true; - else - open_manipulator_is_moving_ = false; - - if(msg->open_manipulator_actuator_state == msg->ACTUATOR_ENABLED) - open_manipulator_actuator_enabled_ = true; - else - open_manipulator_actuator_enabled_ = false; -} -void QNode::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - std::vector temp_angle; - temp_angle.resize(NUM_OF_JOINT_AND_TOOL); - for(int i = 0; i < msg->name.size(); i ++) - { - if(!msg->name.at(i).compare("joint1")) temp_angle.at(0) = (msg->position.at(i)); - else if(!msg->name.at(i).compare("joint2")) temp_angle.at(1) = (msg->position.at(i)); - else if(!msg->name.at(i).compare("joint3")) temp_angle.at(2) = (msg->position.at(i)); - else if(!msg->name.at(i).compare("joint4")) temp_angle.at(3) = (msg->position.at(i)); - else if(!msg->name.at(i).compare("gripper")) temp_angle.at(4) = (msg->position.at(i)); - } - present_joint_angle_ = temp_angle; -} - -void QNode::kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg) -{ - std::vector temp_position; - temp_position.push_back(msg->pose.position.x); - temp_position.push_back(msg->pose.position.y); - temp_position.push_back(msg->pose.position.z); - - present_kinematic_position_ = temp_position; - - kinematics_pose_.pose = msg->pose; -} - -std::vector QNode::getPresentJointAngle() -{ - return present_joint_angle_; -} -std::vector QNode::getPresentKinematicsPose() -{ - return present_kinematic_position_; -} -bool QNode::getOpenManipulatorMovingState() -{ - return open_manipulator_is_moving_; -} -bool QNode::getOpenManipulatorActuatorState() -{ - return open_manipulator_actuator_enabled_; -} - -void QNode::setOption(std::string opt) -{ - std_msgs::String msg; - msg.data = opt; - open_manipulator_option_pub_.publish(msg); -} - -bool QNode::setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name = joint_name; - srv.request.joint_position.position = joint_angle; - srv.request.path_time = path_time; - - if(goal_joint_space_path_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool QNode::setTaskSpacePath(std::vector kinematics_pose, double path_time) -{ - open_manipulator_msgs::SetKinematicsPose srv; - - srv.request.end_effector_name = "gripper"; - - srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0); - srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1); - srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2); - - srv.request.kinematics_pose.pose.orientation.w = kinematics_pose_.pose.orientation.w; - srv.request.kinematics_pose.pose.orientation.x = kinematics_pose_.pose.orientation.x; - srv.request.kinematics_pose.pose.orientation.y = kinematics_pose_.pose.orientation.y; - srv.request.kinematics_pose.pose.orientation.z = kinematics_pose_.pose.orientation.z; - - srv.request.path_time = path_time; - - if(goal_task_space_path_position_only_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool QNode::setDrawingTrajectory(std::string name, std::vector arg, double path_time) -{ - open_manipulator_msgs::SetDrawingTrajectory srv; - srv.request.end_effector_name = "gripper"; - srv.request.drawing_trajectory_name = name; - srv.request.path_time = path_time; - for(int i = 0; i < arg.size(); i ++) - srv.request.param.push_back(arg.at(i)); - - if(goal_drawing_trajectory_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool QNode::setToolControl(std::vector joint_angle) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name.push_back("gripper"); - srv.request.joint_position.position = joint_angle; - - if(goal_tool_control_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool QNode::setActuatorState(bool actuator_state) -{ - open_manipulator_msgs::SetActuatorState srv; - srv.request.set_actuator_state = actuator_state; - - if(set_actuator_state_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - - -} // namespace open_manipulator_control_gui diff --git a/open_manipulator_control_gui/ui/main_window.ui b/open_manipulator_control_gui/ui/main_window.ui deleted file mode 100644 index 257fb545..00000000 --- a/open_manipulator_control_gui/ui/main_window.ui +++ /dev/null @@ -1,1625 +0,0 @@ - - - MainWindowDesign - - - true - - - - 0 - 0 - 793 - 604 - - - - - 0 - 0 - - - - - 793 - 604 - - - - - 793 - 604 - - - - OpenManipulator control GUI - - - - :/images/icon.png:/images/icon.png - - - - - - - - true - - - - 510 - 103 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 460 - 10 - 321 - 31 - - - - Timer Start - - - - - true - - - - 510 - 133 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - true - - - - 510 - 163 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - true - - - - 510 - 193 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 460 - 107 - 59 - 14 - - - - Joint 1 - - - - - - 460 - 137 - 59 - 14 - - - - Joint 2 - - - - - - 460 - 167 - 59 - 14 - - - - Joint 3 - - - - - - 460 - 197 - 59 - 14 - - - - Joint 4 - - - - - true - - - - 670 - 159 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 650 - 163 - 59 - 14 - - - - Z - - - - - true - - - - 670 - 129 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 650 - 133 - 59 - 14 - - - - Y - - - - - true - - - - 670 - 99 - 81 - 22 - - - - Qt::LeftToRight - - - 0.0 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 650 - 103 - 59 - 14 - - - - X - - - - - - 595 - 107 - 59 - 14 - - - - rad - - - - - - 595 - 137 - 59 - 14 - - - - rad - - - - - - 595 - 167 - 59 - 14 - - - - rad - - - - - - 595 - 197 - 59 - 14 - - - - rad - - - - - - 755 - 163 - 59 - 14 - - - - m - - - - - - 755 - 133 - 59 - 14 - - - - m - - - - - - 755 - 103 - 59 - 14 - - - - m - - - - - false - - - - 460 - 233 - 151 - 31 - - - - Init pose - - - - - false - - - - 630 - 233 - 151 - 31 - - - - Home pose - - - - - - 20 - 70 - 59 - 21 - - - - Logging - - - - - - 20 - 100 - 421 - 471 - - - - true - - - - - false - - - - 630 - 273 - 151 - 31 - - - - Gripper close - - - - - false - - - - 460 - 273 - 151 - 31 - - - - Gripper open - - - - - - 460 - 313 - 321 - 221 - - - - 0 - - - - Joint space - - - - false - - - - 9 - 150 - 141 - 31 - - - - Read joint angle - - - - - false - - - - 169 - 150 - 141 - 31 - - - - Send - - - - - - 20 - 80 - 59 - 14 - - - - Joint 3 - - - - - - 20 - 110 - 59 - 14 - - - - Joint 4 - - - - - - 180 - 80 - 59 - 14 - - - - rad - - - - - - 180 - 110 - 59 - 14 - - - - rad - - - - - - 180 - 50 - 59 - 14 - - - - rad - - - - - - 20 - 50 - 59 - 14 - - - - Joint 2 - - - - - - 180 - 20 - 59 - 14 - - - - rad - - - - - - 20 - 20 - 59 - 14 - - - - Joint 1 - - - - - - 70 - 16 - 101 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - -3.140000000000000 - - - 3.140000000000000 - - - 0.050000000000000 - - - - - - 70 - 46 - 101 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - -3.140000000000000 - - - 3.140000000000000 - - - 0.050000000000000 - - - - - - 70 - 76 - 101 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - -3.140000000000000 - - - 3.140000000000000 - - - 0.050000000000000 - - - - - - 70 - 106 - 101 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - -3.140000000000000 - - - 3.140000000000000 - - - 0.050000000000000 - - - - - - 220 - 86 - 59 - 14 - - - - time - - - - - - 217 - 106 - 71 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - -3.140000000000000 - - - 10.000000000000000 - - - 0.500000000000000 - - - 2.000000000000000 - - - - - - 291 - 111 - 59 - 14 - - - - s - - - - - - Task space - - - - - 70 - 28 - 101 - 23 - - - - 3 - - - -1.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - - - false - - - - 169 - 150 - 141 - 31 - - - - Send - - - - - - 180 - 92 - 59 - 14 - - - - m - - - - - - 70 - 88 - 101 - 23 - - - - 3 - - - -1.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - - - - 180 - 62 - 59 - 14 - - - - m - - - - - - 70 - 58 - 101 - 23 - - - - 3 - - - -1.000000000000000 - - - 1.000000000000000 - - - 0.010000000000000 - - - - - false - - - - 7 - 150 - 151 - 31 - - - - Read kinematic pose - - - - - - 20 - 32 - 59 - 14 - - - - X axis - - - - - - 20 - 92 - 59 - 14 - - - - Z axis - - - - - - 180 - 32 - 59 - 14 - - - - m - - - - - - 20 - 59 - 59 - 20 - - - - Y axis - - - - - - 217 - 106 - 71 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - -3.140000000000000 - - - 10.000000000000000 - - - 0.500000000000000 - - - 2.000000000000000 - - - - - - 291 - 111 - 59 - 14 - - - - s - - - - - - 220 - 86 - 59 - 14 - - - - time - - - - - - Drawing - - - - - 10 - 10 - 61 - 20 - - - - Line - - - true - - - - - - 73 - 10 - 71 - 20 - - - - Circle - - - - - - 146 - 10 - 91 - 20 - - - - Rhombus - - - - - - 240 - 10 - 71 - 20 - - - - Heart - - - - - - 20 - 50 - 81 - 21 - - - - Qt::LeftToRight - - - Transpose X - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - 220 - 54 - 59 - 14 - - - - m - - - - - - 20 - 110 - 81 - 21 - - - - Qt::LeftToRight - - - Transpose Z - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - 110 - 50 - 101 - 23 - - - - 3 - - - -10.000000000000000 - - - 10.000000000000000 - - - 0.010000000000000 - - - - - - 220 - 114 - 59 - 14 - - - - m - - - - - - 70 - 155 - 71 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - -3.140000000000000 - - - 10.000000000000000 - - - 0.500000000000000 - - - 2.000000000000000 - - - - - - 110 - 80 - 101 - 23 - - - - 3 - - - -10.000000000000000 - - - 10.000000000000000 - - - 0.010000000000000 - - - - - - 30 - 160 - 59 - 14 - - - - time - - - - - - 20 - 80 - 81 - 21 - - - - Qt::LeftToRight - - - Transpose Y - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - 144 - 160 - 59 - 14 - - - - s - - - - - false - - - - 169 - 150 - 141 - 31 - - - - Send - - - - - - 110 - 110 - 101 - 23 - - - - 3 - - - -10.000000000000000 - - - 10.000000000000000 - - - 0.010000000000000 - - - - - - 220 - 84 - 59 - 14 - - - - m - - - - - - Option - - - - true - - - - 0 - 10 - 311 - 51 - - - - Get Manipulator Setting - - - - - - - - 517 - 546 - 81 - 23 - - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - 3 - - - -0.010000000000000 - - - 0.010000000000000 - - - 0.002000000000000 - - - 0.000000000000000 - - - - - - 463 - 546 - 59 - 20 - - - - Gripper - - - - - false - - - - 630 - 543 - 141 - 31 - - - - Set gripper - - - - - - 635 - 193 - 61 - 20 - - - - Gripper - - - - - true - - - - 692 - 193 - 60 - 22 - - - - Qt::LeftToRight - - - 0.00 - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - true - - - - - - 756 - 196 - 59 - 14 - - - - m - - - - - - 603 - 550 - 21 - 16 - - - - m - - - - - false - - - - 460 - 50 - 151 - 31 - - - - Actuator enable - - - - - false - - - - 630 - 50 - 151 - 31 - - - - Actuator disable - - - - - - 21 - 10 - 191 - 16 - - - - OpenManipulator states - - - - - - 80 - 42 - 171 - 20 - - - - - 13 - - - - Moving state - - - - - - 260 - 42 - 171 - 20 - - - - - 13 - - - - Actuator state - - - - - - - 0 - 0 - 793 - 19 - - - - - &App - - - - - - - - - - - - - &Quit - - - Ctrl+Q - - - Qt::ApplicationShortcut - - - - - &Preferences - - - - - &About - - - - - About &Qt - - - - - - - - - action_Quit - triggered() - MainWindowDesign - close() - - - -1 - -1 - - - 399 - 299 - - - - - diff --git a/open_manipulator_controller/CHANGELOG.rst b/open_manipulator_controller/CHANGELOG.rst deleted file mode 100644 index 2cc688c9..00000000 --- a/open_manipulator_controller/CHANGELOG.rst +++ /dev/null @@ -1,60 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_controller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.2 (2021-06-21) ------------------- -* Noetic support -* Contributors: Will Son - -2.0.1 (2019-02-18) ------------------- -* none - -2.0.0 (2019-02-08) ------------------- -* added jointspace path serv, moveit params -* added moveit config and controller -* added kinematic pose pub -* added mimic param and end effector point -* added execute permission -* added usb rules -* added cdc rules -* removed warn message -* renamed open_manipulator lib files -* changed math function name, namespace -* changed openManipulatorProcess() to processOpenManipulator() -* updated start_state after execution on MoveIt -* updated thread time, dynamixel profiling control method -* updated drawing line -* updated flexible node -* updated tool control -* updated chain to open_manipulator -* updated new kinematics -* used robot_name on joint_state_publisher's source_list -* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Guilherme de Campos Affonso, Pyo - -1.0.0 (2018-06-01) ------------------- -* package reconfiguration for OpenManipulator -* added function to support protocol 1.0 -* modified motor id, msg names -* merged pull request `#34 `_ `#33 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ -* Contributors: Darby Lim, Pyo - -0.1.1 (2018-03-15) ------------------- -* none - -0.1.0 (2018-03-14) ------------------- -* added new dynamixel-workbench and velocity message -* added sync read failed debug code -* modified dxl-wb path -* modified dynamixel controller -* modified messages -* modified moveit -* modified description -* modified cmake, package files for release -* refactoring for release -* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_controller/CMakeLists.txt b/open_manipulator_controller/CMakeLists.txt deleted file mode 100644 index 5192943c..00000000 --- a/open_manipulator_controller/CMakeLists.txt +++ /dev/null @@ -1,95 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(open_manipulator_controller) - -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED - COMPONENTS - roscpp - std_msgs - sensor_msgs - geometry_msgs - trajectory_msgs - open_manipulator_msgs - robotis_manipulator - open_manipulator_libs - cmake_modules -) -find_package(Boost REQUIRED) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -## Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - std_msgs - sensor_msgs - geometry_msgs - trajectory_msgs - open_manipulator_msgs - robotis_manipulator - open_manipulator_libs - cmake_modules - DEPENDS Boost -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} -) - -add_executable(open_manipulator_controller src/open_manipulator_controller.cpp) -add_dependencies(open_manipulator_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open_manipulator_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS open_manipulator_controller - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(PROGRAMS - scripts/create_udev_rules - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(FILES 99-open-manipulator-cdc.rules - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/open_manipulator_controller/include/open_manipulator_controller/open_manipulator_controller.h b/open_manipulator_controller/include/open_manipulator_controller/open_manipulator_controller.h deleted file mode 100644 index 1f9e991b..00000000 --- a/open_manipulator_controller/include/open_manipulator_controller/open_manipulator_controller.h +++ /dev/null @@ -1,174 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef OPEN_MANIPULATOR_CONTROLLER_H_ -#define OPEN_MANIPULATOR_CONTROLLER_H_ - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "open_manipulator_libs/open_manipulator.h" -#include "open_manipulator_msgs/SetJointPosition.h" -#include "open_manipulator_msgs/SetKinematicsPose.h" -#include "open_manipulator_msgs/SetDrawingTrajectory.h" -#include "open_manipulator_msgs/SetActuatorState.h" -#include "open_manipulator_msgs/GetJointPosition.h" -#include "open_manipulator_msgs/GetKinematicsPose.h" -#include "open_manipulator_msgs/OpenManipulatorState.h" - -namespace open_manipulator_controller -{ -class OpenManipulatorController -{ - public: - OpenManipulatorController(std::string usb_port, std::string baud_rate); - ~OpenManipulatorController(); - - // update - void publishCallback(const ros::TimerEvent&); - void startTimerThread(); - static void *timerThread(void *param); - void process(double time); - double getControlPeriod(void){return control_period_;} - - private: - /***************************************************************************** - ** ROS NodeHandle - *****************************************************************************/ - ros::NodeHandle node_handle_; - ros::NodeHandle priv_node_handle_; - - /***************************************************************************** - ** ROS Parameters - *****************************************************************************/ - bool using_platform_; - double control_period_; - - /***************************************************************************** - ** Variables - *****************************************************************************/ - // Thread parameter - pthread_t timer_thread_; - pthread_attr_t attr_; - bool timer_thread_state_; - - // Robotis_manipulator related - OpenManipulator open_manipulator_; - - /***************************************************************************** - ** Init Functions - *****************************************************************************/ - void initPublisher(); - void initSubscriber(); - void initServer(); - - /***************************************************************************** - ** ROS Publishers, Callback Functions and Relevant Functions - *****************************************************************************/ - ros::Publisher open_manipulator_states_pub_; - std::vector open_manipulator_kinematics_pose_pub_; - ros::Publisher open_manipulator_joint_states_pub_; - std::vector gazebo_goal_joint_position_pub_; - - void publishOpenManipulatorStates(); - void publishKinematicsPose(); - void publishJointStates(); - void publishGazeboCommand(); - - /***************************************************************************** - ** ROS Subscribers and Callback Functions - *****************************************************************************/ - ros::Subscriber open_manipulator_option_sub_; - - void openManipulatorOptionCallback(const std_msgs::String::ConstPtr &msg); - - /***************************************************************************** - ** ROS Servers and Callback Functions - *****************************************************************************/ - ros::ServiceServer goal_joint_space_path_server_; - ros::ServiceServer goal_joint_space_path_to_kinematics_pose_server_; - ros::ServiceServer goal_joint_space_path_to_kinematics_position_server_; - ros::ServiceServer goal_joint_space_path_to_kinematics_orientation_server_; - ros::ServiceServer goal_task_space_path_server_; - ros::ServiceServer goal_task_space_path_position_only_server_; - ros::ServiceServer goal_task_space_path_orientation_only_server_; - ros::ServiceServer goal_joint_space_path_from_present_server_; - ros::ServiceServer goal_task_space_path_from_present_position_only_server_; - ros::ServiceServer goal_task_space_path_from_present_orientation_only_server_; - ros::ServiceServer goal_task_space_path_from_present_server_; - ros::ServiceServer goal_tool_control_server_; - ros::ServiceServer set_actuator_state_server_; - ros::ServiceServer goal_drawing_trajectory_server_; - ros::ServiceServer get_joint_position_server_; - ros::ServiceServer get_kinematics_pose_server_; - ros::ServiceServer set_joint_position_server_; - ros::ServiceServer set_kinematics_pose_server_; - - bool goalJointSpacePathCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res); - - bool goalJointSpacePathToKinematicsPoseCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalJointSpacePathToKinematicsPositionCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalJointSpacePathToKinematicsOrientationCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalTaskSpacePathCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalTaskSpacePathPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalTaskSpacePathOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalJointSpacePathFromPresentCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res); - - bool goalTaskSpacePathFromPresentCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalTaskSpacePathFromPresentPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalTaskSpacePathFromPresentOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res); - - bool goalToolControlCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res); - - bool setActuatorStateCallback(open_manipulator_msgs::SetActuatorState::Request &req, - open_manipulator_msgs::SetActuatorState::Response &res); - - bool goalDrawingTrajectoryCallback(open_manipulator_msgs::SetDrawingTrajectory::Request &req, - open_manipulator_msgs::SetDrawingTrajectory::Response &res); -}; -} -#endif //OPEN_MANIPULATOR_CONTROLLER_H_ diff --git a/open_manipulator_controller/launch/open_manipulator_controller.launch b/open_manipulator_controller/launch/open_manipulator_controller.launch deleted file mode 100644 index d642e510..00000000 --- a/open_manipulator_controller/launch/open_manipulator_controller.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - diff --git a/open_manipulator_controller/package.xml b/open_manipulator_controller/package.xml deleted file mode 100644 index 8aaec988..00000000 --- a/open_manipulator_controller/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - - open_manipulator_controller - 2.0.2 - OpenManipulator controller package - - Apache 2.0 - Darby Lim - Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son - http://wiki.ros.org/open_manipulator_controller - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues - - catkin - roscpp - std_msgs - sensor_msgs - geometry_msgs - trajectory_msgs - open_manipulator_msgs - robotis_manipulator - open_manipulator_libs - cmake_modules - boost - diff --git a/open_manipulator_controller/scripts/create_udev_rules b/open_manipulator_controller/scripts/create_udev_rules deleted file mode 100755 index 21953433..00000000 --- a/open_manipulator_controller/scripts/create_udev_rules +++ /dev/null @@ -1,14 +0,0 @@ -#!/bin/bash - -echo "" -echo "This script copies a udev rule to /etc to facilitate bringing" -echo "up the OpenManipulator usb connection." -echo "" - -sudo cp `rospack find open_manipulator_controller`/99-open-manipulator-cdc.rules /etc/udev/rules.d/ - -echo "" -echo "Reload rules" -echo "" -sudo udevadm control --reload-rules -sudo udevadm trigger diff --git a/open_manipulator_controller/src/open_manipulator_controller.cpp b/open_manipulator_controller/src/open_manipulator_controller.cpp deleted file mode 100644 index 6dc173e7..00000000 --- a/open_manipulator_controller/src/open_manipulator_controller.cpp +++ /dev/null @@ -1,661 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "open_manipulator_controller/open_manipulator_controller.h" - -using namespace open_manipulator_controller; - -OpenManipulatorController::OpenManipulatorController(std::string usb_port, std::string baud_rate) -: node_handle_(""), - priv_node_handle_("~"), - timer_thread_state_(false) -{ - /************************************************************ - ** Initialize ROS parameters - ************************************************************/ - control_period_ = priv_node_handle_.param("control_period", 0.010f); - using_platform_ = priv_node_handle_.param("using_platform", false); - - /************************************************************ - ** Initialize variables - ************************************************************/ - open_manipulator_.initOpenManipulator(using_platform_, usb_port, baud_rate, control_period_); - - if (using_platform_ == true) log::info("Succeeded to init " + priv_node_handle_.getNamespace()); - else if (using_platform_ == false) log::info("Ready to simulate " + priv_node_handle_.getNamespace() + " on Gazebo"); - - /************************************************************ - ** Initialize ROS publishers, subscribers and servers - ************************************************************/ - initPublisher(); - initSubscriber(); - initServer(); -} - -OpenManipulatorController::~OpenManipulatorController() -{ - timer_thread_state_ = false; - pthread_join(timer_thread_, NULL); // Wait for the thread associated with thread_p to complete - log::info("Shutdown OpenManipulator Controller"); - open_manipulator_.disableAllActuator(); - ros::shutdown(); -} - -void OpenManipulatorController::startTimerThread() -{ - //////////////////////////////////////////////////////////////////// - /// Use this when you want to increase the priority of threads. - //////////////////////////////////////////////////////////////////// - // pthread_attr_t attr_; - // int error; - // struct sched_param param; - // pthread_attr_init(&attr_); - - // error = pthread_attr_setschedpolicy(&attr_, SCHED_RR); - // if (error != 0) log::error("pthread_attr_setschedpolicy error = ", (double)error); - // error = pthread_attr_setinheritsched(&attr_, PTHREAD_EXPLICIT_SCHED); - // if (error != 0) log::error("pthread_attr_setinheritsched error = ", (double)error); - - // memset(¶m, 0, sizeof(param)); - // param.sched_priority = 31; // RT - // error = pthread_attr_setschedparam(&attr_, ¶m); - // if (error != 0) log::error("pthread_attr_setschedparam error = ", (double)error); - - // if ((error = pthread_create(&this->timer_thread_, &attr_, this->timerThread, this)) != 0) - // { - // log::error("Creating timer thread failed!!", (double)error); - // exit(-1); - // } - // timer_thread_state_ = true; - //////////////////////////////////////////////////////////////////// - - int error; - if ((error = pthread_create(&this->timer_thread_, NULL, this->timerThread, this)) != 0) - { - log::error("Creating timer thread failed!!", (double)error); - exit(-1); - } - timer_thread_state_ = true; -} - -void *OpenManipulatorController::timerThread(void *param) -{ - OpenManipulatorController *controller = (OpenManipulatorController *) param; - static struct timespec next_time; - static struct timespec curr_time; - - clock_gettime(CLOCK_MONOTONIC, &next_time); - - while(controller->timer_thread_state_) - { - next_time.tv_sec += (next_time.tv_nsec + ((int)(controller->getControlPeriod() * 1000)) * 1000000) / 1000000000; - next_time.tv_nsec = (next_time.tv_nsec + ((int)(controller->getControlPeriod() * 1000)) * 1000000) % 1000000000; - - double time = next_time.tv_sec + (next_time.tv_nsec*0.000000001); - controller->process(time); - - clock_gettime(CLOCK_MONOTONIC, &curr_time); - ///// - double delta_nsec = controller->getControlPeriod() - ((next_time.tv_sec - curr_time.tv_sec) + ((double)(next_time.tv_nsec - curr_time.tv_nsec)*0.000000001)); - //log::info("control time : ", controller->getControlPeriod() - delta_nsec); - if (delta_nsec > controller->getControlPeriod()) - { - //log::warn("Over the control time : ", delta_nsec); - next_time = curr_time; - } - else - clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, NULL); - ///// - } - return 0; -} - -/******************************************************************************** -** Init Functions -********************************************************************************/ -void OpenManipulatorController::initPublisher() -{ - // ros message publisher - auto om_tools_name = open_manipulator_.getManipulator()->getAllToolComponentName(); - - for (auto const& name:om_tools_name) - { - ros::Publisher pb; - pb = node_handle_.advertise(name + "/kinematics_pose", 10); - open_manipulator_kinematics_pose_pub_.push_back(pb); - } - open_manipulator_states_pub_ = node_handle_.advertise("states", 10); - - if (using_platform_ == true) - { - open_manipulator_joint_states_pub_ = node_handle_.advertise("joint_states", 10); - } - else - { - auto gazebo_joints_name = open_manipulator_.getManipulator()->getAllActiveJointComponentName(); - gazebo_joints_name.reserve(gazebo_joints_name.size() + om_tools_name.size()); - gazebo_joints_name.insert(gazebo_joints_name.end(), om_tools_name.begin(), om_tools_name.end()); - - for (auto const& name:gazebo_joints_name) - { - ros::Publisher pb; - pb = node_handle_.advertise(name + "_position/command", 10); - gazebo_goal_joint_position_pub_.push_back(pb); - } - } -} -void OpenManipulatorController::initSubscriber() -{ - // ros message subscriber - open_manipulator_option_sub_ = node_handle_.subscribe("option", 10, &OpenManipulatorController::openManipulatorOptionCallback, this); -} - -void OpenManipulatorController::initServer() -{ - goal_joint_space_path_server_ = node_handle_.advertiseService("goal_joint_space_path", &OpenManipulatorController::goalJointSpacePathCallback, this); - goal_joint_space_path_to_kinematics_pose_server_ = node_handle_.advertiseService("goal_joint_space_path_to_kinematics_pose", &OpenManipulatorController::goalJointSpacePathToKinematicsPoseCallback, this); - goal_joint_space_path_to_kinematics_position_server_ = node_handle_.advertiseService("goal_joint_space_path_to_kinematics_position", &OpenManipulatorController::goalJointSpacePathToKinematicsPositionCallback, this); - goal_joint_space_path_to_kinematics_orientation_server_ = node_handle_.advertiseService("goal_joint_space_path_to_kinematics_orientation", &OpenManipulatorController::goalJointSpacePathToKinematicsOrientationCallback, this); - - goal_task_space_path_server_ = node_handle_.advertiseService("goal_task_space_path", &OpenManipulatorController::goalTaskSpacePathCallback, this); - goal_task_space_path_position_only_server_ = node_handle_.advertiseService("goal_task_space_path_position_only", &OpenManipulatorController::goalTaskSpacePathPositionOnlyCallback, this); - goal_task_space_path_orientation_only_server_ = node_handle_.advertiseService("goal_task_space_path_orientation_only", &OpenManipulatorController::goalTaskSpacePathOrientationOnlyCallback, this); - - goal_joint_space_path_from_present_server_ = node_handle_.advertiseService("goal_joint_space_path_from_present", &OpenManipulatorController::goalJointSpacePathFromPresentCallback, this); - - goal_task_space_path_from_present_server_ = node_handle_.advertiseService("goal_task_space_path_from_present", &OpenManipulatorController::goalTaskSpacePathFromPresentCallback, this); - goal_task_space_path_from_present_position_only_server_ = node_handle_.advertiseService("goal_task_space_path_from_present_position_only", &OpenManipulatorController::goalTaskSpacePathFromPresentPositionOnlyCallback, this); - goal_task_space_path_from_present_orientation_only_server_ = node_handle_.advertiseService("goal_task_space_path_from_present_orientation_only", &OpenManipulatorController::goalTaskSpacePathFromPresentOrientationOnlyCallback, this); - - goal_tool_control_server_ = node_handle_.advertiseService("goal_tool_control", &OpenManipulatorController::goalToolControlCallback, this); - set_actuator_state_server_ = node_handle_.advertiseService("set_actuator_state", &OpenManipulatorController::setActuatorStateCallback, this); - goal_drawing_trajectory_server_ = node_handle_.advertiseService("goal_drawing_trajectory", &OpenManipulatorController::goalDrawingTrajectoryCallback, this); -} - -/***************************************************************************** -** Callback Functions for ROS Subscribers -*****************************************************************************/ -void OpenManipulatorController::openManipulatorOptionCallback(const std_msgs::String::ConstPtr &msg) -{ - if (msg->data == "print_open_manipulator_setting") - open_manipulator_.printManipulatorSetting(); -} - -/***************************************************************************** -** Callback Functions for ROS Servers -*****************************************************************************/ -bool OpenManipulatorController::goalJointSpacePathCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res) -{ - std::vector target_angle; - - for (int i = 0; i < req.joint_position.joint_name.size(); i ++) - target_angle.push_back(req.joint_position.position.at(i)); - - if (!open_manipulator_.makeJointTrajectory(target_angle, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalJointSpacePathToKinematicsPoseCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - KinematicPose target_pose; - target_pose.position[0] = req.kinematics_pose.pose.position.x; - target_pose.position[1] = req.kinematics_pose.pose.position.y; - target_pose.position[2] = req.kinematics_pose.pose.position.z; - - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - target_pose.orientation = math::convertQuaternionToRotationMatrix(q); - - if (!open_manipulator_.makeJointTrajectory(req.end_effector_name, target_pose, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalJointSpacePathToKinematicsPositionCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - KinematicPose target_pose; - target_pose.position[0] = req.kinematics_pose.pose.position.x; - target_pose.position[1] = req.kinematics_pose.pose.position.y; - target_pose.position[2] = req.kinematics_pose.pose.position.z; - - if (!open_manipulator_.makeJointTrajectory(req.end_effector_name, target_pose.position, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalJointSpacePathToKinematicsOrientationCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - KinematicPose target_pose; - - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - target_pose.orientation = math::convertQuaternionToRotationMatrix(q); - - if (!open_manipulator_.makeJointTrajectory(req.end_effector_name, target_pose.orientation, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - KinematicPose target_pose; - target_pose.position[0] = req.kinematics_pose.pose.position.x; - target_pose.position[1] = req.kinematics_pose.pose.position.y; - target_pose.position[2] = req.kinematics_pose.pose.position.z; - - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - target_pose.orientation = math::convertQuaternionToRotationMatrix(q); - if (!open_manipulator_.makeTaskTrajectory(req.end_effector_name, target_pose, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - Eigen::Vector3d position; - position[0] = req.kinematics_pose.pose.position.x; - position[1] = req.kinematics_pose.pose.position.y; - position[2] = req.kinematics_pose.pose.position.z; - - if (!open_manipulator_.makeTaskTrajectory(req.end_effector_name, position, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - Eigen::Matrix3d orientation = math::convertQuaternionToRotationMatrix(q); - - if (!open_manipulator_.makeTaskTrajectory(req.end_effector_name, orientation, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalJointSpacePathFromPresentCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res) -{ - std::vector target_angle; - - for (int i = 0; i < req.joint_position.joint_name.size(); i ++) - target_angle.push_back(req.joint_position.position.at(i)); - - if (!open_manipulator_.makeJointTrajectoryFromPresentPosition(target_angle, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathFromPresentCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - KinematicPose target_pose; - target_pose.position[0] = req.kinematics_pose.pose.position.x; - target_pose.position[1] = req.kinematics_pose.pose.position.y; - target_pose.position[2] = req.kinematics_pose.pose.position.z; - - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - target_pose.orientation = math::convertQuaternionToRotationMatrix(q); - - if (!open_manipulator_.makeTaskTrajectoryFromPresentPose(req.planning_group, target_pose, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathFromPresentPositionOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - Eigen::Vector3d position; - position[0] = req.kinematics_pose.pose.position.x; - position[1] = req.kinematics_pose.pose.position.y; - position[2] = req.kinematics_pose.pose.position.z; - - if (!open_manipulator_.makeTaskTrajectoryFromPresentPose(req.planning_group, position, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalTaskSpacePathFromPresentOrientationOnlyCallback(open_manipulator_msgs::SetKinematicsPose::Request &req, - open_manipulator_msgs::SetKinematicsPose::Response &res) -{ - Eigen::Quaterniond q(req.kinematics_pose.pose.orientation.w, - req.kinematics_pose.pose.orientation.x, - req.kinematics_pose.pose.orientation.y, - req.kinematics_pose.pose.orientation.z); - - Eigen::Matrix3d orientation = math::convertQuaternionToRotationMatrix(q); - - if (!open_manipulator_.makeTaskTrajectoryFromPresentPose(req.planning_group, orientation, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalToolControlCallback(open_manipulator_msgs::SetJointPosition::Request &req, - open_manipulator_msgs::SetJointPosition::Response &res) -{ - for (int i = 0; i < req.joint_position.joint_name.size(); i ++) - { - if (!open_manipulator_.makeToolTrajectory(req.joint_position.joint_name.at(i), req.joint_position.position.at(i))) - res.is_planned = false; - else - res.is_planned = true; - } - - return true; -} - -bool OpenManipulatorController::setActuatorStateCallback(open_manipulator_msgs::SetActuatorState::Request &req, - open_manipulator_msgs::SetActuatorState::Response &res) -{ - if (req.set_actuator_state == true) // enable actuators - { - log::println("Wait a second for actuator enable", "GREEN"); - timer_thread_state_ = false; - pthread_join(timer_thread_, NULL); // Wait for the thread associated with thread_p to complete - open_manipulator_.enableAllActuator(); - startTimerThread(); - } - else // disable actuators - { - log::println("Wait a second for actuator disable", "GREEN"); - timer_thread_state_ = false; - pthread_join(timer_thread_, NULL); // Wait for the thread associated with thread_p to complete - open_manipulator_.disableAllActuator(); - startTimerThread(); - } - - res.is_planned = true; - - return true; -} - -bool OpenManipulatorController::goalDrawingTrajectoryCallback(open_manipulator_msgs::SetDrawingTrajectory::Request &req, - open_manipulator_msgs::SetDrawingTrajectory::Response &res) -{ - try - { - if (req.drawing_trajectory_name == "circle") - { - double draw_circle_arg[3]; - draw_circle_arg[0] = req.param[0]; // radius (m) - draw_circle_arg[1] = req.param[1]; // revolution (rev) - draw_circle_arg[2] = req.param[2]; // start angle position (rad) - void* p_draw_circle_arg = &draw_circle_arg; - - if (!open_manipulator_.makeCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, req.end_effector_name, p_draw_circle_arg, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - } - else if (req.drawing_trajectory_name == "line") - { - TaskWaypoint draw_line_arg; - draw_line_arg.kinematic.position(0) = req.param[0]; // x axis (m) - draw_line_arg.kinematic.position(1) = req.param[1]; // y axis (m) - draw_line_arg.kinematic.position(2) = req.param[2]; // z axis (m) - void *p_draw_line_arg = &draw_line_arg; - - if (!open_manipulator_.makeCustomTrajectory(CUSTOM_TRAJECTORY_LINE, req.end_effector_name, p_draw_line_arg, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - } - else if (req.drawing_trajectory_name == "rhombus") - { - double draw_rhombus_arg[3]; - draw_rhombus_arg[0] = req.param[0]; // radius (m) - draw_rhombus_arg[1] = req.param[1]; // revolution (rev) - draw_rhombus_arg[2] = req.param[2]; // start angle position (rad) - void* p_draw_rhombus_arg = &draw_rhombus_arg; - - if (!open_manipulator_.makeCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, req.end_effector_name, p_draw_rhombus_arg, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - } - else if (req.drawing_trajectory_name == "heart") - { - double draw_heart_arg[3]; - draw_heart_arg[0] = req.param[0]; // radius (m) - draw_heart_arg[1] = req.param[1]; // revolution (rev) - draw_heart_arg[2] = req.param[2]; // start angle position (rad) - void* p_draw_heart_arg = &draw_heart_arg; - - if (!open_manipulator_.makeCustomTrajectory(CUSTOM_TRAJECTORY_HEART, req.end_effector_name, p_draw_heart_arg, req.path_time)) - res.is_planned = false; - else - res.is_planned = true; - } - - return true; - } - catch ( ros::Exception &e ) - { - log::error("Creation the custom trajectory is failed!"); - } - - return true; -} - -/******************************************************************************** -** Callback function for process timer -********************************************************************************/ -void OpenManipulatorController::process(double time) -{ - open_manipulator_.processOpenManipulator(time); -} - -/******************************************************************************** -** Callback function for publish timer -********************************************************************************/ -void OpenManipulatorController::publishCallback(const ros::TimerEvent&) -{ - if (using_platform_ == true) publishJointStates(); - else publishGazeboCommand(); - - publishOpenManipulatorStates(); - publishKinematicsPose(); -} - -void OpenManipulatorController::publishOpenManipulatorStates() -{ - open_manipulator_msgs::OpenManipulatorState msg; - if (open_manipulator_.getMovingState()) - msg.open_manipulator_moving_state = msg.IS_MOVING; - else - msg.open_manipulator_moving_state = msg.STOPPED; - - if (open_manipulator_.getActuatorEnabledState(JOINT_DYNAMIXEL)) - msg.open_manipulator_actuator_state = msg.ACTUATOR_ENABLED; - else - msg.open_manipulator_actuator_state = msg.ACTUATOR_DISABLED; - - open_manipulator_states_pub_.publish(msg); -} - -void OpenManipulatorController::publishKinematicsPose() -{ - open_manipulator_msgs::KinematicsPose msg; - auto om_tools_name = open_manipulator_.getManipulator()->getAllToolComponentName(); - - uint8_t index = 0; - for (auto const& tools:om_tools_name) - { - KinematicPose pose = open_manipulator_.getKinematicPose(tools); - msg.pose.position.x = pose.position[0]; - msg.pose.position.y = pose.position[1]; - msg.pose.position.z = pose.position[2]; - Eigen::Quaterniond orientation = math::convertRotationMatrixToQuaternion(pose.orientation); - msg.pose.orientation.w = orientation.w(); - msg.pose.orientation.x = orientation.x(); - msg.pose.orientation.y = orientation.y(); - msg.pose.orientation.z = orientation.z(); - - open_manipulator_kinematics_pose_pub_.at(index).publish(msg); - index++; - } -} - -void OpenManipulatorController::publishJointStates() -{ - sensor_msgs::JointState msg; - msg.header.stamp = ros::Time::now(); - - auto joints_name = open_manipulator_.getManipulator()->getAllActiveJointComponentName(); - auto tool_name = open_manipulator_.getManipulator()->getAllToolComponentName(); - - auto joint_value = open_manipulator_.getAllActiveJointValue(); - auto tool_value = open_manipulator_.getAllToolValue(); - - for (uint8_t i = 0; i < joints_name.size(); i ++) - { - msg.name.push_back(joints_name.at(i)); - - msg.position.push_back(joint_value.at(i).position); - msg.velocity.push_back(joint_value.at(i).velocity); - msg.effort.push_back(joint_value.at(i).effort); - } - - for (uint8_t i = 0; i < tool_name.size(); i ++) - { - msg.name.push_back(tool_name.at(i)); - - msg.position.push_back(tool_value.at(i).position); - msg.velocity.push_back(0.0f); - msg.effort.push_back(0.0f); - } - open_manipulator_joint_states_pub_.publish(msg); -} - -void OpenManipulatorController::publishGazeboCommand() -{ - JointWaypoint joint_value = open_manipulator_.getAllActiveJointValue(); - JointWaypoint tool_value = open_manipulator_.getAllToolValue(); - - for (uint8_t i = 0; i < joint_value.size(); i ++) - { - std_msgs::Float64 msg; - msg.data = joint_value.at(i).position; - - gazebo_goal_joint_position_pub_.at(i).publish(msg); - } - - for (uint8_t i = 0; i < tool_value.size(); i ++) - { - std_msgs::Float64 msg; - msg.data = tool_value.at(i).position; - - gazebo_goal_joint_position_pub_.at(joint_value.size() + i).publish(msg); - } -} - -/***************************************************************************** -** Main -*****************************************************************************/ -int main(int argc, char **argv) -{ - // init - ros::init(argc, argv, "open_manipulator_controller"); - ros::NodeHandle node_handle(""); - - std::string usb_port = "/dev/ttyUSB0"; - std::string baud_rate = "1000000"; - - if (argc = 3) - { - usb_port = argv[1]; - baud_rate = argv[2]; - printf("port_name and baud_rate are set to %s, %s \n", usb_port.c_str(), baud_rate.c_str()); - } - else - { - log::error("Please set '-port_name' and '-baud_rate' arguments for connected Dynamixels"); - return 1; - } - - OpenManipulatorController om_controller(usb_port, baud_rate); - - // update - om_controller.startTimerThread(); - ros::Timer publish_timer = node_handle.createTimer(ros::Duration(om_controller.getControlPeriod()), &OpenManipulatorController::publishCallback, &om_controller); - ros::Rate loop_rate(100); - while (ros::ok()) - { - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} diff --git a/open_manipulator_description/CHANGELOG.rst b/open_manipulator_description/CHANGELOG.rst deleted file mode 100644 index 5f962a97..00000000 --- a/open_manipulator_description/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_description -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.2 (2021-06-21) ------------------- -* Noetic support -* Remove use_gui param from joint_state_publisher package -* Contributors: Will Son - -2.0.1 (2019-02-18) ------------------- -* none - -2.0.0 (2019-02-08) ------------------- -* deleted model.launch -* modified gripper origin -* modified end_effector origin -* modified link2 and joint2 position -* updated inertia -* changed calculated inertia param -* changed gripper link name -* changed axis for grip_joint -* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo - -1.0.0 (2018-06-01) ------------------- -* package reconfiguration for OpenManipulator -* added new stl files -* added urdf, rviz param, gazebo params, group -* modified color, xacro server, mu1, mu2, collision range, joint limit -* modified joint_state_publisher, joint_states_publisher -* modified params of inertial, xacro, gazebo, collision, friction -* modified urdf file names and collision geometry -* deleted unnecessary packages -* merged pull request `#34 `_ `#33 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ -* Contributors: Darby Lim, Pyo - -0.1.1 (2018-03-15) ------------------- -* none - -0.1.0 (2018-03-14) ------------------- -* modified joint control -* modified gripper topic -* modified gazebo set -* modified URDF -* modified description -* modified messages -* modified moveit -* modified cmake, package files for release -* refactoring for release -* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_description/CMakeLists.txt b/open_manipulator_description/CMakeLists.txt deleted file mode 100644 index 3edaf19b..00000000 --- a/open_manipulator_description/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(open_manipulator_description) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -# Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package() - -################################################################################ -# Build -################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} -) - -################################################################################ -# Install -################################################################################ -install(DIRECTORY launch meshes rviz urdf - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/open_manipulator_description/launch/open_manipulator_rviz.launch b/open_manipulator_description/launch/open_manipulator_rviz.launch deleted file mode 100644 index 1c29d4f7..00000000 --- a/open_manipulator_description/launch/open_manipulator_rviz.launch +++ /dev/null @@ -1,14 +0,0 @@ - - - - - - ["joint_states"] - - - - - - - - diff --git a/open_manipulator_description/launch/open_manipulator_upload.launch b/open_manipulator_description/launch/open_manipulator_upload.launch deleted file mode 100644 index 38d96df0..00000000 --- a/open_manipulator_description/launch/open_manipulator_upload.launch +++ /dev/null @@ -1,4 +0,0 @@ - - - diff --git a/open_manipulator_description/urdf/open_manipulator.gazebo.xacro b/open_manipulator_description/urdf/open_manipulator.gazebo.xacro deleted file mode 100644 index a920ae96..00000000 --- a/open_manipulator_description/urdf/open_manipulator.gazebo.xacro +++ /dev/null @@ -1,106 +0,0 @@ - - - - - - - 1000000.0 - 100.0 - 30.0 - 30.0 - 1.0 - 0.001 - Gazebo/DarkGrey - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Red - - - - - - 0.001 - gazebo_ros_control/DefaultRobotHWSim - true - - - - - - - gripper - gripper_sub - 1.0 - 0.0 - - - - - - - 30.0 - - 1.02974 - - 1920 - 1080 - R8G8B8 - - - 0.02 - 300 - - - gaussian - - 0.0 - 0.007 - - - - true - 0.0 - camera/color - image_raw - camera_info - camera_link - 0.07 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - diff --git a/open_manipulator_description/urdf/open_manipulator.transmission.xacro b/open_manipulator_description/urdf/open_manipulator.transmission.xacro deleted file mode 100644 index ca25c89f..00000000 --- a/open_manipulator_description/urdf/open_manipulator.transmission.xacro +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - 1 - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - 1 - - - - - - - - - - - - - - - - - - - - - - - diff --git a/open_manipulator_description/urdf/open_manipulator.urdf.xacro b/open_manipulator_description/urdf/open_manipulator.urdf.xacro deleted file mode 100644 index 71ce2a62..00000000 --- a/open_manipulator_description/urdf/open_manipulator.urdf.xacro +++ /dev/null @@ -1,278 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/open_manipulator_description/urdf/open_manipulator_robot.urdf.xacro b/open_manipulator_description/urdf/open_manipulator_robot.urdf.xacro deleted file mode 100644 index a900a2f5..00000000 --- a/open_manipulator_description/urdf/open_manipulator_robot.urdf.xacro +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/open_manipulator_libs/CHANGELOG.rst b/open_manipulator_libs/CHANGELOG.rst deleted file mode 100644 index bc724786..00000000 --- a/open_manipulator_libs/CHANGELOG.rst +++ /dev/null @@ -1,43 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_libs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.2 (2021-06-21) ------------------- -* Noetic support -* Contributors: Will Son - -2.0.1 (2019-02-18) ------------------- -* none - -2.0.0 (2019-02-08) ------------------- -* changed open_manipulator_dynamixel_ctrl to open_manipulator_libs -* updated the CHANGELOG and version to release binary packages -* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo - -1.0.0 (2018-06-01) ------------------- -* package reconfiguration for OpenManipulator -* added function to support protocol 1.0 -* modified motor id, msg names -* merged pull request `#34 `_ `#33 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ -* Contributors: Darby Lim, Pyo - -0.1.1 (2018-03-15) ------------------- -* none - -0.1.0 (2018-03-14) ------------------- -* added new dynamixel-workbench and velocity message -* added sync read failed debug code -* modified dxl-wb path -* modified dynamixel controller -* modified messages -* modified moveit -* modified description -* modified cmake, package files for release -* refactoring for release -* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_libs/CMakeLists.txt b/open_manipulator_libs/CMakeLists.txt deleted file mode 100644 index d43b3aa0..00000000 --- a/open_manipulator_libs/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(open_manipulator_libs) - -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - robotis_manipulator - roscpp - cmake_modules - dynamixel_workbench_toolbox -) -find_package(Eigen3 REQUIRED) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -## Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - LIBRARIES open_manipulator_libs - CATKIN_DEPENDS roscpp robotis_manipulator dynamixel_workbench_toolbox cmake_modules - DEPENDS EIGEN3 -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} -) - -add_library(open_manipulator_libs - src/open_manipulator.cpp - src/custom_trajectory.cpp - src/dynamixel.cpp - src/kinematics.cpp -) - -add_dependencies(open_manipulator_libs ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open_manipulator_libs ${catkin_LIBRARIES} ${Eigen3_LIBRARIES}) - -################################################################################ -# Install -################################################################################ -install(TARGETS open_manipulator_libs - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/open_manipulator_libs/include/open_manipulator_libs/custom_trajectory.h b/open_manipulator_libs/include/open_manipulator_libs/custom_trajectory.h deleted file mode 100644 index 28a4c5e8..00000000 --- a/open_manipulator_libs/include/open_manipulator_libs/custom_trajectory.h +++ /dev/null @@ -1,159 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef CUSTOM_TRAJECTORY_H_ -#define CUSTOM_TRAJECTORY_H_ - -#if defined(__OPENCR__) - #include -#else - #include -#endif - -using namespace robotis_manipulator; -using namespace Eigen; - -namespace custom_trajectory -{ - -enum AXIS{ - X_AXIS, - Y_AXIS, - Z_AXIS, -}; - - -/***************************************************************************** -** Line -*****************************************************************************/ -class Line : public robotis_manipulator::CustomTaskTrajectory -{ -private: - TaskWaypoint start_pose_; - TaskWaypoint goal_pose_; - - double acc_dec_time_; - double move_time_; - std::vector vel_max_; - -public: - Line() {} - virtual ~Line() {} - - void initLine(double move_time, TaskWaypoint start, TaskWaypoint delta); - TaskWaypoint drawLine(double time_var); - - virtual void setOption(const void *arg); - virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); - virtual TaskWaypoint getTaskWaypoint(double tick); -}; - - -/***************************************************************************** -** Circle -*****************************************************************************/ -class Circle : public robotis_manipulator::CustomTaskTrajectory -{ -private: - robotis_manipulator::MinimumJerk path_generator_; - VectorXd coefficient_; - - TaskWaypoint start_pose_; - TaskWaypoint goal_pose_; - - double radius_; - double start_angular_position_; - double revolution_; - -public: - Circle() {} - virtual ~Circle() {} - - void initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); - TaskWaypoint drawCircle(double time_var); - - virtual void setOption(const void *arg); - virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); - virtual TaskWaypoint getTaskWaypoint(double tick); -}; - - -/***************************************************************************** -** Rhombus -*****************************************************************************/ -class Rhombus : public robotis_manipulator::CustomTaskTrajectory -{ -private: - robotis_manipulator::MinimumJerk path_generator_; - VectorXd coefficient_; - - TaskWaypoint start_pose_; - TaskWaypoint goal_pose_; - - double radius_; - double start_angular_position_; - double revolution_; - -public: - Rhombus() {} - virtual ~Rhombus() {} - - void initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); - TaskWaypoint drawRhombus(double time_var); - - virtual void setOption(const void *arg); - virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); - virtual TaskWaypoint getTaskWaypoint(double tick); -}; - - -/***************************************************************************** -** Heart -*****************************************************************************/ -class Heart : public robotis_manipulator::CustomTaskTrajectory -{ -private: - robotis_manipulator::MinimumJerk path_generator_; - VectorXd coefficient_; - - TaskWaypoint start_pose_; - TaskWaypoint goal_pose_; - - double radius_; - double start_angular_position_; - double revolution_; - -public: - Heart() {} - virtual ~Heart() {} - - void initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position); - TaskWaypoint drawHeart(double tick); - - virtual void setOption(const void *arg); - virtual void makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg); - virtual TaskWaypoint getTaskWaypoint(double tick); -}; - - -} // namespace CUSTOM_TRAJECTORY -#endif // CUSTOM_TRAJECTORY_H_ - - - - diff --git a/open_manipulator_libs/include/open_manipulator_libs/dynamixel.h b/open_manipulator_libs/include/open_manipulator_libs/dynamixel.h deleted file mode 100644 index 9321359c..00000000 --- a/open_manipulator_libs/include/open_manipulator_libs/dynamixel.h +++ /dev/null @@ -1,189 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef DYNAMIXEL_H_ -#define DYNAMIXEL_H_ - -#if defined(__OPENCR__) - #include - #include -#else - #include - #include -#endif - -namespace dynamixel -{ - -#define SYNC_WRITE_HANDLER 0 -#define SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT 0 - -//#define CONTROL_LOOP_TIME 10; //ms - -// Protocol 2.0 -#define ADDR_PRESENT_CURRENT_2 126 -#define ADDR_PRESENT_VELOCITY_2 128 -#define ADDR_PRESENT_POSITION_2 132 -#define ADDR_VELOCITY_TRAJECTORY_2 136 -#define ADDR_POSITION_TRAJECTORY_2 140 -#define ADDR_PROFILE_ACCELERATION_2 108 -#define ADDR_PROFILE_VELOCITY_2 112 -#define ADDR_GOAL_POSITION_2 116 - - -#define LENGTH_PRESENT_CURRENT_2 2 -#define LENGTH_PRESENT_VELOCITY_2 4 -#define LENGTH_PRESENT_POSITION_2 4 -#define LENGTH_VELOCITY_TRAJECTORY_2 4 -#define LENGTH_POSITION_TRAJECTORY_2 4 -#define LENGTH_PROFILE_ACCELERATION_2 4 -#define LENGTH_PROFILE_VELOCITY_2 4 -#define LENGTH_GOAL_POSITION_2 4 - - -// Protocol 1.0 -#define ADDR_PRESENT_CURRENT_1 = 40; -#define ADDR_PRESENT_VELOCITY_1 = 38; -#define ADDR_PRESENT_POSITION_1 = 36; - -#define LENGTH_PRESENT_CURRENT_1 = 2; -#define LENGTH_PRESENT_VELOCITY_1 = 2; -#define LENGTH_PRESENT_POSITION_1 = 2; - -typedef struct -{ - std::vector id; - uint8_t num; -} Joint; - -class JointDynamixel : public robotis_manipulator::JointActuator -{ -private: - DynamixelWorkbench *dynamixel_workbench_; - Joint dynamixel_; - -public: - JointDynamixel(){} - virtual ~JointDynamixel(){} - - - /***************************************************************************** - ** Joint Dynamixel Control Functions - *****************************************************************************/ - virtual void init(std::vector actuator_id, const void *arg); - virtual void setMode(std::vector actuator_id, const void *arg); - virtual std::vector getId(); - - virtual void enable(); - virtual void disable(); - - virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); - virtual std::vector receiveJointActuatorValue(std::vector actuator_id); - - - /***************************************************************************** - ** Functions called in Joint Dynamixel Control Functions - *****************************************************************************/ - bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); - bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); - bool setSDKHandler(uint8_t actuator_id); - bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); - bool writeGoalPosition(std::vector actuator_id, std::vector radian_vector); - std::vector receiveAllDynamixelValue(std::vector actuator_id); -}; - -class JointDynamixelProfileControl : public robotis_manipulator::JointActuator -{ -private: - DynamixelWorkbench *dynamixel_workbench_; - Joint dynamixel_; - float control_loop_time_; // unit: ms - std::map previous_goal_value_; - -public: - JointDynamixelProfileControl(float control_loop_time = 0.010); - virtual ~JointDynamixelProfileControl(){} - - - /***************************************************************************** - ** Joint Dynamixel Profile Control Functions - *****************************************************************************/ - virtual void init(std::vector actuator_id, const void *arg); - virtual void setMode(std::vector actuator_id, const void *arg); - virtual std::vector getId(); - - virtual void enable(); - virtual void disable(); - - virtual bool sendJointActuatorValue(std::vector actuator_id, std::vector value_vector); - virtual std::vector receiveJointActuatorValue(std::vector actuator_id); - - - /***************************************************************************** - ** Functions called in Joint Dynamixel Profile Control Functions - *****************************************************************************/ - bool initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); - bool setOperatingMode(std::vector actuator_id, STRING dynamixel_mode = "position_mode"); - bool setSDKHandler(uint8_t actuator_id); - bool writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value); - bool writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector); - std::vector receiveAllDynamixelValue(std::vector actuator_id); -}; - -class GripperDynamixel : public robotis_manipulator::ToolActuator -{ - private: - DynamixelWorkbench *dynamixel_workbench_; - Joint dynamixel_; - - public: - GripperDynamixel() {} - virtual ~GripperDynamixel() {} - - - /***************************************************************************** - ** Tool Dynamixel Control Functions - *****************************************************************************/ - virtual void init(uint8_t actuator_id, const void *arg); - virtual void setMode(const void *arg); - virtual uint8_t getId(); - - virtual void enable(); - virtual void disable(); - - virtual bool sendToolActuatorValue(robotis_manipulator::ActuatorValue value); - virtual robotis_manipulator::ActuatorValue receiveToolActuatorValue(); - - - /***************************************************************************** - ** Functions called in Tool Dynamixel Profile Control Functions - *****************************************************************************/ - bool initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate); - bool setOperatingMode(STRING dynamixel_mode = "position_mode"); - bool writeProfileValue(STRING profile_mode, uint32_t value); - bool setSDKHandler(); - bool writeGoalPosition(double radian); - double receiveDynamixelValue(); -}; - -} // namespace DYNAMIXEL -#endif // DYNAMIXEL_H_ - - - - diff --git a/open_manipulator_libs/include/open_manipulator_libs/kinematics.h b/open_manipulator_libs/include/open_manipulator_libs/kinematics.h deleted file mode 100644 index 38c8311d..00000000 --- a/open_manipulator_libs/include/open_manipulator_libs/kinematics.h +++ /dev/null @@ -1,118 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef KINEMATICS_H_ -#define KINEMATICS_H_ - -#if defined(__OPENCR__) - #include -#else - #include -#endif - -//#define KINEMATICS_DEBUG - -using namespace Eigen; -using namespace robotis_manipulator; - -namespace kinematics -{ - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Jacobian -*****************************************************************************/ -class SolverUsingCRAndJacobian : public robotis_manipulator::Kinematics -{ -private: - void forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name); - bool inverseSolverUsingJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); - -public: - SolverUsingCRAndJacobian(){} - virtual ~SolverUsingCRAndJacobian(){} - - virtual void setOption(const void *arg); - virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); - virtual void solveForwardKinematics(Manipulator *manipulator); - virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); -}; - - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Singularity Robust Jacobian -*****************************************************************************/ -class SolverUsingCRAndSRJacobian : public robotis_manipulator::Kinematics -{ -private: - void forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name); - bool inverseSolverUsingSRJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); - -public: - SolverUsingCRAndSRJacobian(){} - virtual ~SolverUsingCRAndSRJacobian(){} - - virtual void setOption(const void *arg); - virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); - virtual void solveForwardKinematics(Manipulator *manipulator); - virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); -}; - - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Singularity Robust Position Only Jacobian -*****************************************************************************/ -class SolverUsingCRAndSRPositionOnlyJacobian : public robotis_manipulator::Kinematics -{ -private: - void forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name); - bool inverseSolverUsingPositionOnlySRJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); - -public: - SolverUsingCRAndSRPositionOnlyJacobian(){} - virtual ~SolverUsingCRAndSRPositionOnlyJacobian(){} - - virtual void setOption(const void *arg); - virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); - virtual void solveForwardKinematics(Manipulator *manipulator); - virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); -}; - - -/***************************************************************************** -** Kinematics Solver Customized for OpenManipulator Chain -*****************************************************************************/ -class SolverCustomizedforOMChain : public robotis_manipulator::Kinematics -{ -private: - void forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name); - bool chainCustomInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); - -public: - SolverCustomizedforOMChain(){} - virtual ~SolverCustomizedforOMChain(){} - - virtual void setOption(const void *arg); - virtual MatrixXd jacobian(Manipulator *manipulator, Name tool_name); - virtual void solveForwardKinematics(Manipulator *manipulator); - virtual bool solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector* goal_joint_value); -}; - -} // namespace KINEMATICS - - -#endif // KINEMATICS_H_ diff --git a/open_manipulator_libs/include/open_manipulator_libs/open_manipulator.h b/open_manipulator_libs/include/open_manipulator_libs/open_manipulator.h deleted file mode 100644 index a6a82af8..00000000 --- a/open_manipulator_libs/include/open_manipulator_libs/open_manipulator.h +++ /dev/null @@ -1,56 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef OPEN_MANIPULTOR_H_ -#define OPEN_MANIPULTOR_H_ - -#include "dynamixel.h" -#include "custom_trajectory.h" -#include "kinematics.h" - -#define CUSTOM_TRAJECTORY_SIZE 4 -#define CUSTOM_TRAJECTORY_LINE "custom_trajectory_line" -#define CUSTOM_TRAJECTORY_CIRCLE "custom_trajectory_circle" -#define CUSTOM_TRAJECTORY_RHOMBUS "custom_trajectory_rhombus" -#define CUSTOM_TRAJECTORY_HEART "custom_trajectory_heart" - -#define JOINT_DYNAMIXEL "joint_dxl" -#define TOOL_DYNAMIXEL "tool_dxl" - -#define X_AXIS robotis_manipulator::math::vector3(1.0, 0.0, 0.0) -#define Y_AXIS robotis_manipulator::math::vector3(0.0, 1.0, 0.0) -#define Z_AXIS robotis_manipulator::math::vector3(0.0, 0.0, 1.0) - -class OpenManipulator : public robotis_manipulator::RobotisManipulator -{ - -private: - robotis_manipulator::Kinematics *kinematics_; - robotis_manipulator::JointActuator *actuator_; - robotis_manipulator::ToolActuator *tool_; - robotis_manipulator::CustomTaskTrajectory *custom_trajectory_[CUSTOM_TRAJECTORY_SIZE]; - -public: - OpenManipulator(); - virtual ~OpenManipulator(); - - void initOpenManipulator(bool using_actual_robot_state, STRING usb_port = "/dev/ttyUSB0", STRING baud_rate = "1000000", float control_loop_time = 0.010); - void processOpenManipulator(double present_time); -}; - -#endif // OPEN_MANIPULTOR_H_ diff --git a/open_manipulator_libs/package.xml b/open_manipulator_libs/package.xml deleted file mode 100644 index 14116160..00000000 --- a/open_manipulator_libs/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - open_manipulator_libs - 2.0.2 - - OpenManipulator library (Kinematics lib., Dynamixel lib., Drawing path lib.) - - Apache 2.0 - Darby Lim - Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son - http://wiki.ros.org/open_manipulator_libs - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues - catkin - robotis_manipulator - cmake_modules - dynamixel_workbench_toolbox - eigen - robotis_manipulator - roscpp - diff --git a/open_manipulator_libs/src/custom_trajectory.cpp b/open_manipulator_libs/src/custom_trajectory.cpp deleted file mode 100644 index 4da2a52b..00000000 --- a/open_manipulator_libs/src/custom_trajectory.cpp +++ /dev/null @@ -1,346 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "../include/open_manipulator_libs/custom_trajectory.h" - -using namespace custom_trajectory; -using namespace Eigen; - - -/***************************************************************************** -** Line -*****************************************************************************/ -void Line::initLine(double move_time, TaskWaypoint start, TaskWaypoint delta) -{ - move_time_ = move_time; - acc_dec_time_ = move_time_ * 0.2; - vel_max_.resize(3); - - TaskWaypoint start_to_goal; - - start_pose_ = start; - - goal_pose_ .kinematic.orientation = start_pose_.kinematic.orientation; - goal_pose_ .kinematic.position = start.kinematic.position + delta.kinematic.position; - - vel_max_.at(X_AXIS) = delta.kinematic.position(X_AXIS)/(move_time_ - acc_dec_time_); - vel_max_.at(Y_AXIS) = delta.kinematic.position(Y_AXIS)/(move_time_ - acc_dec_time_); - vel_max_.at(Z_AXIS) = delta.kinematic.position(Z_AXIS)/(move_time_ - acc_dec_time_); -} - -TaskWaypoint Line::drawLine(double time_var) -{ - TaskWaypoint pose; - - if(acc_dec_time_ >= time_var) - { - pose.kinematic.position(X_AXIS) = 0.5*vel_max_.at(X_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(X_AXIS); - pose.kinematic.position(Y_AXIS) = 0.5*vel_max_.at(Y_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Y_AXIS); - pose.kinematic.position(Z_AXIS) = 0.5*vel_max_.at(Z_AXIS)*pow(time_var, 2)/acc_dec_time_ + start_pose_.kinematic.position(Z_AXIS); - } - else if(time_var > acc_dec_time_ && (move_time_ - acc_dec_time_) >= time_var ) - { - pose.kinematic.position(X_AXIS) = vel_max_.at(X_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(X_AXIS); - pose.kinematic.position(Y_AXIS) = vel_max_.at(Y_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Y_AXIS); - pose.kinematic.position(Z_AXIS) = vel_max_.at(Z_AXIS)*(time_var-(acc_dec_time_*0.5)) + start_pose_.kinematic.position(Z_AXIS); - } - else if(time_var > (move_time_ - acc_dec_time_) && (time_var < move_time_)) - { - pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS) - vel_max_.at(X_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); - pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS) - vel_max_.at(Y_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); - pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS) - vel_max_.at(Z_AXIS)*0.5/acc_dec_time_*(pow((move_time_-time_var),2)); - } - else if(time_var <= move_time_) - { - pose.kinematic.position(X_AXIS) = goal_pose_.kinematic.position(X_AXIS); - pose.kinematic.position(Y_AXIS) = goal_pose_.kinematic.position(Y_AXIS); - pose.kinematic.position(Z_AXIS) = goal_pose_.kinematic.position(Z_AXIS); - } - - pose.kinematic.orientation = start_pose_.kinematic.orientation; - pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); - - return pose; -} - -TaskWaypoint Line::getTaskWaypoint(double tick) -{ - return drawLine(tick); -} - - -void Line::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) -{ - TaskWaypoint *c_arg = (TaskWaypoint *)arg; - initLine(move_time, start, c_arg[0]); -} -void Line::setOption(const void *arg) {} - - -/***************************************************************************** -** Circle -*****************************************************************************/ -void Circle::initCircle(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) -{ - start_pose_ = start; - - radius_ = radius; - revolution_ = revolution; - start_angular_position_ = start_angular_position; - - Point drawingStart, drawingGoal; - - drawingStart.position = 0.0; - drawingStart.velocity = 0.0; - drawingStart.acceleration = 0.0; - drawingStart.effort = 0.0; - - drawingGoal.position = revolution_ * 2*M_PI; - drawingGoal.velocity = 0.0; - drawingGoal.acceleration = 0.0; - drawingGoal.effort = 0.0; - - path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); - coefficient_ = path_generator_.getCoefficient(); -} - -TaskWaypoint Circle::drawCircle(double tick) -{ - // get time variable - double get_time_var = 0.0; - - get_time_var = coefficient_(0) + - coefficient_(1) * pow(tick, 1) + - coefficient_(2) * pow(tick, 2) + - coefficient_(3) * pow(tick, 3) + - coefficient_(4) * pow(tick, 4) + - coefficient_(5) * pow(tick, 5); - - // set drawing trajectory - TaskWaypoint pose; - - double diff_pose[2]; - - diff_pose[0] = (cos(get_time_var)-1)*cos(start_angular_position_) - sin(get_time_var)*sin(start_angular_position_); - diff_pose[1] = (cos(get_time_var)-1)*sin(start_angular_position_) + sin(get_time_var)*cos(start_angular_position_); - - pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; - pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; - pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); - - pose.kinematic.orientation = start_pose_.kinematic.orientation; - - pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); - - return pose; -} - -TaskWaypoint Circle::getTaskWaypoint(double tick) -{ - return drawCircle(tick); -} - -void Circle::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) -{ - double *get_arg_ = (double *)arg; - initCircle(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); -} - -void Circle::setOption(const void *arg){} - - -/***************************************************************************** -** Rhombus -*****************************************************************************/ -void Rhombus::initRhombus(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) -{ - start_pose_ = start; - - radius_ = radius; - revolution_ = revolution; - start_angular_position_ = start_angular_position; - - Point drawingStart, drawingGoal; - - drawingStart.position = 0.0; - drawingStart.velocity = 0.0; - drawingStart.acceleration = 0.0; - drawingStart.effort = 0.0; - - drawingGoal.position = revolution_ * 2*M_PI; - drawingGoal.velocity = 0.0; - drawingGoal.acceleration = 0.0; - drawingGoal.effort = 0.0; - - path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); - coefficient_ = path_generator_.getCoefficient(); -} - - -TaskWaypoint Rhombus::drawRhombus(double tick) -{ - // get time variable - double get_time_var = 0.0; - - get_time_var = coefficient_(0) + - coefficient_(1) * pow(tick, 1) + - coefficient_(2) * pow(tick, 2) + - coefficient_(3) * pow(tick, 3) + - coefficient_(4) * pow(tick, 4) + - coefficient_(5) * pow(tick, 5); - - // set drawing trajectory - TaskWaypoint pose; - double diff_pose[2]; - double traj[2]; - - while(true) - { - if (get_time_var < PI*2) break; - get_time_var = get_time_var - PI*2; - } - - if (get_time_var >= 0 && get_time_var < PI/2){ - traj[0] = - get_time_var / (PI/2); - traj[1] = - get_time_var / (PI/2); - } else if (get_time_var >= PI/2 && get_time_var < PI){ - traj[0] = - get_time_var / (PI/2); - traj[1] = get_time_var / (PI/2) - 2; - } else if (get_time_var >= PI && get_time_var < PI*3/2){ - traj[0] = get_time_var / (PI/2) - 4; - traj[1] = get_time_var / (PI/2) - 2; - } else { - traj[0] = get_time_var / (PI/2) - 4; - traj[1] = - get_time_var / (PI/2) + 4; - } - - - diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); - diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); - - pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; - pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; - pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); - - pose.kinematic.orientation = start_pose_.kinematic.orientation; - - pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); - - return pose; -} - - -void Rhombus::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) -{ - double *get_arg_ = (double *)arg; - initRhombus(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); -} - -TaskWaypoint Rhombus::getTaskWaypoint(double tick) -{ - return drawRhombus(tick); -} -void Rhombus::setOption(const void *arg){} - - -/***************************************************************************** -** Heart -*****************************************************************************/ -void Heart::initHeart(double move_time, TaskWaypoint start, double radius, double revolution, double start_angular_position) -{ - start_pose_ = start; - - radius_ = radius; - revolution_ = revolution; - start_angular_position_ = start_angular_position; - - Point drawingStart, drawingGoal; - - drawingStart.position = 0.0; - drawingStart.velocity = 0.0; - drawingStart.acceleration = 0.0; - drawingStart.effort = 0.0; - - drawingGoal.position = revolution_ * 2*M_PI; - drawingGoal.velocity = 0.0; - drawingGoal.acceleration = 0.0; - drawingGoal.effort = 0.0; - - path_generator_.calcCoefficient(drawingStart, drawingGoal, move_time); - coefficient_ = path_generator_.getCoefficient(); -} - -TaskWaypoint Heart::drawHeart(double tick) -{ - // get time variable - double get_time_var = 0.0; - - get_time_var = coefficient_(0) + - coefficient_(1) * pow(tick, 1) + - coefficient_(2) * pow(tick, 2) + - coefficient_(3) * pow(tick, 3) + - coefficient_(4) * pow(tick, 4) + - coefficient_(5) * pow(tick, 5); - - // set drawing trajectory - TaskWaypoint pose; - double diff_pose[2]; - double traj[2]; - - double shift_offset = - 5.0; - - traj[0] = (shift_offset + (13*cos(get_time_var) - 5*cos(2*get_time_var) - 2*cos(3*get_time_var) - cos(4*get_time_var))) / 16; - traj[1] = (16*sin(get_time_var)*sin(get_time_var)*sin(get_time_var)) / 16; - - diff_pose[0] = traj[0]*cos(start_angular_position_) - traj[1]*sin(start_angular_position_); - diff_pose[1] = traj[0]*sin(start_angular_position_) + traj[1]*cos(start_angular_position_); - - pose.kinematic.position(X_AXIS) = start_pose_.kinematic.position(X_AXIS) + radius_ * diff_pose[0]; - pose.kinematic.position(Y_AXIS) = start_pose_.kinematic.position(Y_AXIS) + radius_ * diff_pose[1]; - pose.kinematic.position(Z_AXIS) = start_pose_.kinematic.position(Z_AXIS); - - pose.kinematic.orientation = start_pose_.kinematic.orientation; - - pose.dynamic.linear.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.linear.acceleration = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.velocity = Eigen::Vector3d::Zero(3); - pose.dynamic.angular.acceleration = Eigen::Vector3d::Zero(3); - - return pose; -} - -void Heart::makeTaskTrajectory(double move_time, TaskWaypoint start, const void *arg) -{ - double *get_arg_ = (double *)arg; - initHeart(move_time, start, get_arg_[0], get_arg_[1], get_arg_[2]); -} -void Heart::setOption(const void *arg){} - -TaskWaypoint Heart::getTaskWaypoint(double tick) -{ - return drawHeart(tick); -} diff --git a/open_manipulator_libs/src/dynamixel.cpp b/open_manipulator_libs/src/dynamixel.cpp deleted file mode 100644 index c7760b84..00000000 --- a/open_manipulator_libs/src/dynamixel.cpp +++ /dev/null @@ -1,973 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "../include/open_manipulator_libs/dynamixel.h" - -using namespace dynamixel; -using namespace robotis_manipulator; - -/***************************************************************************** -** Joint Dynamixel Control Functions -*****************************************************************************/ -void JointDynamixel::init(std::vector actuator_id, const void *arg) -{ - STRING *get_arg_ = (STRING *)arg; - - bool result = JointDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); - - if (result == false) - return; -} - -void JointDynamixel::setMode(std::vector actuator_id, const void *arg) -{ - bool result = false; - // const char* log = NULL; - - STRING *get_arg_ = (STRING *)arg; - - if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") - { - result = JointDynamixel::setOperatingMode(actuator_id, get_arg_[0]); - if (result == false) - return; - - result = JointDynamixel::setSDKHandler(actuator_id.at(0)); - if (result == false) - return; - } - else - { - result = JointDynamixel::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); - if (result == false) - return; - } - return; -} - -std::vector JointDynamixel::getId() -{ - return dynamixel_.id; -} - -void JointDynamixel::enable() -{ - const char* log = NULL; - bool result = false; - - for (uint32_t index = 0; index < dynamixel_.num; index++) - { - result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); - if (result == false) - { - log::error(log); - } - } - enabled_state_ = true; -} - -void JointDynamixel::disable() -{ - const char* log = NULL; - bool result = false; - - for (uint32_t index = 0; index < dynamixel_.num; index++) - { - result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); - if (result == false) - { - log::error(log); - } - } - enabled_state_ = false; -} - -bool JointDynamixel::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) -{ - bool result = false; - - std::vector radian_vector; - for(uint32_t index = 0; index < value_vector.size(); index++) - { - radian_vector.push_back(value_vector.at(index).position); - } - result = JointDynamixel::writeGoalPosition(actuator_id, radian_vector); - if (result == false) - return false; - - return true; -} - -std::vector JointDynamixel::receiveJointActuatorValue(std::vector actuator_id) -{ - return JointDynamixel::receiveAllDynamixelValue(actuator_id); -} - - -/***************************************************************************** -** Functions called in Joint Dynamixel Control Functions -*****************************************************************************/ -bool JointDynamixel::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) -{ - bool result = false; - const char* log = NULL; - - STRING return_delay_time_st = "Return_Delay_Time"; - const char * return_delay_time_char = return_delay_time_st.c_str(); - - dynamixel_.id = actuator_id; - dynamixel_.num = actuator_id.size(); - - dynamixel_workbench_ = new DynamixelWorkbench; - - result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); - if (result == false) - { - log::error(log); - } - - uint16_t get_model_number; - for (uint8_t index = 0; index < dynamixel_.num; index++) - { - uint8_t id = dynamixel_.id.at(index); - result = dynamixel_workbench_->ping(id, &get_model_number, &log); - - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel ID"); - } - else - { - char str[100]; - sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); - log::println(str); - - result = dynamixel_workbench_->setVelocityBasedProfile(id, &log); - if(result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version (v38~)"); - } - - result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version"); - } - } - } - return true; -} - -bool JointDynamixel::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) -{ - const char* log = NULL; - bool result = false; - - const uint32_t velocity = 0; - const uint32_t acceleration = 0; - const uint32_t current = 0; - - if (dynamixel_mode == "position_mode") - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - } - else if (dynamixel_mode == "current_based_position_mode") - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); - if (result == false) - { - log::error(log); - } - } - } - else - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - } - - return true; -} - -bool JointDynamixel::setSDKHandler(uint8_t actuator_id) -{ - bool result = false; - const char* log = NULL; - - result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, - (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), - &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -bool JointDynamixel::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) -{ - const char* log = NULL; - bool result = false; - - const char * char_profile_mode = profile_mode.c_str(); - - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); - if (result == false) - { - log::error(log); - } - } - - return true; -} - -bool JointDynamixel::writeGoalPosition(std::vector actuator_id, std::vector radian_vector) -{ - bool result = false; - const char* log = NULL; - - uint8_t id_array[actuator_id.size()]; - int32_t goal_position[actuator_id.size()]; - - for (uint8_t index = 0; index < actuator_id.size(); index++) - { - id_array[index] = actuator_id.at(index); - goal_position[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), radian_vector.at(index)); - } - - result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_position, 1, &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -std::vector JointDynamixel::receiveAllDynamixelValue(std::vector actuator_id) -{ - bool result = false; - const char* log = NULL; - - std::vector all_actuator; - - uint8_t id_array[actuator_id.size()]; - for (uint8_t index = 0; index < actuator_id.size(); index++) - id_array[index] = actuator_id.at(index); - - int32_t get_current[actuator_id.size()]; - int32_t get_velocity[actuator_id.size()]; - int32_t get_position[actuator_id.size()]; - - result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_CURRENT_2, - LENGTH_PRESENT_CURRENT_2, - get_current, - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_VELOCITY_2, - LENGTH_PRESENT_VELOCITY_2, - get_velocity, - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_POSITION_2, - LENGTH_PRESENT_POSITION_2, - get_position, - &log); - if (result == false) - { - log::error(log); - } - - for (uint8_t index = 0; index < actuator_id.size(); index++) - { - robotis_manipulator::ActuatorValue actuator; - actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); - actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); - actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); - - all_actuator.push_back(actuator); - } - - return all_actuator; -} - - -/***************************************************************************** -** Joint Dynamixel Profile Control Functions -*****************************************************************************/ -JointDynamixelProfileControl::JointDynamixelProfileControl(float control_loop_time) -{ - control_loop_time_ = control_loop_time; -} - -void JointDynamixelProfileControl::init(std::vector actuator_id, const void *arg) -{ - STRING *get_arg_ = (STRING *)arg; - - bool result = JointDynamixelProfileControl::initialize(actuator_id ,get_arg_[0], get_arg_[1]); - - if (result == false) - return; -} - -void JointDynamixelProfileControl::setMode(std::vector actuator_id, const void *arg) -{ - bool result = false; - // const char* log = NULL; - - STRING *get_arg_ = (STRING *)arg; - - if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") - { - result = JointDynamixelProfileControl::setOperatingMode(actuator_id, get_arg_[0]); - if (result == false) - return; - - result = JointDynamixelProfileControl::setSDKHandler(actuator_id.at(0)); - if (result == false) - return; - } - else - { - result = JointDynamixelProfileControl::writeProfileValue(actuator_id, get_arg_[0], std::atoi(get_arg_[1].c_str())); - if (result == false) - return; - } - return; -} - -std::vector JointDynamixelProfileControl::getId() -{ - return dynamixel_.id; -} - -void JointDynamixelProfileControl::enable() -{ - const char* log = NULL; - bool result = false; - - for (uint32_t index = 0; index < dynamixel_.num; index++) - { - result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(index), &log); - if (result == false) - { - log::error(log); - } - } - enabled_state_ = true; -} - -void JointDynamixelProfileControl::disable() -{ - const char* log = NULL; - bool result = false; - - for (uint32_t index = 0; index < dynamixel_.num; index++) - { - result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(index), &log); - if (result == false) - { - log::error(log); - } - } - enabled_state_ = false; -} - -bool JointDynamixelProfileControl::sendJointActuatorValue(std::vector actuator_id, std::vector value_vector) -{ - bool result = false; - - result = JointDynamixelProfileControl::writeGoalProfilingControlValue(actuator_id, value_vector); - if (result == false) - return false; - - return true; -} - -std::vector JointDynamixelProfileControl::receiveJointActuatorValue(std::vector actuator_id) -{ - return JointDynamixelProfileControl::receiveAllDynamixelValue(actuator_id); -} - - -/***************************************************************************** -** Functions called in Joint Dynamixel Profile Control Functions -*****************************************************************************/ -bool JointDynamixelProfileControl::initialize(std::vector actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) -{ - bool result = false; - const char* log = NULL; - - STRING return_delay_time_st = "Return_Delay_Time"; - const char * return_delay_time_char = return_delay_time_st.c_str(); - - dynamixel_.id = actuator_id; - dynamixel_.num = actuator_id.size(); - - dynamixel_workbench_ = new DynamixelWorkbench; - - result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); - if (result == false) - { - log::error(log); - } - - uint16_t get_model_number; - for (uint8_t index = 0; index < dynamixel_.num; index++) - { - uint8_t id = dynamixel_.id.at(index); - result = dynamixel_workbench_->ping(id, &get_model_number, &log); - - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel ID"); - } - else - { - char str[100]; - sprintf(str, "Joint Dynamixel ID : %d, Model Name : %s", id, dynamixel_workbench_->getModelName(id)); - log::println(str); - - result = dynamixel_workbench_->setTimeBasedProfile(id, &log); - if(result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version (v38~)"); - } - - result = dynamixel_workbench_->writeRegister(id, return_delay_time_char, 0, &log); - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version"); - } - } - } - return true; -} - -bool JointDynamixelProfileControl::setOperatingMode(std::vector actuator_id, STRING dynamixel_mode) -{ - const char* log = NULL; - bool result = false; - - const uint32_t velocity = uint32_t(control_loop_time_*1000) * 3; - const uint32_t acceleration = uint32_t(control_loop_time_*1000); - const uint32_t current = 0; - - if (dynamixel_mode == "position_mode") - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - } - else if (dynamixel_mode == "current_based_position_mode") - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->currentBasedPositionMode(actuator_id.at(num), current, &log); - if (result == false) - { - log::error(log); - } - } - } - else - { - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->jointMode(actuator_id.at(num), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - } - - return true; -} - -bool JointDynamixelProfileControl::setSDKHandler(uint8_t actuator_id) -{ - bool result = false; - const char* log = NULL; - - result = dynamixel_workbench_->addSyncWriteHandler(actuator_id, "Goal_Position", &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->addSyncReadHandler(ADDR_PRESENT_CURRENT_2, - (LENGTH_PRESENT_CURRENT_2 + LENGTH_PRESENT_VELOCITY_2 + LENGTH_PRESENT_POSITION_2), - &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -bool JointDynamixelProfileControl::writeProfileValue(std::vector actuator_id, STRING profile_mode, uint32_t value) -{ - const char* log = NULL; - bool result = false; - - const char * char_profile_mode = profile_mode.c_str(); - - for (uint8_t num = 0; num < actuator_id.size(); num++) - { - result = dynamixel_workbench_->writeRegister(actuator_id.at(num), char_profile_mode, value, &log); - if (result == false) - { - log::error(log); - } - } - return true; -} - -bool JointDynamixelProfileControl::writeGoalProfilingControlValue(std::vector actuator_id, std::vector value_vector) -{ - bool result = false; - const char* log = NULL; - - uint8_t id_array[actuator_id.size()]; - int32_t goal_value[actuator_id.size()]; - - //add tarajectory eq. - for(uint8_t index = 0; index < actuator_id.size(); index++) - { - float result_position; - float time_control = control_loop_time_; //ms - - if(previous_goal_value_.find(actuator_id.at(index)) == previous_goal_value_.end()) - { - previous_goal_value_.insert(std::make_pair(actuator_id.at(index), value_vector.at(index))); - } - - result_position = value_vector.at(index).position + 3*(value_vector.at(index).velocity * (time_control))/2; - - id_array[index] = actuator_id.at(index); - goal_value[index] = dynamixel_workbench_->convertRadian2Value(actuator_id.at(index), result_position); - - previous_goal_value_[actuator_id.at(index)] = value_vector.at(index); - } - - result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, id_array, actuator_id.size(), goal_value, 1, &log); - if (result == false) - { - log::error(log); - } - return true; -} - -std::vector JointDynamixelProfileControl::receiveAllDynamixelValue(std::vector actuator_id) -{ - bool result = false; - const char* log = NULL; - - std::vector all_actuator; - - uint8_t id_array[actuator_id.size()]; - for (uint8_t index = 0; index < actuator_id.size(); index++) - id_array[index] = actuator_id.at(index); - - int32_t get_current[actuator_id.size()]; - int32_t get_velocity[actuator_id.size()]; - int32_t get_position[actuator_id.size()]; - - result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_CURRENT_2, - LENGTH_PRESENT_CURRENT_2, - get_current, - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_VELOCITY_2, - LENGTH_PRESENT_VELOCITY_2, - get_velocity, - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - actuator_id.size(), - ADDR_PRESENT_POSITION_2, - LENGTH_PRESENT_POSITION_2, - get_position, - &log); - if (result == false) - { - log::error(log); - } - - for (uint8_t index = 0; index < actuator_id.size(); index++) - { - robotis_manipulator::ActuatorValue actuator; - actuator.effort = dynamixel_workbench_->convertValue2Current(get_current[index]); - actuator.velocity = dynamixel_workbench_->convertValue2Velocity(actuator_id.at(index), get_velocity[index]); - actuator.position = dynamixel_workbench_->convertValue2Radian(actuator_id.at(index), get_position[index]); - - all_actuator.push_back(actuator); - } - - return all_actuator; -} - - -/***************************************************************************** -** Tool Dynamixel Control Functions -*****************************************************************************/ -void GripperDynamixel::init(uint8_t actuator_id, const void *arg) -{ - STRING *get_arg_ = (STRING *)arg; - - bool result = GripperDynamixel::initialize(actuator_id ,get_arg_[0], get_arg_[1]); - - if (result == false) - return; -} - -void GripperDynamixel::setMode(const void *arg) -{ - bool result = false; -// const char* log = NULL; - - STRING *get_arg_ = (STRING *)arg; - - if (get_arg_[0] == "position_mode" || get_arg_[0] == "current_based_position_mode") - { - result = GripperDynamixel::setOperatingMode(get_arg_[0]); - if (result == false) - return; - } - else - { - result = GripperDynamixel::writeProfileValue(get_arg_[0], std::atoi(get_arg_[1].c_str())); - if (result == false) - return; - } - - result = GripperDynamixel::setSDKHandler(); - if (result == false) - return; -} - -uint8_t GripperDynamixel::getId() -{ - return dynamixel_.id.at(0); -} - -void GripperDynamixel::enable() -{ - const char* log = NULL; - bool result = false; - - result = dynamixel_workbench_->torqueOn(dynamixel_.id.at(0), &log); - if (result == false) - { - log::error(log); - } - enabled_state_ = true; -} - -void GripperDynamixel::disable() -{ - const char* log = NULL; - bool result = false; - - result = dynamixel_workbench_->torqueOff(dynamixel_.id.at(0), &log); - if (result == false) - { - log::error(log); - } - enabled_state_ = false; -} - -bool GripperDynamixel::sendToolActuatorValue(robotis_manipulator::ActuatorValue value) -{ - return GripperDynamixel::writeGoalPosition(value.position); -} - -robotis_manipulator::ActuatorValue GripperDynamixel::receiveToolActuatorValue() -{ - robotis_manipulator::ActuatorValue result; - result.position = GripperDynamixel::receiveDynamixelValue(); - result.velocity = 0.0; - result.acceleration = 0.0; - result.effort = 0.0; - return result; -} - - -/***************************************************************************** -** Functions called in Tool Dynamixel Profile Control Functions -*****************************************************************************/ -bool GripperDynamixel::initialize(uint8_t actuator_id, STRING dxl_device_name, STRING dxl_baud_rate) -{ - const char* log = NULL; - bool result = false; - - STRING return_delay_time_st = "Return_Delay_Time"; - const char * return_delay_time_char = return_delay_time_st.c_str(); - - dynamixel_.id.push_back(actuator_id); - dynamixel_.num = 1; - - dynamixel_workbench_ = new DynamixelWorkbench; - - result = dynamixel_workbench_->init(dxl_device_name.c_str(), std::atoi(dxl_baud_rate.c_str()), &log); - if (result == false) - { - log::error(log); - } - - uint16_t get_model_number; - result = dynamixel_workbench_->ping(dynamixel_.id.at(0), &get_model_number, &log); - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel ID"); - } - else - { - char str[100]; - sprintf(str, "Gripper Dynamixel ID : %d, Model Name :", dynamixel_.id.at(0)); - strcat(str, dynamixel_workbench_->getModelName(dynamixel_.id.at(0))); - log::println(str); - - result = dynamixel_workbench_->setVelocityBasedProfile(dynamixel_.id.at(0), &log); - if(result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version (v38~)"); - } - - result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), return_delay_time_char, 0, &log); - if (result == false) - { - log::error(log); - log::error("Please check your Dynamixel firmware version"); - } - } - - return true; -} - -bool GripperDynamixel::setOperatingMode(STRING dynamixel_mode) -{ - const char* log = NULL; - bool result = false; - - const uint32_t velocity = 0; - const uint32_t acceleration = 0; - const uint32_t current = 200; - - if (dynamixel_mode == "position_mode") - { - result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - else if (dynamixel_mode == "current_based_position_mode") - { - result = dynamixel_workbench_->currentBasedPositionMode(dynamixel_.id.at(0), current, &log); - if (result == false) - { - log::error(log); - } - } - else - { - result = dynamixel_workbench_->jointMode(dynamixel_.id.at(0), velocity, acceleration, &log); - if (result == false) - { - log::error(log); - } - } - - return true; -} - -bool GripperDynamixel::writeProfileValue(STRING profile_mode, uint32_t value) -{ - const char* log = NULL; - bool result = false; - - const char * char_profile_mode = profile_mode.c_str(); - - result = dynamixel_workbench_->writeRegister(dynamixel_.id.at(0), char_profile_mode, value, &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -bool GripperDynamixel::setSDKHandler() -{ - bool result = false; - const char* log = NULL; - - result = dynamixel_workbench_->addSyncWriteHandler(dynamixel_.id.at(0), "Goal_Position", &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->addSyncReadHandler(dynamixel_.id.at(0), - "Present_Position", - &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -bool GripperDynamixel::writeGoalPosition(double radian) -{ - bool result = false; - const char* log = NULL; - - int32_t goal_position = 0; - - goal_position = dynamixel_workbench_->convertRadian2Value(dynamixel_.id.at(0), radian); - - result = dynamixel_workbench_->syncWrite(SYNC_WRITE_HANDLER, &goal_position, &log); - if (result == false) - { - log::error(log); - } - - return true; -} - -double GripperDynamixel::receiveDynamixelValue() -{ - bool result = false; - const char* log = NULL; - - int32_t get_value = 0; - uint8_t id_array[1] = {dynamixel_.id.at(0)}; - - result = dynamixel_workbench_->syncRead(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - (uint8_t)1, - &log); - if (result == false) - { - log::error(log); - } - - result = dynamixel_workbench_->getSyncReadData(SYNC_READ_HANDLER_FOR_PRESENT_POSITION_VELOCITY_CURRENT, - id_array, - (uint8_t)1, - &get_value, - &log); - if (result == false) - { - log::error(log); - } - - return dynamixel_workbench_->convertValue2Radian(dynamixel_.id.at(0), get_value); -} diff --git a/open_manipulator_libs/src/kinematics.cpp b/open_manipulator_libs/src/kinematics.cpp deleted file mode 100644 index f77281cd..00000000 --- a/open_manipulator_libs/src/kinematics.cpp +++ /dev/null @@ -1,1041 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "../include/open_manipulator_libs/kinematics.h" - -using namespace robotis_manipulator; -using namespace kinematics; - - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Jacobian -*****************************************************************************/ -void SolverUsingCRAndJacobian::setOption(const void *arg){} - -Eigen::MatrixXd SolverUsingCRAndJacobian::jacobian(Manipulator *manipulator, Name tool_name) -{ - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, manipulator->getDOF()); - - Eigen::Vector3d joint_axis = Eigen::Vector3d::Zero(3); - - Eigen::Vector3d position_changed = Eigen::Vector3d::Zero(3); - Eigen::Vector3d orientation_changed = Eigen::Vector3d::Zero(3); - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - - ////////////////////////////////////////////////////////////////////////////////// - - int8_t index = 0; - Name my_name = manipulator->getWorldChildName(); - - for (int8_t size = 0; size < manipulator->getDOF(); size++) - { - Name parent_name = manipulator->getComponentParentName(my_name); - if (parent_name == manipulator->getWorldName()) - { - joint_axis = manipulator->getWorldOrientation() * manipulator->getAxis(my_name); - } - else - { - joint_axis = manipulator->getComponentOrientationFromWorld(parent_name) * manipulator->getAxis(my_name); - } - - position_changed = math::skewSymmetricMatrix(joint_axis) * - (manipulator->getComponentPositionFromWorld(tool_name) - manipulator->getComponentPositionFromWorld(my_name)); - orientation_changed = joint_axis; - - pose_changed << position_changed(0), - position_changed(1), - position_changed(2), - orientation_changed(0), - orientation_changed(1), - orientation_changed(2); - - jacobian.col(index) = pose_changed; - index++; - my_name = manipulator->getComponentChildName(my_name).at(0); // Get Child name which has active joint - } - return jacobian; -} - -void SolverUsingCRAndJacobian::solveForwardKinematics(Manipulator *manipulator) -{ - forwardSolverUsingChainRule(manipulator, manipulator->getWorldChildName()); -} - -bool SolverUsingCRAndJacobian::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - return inverseSolverUsingJacobian(manipulator, tool_name, target_pose, goal_joint_value); -} - - -//private -void SolverUsingCRAndJacobian::forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name) -{ - Name my_name = component_name; - Name parent_name = manipulator->getComponentParentName(my_name); - int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); - - Pose parent_pose_value; - Pose my_pose_value; - - //Get Parent Pose - if (parent_name == manipulator->getWorldName()) - { - parent_pose_value = manipulator->getWorldPose(); - } - else - { - parent_pose_value = manipulator->getComponentPoseFromWorld(parent_name); - } - - //position - my_pose_value.kinematic.position = parent_pose_value.kinematic.position - + (parent_pose_value.kinematic.orientation * manipulator->getComponentRelativePositionFromParent(my_name)); - //orientation - my_pose_value.kinematic.orientation = parent_pose_value.kinematic.orientation * manipulator->getComponentRelativeOrientationFromParent(my_name) * math::rodriguesRotationMatrix(manipulator->getAxis(my_name), manipulator->getJointPosition(my_name)); - //linear velocity - my_pose_value.dynamic.linear.velocity = math::vector3(0.0, 0.0, 0.0); - //angular velocity - my_pose_value.dynamic.angular.velocity = math::vector3(0.0, 0.0, 0.0); - //linear acceleration - my_pose_value.dynamic.linear.acceleration = math::vector3(0.0, 0.0, 0.0); - //angular acceleration - my_pose_value.dynamic.angular.acceleration = math::vector3(0.0, 0.0, 0.0); - - manipulator->setComponentPoseFromWorld(my_name, my_pose_value); - - for (int8_t index = 0; index < number_of_child; index++) - { - Name child_name = manipulator->getComponentChildName(my_name).at(index); - forwardSolverUsingChainRule(manipulator, child_name); - } -} - -bool SolverUsingCRAndJacobian::inverseSolverUsingJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - const double lambda = 0.7; - const int8_t iteration = 10; - - Manipulator _manipulator = *manipulator; - - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, _manipulator.getDOF()); - - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - Eigen::VectorXd delta_angle = Eigen::VectorXd::Zero(_manipulator.getDOF()); - - for (int8_t count = 0; count < iteration; count++) - { - //Forward kinematics solve - solveForwardKinematics(&_manipulator); - //Get jacobian - jacobian = this->jacobian(&_manipulator, tool_name); - - //Pose Difference - pose_changed = math::poseDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name), - target_pose.kinematic.orientation, _manipulator.getComponentOrientationFromWorld(tool_name)); - - //pose sovler success - if (pose_changed.norm() < 1E-6) - { - *goal_joint_value = _manipulator.getAllActiveJointValue(); - for(int8_t index = 0; index < _manipulator.getDOF(); index++) - { - goal_joint_value->at(index).velocity = 0.0; - goal_joint_value->at(index).acceleration = 0.0; - goal_joint_value->at(index).effort = 0.0; - } - return true; - } - - //get delta angle - ColPivHouseholderQR dec(jacobian); - delta_angle = lambda * dec.solve(pose_changed); - - //set changed angle - std::vector changed_angle; - for(int8_t index = 0; index < _manipulator.getDOF(); index++) - changed_angle.push_back(_manipulator.getAllActiveJointPosition().at(index) + delta_angle(index)); - _manipulator.setAllActiveJointPosition(changed_angle); - } - *goal_joint_value = {}; - return false; -} - - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Singularity Robust Jacobian -*****************************************************************************/ -void SolverUsingCRAndSRJacobian::setOption(const void *arg){} - -Eigen::MatrixXd SolverUsingCRAndSRJacobian::jacobian(Manipulator *manipulator, Name tool_name) -{ - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, manipulator->getDOF()); - - Eigen::Vector3d joint_axis = Eigen::Vector3d::Zero(3); - - Eigen::Vector3d position_changed = Eigen::Vector3d::Zero(3); - Eigen::Vector3d orientation_changed = Eigen::Vector3d::Zero(3); - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - - ////////////////////////////////////////////////////////////////////////////////// - - int8_t index = 0; - Name my_name = manipulator->getWorldChildName(); - - for (int8_t size = 0; size < manipulator->getDOF(); size++) - { - Name parent_name = manipulator->getComponentParentName(my_name); - if (parent_name == manipulator->getWorldName()) - { - joint_axis = manipulator->getWorldOrientation() * manipulator->getAxis(my_name); - } - else - { - joint_axis = manipulator->getComponentOrientationFromWorld(parent_name) * manipulator->getAxis(my_name); - } - - position_changed = math::skewSymmetricMatrix(joint_axis) * - (manipulator->getComponentPositionFromWorld(tool_name) - manipulator->getComponentPositionFromWorld(my_name)); - orientation_changed = joint_axis; - - pose_changed << position_changed(0), - position_changed(1), - position_changed(2), - orientation_changed(0), - orientation_changed(1), - orientation_changed(2); - - jacobian.col(index) = pose_changed; - index++; - my_name = manipulator->getComponentChildName(my_name).at(0); // Get Child name which has active joint - } - return jacobian; -} - -void SolverUsingCRAndSRJacobian::solveForwardKinematics(Manipulator *manipulator) -{ - forwardSolverUsingChainRule(manipulator, manipulator->getWorldChildName()); -} - -bool SolverUsingCRAndSRJacobian::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - return inverseSolverUsingSRJacobian(manipulator, tool_name, target_pose, goal_joint_value); -} - - -//private -void SolverUsingCRAndSRJacobian::forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name) -{ - Name my_name = component_name; - Name parent_name = manipulator->getComponentParentName(my_name); - int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); - - Pose parent_pose_value; - Pose my_pose_value; - - //Get Parent Pose - if (parent_name == manipulator->getWorldName()) - { - parent_pose_value = manipulator->getWorldPose(); - } - else - { - parent_pose_value = manipulator->getComponentPoseFromWorld(parent_name); - } - - //position - my_pose_value.kinematic.position = parent_pose_value.kinematic.position - + (parent_pose_value.kinematic.orientation * manipulator->getComponentRelativePositionFromParent(my_name)); - //orientation - my_pose_value.kinematic.orientation = parent_pose_value.kinematic.orientation * manipulator->getComponentRelativeOrientationFromParent(my_name) * math::rodriguesRotationMatrix(manipulator->getAxis(my_name), manipulator->getJointPosition(my_name)); - //linear velocity - my_pose_value.dynamic.linear.velocity = math::vector3(0.0, 0.0, 0.0); - //angular velocity - my_pose_value.dynamic.angular.velocity = math::vector3(0.0, 0.0, 0.0); - //linear acceleration - my_pose_value.dynamic.linear.acceleration = math::vector3(0.0, 0.0, 0.0); - //angular acceleration - my_pose_value.dynamic.angular.acceleration = math::vector3(0.0, 0.0, 0.0); - - manipulator->setComponentPoseFromWorld(my_name, my_pose_value); - - for (int8_t index = 0; index < number_of_child; index++) - { - Name child_name = manipulator->getComponentChildName(my_name).at(index); - forwardSolverUsingChainRule(manipulator, child_name); - } -} - -bool SolverUsingCRAndSRJacobian::inverseSolverUsingSRJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - //manipulator - Manipulator _manipulator = *manipulator; - - //solver parameter - double lambda = 0.0; - const double param = 0.002; - const int8_t iteration = 10; - - const double gamma = 0.5; //rollback delta - - //sr sovler parameter - double wn_pos = 1 / 0.3; - double wn_ang = 1 / (2 * M_PI); - double pre_Ek = 0.0; - double new_Ek = 0.0; - - Eigen::MatrixXd We(6, 6); - We << wn_pos, 0, 0, 0, 0, 0, - 0, wn_pos, 0, 0, 0, 0, - 0, 0, wn_pos, 0, 0, 0, - 0, 0, 0, wn_ang, 0, 0, - 0, 0, 0, 0, wn_ang, 0, - 0, 0, 0, 0, 0, wn_ang; - - Eigen::MatrixXd Wn = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //jacobian - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, _manipulator.getDOF()); - Eigen::MatrixXd sr_jacobian = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //delta parameter - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - Eigen::VectorXd angle_changed = Eigen::VectorXd::Zero(_manipulator.getDOF()); //delta angle (dq) - Eigen::VectorXd gerr(_manipulator.getDOF()); - - //angle parameter - std::vector present_angle; //angle (q) - std::vector set_angle; //set angle (q + dq) - - ////////////////////////////solving////////////////////////////////// - - solveForwardKinematics(&_manipulator); - //////////////checking dx/////////////// - pose_changed = math::poseDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name), target_pose.kinematic.orientation, _manipulator.getComponentOrientationFromWorld(tool_name)); - pre_Ek = pose_changed.transpose() * We * pose_changed; - /////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - Eigen::Vector3d target_orientation_rpy = math::convertRotationToRPY(target_pose.orientation); - Eigen::VectorXd debug_target_pose(6); - for(int t=0; t<3; t++) - debug_target_pose(t) = target_pose.position(t); - for(int t=0; t<3; t++) - debug_target_pose(t+3) = target_orientation_rpy(t); - - Eigen::Vector3d present_position = _manipulator.getComponentPositionFromWorld(tool_name); - Eigen::MatrixXd present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - Eigen::Vector3d present_orientation_rpy = math::convertRotationToRPY(present_orientation); - Eigen::VectorXd debug_present_pose(6); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - - log::println("------------------------------------"); - log::warn("iter : first"); - log::warn("Ek : ", pre_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - //////////////////////////solving loop/////////////////////////////// - for (int8_t count = 0; count < iteration; count++) - { - //////////solve using jacobian////////// - jacobian = this->jacobian(&_manipulator, tool_name); - lambda = pre_Ek + param; - - sr_jacobian = (jacobian.transpose() * We * jacobian) + (lambda * Wn); //calculate sr_jacobian (J^T*we*J + lamda*Wn) - gerr = jacobian.transpose() * We * pose_changed; //calculate gerr (J^T*we) dx - - ColPivHouseholderQR dec(sr_jacobian); //solving (get dq) - angle_changed = dec.solve(gerr); //(J^T*we) * dx = (J^T*we*J + lamda*Wn) * dq - - present_angle = _manipulator.getAllActiveJointPosition(); - set_angle.clear(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(present_angle.at(index) + angle_changed(index)); - _manipulator.setAllActiveJointPosition(set_angle); - solveForwardKinematics(&_manipulator); - //////////////////////////////////////// - - //////////////checking dx/////////////// - pose_changed = math::poseDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name), target_pose.kinematic.orientation, _manipulator.getComponentOrientationFromWorld(tool_name)); - new_Ek = pose_changed.transpose() * We * pose_changed; - //////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - present_position = _manipulator.getComponentPositionFromWorld(tool_name); - present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - present_orientation_rpy = math::convertRotationToRPY(present_orientation); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - if (new_Ek < 1E-12) - { - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::error("Success"); - log::println("------------------------------------"); - #endif - //////////////////////////debug////////////////////////////////// - *goal_joint_value = _manipulator.getAllActiveJointValue(); - for(int8_t index = 0; index < _manipulator.getDOF(); index++) - { - goal_joint_value->at(index).velocity = 0.0; - goal_joint_value->at(index).acceleration = 0.0; - goal_joint_value->at(index).effort = 0.0; - } - return true; - } - else if (new_Ek < pre_Ek) - { - pre_Ek = new_Ek; - } - else - { - present_angle = _manipulator.getAllActiveJointPosition(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(present_angle.at(index) - (gamma * angle_changed(index))); - _manipulator.setAllActiveJointPosition(set_angle); - - solveForwardKinematics(&_manipulator); - } - } - log::error("[sr]fail to solve inverse kinematics (please change the solver)"); - *goal_joint_value = {}; - return false; -} - - -/***************************************************************************** -** Kinematics Solver Using Chain Rule and Singularity Robust Position Only Jacobian -*****************************************************************************/ -void SolverUsingCRAndSRPositionOnlyJacobian::setOption(const void *arg){} - -Eigen::MatrixXd SolverUsingCRAndSRPositionOnlyJacobian::jacobian(Manipulator *manipulator, Name tool_name) -{ - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, manipulator->getDOF()); - - Eigen::Vector3d joint_axis = Eigen::Vector3d::Zero(3); - - Eigen::Vector3d position_changed = Eigen::Vector3d::Zero(3); - Eigen::Vector3d orientation_changed = Eigen::Vector3d::Zero(3); - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - - ////////////////////////////////////////////////////////////////////////////////// - - int8_t index = 0; - Name my_name = manipulator->getWorldChildName(); - - for (int8_t size = 0; size < manipulator->getDOF(); size++) - { - Name parent_name = manipulator->getComponentParentName(my_name); - if (parent_name == manipulator->getWorldName()) - { - joint_axis = manipulator->getWorldOrientation() * manipulator->getAxis(my_name); - } - else - { - joint_axis = manipulator->getComponentOrientationFromWorld(parent_name) * manipulator->getAxis(my_name); - } - - position_changed = math::skewSymmetricMatrix(joint_axis) * - (manipulator->getComponentPositionFromWorld(tool_name) - manipulator->getComponentPositionFromWorld(my_name)); - orientation_changed = joint_axis; - - pose_changed << position_changed(0), - position_changed(1), - position_changed(2), - orientation_changed(0), - orientation_changed(1), - orientation_changed(2); - - jacobian.col(index) = pose_changed; - index++; - my_name = manipulator->getComponentChildName(my_name).at(0); // Get Child name which has active joint - } - return jacobian; -} - -void SolverUsingCRAndSRPositionOnlyJacobian::solveForwardKinematics(Manipulator *manipulator) -{ - forwardSolverUsingChainRule(manipulator, manipulator->getWorldChildName()); -} - -bool SolverUsingCRAndSRPositionOnlyJacobian::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - return inverseSolverUsingPositionOnlySRJacobian(manipulator, tool_name, target_pose, goal_joint_value); -} - - -//private -void SolverUsingCRAndSRPositionOnlyJacobian::forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name) -{ - Name my_name = component_name; - Name parent_name = manipulator->getComponentParentName(my_name); - int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); - - Pose parent_pose_value; - Pose my_pose_value; - - //Get Parent Pose - if (parent_name == manipulator->getWorldName()) - { - parent_pose_value = manipulator->getWorldPose(); - } - else - { - parent_pose_value = manipulator->getComponentPoseFromWorld(parent_name); - } - - //position - my_pose_value.kinematic.position = parent_pose_value.kinematic.position - + (parent_pose_value.kinematic.orientation * manipulator->getComponentRelativePositionFromParent(my_name)); - //orientation - my_pose_value.kinematic.orientation = parent_pose_value.kinematic.orientation * manipulator->getComponentRelativeOrientationFromParent(my_name) * math::rodriguesRotationMatrix(manipulator->getAxis(my_name), manipulator->getJointPosition(my_name)); - //linear velocity - my_pose_value.dynamic.linear.velocity = math::vector3(0.0, 0.0, 0.0); - //angular velocity - my_pose_value.dynamic.angular.velocity = math::vector3(0.0, 0.0, 0.0); - //linear acceleration - my_pose_value.dynamic.linear.acceleration = math::vector3(0.0, 0.0, 0.0); - //angular acceleration - my_pose_value.dynamic.angular.acceleration = math::vector3(0.0, 0.0, 0.0); - - manipulator->setComponentPoseFromWorld(my_name, my_pose_value); - - for (int8_t index = 0; index < number_of_child; index++) - { - Name child_name = manipulator->getComponentChildName(my_name).at(index); - forwardSolverUsingChainRule(manipulator, child_name); - } -} - -bool SolverUsingCRAndSRPositionOnlyJacobian::inverseSolverUsingPositionOnlySRJacobian(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - //manipulator - Manipulator _manipulator = *manipulator; - - //solver parameter - double lambda = 0.0; - const double param = 0.002; - const int8_t iteration = 10; - - const double gamma = 0.5; //rollback delta - - //jacobian - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, _manipulator.getDOF()); - Eigen::MatrixXd position_jacobian = Eigen::MatrixXd::Identity(3, _manipulator.getDOF()); - Eigen::MatrixXd sr_jacobian = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //delta parameter - Eigen::Vector3d position_changed = Eigen::VectorXd::Zero(3); - Eigen::VectorXd angle_changed = Eigen::VectorXd::Zero(_manipulator.getDOF()); //delta angle (dq) - Eigen::VectorXd gerr(_manipulator.getDOF()); - - //sr sovler parameter - double wn_pos = 1 / 0.3; - double pre_Ek = 0.0; - double new_Ek = 0.0; - - Eigen::MatrixXd We(3, 3); - We << wn_pos, 0, 0, - 0, wn_pos, 0, - 0, 0, wn_pos; - - Eigen::MatrixXd Wn = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //angle parameter - std::vector present_angle; //angle (q) - std::vector set_angle; //set angle (q + dq) - - ////////////////////////////solving////////////////////////////////// - - solveForwardKinematics(&_manipulator); - //////////////checking dx/////////////// - position_changed = math::positionDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name)); - pre_Ek = position_changed.transpose() * We * position_changed; - /////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - Eigen::Vector3d target_orientation_rpy = math::convertRotationToRPY(target_pose.orientation); - Eigen::VectorXd debug_target_pose(6); - for(int t=0; t<3; t++) - debug_target_pose(t) = target_pose.position(t); - for(int t=0; t<3; t++) - debug_target_pose(t+3) = target_orientation_rpy(t); - - Eigen::Vector3d present_position = _manipulator.getComponentPositionFromWorld(tool_name); - Eigen::MatrixXd present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - Eigen::Vector3d present_orientation_rpy = math::convertRotationToRPY(present_orientation); - Eigen::VectorXd debug_present_pose(6); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - - log::println("------------------------------------"); - log::warn("iter : first"); - log::warn("Ek : ", pre_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - //////////////////////////solving loop/////////////////////////////// - for (int8_t count = 0; count < iteration; count++) - { - //////////solve using jacobian////////// - jacobian = this->jacobian(&_manipulator, tool_name); - position_jacobian.row(0) = jacobian.row(0); - position_jacobian.row(1) = jacobian.row(1); - position_jacobian.row(2) = jacobian.row(2); - lambda = pre_Ek + param; - - sr_jacobian = (position_jacobian.transpose() * We * position_jacobian) + (lambda * Wn); //calculate sr_jacobian (J^T*we*J + lamda*Wn) - gerr = position_jacobian.transpose() * We * position_changed; //calculate gerr (J^T*we) dx - - ColPivHouseholderQR dec(sr_jacobian); //solving (get dq) - angle_changed = dec.solve(gerr); //(J^T*we) * dx = (J^T*we*J + lamda*Wn) * dq - - present_angle = _manipulator.getAllActiveJointPosition(); - set_angle.clear(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(_manipulator.getAllActiveJointPosition().at(index) + angle_changed(index)); - _manipulator.setAllActiveJointPosition(set_angle); - solveForwardKinematics(&_manipulator); - //////////////////////////////////////// - - //////////////checking dx/////////////// - position_changed = math::positionDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name)); - new_Ek = position_changed.transpose() * We * position_changed; - //////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - present_position = _manipulator.getComponentPositionFromWorld(tool_name); - present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - present_orientation_rpy = math::convertRotationToRPY(present_orientation); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - if (new_Ek < 1E-12) - { - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::error("IK Success"); - log::println("------------------------------------"); - #endif - //////////////////////////debug////////////////////////////////// - *goal_joint_value = _manipulator.getAllActiveJointValue(); - for(int8_t index = 0; index < _manipulator.getDOF(); index++) - { - goal_joint_value->at(index).velocity = 0.0; - goal_joint_value->at(index).acceleration = 0.0; - goal_joint_value->at(index).effort = 0.0; - } - return true; - } - else if (new_Ek < pre_Ek) - { - pre_Ek = new_Ek; - } - else - { - present_angle = _manipulator.getAllActiveJointPosition(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(_manipulator.getAllActiveJointPosition().at(index) - (gamma * angle_changed(index))); - _manipulator.setAllActiveJointPosition(set_angle); - - solveForwardKinematics(&_manipulator); - } - } - log::error("[position_only]fail to solve inverse kinematics (please change the solver)"); - *goal_joint_value = {}; - return false; -} - - -/***************************************************************************** -** Kinematics Solver Customized for OpenManipulator Chain -*****************************************************************************/ -void SolverCustomizedforOMChain::setOption(const void *arg){} - -Eigen::MatrixXd SolverCustomizedforOMChain::jacobian(Manipulator *manipulator, Name tool_name) -{ - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, manipulator->getDOF()); - - Eigen::Vector3d joint_axis = Eigen::Vector3d::Zero(3); - - Eigen::Vector3d position_changed = Eigen::Vector3d::Zero(3); - Eigen::Vector3d orientation_changed = Eigen::Vector3d::Zero(3); - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - - ////////////////////////////////////////////////////////////////////////////////// - - int8_t index = 0; - Name my_name = manipulator->getWorldChildName(); - - for (int8_t size = 0; size < manipulator->getDOF(); size++) - { - Name parent_name = manipulator->getComponentParentName(my_name); - if (parent_name == manipulator->getWorldName()) - { - joint_axis = manipulator->getWorldOrientation() * manipulator->getAxis(my_name); - } - else - { - joint_axis = manipulator->getComponentOrientationFromWorld(parent_name) * manipulator->getAxis(my_name); - } - - position_changed = math::skewSymmetricMatrix(joint_axis) * - (manipulator->getComponentPositionFromWorld(tool_name) - manipulator->getComponentPositionFromWorld(my_name)); - orientation_changed = joint_axis; - - pose_changed << position_changed(0), - position_changed(1), - position_changed(2), - orientation_changed(0), - orientation_changed(1), - orientation_changed(2); - - jacobian.col(index) = pose_changed; - index++; - my_name = manipulator->getComponentChildName(my_name).at(0); // Get Child name which has active joint - } - return jacobian; -} - -void SolverCustomizedforOMChain::solveForwardKinematics(Manipulator *manipulator) -{ - forwardSolverUsingChainRule(manipulator, manipulator->getWorldChildName()); -} - -bool SolverCustomizedforOMChain::solveInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - return chainCustomInverseKinematics(manipulator, tool_name, target_pose, goal_joint_value); -} - - -//private -void SolverCustomizedforOMChain::forwardSolverUsingChainRule(Manipulator *manipulator, Name component_name) -{ - Name my_name = component_name; - Name parent_name = manipulator->getComponentParentName(my_name); - int8_t number_of_child = manipulator->getComponentChildName(my_name).size(); - - Pose parent_pose_value; - Pose my_pose_value; - - //Get Parent Pose - if (parent_name == manipulator->getWorldName()) - { - parent_pose_value = manipulator->getWorldPose(); - } - else - { - parent_pose_value = manipulator->getComponentPoseFromWorld(parent_name); - } - - //position - my_pose_value.kinematic.position = parent_pose_value.kinematic.position - + (parent_pose_value.kinematic.orientation * manipulator->getComponentRelativePositionFromParent(my_name)); - //orientation - my_pose_value.kinematic.orientation = parent_pose_value.kinematic.orientation * manipulator->getComponentRelativeOrientationFromParent(my_name) * math::rodriguesRotationMatrix(manipulator->getAxis(my_name), manipulator->getJointPosition(my_name)); - //linear velocity - my_pose_value.dynamic.linear.velocity = math::vector3(0.0, 0.0, 0.0); - //angular velocity - my_pose_value.dynamic.angular.velocity = math::vector3(0.0, 0.0, 0.0); - //linear acceleration - my_pose_value.dynamic.linear.acceleration = math::vector3(0.0, 0.0, 0.0); - //angular acceleration - my_pose_value.dynamic.angular.acceleration = math::vector3(0.0, 0.0, 0.0); - - manipulator->setComponentPoseFromWorld(my_name, my_pose_value); - - for (int8_t index = 0; index < number_of_child; index++) - { - Name child_name = manipulator->getComponentChildName(my_name).at(index); - forwardSolverUsingChainRule(manipulator, child_name); - } -} -bool SolverCustomizedforOMChain::chainCustomInverseKinematics(Manipulator *manipulator, Name tool_name, Pose target_pose, std::vector *goal_joint_value) -{ - //manipulator - Manipulator _manipulator = *manipulator; - - //solver parameter - double lambda = 0.0; - const double param = 0.002; - const int8_t iteration = 10; - - const double gamma = 0.5; //rollback delta - - //sr sovler parameter - double wn_pos = 1 / 0.3; - double wn_ang = 1 / (2 * M_PI); - double pre_Ek = 0.0; - double new_Ek = 0.0; - - Eigen::MatrixXd We(6, 6); - We << wn_pos, 0, 0, 0, 0, 0, - 0, wn_pos, 0, 0, 0, 0, - 0, 0, wn_pos, 0, 0, 0, - 0, 0, 0, wn_ang, 0, 0, - 0, 0, 0, 0, wn_ang, 0, - 0, 0, 0, 0, 0, wn_ang; - - Eigen::MatrixXd Wn = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //jacobian - Eigen::MatrixXd jacobian = Eigen::MatrixXd::Identity(6, _manipulator.getDOF()); - Eigen::MatrixXd sr_jacobian = Eigen::MatrixXd::Identity(_manipulator.getDOF(), _manipulator.getDOF()); - - //delta parameter - Eigen::VectorXd pose_changed = Eigen::VectorXd::Zero(6); - Eigen::VectorXd angle_changed = Eigen::VectorXd::Zero(_manipulator.getDOF()); //delta angle (dq) - Eigen::VectorXd gerr(_manipulator.getDOF()); - - //angle parameter - std::vector present_angle; //angle (q) - std::vector set_angle; //set angle (q + dq) - - ////////////////////////////solving////////////////////////////////// - - solveForwardKinematics(&_manipulator); - - //////////////make target ori////////// //only OpenManipulator Chain - Eigen::Matrix3d present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - Eigen::Vector3d present_orientation_rpy = math::convertRotationMatrixToRPYVector(present_orientation); - Eigen::Matrix3d target_orientation = target_pose.kinematic.orientation; - Eigen::Vector3d target_orientation_rpy = math::convertRotationMatrixToRPYVector(target_orientation); - - Eigen::Vector3d joint1_rlative_position = _manipulator.getComponentRelativePositionFromParent(_manipulator.getWorldChildName()); - Eigen::Vector3d target_position_from_joint1 = target_pose.kinematic.position - joint1_rlative_position; - - target_orientation_rpy(0) = present_orientation_rpy(0); - target_orientation_rpy(1) = target_orientation_rpy(1); - target_orientation_rpy(2) = atan2(target_position_from_joint1(1) ,target_position_from_joint1(0)); - - target_pose.kinematic.orientation = math::convertRPYToRotationMatrix(target_orientation_rpy(0), target_orientation_rpy(1), target_orientation_rpy(2)); - /////////////////////////////////////// - - //////////////checking dx/////////////// - pose_changed = math::poseDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name), target_pose.kinematic.orientation, _manipulator.getComponentOrientationFromWorld(tool_name)); - pre_Ek = pose_changed.transpose() * We * pose_changed; - /////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - Eigen::VectorXd debug_target_pose(6); - for(int t=0; t<3; t++) - debug_target_pose(t) = target_pose.position(t); - for(int t=0; t<3; t++) - debug_target_pose(t+3) = target_orientation_rpy(t); - - Eigen::Vector3d present_position = _manipulator.getComponentPositionFromWorld(tool_name); - Eigen::VectorXd debug_present_pose(6); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - - log::println("------------------------------------"); - log::warn("iter : first"); - log::warn("Ek : ", pre_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - //////////////////////////solving loop/////////////////////////////// - for (int8_t count = 0; count < iteration; count++) - { - //////////solve using jacobian////////// - jacobian = this->jacobian(&_manipulator, tool_name); - lambda = pre_Ek + param; - - sr_jacobian = (jacobian.transpose() * We * jacobian) + (lambda * Wn); //calculate sr_jacobian (J^T*we*J + lamda*Wn) - gerr = jacobian.transpose() * We * pose_changed; //calculate gerr (J^T*we) dx - - ColPivHouseholderQR dec(sr_jacobian); //solving (get dq) - angle_changed = dec.solve(gerr); //(J^T*we) * dx = (J^T*we*J + lamda*Wn) * dq - - present_angle = _manipulator.getAllActiveJointPosition(); - set_angle.clear(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(present_angle.at(index) + angle_changed(index)); - _manipulator.setAllActiveJointPosition(set_angle); - solveForwardKinematics(&_manipulator); - //////////////////////////////////////// - - //////////////checking dx/////////////// - pose_changed = math::poseDifference(target_pose.kinematic.position, _manipulator.getComponentPositionFromWorld(tool_name), target_pose.kinematic.orientation, _manipulator.getComponentOrientationFromWorld(tool_name)); - new_Ek = pose_changed.transpose() * We * pose_changed; - //////////////////////////////////////// - - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - present_position = _manipulator.getComponentPositionFromWorld(tool_name); - present_orientation = _manipulator.getComponentOrientationFromWorld(tool_name); - present_orientation_rpy = math::convertRotationToRPY(present_orientation); - for(int t=0; t<3; t++) - debug_present_pose(t) = present_position(t); - for(int t=0; t<3; t++) - debug_present_pose(t+3) = present_orientation_rpy(t); - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::println("tar_pose"); - log::println_VECTOR(debug_target_pose,16); - log::println("pre_pose"); - log::println_VECTOR(debug_present_pose,16); - log::println("delta_pose"); - log::println_VECTOR(debug_target_pose-debug_present_pose,16); - #endif - ////////////////////////////debug////////////////////////////////// - - if (new_Ek < 1E-12) - { - /////////////////////////////debug///////////////////////////////// - #if defined(KINEMATICS_DEBUG) - log::warn("iter : ", count,0); - log::warn("Ek : ", new_Ek*1000000000000); - log::error("Success"); - log::println("------------------------------------"); - #endif - //////////////////////////debug////////////////////////////////// - *goal_joint_value = _manipulator.getAllActiveJointValue(); - for(int8_t index = 0; index < _manipulator.getDOF(); index++) - { - goal_joint_value->at(index).velocity = 0.0; - goal_joint_value->at(index).acceleration = 0.0; - goal_joint_value->at(index).effort = 0.0; - } - return true; - } - else if (new_Ek < pre_Ek) - { - pre_Ek = new_Ek; - } - else - { - present_angle = _manipulator.getAllActiveJointPosition(); - for (int8_t index = 0; index < _manipulator.getDOF(); index++) - set_angle.push_back(present_angle.at(index) - (gamma * angle_changed(index))); - _manipulator.setAllActiveJointPosition(set_angle); - - solveForwardKinematics(&_manipulator); - } - } - log::error("[OpenManipulator Chain Custom]fail to solve inverse kinematics"); - *goal_joint_value = {}; - return false; -} - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/open_manipulator_libs/src/open_manipulator.cpp b/open_manipulator_libs/src/open_manipulator.cpp deleted file mode 100644 index 8ad323bb..00000000 --- a/open_manipulator_libs/src/open_manipulator.cpp +++ /dev/null @@ -1,217 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "../include/open_manipulator_libs/open_manipulator.h" - -OpenManipulator::OpenManipulator() -{} -OpenManipulator::~OpenManipulator() -{ - delete kinematics_; - delete actuator_; - delete tool_; - for(uint8_t index = 0; index < CUSTOM_TRAJECTORY_SIZE; index++) - delete custom_trajectory_[index]; -} - -void OpenManipulator::initOpenManipulator(bool using_actual_robot_state, STRING usb_port, STRING baud_rate, float control_loop_time) -{ - /***************************************************************************** - ** Initialize Manipulator Parameter - *****************************************************************************/ - addWorld("world", // world name - "joint1"); // child name - - addJoint("joint1", // my name - "world", // parent name - "joint2", // child name - math::vector3(0.012, 0.0, 0.017), // relative position - math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation - Z_AXIS, // axis of rotation - 11, // actuator id - M_PI, // max joint limit (3.14 rad) - -M_PI, // min joint limit (-3.14 rad) - 1.0, // coefficient - 9.8406837e-02, // mass - math::inertiaMatrix(3.4543422e-05, -1.6031095e-08, -3.8375155e-07, - 3.2689329e-05, 2.8511935e-08, - 1.8850320e-05), // inertial tensor - math::vector3(-3.0184870e-04, 5.4043684e-04, 0.018 + 2.9433464e-02) // COM - ); - - addJoint("joint2", // my name - "joint1", // parent name - "joint3", // child name - math::vector3(0.0, 0.0, 0.0595), // relative position - math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation - Y_AXIS, // axis of rotation - 12, // actuator id - M_PI_2, // max joint limit (1.67 rad) - -2.05, // min joint limit (-2.05 rad) - 1.0, // coefficient - 1.3850917e-01, // mass - math::inertiaMatrix(3.3055381e-04, 9.7940978e-08, -3.8505711e-05, - 3.4290447e-04, -1.5717516e-06, - 6.0346498e-05), // inertial tensor - math::vector3(1.0308393e-02, 3.7743363e-04, 1.0170197e-01) // COM - ); - - addJoint("joint3", // my name - "joint2", // parent name - "joint4", // child name - math::vector3(0.024, 0.0, 0.128), // relative position - math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation - Y_AXIS, // axis of rotation - 13, // actuator id - 1.53, // max joint limit (1.53 rad) - -M_PI_2, // min joint limit (-1.67 rad) - 1.0, // coefficient - 1.3274562e-01, // mass - math::inertiaMatrix(3.0654178e-05, -1.2764155e-06, -2.6874417e-07, - 2.4230292e-04, 1.1559550e-08, - 2.5155057e-04), // inertial tensor - math::vector3(9.0909590e-02, 3.8929816e-04, 2.2413279e-04) // COM - ); - - addJoint("joint4", // my name - "joint3", // parent name - "gripper", // child name - math::vector3(0.124, 0.0, 0.0), // relative position - math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation - Y_AXIS, // axis of rotation - 14, // actuator id - 2.0, // max joint limit (2.0 rad) - -1.8, // min joint limit (-1.8 rad) - 1.0, // coefficient - 1.4327573e-01, // mass - math::inertiaMatrix(8.0870749e-05, 0.0, -1.0157896e-06, - 7.5980465e-05, 0.0, - 9.3127351e-05), // inertial tensor - math::vector3(4.4206755e-02, 3.6839985e-07, 8.9142216e-03) // COM - ); - - addTool("gripper", // my name - "joint4", // parent name - math::vector3(0.126, 0.0, 0.0), // relative position - math::convertRPYToRotationMatrix(0.0, 0.0, 0.0), // relative orientation - 15, // actuator id - 0.010, // max gripper limit (0.01 m) - -0.010, // min gripper limit (-0.01 m) - -0.015, // Change unit from `meter` to `radian` - 3.2218127e-02 * 2, // mass - math::inertiaMatrix(9.5568826e-06, 2.8424644e-06, -3.2829197e-10, - 2.2552871e-05, -3.1463634e-10, - 1.7605306e-05), // inertial tensor - math::vector3(0.028 + 8.3720668e-03, 0.0246, -4.2836895e-07) // COM - ); - - /***************************************************************************** - ** Initialize Kinematics - *****************************************************************************/ - kinematics_ = new kinematics::SolverCustomizedforOMChain(); -// kinematics_ = new kinematics::SolverUsingCRAndSRPositionOnlyJacobian(); - addKinematics(kinematics_); - - if(using_actual_robot_state) - { - /***************************************************************************** - ** Initialize Joint Actuator - *****************************************************************************/ - // actuator_ = new dynamixel::JointDynamixel(); - actuator_ = new dynamixel::JointDynamixelProfileControl(control_loop_time); - - // Set communication arguments - STRING dxl_comm_arg[2] = {usb_port, baud_rate}; - void *p_dxl_comm_arg = &dxl_comm_arg; - - // Set joint actuator id - std::vector jointDxlId; - jointDxlId.push_back(11); - jointDxlId.push_back(12); - jointDxlId.push_back(13); - jointDxlId.push_back(14); - addJointActuator(JOINT_DYNAMIXEL, actuator_, jointDxlId, p_dxl_comm_arg); - - // Set joint actuator control mode - STRING joint_dxl_mode_arg = "position_mode"; - void *p_joint_dxl_mode_arg = &joint_dxl_mode_arg; - setJointActuatorMode(JOINT_DYNAMIXEL, jointDxlId, p_joint_dxl_mode_arg); - - - /***************************************************************************** - ** Initialize Tool Actuator - *****************************************************************************/ - tool_ = new dynamixel::GripperDynamixel(); - - uint8_t gripperDxlId = 15; - addToolActuator(TOOL_DYNAMIXEL, tool_, gripperDxlId, p_dxl_comm_arg); - - // Set gripper actuator control mode - STRING gripper_dxl_mode_arg = "current_based_position_mode"; - void *p_gripper_dxl_mode_arg = &gripper_dxl_mode_arg; - setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_mode_arg); - - // Set gripper actuator parameter - STRING gripper_dxl_opt_arg[2]; - void *p_gripper_dxl_opt_arg = &gripper_dxl_opt_arg; - gripper_dxl_opt_arg[0] = "Profile_Acceleration"; - gripper_dxl_opt_arg[1] = "20"; - setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg); - - gripper_dxl_opt_arg[0] = "Profile_Velocity"; - gripper_dxl_opt_arg[1] = "200"; - setToolActuatorMode(TOOL_DYNAMIXEL, p_gripper_dxl_opt_arg); - - // Enable All Actuators - enableAllActuator(); - - // Receive current angles from all actuators - receiveAllJointActuatorValue(); - receiveAllToolActuatorValue(); - } - - /***************************************************************************** - ** Initialize Custom Trajectory - *****************************************************************************/ - custom_trajectory_[0] = new custom_trajectory::Line(); - custom_trajectory_[1] = new custom_trajectory::Circle(); - custom_trajectory_[2] = new custom_trajectory::Rhombus(); - custom_trajectory_[3] = new custom_trajectory::Heart(); - - addCustomTrajectory(CUSTOM_TRAJECTORY_LINE, custom_trajectory_[0]); - addCustomTrajectory(CUSTOM_TRAJECTORY_CIRCLE, custom_trajectory_[1]); - addCustomTrajectory(CUSTOM_TRAJECTORY_RHOMBUS, custom_trajectory_[2]); - addCustomTrajectory(CUSTOM_TRAJECTORY_HEART, custom_trajectory_[3]); -} - -void OpenManipulator::processOpenManipulator(double present_time) -{ - // Planning (ik) - JointWaypoint goal_joint_value = getJointGoalValueFromTrajectory(present_time); - JointWaypoint goal_tool_value = getToolGoalValue(); - - // Control (motor) - receiveAllJointActuatorValue(); - receiveAllToolActuatorValue(); - - if(goal_joint_value.size() != 0) sendAllJointActuatorValue(goal_joint_value); - if(goal_tool_value.size() != 0) sendAllToolActuatorValue(goal_tool_value); - - // Perception (fk) - solveForwardKinematics(); -} diff --git a/open_manipulator_teleop/CHANGELOG.rst b/open_manipulator_teleop/CHANGELOG.rst deleted file mode 100644 index 7e2fa1d9..00000000 --- a/open_manipulator_teleop/CHANGELOG.rst +++ /dev/null @@ -1,19 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_teleop -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.0.2 (2021-06-21) ------------------- -* Noetic support -* Contributors: Will Son - -2.0.1 (2019-02-18) ------------------- -* none - -2.0.0 (2019-02-08) ------------------- -* added new package for teleoperation -* added teleop launch -* updated the CHANGELOG and version to release binary packages -* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Guilherme de Campos Affonso, Pyo diff --git a/open_manipulator_teleop/CMakeLists.txt b/open_manipulator_teleop/CMakeLists.txt deleted file mode 100644 index 1a0aa9ce..00000000 --- a/open_manipulator_teleop/CMakeLists.txt +++ /dev/null @@ -1,72 +0,0 @@ -################################################################################ -# Set minimum required version of cmake, project name and compile options -################################################################################ -cmake_minimum_required(VERSION 3.0.2) -project(open_manipulator_teleop) - -add_compile_options(-std=c++11) - -################################################################################ -# Find catkin packages and libraries for catkin and system dependencies -################################################################################ -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - sensor_msgs - open_manipulator_msgs -) - -################################################################################ -# Setup for python modules and scripts -################################################################################ - -################################################################################ -# Declare ROS messages, services and actions -################################################################################ - -################################################################################ -## Declare ROS dynamic reconfigure parameters -################################################################################ - -################################################################################ -# Declare catkin specific configuration to be passed to dependent projects -################################################################################ -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS roscpp std_msgs sensor_msgs open_manipulator_msgs -) - -################################################################################ -# Build -################################################################################ -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_executable(open_manipulator_teleop_keyboard src/open_manipulator_teleop_keyboard.cpp) -add_dependencies(open_manipulator_teleop_keyboard ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open_manipulator_teleop_keyboard ${catkin_LIBRARIES} ) - -add_executable(open_manipulator_teleop_joystick src/open_manipulator_teleop_joystick.cpp) -add_dependencies(open_manipulator_teleop_joystick ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(open_manipulator_teleop_joystick ${catkin_LIBRARIES} ) - -################################################################################ -# Install -################################################################################ -install(TARGETS open_manipulator_teleop_keyboard open_manipulator_teleop_joystick - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -################################################################################ -# Test -################################################################################ diff --git a/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_joystick.h b/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_joystick.h deleted file mode 100644 index be0314f4..00000000 --- a/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_joystick.h +++ /dev/null @@ -1,85 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef OPEN_MANIPULATOR_TELEOP_JOYSTICK_H_ -#define OPEN_MANIPULATOR_TELEOP_JOYSTICK_H_ - -#include - -#include -#include -#include - -#include "open_manipulator_msgs/SetJointPosition.h" -#include "open_manipulator_msgs/SetKinematicsPose.h" - -#define NUM_OF_JOINT 4 -#define DELTA 0.01 -#define JOINT_DELTA 0.05 -#define PATH_TIME 0.5 - -class OpenManipulatorTeleop -{ - public: - OpenManipulatorTeleop(); - ~OpenManipulatorTeleop(); - - private: - /***************************************************************************** - ** ROS NodeHandle - *****************************************************************************/ - ros::NodeHandle node_handle_; - ros::NodeHandle priv_node_handle_; - - /***************************************************************************** - ** Init Functions - *****************************************************************************/ - void initSubscriber(); - void initClient(); - - /***************************************************************************** - ** Variables - *****************************************************************************/ - std::vector present_joint_angle_; - std::vector present_kinematic_position_; - - /***************************************************************************** - ** ROS Subscribers, Callback Functions and Relevant Functions - *****************************************************************************/ - ros::Subscriber joint_states_sub_; - ros::Subscriber kinematics_pose_sub_; - ros::Subscriber joy_command_sub_; - - void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg); - void joyCallback(const sensor_msgs::Joy::ConstPtr &msg); - - /***************************************************************************** - ** ROS Clients and Callback Functions - *****************************************************************************/ - ros::ServiceClient goal_joint_space_path_client_; - ros::ServiceClient goal_task_space_path_from_present_position_only_client_; - ros::ServiceClient goal_tool_control_client_; - - bool setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time); - bool setTaskSpacePathFromPresentPositionOnly(std::vector kinematics_pose, double path_time); - bool setToolControl(std::vector joint_angle); - void setGoal(const char *str); -}; - -#endif // OPEN_MANIPULATOR_TELEOP_JOYSTICK_H_ diff --git a/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_keyboard.h b/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_keyboard.h deleted file mode 100644 index ef35ca12..00000000 --- a/open_manipulator_teleop/include/open_manipulator_teleop/open_manipulator_teleop_keyboard.h +++ /dev/null @@ -1,97 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#ifndef OPEN_MANIPULATOR_TELEOP_KEYBOARD_H_ -#define OPEN_MANIPULATOR_TELEOP_KEYBOARD_H_ - -#include - -#include -#include - -#include "open_manipulator_msgs/SetJointPosition.h" -#include "open_manipulator_msgs/SetKinematicsPose.h" - -#define NUM_OF_JOINT 4 -#define DELTA 0.01 -#define JOINT_DELTA 0.05 -#define PATH_TIME 0.5 - -class OpenManipulatorTeleop -{ - public: - OpenManipulatorTeleop(); - ~OpenManipulatorTeleop(); - - // update - void printText(); - void setGoal(char ch); - - private: - /***************************************************************************** - ** ROS NodeHandle - *****************************************************************************/ - ros::NodeHandle node_handle_; - ros::NodeHandle priv_node_handle_; - - /***************************************************************************** - ** Variables - *****************************************************************************/ - std::vector present_joint_angle_; - std::vector present_kinematic_position_; - - /***************************************************************************** - ** Init Functions - *****************************************************************************/ - void initSubscriber(); - void initClient(); - - /***************************************************************************** - ** ROS Subscribers, Callback Functions and Relevant Functions - *****************************************************************************/ - ros::Subscriber joint_states_sub_; - ros::Subscriber kinematics_pose_sub_; - - void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); - void kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg); - - /***************************************************************************** - ** ROS Clients and Callback Functions - *****************************************************************************/ - ros::ServiceClient goal_joint_space_path_client_; - ros::ServiceClient goal_joint_space_path_from_present_client_; - ros::ServiceClient goal_task_space_path_from_present_position_only_client_; - ros::ServiceClient goal_tool_control_client_; - - bool setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time); - bool setJointSpacePathFromPresent(std::vector joint_name, std::vector joint_angle, double path_time); - bool setTaskSpacePathFromPresentPositionOnly(std::vector kinematics_pose, double path_time); - bool setToolControl(std::vector joint_angle); - - /***************************************************************************** - ** Others - *****************************************************************************/ - struct termios oldt_; - - void disableWaitingForEnter(void); - void restoreTerminalSettings(void); - std::vector getPresentJointAngle(); - std::vector getPresentKinematicsPose(); -}; - -#endif //OPEN_MANIPULATOR_TELEOP_KEYBOARD_H_ diff --git a/open_manipulator_teleop/launch/open_manipulator_teleop_joystick.launch b/open_manipulator_teleop/launch/open_manipulator_teleop_joystick.launch deleted file mode 100644 index 99c27225..00000000 --- a/open_manipulator_teleop/launch/open_manipulator_teleop_joystick.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/open_manipulator_teleop/launch/open_manipulator_teleop_keyboard.launch b/open_manipulator_teleop/launch/open_manipulator_teleop_keyboard.launch deleted file mode 100644 index 24deb3c9..00000000 --- a/open_manipulator_teleop/launch/open_manipulator_teleop_keyboard.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/open_manipulator_teleop/package.xml b/open_manipulator_teleop/package.xml deleted file mode 100644 index d1200783..00000000 --- a/open_manipulator_teleop/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - open_manipulator_teleop - 2.0.2 - - Provides teleoperation using keyboard for OpenManipulator. - - Apache 2.0 - Darby Lim - Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son - http://wiki.ros.org/open_manipulator_teleop - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues - catkin - roscpp - std_msgs - sensor_msgs - open_manipulator_msgs - diff --git a/open_manipulator_teleop/src/open_manipulator_teleop_joystick.cpp b/open_manipulator_teleop/src/open_manipulator_teleop_joystick.cpp deleted file mode 100644 index 81f89ac6..00000000 --- a/open_manipulator_teleop/src/open_manipulator_teleop_joystick.cpp +++ /dev/null @@ -1,228 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "open_manipulator_teleop/open_manipulator_teleop_joystick.h" - -OpenManipulatorTeleop::OpenManipulatorTeleop() -: node_handle_(""), - priv_node_handle_("~") -{ - /************************************************************ - ** Initialize variables - ************************************************************/ - present_joint_angle_.resize(NUM_OF_JOINT); - present_kinematic_position_.resize(3); - - /************************************************************ - ** Initialize ROS Subscribers and Clients - ************************************************************/ - initSubscriber(); - initClient(); - - ROS_INFO("OpenManipulator teleoperation using joystick start"); -} - -OpenManipulatorTeleop::~OpenManipulatorTeleop() -{ - ROS_INFO("Terminate OpenManipulator Joystick"); - ros::shutdown(); -} - -void OpenManipulatorTeleop::initClient() -{ - goal_joint_space_path_client_ = node_handle_.serviceClient("goal_joint_space_path"); - goal_task_space_path_from_present_position_only_client_ = node_handle_.serviceClient("goal_task_space_path_from_present_position_only"); - goal_tool_control_client_ = node_handle_.serviceClient("goal_tool_control"); -} - -void OpenManipulatorTeleop::initSubscriber() -{ - joint_states_sub_ = node_handle_.subscribe("joint_states", 10, &OpenManipulatorTeleop::jointStatesCallback, this); - kinematics_pose_sub_ = node_handle_.subscribe("kinematics_pose", 10, &OpenManipulatorTeleop::kinematicsPoseCallback, this); - joy_command_sub_ = node_handle_.subscribe("joy", 10, &OpenManipulatorTeleop::joyCallback, this); -} - -void OpenManipulatorTeleop::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - std::vector temp_angle; - temp_angle.resize(NUM_OF_JOINT); - for (std::vector::size_type i = 0; i < msg->name.size(); i ++) - { - if (!msg->name.at(i).compare("joint1")) temp_angle.at(0) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint2")) temp_angle.at(1) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint3")) temp_angle.at(2) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint4")) temp_angle.at(3) = (msg->position.at(i)); - } - present_joint_angle_ = temp_angle; -} - -void OpenManipulatorTeleop::kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg) -{ - present_kinematic_position_ = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}; -} - -void OpenManipulatorTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr &msg) -{ - if (msg->axes.at(1) >= 0.9) setGoal("x+"); - else if (msg->axes.at(1) <= -0.9) setGoal("x-"); - else if (msg->axes.at(0) >= 0.9) setGoal("y+"); - else if (msg->axes.at(0) <= -0.9) setGoal("y-"); - else if (msg->buttons.at(3) == 1) setGoal("z+"); - else if (msg->buttons.at(0) == 1) setGoal("z-"); - else if (msg->buttons.at(5) == 1) setGoal("home"); - else if (msg->buttons.at(4) == 1) setGoal("init"); - - if (msg->buttons.at(2) == 1) setGoal("gripper close"); - else if (msg->buttons.at(1) == 1) setGoal("gripper open"); -} - -bool OpenManipulatorTeleop::setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name = joint_name; - srv.request.joint_position.position = joint_angle; - srv.request.path_time = path_time; - - if (goal_joint_space_path_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool OpenManipulatorTeleop::setToolControl(std::vector joint_angle) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name.push_back(priv_node_handle_.param("end_effector_name", "gripper")); - srv.request.joint_position.position = joint_angle; - - if (goal_tool_control_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool OpenManipulatorTeleop::setTaskSpacePathFromPresentPositionOnly(std::vector kinematics_pose, double path_time) -{ - open_manipulator_msgs::SetKinematicsPose srv; - srv.request.planning_group = priv_node_handle_.param("end_effector_name", "gripper"); - srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0); - srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1); - srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2); - srv.request.path_time = path_time; - - if (goal_task_space_path_from_present_position_only_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -void OpenManipulatorTeleop::setGoal(const char* str) -{ - std::vector goalPose; goalPose.resize(3, 0.0); - std::vector goalJoint; goalJoint.resize(NUM_OF_JOINT, 0.0); - - if (str == "x+") - { - printf("increase(++) x axis in cartesian space\n"); - goalPose.at(0) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "x-") - { - printf("decrease(--) x axis in cartesian space\n"); - goalPose.at(0) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "y+") - { - printf("increase(++) y axis in cartesian space\n"); - goalPose.at(1) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "y-") - { - printf("decrease(--) y axis in cartesian space\n"); - goalPose.at(1) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "z+") - { - printf("increase(++) z axis in cartesian space\n"); - goalPose.at(2) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "z-") - { - printf("decrease(--) z axis in cartesian space\n"); - goalPose.at(2) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (str == "gripper open") - { - printf("open gripper\n"); - std::vector joint_angle; - - joint_angle.push_back(0.01); - setToolControl(joint_angle); - } - else if (str == "gripper close") - { - printf("close gripper\n"); - std::vector joint_angle; - joint_angle.push_back(-0.01); - setToolControl(joint_angle); - } - else if (str == "home") - { - printf("home pose\n"); - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(-1.05); - joint_name.push_back("joint3"); joint_angle.push_back(0.35); - joint_name.push_back("joint4"); joint_angle.push_back(0.70); - setJointSpacePath(joint_name, joint_angle, path_time); - } - else if (str == "init") - { - printf("init pose\n"); - - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(0.0); - joint_name.push_back("joint3"); joint_angle.push_back(0.0); - joint_name.push_back("joint4"); joint_angle.push_back(0.0); - setJointSpacePath(joint_name, joint_angle, path_time); - } -} - -int main(int argc, char **argv) -{ - // Init ROS node - ros::init(argc, argv, "open_manipulator_TELEOP"); - OpenManipulatorTeleop openManipulatorTeleop; - ros::spin(); - return 0; -} diff --git a/open_manipulator_teleop/src/open_manipulator_teleop_keyboard.cpp b/open_manipulator_teleop/src/open_manipulator_teleop_keyboard.cpp deleted file mode 100644 index dfcb3bb3..00000000 --- a/open_manipulator_teleop/src/open_manipulator_teleop_keyboard.cpp +++ /dev/null @@ -1,391 +0,0 @@ -/******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* you may not use this file except in compliance with the License. -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -*******************************************************************************/ - -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ - -#include "open_manipulator_teleop/open_manipulator_teleop_keyboard.h" - -OpenManipulatorTeleop::OpenManipulatorTeleop() -: node_handle_(""), - priv_node_handle_("~") -{ - /************************************************************ - ** Initialize variables - ************************************************************/ - present_joint_angle_.resize(NUM_OF_JOINT); - present_kinematic_position_.resize(3); - - /************************************************************ - ** Initialize ROS Subscribers and Clients - ************************************************************/ - initSubscriber(); - initClient(); - - disableWaitingForEnter(); - ROS_INFO("OpenManipulator teleoperation using keyboard start"); -} - -OpenManipulatorTeleop::~OpenManipulatorTeleop() -{ - restoreTerminalSettings(); - ROS_INFO("Terminate OpenManipulator Joystick"); - ros::shutdown(); -} - -void OpenManipulatorTeleop::initClient() -{ - goal_joint_space_path_client_ = node_handle_.serviceClient("goal_joint_space_path"); - goal_joint_space_path_from_present_client_ = node_handle_.serviceClient("goal_joint_space_path_from_present"); - goal_task_space_path_from_present_position_only_client_ = node_handle_.serviceClient("goal_task_space_path_from_present_position_only"); - goal_tool_control_client_ = node_handle_.serviceClient("goal_tool_control"); -} - -void OpenManipulatorTeleop::initSubscriber() -{ - joint_states_sub_ = node_handle_.subscribe("joint_states", 10, &OpenManipulatorTeleop::jointStatesCallback, this); - kinematics_pose_sub_ = node_handle_.subscribe("kinematics_pose", 10, &OpenManipulatorTeleop::kinematicsPoseCallback, this); -} - -void OpenManipulatorTeleop::jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ - std::vector temp_angle; - temp_angle.resize(NUM_OF_JOINT); - for (std::vector::size_type i = 0; i < msg->name.size(); i ++) - { - if (!msg->name.at(i).compare("joint1")) temp_angle.at(0) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint2")) temp_angle.at(1) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint3")) temp_angle.at(2) = (msg->position.at(i)); - else if (!msg->name.at(i).compare("joint4")) temp_angle.at(3) = (msg->position.at(i)); - } - present_joint_angle_ = temp_angle; -} - -void OpenManipulatorTeleop::kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg) -{ - std::vector temp_position; - temp_position.push_back(msg->pose.position.x); - temp_position.push_back(msg->pose.position.y); - temp_position.push_back(msg->pose.position.z); - present_kinematic_position_ = temp_position; -} - -std::vector OpenManipulatorTeleop::getPresentJointAngle() -{ - return present_joint_angle_; -} - -std::vector OpenManipulatorTeleop::getPresentKinematicsPose() -{ - return present_kinematic_position_; -} - -bool OpenManipulatorTeleop::setJointSpacePathFromPresent(std::vector joint_name, std::vector joint_angle, double path_time) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name = joint_name; - srv.request.joint_position.position = joint_angle; - srv.request.path_time = path_time; - - if (goal_joint_space_path_from_present_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool OpenManipulatorTeleop::setJointSpacePath(std::vector joint_name, std::vector joint_angle, double path_time) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name = joint_name; - srv.request.joint_position.position = joint_angle; - srv.request.path_time = path_time; - - if (goal_joint_space_path_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool OpenManipulatorTeleop::setToolControl(std::vector joint_angle) -{ - open_manipulator_msgs::SetJointPosition srv; - srv.request.joint_position.joint_name.push_back(priv_node_handle_.param("end_effector_name", "gripper")); - srv.request.joint_position.position = joint_angle; - - if (goal_tool_control_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -bool OpenManipulatorTeleop::setTaskSpacePathFromPresentPositionOnly(std::vector kinematics_pose, double path_time) -{ - open_manipulator_msgs::SetKinematicsPose srv; - srv.request.planning_group = priv_node_handle_.param("end_effector_name", "gripper"); - srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0); - srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1); - srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2); - srv.request.path_time = path_time; - - if (goal_task_space_path_from_present_position_only_client_.call(srv)) - { - return srv.response.is_planned; - } - return false; -} - -void OpenManipulatorTeleop::printText() -{ - printf("\n"); - printf("---------------------------\n"); - printf("Control Your OpenManipulator!\n"); - printf("---------------------------\n"); - printf("w : increase x axis in task space\n"); - printf("s : decrease x axis in task space\n"); - printf("a : increase y axis in task space\n"); - printf("d : decrease y axis in task space\n"); - printf("z : increase z axis in task space\n"); - printf("x : decrease z axis in task space\n"); - printf("\n"); - printf("y : increase joint 1 angle\n"); - printf("h : decrease joint 1 angle\n"); - printf("u : increase joint 2 angle\n"); - printf("j : decrease joint 2 angle\n"); - printf("i : increase joint 3 angle\n"); - printf("k : decrease joint 3 angle\n"); - printf("o : increase joint 4 angle\n"); - printf("l : decrease joint 4 angle\n"); - printf("\n"); - printf("g : gripper open\n"); - printf("f : gripper close\n"); - printf(" \n"); - printf("1 : init pose\n"); - printf("2 : home pose\n"); - printf(" \n"); - printf("q to quit\n"); - printf("---------------------------\n"); - printf("Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n", - getPresentJointAngle().at(0), - getPresentJointAngle().at(1), - getPresentJointAngle().at(2), - getPresentJointAngle().at(3)); - printf("Present Kinematics Position X: %.3lf Y: %.3lf Z: %.3lf\n", - getPresentKinematicsPose().at(0), - getPresentKinematicsPose().at(1), - getPresentKinematicsPose().at(2)); - printf("---------------------------\n"); - -} - -void OpenManipulatorTeleop::setGoal(char ch) -{ - std::vector goalPose; goalPose.resize(3, 0.0); - std::vector goalJoint; goalJoint.resize(NUM_OF_JOINT, 0.0); - - if (ch == 'w' || ch == 'W') - { - printf("input : w \tincrease(++) x axis in task space\n"); - goalPose.at(0) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 's' || ch == 'S') - { - printf("input : s \tdecrease(--) x axis in task space\n"); - goalPose.at(0) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 'a' || ch == 'A') - { - printf("input : a \tincrease(++) y axis in task space\n"); - goalPose.at(1) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 'd' || ch == 'D') - { - printf("input : d \tdecrease(--) y axis in task space\n"); - goalPose.at(1) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 'z' || ch == 'Z') - { - printf("input : z \tincrease(++) z axis in task space\n"); - goalPose.at(2) = DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 'x' || ch == 'X') - { - printf("input : x \tdecrease(--) z axis in task space\n"); - goalPose.at(2) = -DELTA; - setTaskSpacePathFromPresentPositionOnly(goalPose, PATH_TIME); - } - else if (ch == 'y' || ch == 'Y') - { - printf("input : y \tincrease(++) joint 1 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); goalJoint.at(0) = JOINT_DELTA; - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'h' || ch == 'H') - { - printf("input : h \tdecrease(--) joint 1 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); goalJoint.at(0) = -JOINT_DELTA; - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'u' || ch == 'U') - { - printf("input : u \tincrease(++) joint 2 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); goalJoint.at(1) = JOINT_DELTA; - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'j' || ch == 'J') - { - printf("input : j \tdecrease(--) joint 2 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); goalJoint.at(1) = -JOINT_DELTA; - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'i' || ch == 'I') - { - printf("input : i \tincrease(++) joint 3 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); goalJoint.at(2) = JOINT_DELTA; - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'k' || ch == 'K') - { - printf("input : k \tdecrease(--) joint 3 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); goalJoint.at(2) = -JOINT_DELTA; - joint_name.push_back("joint4"); - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'o' || ch == 'O') - { - printf("input : o \tincrease(++) joint 4 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); goalJoint.at(3) = JOINT_DELTA; - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'l' || ch == 'L') - { - printf("input : l \tdecrease(--) joint 4 angle\n"); - std::vector joint_name; - joint_name.push_back("joint1"); - joint_name.push_back("joint2"); - joint_name.push_back("joint3"); - joint_name.push_back("joint4"); goalJoint.at(3) = -JOINT_DELTA; - setJointSpacePathFromPresent(joint_name, goalJoint, PATH_TIME); - } - else if (ch == 'g' || ch == 'G') - { - printf("input : g \topen gripper\n"); - std::vector joint_angle; - - joint_angle.push_back(0.01); - setToolControl(joint_angle); - } - else if (ch == 'f' || ch == 'F') - { - printf("input : f \tclose gripper\n"); - std::vector joint_angle; - joint_angle.push_back(-0.01); - setToolControl(joint_angle); - } - else if (ch == '2') - { - printf("input : 2 \thome pose\n"); - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(-1.05); - joint_name.push_back("joint3"); joint_angle.push_back(0.35); - joint_name.push_back("joint4"); joint_angle.push_back(0.70); - setJointSpacePath(joint_name, joint_angle, path_time); - } - else if (ch == '1') - { - printf("input : 1 \tinit pose\n"); - - std::vector joint_name; - std::vector joint_angle; - double path_time = 2.0; - joint_name.push_back("joint1"); joint_angle.push_back(0.0); - joint_name.push_back("joint2"); joint_angle.push_back(0.0); - joint_name.push_back("joint3"); joint_angle.push_back(0.0); - joint_name.push_back("joint4"); joint_angle.push_back(0.0); - setJointSpacePath(joint_name, joint_angle, path_time); - } -} - -void OpenManipulatorTeleop::restoreTerminalSettings(void) -{ - tcsetattr(0, TCSANOW, &oldt_); /* Apply saved settings */ -} - -void OpenManipulatorTeleop::disableWaitingForEnter(void) -{ - struct termios newt; - - tcgetattr(0, &oldt_); /* Save terminal settings */ - newt = oldt_; /* Init new settings */ - newt.c_lflag &= ~(ICANON | ECHO); /* Change settings */ - tcsetattr(0, TCSANOW, &newt); /* Apply settings */ -} - -int main(int argc, char **argv) -{ - // Init ROS node - ros::init(argc, argv, "open_manipulator_teleop_keyboard"); - OpenManipulatorTeleop openManipulatorTeleop; - - char ch; - openManipulatorTeleop.printText(); - while (ros::ok() && (ch = std::getchar()) != 'q') - { - ros::spinOnce(); - openManipulatorTeleop.printText(); - ros::spinOnce(); - openManipulatorTeleop.setGoal(ch); - } - - return 0; -} diff --git a/openmanipulator.repos b/open_manipulator_x.repos similarity index 51% rename from openmanipulator.repos rename to open_manipulator_x.repos index ae8a3412..9d396e1d 100644 --- a/openmanipulator.repos +++ b/open_manipulator_x.repos @@ -2,20 +2,24 @@ repositories: DynamixelSDK: type: git url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git - version: develop - dynamixel_workbench: + version: ros2-devel + dynamixel-workbench: type: git url: https://github.com/ROBOTIS-GIT/dynamixel-workbench.git - version: develop - dynamixel_workbench_msgs: + version: ros2-devel + open_manipulator: type: git - url: https://github.com/ROBOTIS-GIT/dynamixel-workbench-msgs.git - version: develop + url: https://github.com/ROBOTIS-GIT/open_manipulator.git + version: ros2-devel open_manipulator_msgs: type: git url: https://github.com/ROBOTIS-GIT/open_manipulator_msgs.git - version: develop + version: ros2-devel + open_manipulator_dependencies: + type: git + url: https://github.com/ROBOTIS-GIT/open_manipulator_dependencies.git + version: ros2-devel robotis_manipulator: type: git url: https://github.com/ROBOTIS-GIT/robotis_manipulator.git - version: develop + version: ros2-devel diff --git a/open_manipulator_controller/99-open-manipulator-cdc.rules b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules similarity index 50% rename from open_manipulator_controller/99-open-manipulator-cdc.rules rename to open_manipulator_x_bringup/99-open-manipulator-cdc.rules index 52e361d1..a23da3d6 100644 --- a/open_manipulator_controller/99-open-manipulator-cdc.rules +++ b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules @@ -1,3 +1,3 @@ -# http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2 +#http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2 KERNEL=="ttyUSB*", DRIVERS=="ftdi_sio", MODE="0666", ATTR{device/latency_timer}="1" diff --git a/open_manipulator_x_bringup/CMakeLists.txt b/open_manipulator_x_bringup/CMakeLists.txt new file mode 100644 index 00000000..c1d93e12 --- /dev/null +++ b/open_manipulator_x_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(open_manipulator_x_bringup) + +################################################################################ +# Find and load build settings from external packages +################################################################################ +find_package(ament_cmake REQUIRED) + +################################################################################ +# Install +################################################################################ +install( + DIRECTORY launch config rviz worlds + DESTINATION share/${PROJECT_NAME} +) + +install(FILES 99-open-manipulator-cdc.rules + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS scripts/create_udev_rules + DESTINATION lib/${PROJECT_NAME} +) + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") +################################################################################ +# Macro for ament package +################################################################################ +ament_package() \ No newline at end of file diff --git a/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml b/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml new file mode 100644 index 00000000..0d725bbc --- /dev/null +++ b/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml @@ -0,0 +1,59 @@ +controller_manager: + ros__parameters: + use_sim_time: True + update_rate: 1000 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: position_controllers/GripperActionController + +arm_controller: + ros__parameters: + use_sim_time: True + joints: + - joint1 + - joint2 + - joint3 + - joint4 + + + interface_name: position + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + state_publish_rate: 200.0 + action_monitor_rate: 20.0 + + allow_partial_joints_goal: false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +gripper_controller: + ros__parameters: + use_sim_time: True + joint: gripper_left_joint + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + - effort + state_publish_rate: 50.0 + action_monitor_rate: 20.0 + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 diff --git a/open_manipulator_x_bringup/config/hardware_controller_manager.yaml b/open_manipulator_x_bringup/config/hardware_controller_manager.yaml new file mode 100644 index 00000000..6629431a --- /dev/null +++ b/open_manipulator_x_bringup/config/hardware_controller_manager.yaml @@ -0,0 +1,49 @@ +controller_manager: + ros__parameters: + use_sim_time: False + update_rate: 1000 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + gripper_controller: + type: position_controllers/GripperActionController + +arm_controller: + ros__parameters: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + + interface_name: position + + command_interfaces: + - position + + state_interfaces: + - position + - velocity + + state_publish_rate: 200.0 + action_monitor_rate: 20.0 + + allow_partial_joints_goal: false + open_loop_control: true + allow_integration_in_goal_trajectories: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 # Defaults to 0.0 (start immediately) + +gripper_controller: + ros__parameters: + joint: gripper_left_joint + state_interfaces: + - position + command_interfaces: + - position + conversion_factor: 0.0001 diff --git a/open_manipulator_x_bringup/env-hooks/open_manipulator_x_bringup.dsv.in b/open_manipulator_x_bringup/env-hooks/open_manipulator_x_bringup.dsv.in new file mode 100644 index 00000000..cc582e4d --- /dev/null +++ b/open_manipulator_x_bringup/env-hooks/open_manipulator_x_bringup.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GAZEBO_MODEL_PATH;share/open_manipulator_x_bringup/worlds diff --git a/open_manipulator_x_bringup/launch/base.launch.py b/open_manipulator_x_bringup/launch/base.launch.py new file mode 100644 index 00000000..8b2d81dd --- /dev/null +++ b/open_manipulator_x_bringup/launch/base.launch.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Darby Lim, Sungho Woo + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import RegisterEventHandler +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command +from launch.substitutions import FindExecutable +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='Whether execute rviz2' + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + 'prefix', + default_value='""', + description='Prefix of the joint and link names' + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + 'use_sim', + default_value='true', + description='Start robot in Gazebo simulation.' + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + 'use_fake_hardware', + default_value='false', + description='Start robot with fake hardware mirroring command to its states.' + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + 'fake_sensor_commands', + default_value='false', + description='Enable fake command interfaces for sensors used for simple simulations. \ + Used only if "use_fake_hardware" parameter is true.' + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + 'port_name', + default_value='/dev/ttyUSB0', + description='The port name to connect to hardware.' + ) + ) + + start_rviz = LaunchConfiguration('start_rviz') + prefix = LaunchConfiguration('prefix') + use_sim = LaunchConfiguration('use_sim') + use_fake_hardware = LaunchConfiguration('use_fake_hardware') + fake_sensor_commands = LaunchConfiguration('fake_sensor_commands') + port_name = LaunchConfiguration('port_name') + + urdf_file = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_description'), + 'urdf', + 'open_manipulator_x_robot.urdf.xacro' + ] + ), + ' ', + 'prefix:=', + prefix, + ' ', + 'use_sim:=', + use_sim, + ' ', + 'use_fake_hardware:=', + use_fake_hardware, + ' ', + 'fake_sensor_commands:=', + fake_sensor_commands, + ' ', + 'port_name:=', + port_name, + ] + ) + + controller_manager_config = PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_bringup'), + 'config', + 'hardware_controller_manager.yaml', + ] + ) + + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_bringup'), + 'rviz', + 'open_manipulator_x.rviz' + ] + ) + + control_node = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ + {'robot_description': urdf_file}, + controller_manager_config + ], + output="both", + condition=UnlessCondition(use_sim)) + + robot_state_pub_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': urdf_file, 'use_sim_time': use_sim}], + output='screen' + ) + + rviz_node = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file], + output='screen', + condition=IfCondition(start_rviz) + ) + + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'], + output='screen', + ) + + arm_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['arm_controller'], + output='screen', + ) + + gripper_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['gripper_controller'], + output='screen', + ) + + delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[rviz_node], + ) + ) + + delay_arm_controller_spawner_after_joint_state_broadcaster_spawner = \ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[arm_controller_spawner], + ) + ) + + delay_gripper_controller_spawner_after_joint_state_broadcaster_spawner = \ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[gripper_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + joint_state_broadcaster_spawner, + delay_rviz_after_joint_state_broadcaster_spawner, + delay_arm_controller_spawner_after_joint_state_broadcaster_spawner, + delay_gripper_controller_spawner_after_joint_state_broadcaster_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/open_manipulator_x_bringup/launch/fake.launch.py b/open_manipulator_x_bringup/launch/fake.launch.py new file mode 100644 index 00000000..45ecd956 --- /dev/null +++ b/open_manipulator_x_bringup/launch/fake.launch.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Darby Lim, Sungho Woo + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.substitutions import ThisLaunchFileDir + + +def is_valid_to_launch(): + # Path includes model name of Raspberry Pi series + path = '/sys/firmware/devicetree/base/model' + if os.path.exists(path): + return False + else: + return True + + +def generate_launch_description(): + if not is_valid_to_launch(): + print('Can not launch fake robot in Raspberry Pi') + return LaunchDescription([]) + + start_rviz = LaunchConfiguration('start_rviz') + prefix = LaunchConfiguration('prefix') + use_fake_hardware = LaunchConfiguration('use_fake_hardware') + fake_sensor_commands = LaunchConfiguration('fake_sensor_commands') + + return LaunchDescription([ + DeclareLaunchArgument( + 'start_rviz', + default_value='true', + description='Whether execute rviz2'), + + DeclareLaunchArgument( + 'prefix', + default_value='""', + description='Prefix of the joint and link names'), + + DeclareLaunchArgument( + 'use_fake_hardware', + default_value='true', + description='Start robot with fake hardware mirroring command to its states.'), + + DeclareLaunchArgument( + 'fake_sensor_commands', + default_value='true', + description='Enable fake command interfaces for sensors used for simple simulations. \ + Used only if "use_fake_hardware" parameter is true.'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/base.launch.py']), + launch_arguments={ + 'start_rviz': start_rviz, + 'prefix': prefix, + 'use_fake_hardware': use_fake_hardware, + 'fake_sensor_commands': fake_sensor_commands, + }.items(), + ) + ]) diff --git a/open_manipulator_x_bringup/launch/gazebo.launch.py b/open_manipulator_x_bringup/launch/gazebo.launch.py new file mode 100644 index 00000000..1887304c --- /dev/null +++ b/open_manipulator_x_bringup/launch/gazebo.launch.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Darby Lim, Sungho Woo + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution +from launch.substitutions import ThisLaunchFileDir + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def is_valid_to_launch(): + # Path includes model name of Raspberry Pi series + path = '/sys/firmware/devicetree/base/model' + if os.path.exists(path): + return False + else: + return True + + +def generate_launch_description(): + if not is_valid_to_launch(): + print('Can not launch fake robot in Raspberry Pi') + return LaunchDescription([]) + + start_rviz = LaunchConfiguration('start_rviz') + prefix = LaunchConfiguration('prefix') + use_sim = LaunchConfiguration('use_sim') + + world = LaunchConfiguration( + 'world', + default=PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_bringup'), + 'worlds', + 'empty_world.model' + ] + ) + ) + + pose = {'x': LaunchConfiguration('x_pose', default='0.00'), + 'y': LaunchConfiguration('y_pose', default='0.00'), + 'z': LaunchConfiguration('z_pose', default='0.01'), + 'R': LaunchConfiguration('roll', default='0.00'), + 'P': LaunchConfiguration('pitch', default='0.00'), + 'Y': LaunchConfiguration('yaw', default='0.00')} + + return LaunchDescription([ + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='Whether execute rviz2'), + + DeclareLaunchArgument( + 'prefix', + default_value='""', + description='Prefix of the joint and link names'), + + DeclareLaunchArgument( + 'use_sim', + default_value='true', + description='Start robot in Gazebo simulation.'), + + DeclareLaunchArgument( + 'world', + default_value=world, + description='Directory of gazebo world file'), + + DeclareLaunchArgument( + 'x_pose', + default_value=pose['x'], + description='position of open_manipulator_x'), + + DeclareLaunchArgument( + 'y_pose', + default_value=pose['y'], + description='position of open_manipulator_x'), + + DeclareLaunchArgument( + 'z_pose', + default_value=pose['z'], + description='position of open_manipulator_x'), + + DeclareLaunchArgument( + 'roll', + default_value=pose['R'], + description='orientation of open_manipulator_x'), + + DeclareLaunchArgument( + 'pitch', + default_value=pose['P'], + description='orientation of open_manipulator_x'), + + DeclareLaunchArgument( + 'yaw', + default_value=pose['Y'], + description='orientation of open_manipulator_x'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/base.launch.py']), + launch_arguments={ + 'start_rviz': start_rviz, + 'prefix': prefix, + 'use_sim': use_sim, + }.items(), + ), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + PathJoinSubstitution( + [ + FindPackageShare('gazebo_ros'), + 'launch', + 'gazebo.launch.py' + ] + ) + ] + ), + launch_arguments={ + 'verbose': 'false', + 'world': world, + }.items(), + ), + + Node( + package='gazebo_ros', + executable='spawn_entity.py', + arguments=[ + '-topic', 'robot_description', + '-entity', 'open_manipulator_x_system', + '-x', pose['x'], '-y', pose['y'], '-z', pose['z'], + '-R', pose['R'], '-P', pose['P'], '-Y', pose['Y'], + ], + output='screen', + ), + ]) diff --git a/open_manipulator_x_bringup/launch/hardware.launch.py b/open_manipulator_x_bringup/launch/hardware.launch.py new file mode 100644 index 00000000..5d30f2a3 --- /dev/null +++ b/open_manipulator_x_bringup/launch/hardware.launch.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Darby Lim, Sungho Woo + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch.substitutions import LaunchConfiguration +from launch.substitutions import ThisLaunchFileDir + +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + start_rviz = LaunchConfiguration('start_rviz') + prefix = LaunchConfiguration('prefix') + use_fake_hardware = LaunchConfiguration('use_fake_hardware') + use_sim = LaunchConfiguration('use_sim') + port_name = LaunchConfiguration('port_name') + + return LaunchDescription([ + DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='Whether execute rviz2'), + + DeclareLaunchArgument( + 'prefix', + default_value='""', + description='Prefix of the joint and link names'), + + DeclareLaunchArgument( + 'use_fake_hardware', + default_value='false', + description='Start robot with fake hardware mirroring command to its states.'), + + DeclareLaunchArgument( + 'use_sim', + default_value='false', + description='Start robot in Gazebo simulation.'), + + DeclareLaunchArgument( + 'port_name', + default_value='/dev/ttyUSB0', + description='The port name to connect to hardware.'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/base.launch.py']), + launch_arguments={ + 'start_rviz': start_rviz, + 'prefix': prefix, + 'use_fake_hardware': use_fake_hardware, + 'use_sim': use_sim, + 'port_name': port_name, + }.items(), + ), + ]) diff --git a/open_manipulator_x_bringup/package.xml b/open_manipulator_x_bringup/package.xml new file mode 100644 index 00000000..3b57bda7 --- /dev/null +++ b/open_manipulator_x_bringup/package.xml @@ -0,0 +1,22 @@ + + + + open_manipulator_x_bringup + 0.0.0 + TODO: Package description + yunwonho + TODO: License declaration + + ament_cmake + gazebo_ros + robot_state_publisher + ros2_control + ros2_controllers + gripper_controllers + rviz2 + open_manipulator_x_description + xacro + + ament_cmake + + \ No newline at end of file diff --git a/open_manipulator_description/rviz/open_manipulator.rviz b/open_manipulator_x_bringup/rviz/open_manipulator_x.rviz similarity index 54% rename from open_manipulator_description/rviz/open_manipulator.rviz rename to open_manipulator_x_bringup/rviz/open_manipulator_x.rviz index 9233feba..6c8b9ce1 100644 --- a/open_manipulator_description/rviz/open_manipulator.rviz +++ b/open_manipulator_x_bringup/rviz/open_manipulator_x.rviz @@ -1,27 +1,29 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /Status1 + - /RobotModel1 + - /TF1 Splitter Ratio: 0.5 - Tree Height: 775 - - Class: rviz/Selection + Tree Height: 549 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 - - Class: rviz/Views + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 @@ -31,11 +33,11 @@ Visualization Manager: Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid - Color: 35; 35; 36 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -48,8 +50,16 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Class: rviz/RobotModel + Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description Enabled: true Links: All Links Enabled: true @@ -57,17 +67,21 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order + dummy_mimic_fix: + Alpha: 1 + Show Axes: false + Show Trail: false end_effector_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - grip_link: + gripper_left_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - grip_link_sub: + gripper_right_link: Alpha: 1 Show Axes: false Show Trail: false @@ -101,22 +115,26 @@ Visualization Manager: Alpha: 1 Show Axes: false Show Trail: false + Mass Properties: + Inertia: false + Mass: false Name: RobotModel - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz/TF + - Class: rviz_default_plugins/TF Enabled: true Frame Timeout: 15 Frames: All Enabled: true + dummy_mimic_fix: + Value: true end_effector_link: Value: true - grip_link: + gripper_left_link: Value: true - grip_link_sub: + gripper_right_link: Value: true link1: Value: true @@ -130,7 +148,7 @@ Visualization Manager: Value: true world: Value: true - Marker Scale: 0.300000012 + Marker Scale: 0.3499999940395355 Name: TF Show Arrows: true Show Axes: true @@ -142,66 +160,89 @@ Visualization Manager: link3: link4: link5: + dummy_mimic_fix: + {} end_effector_link: {} - grip_link: + gripper_left_link: {} - grip_link_sub: + gripper_right_link: {} Update Interval: 0 Value: true Enabled: true Global Options: - Background Color: 225; 225; 225 - Default Light: true + Background Color: 48; 48; 48 Fixed Frame: world Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Topic: /initialpose - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 1.0015856 + Class: rviz_default_plugins/Orbit + Distance: 1.3669214248657227 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0915589333 - Y: -0.119371392 - Z: 0.0927140638 + X: 0.09866781532764435 + Y: -0.062461238354444504 + Z: -0.023534774780273438 Focal Shape Fixed Size: true - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.485398322 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.84539794921875 Target Frame: Value: Orbit (rviz) - Yaw: 0.985397935 + Yaw: 1.155397653579712 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 846 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000043000002a7000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009bf0000003efc0100000002fb0000000800540069006d00650100000000000009bf0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000084f0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -209,7 +250,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 2495 - X: 65 - Y: 1104 + collapsed: false + Width: 1200 + X: 130 + Y: 71 diff --git a/open_manipulator_x_bringup/scripts/create_udev_rules b/open_manipulator_x_bringup/scripts/create_udev_rules new file mode 100644 index 00000000..e79028ca --- /dev/null +++ b/open_manipulator_x_bringup/scripts/create_udev_rules @@ -0,0 +1,14 @@ +#!/bin/bash + +echo "" +echo "This script copies the udev rule to /etc/udev/rules.d/" +echo "to configure the U2D2 device for the OpenMANIPULATOR." +echo "" + +sudo cp `ros2 pkg prefix open_manipulator_x_bringup`/share/open_manipulator_x_bringup/99-open-manipulator-cdc.rules /etc/udev/rules.d/ + +echo "" +echo "Reload rules" +echo "" +sudo udevadm control --reload-rules +sudo udevadm trigger diff --git a/open_manipulator_x_bringup/worlds/empty_world.model b/open_manipulator_x_bringup/worlds/empty_world.model new file mode 100644 index 00000000..64e9476b --- /dev/null +++ b/open_manipulator_x_bringup/worlds/empty_world.model @@ -0,0 +1,46 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 1.226730 -1.270682 1.172346 0 0.545800 2.301601 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world.model b/open_manipulator_x_bringup/worlds/turtlebot3_world.model new file mode 100644 index 00000000..11bfcc22 --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world.model @@ -0,0 +1,53 @@ + + + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + 1 + + model://turtlebot3_world + + + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae new file mode 100644 index 00000000..c548e5f1 --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae @@ -0,0 +1,76 @@ + + + 2017-03-30T01:06:40 + 2017-03-30T01:06:40 + + Y_UP + + + + + + + + + + + + + + + + + + + -28.8675 50 100 -57.735 9.66338e-13 0 -57.735 1.13687e-12 100 -28.8675 50 0 -57.735 1.13687e-12 100 -28.8675 -50 0 -28.8675 -50 100 -57.735 9.66338e-13 0 -28.8675 -50 100 28.8675 -50 0 28.8675 -50 100 -28.8675 -50 0 28.8675 -50 100 57.735 1.7053e-12 0 57.735 1.36424e-12 100 28.8675 -50 0 57.735 1.36424e-12 100 28.8675 50 2.498e-13 28.8675 50 100 57.735 1.7053e-12 0 28.8675 50 100 -28.8675 50 0 -28.8675 50 100 28.8675 50 2.498e-13 -57.735 1.13687e-12 100 57.735 1.36424e-12 100 -28.8675 50 100 -28.8675 -50 100 28.8675 -50 100 28.8675 50 100 -57.735 9.66338e-13 0 28.8675 -50 0 -28.8675 -50 0 57.735 1.7053e-12 0 -28.8675 50 0 28.8675 50 2.498e-13 + + + + + + + + + + -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 + + + + + + + + + + + + + + +

0 1 2 1 0 3 4 5 6 5 4 7 8 9 10 9 8 11 12 13 14 13 12 15 16 17 18 17 16 19 20 21 22 21 20 23 24 25 26 24 27 28 28 25 24 26 25 29 30 31 32 31 30 33 33 34 35 33 30 34

+ + + + + + + + + + + + + + + + 0.392157 0.976471 0.121569 1 + + + + + + + + + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae new file mode 100644 index 00000000..aa738ce7 --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae @@ -0,0 +1,76 @@ + + + 2017-03-30T02:35:53 + 2017-03-30T02:35:53 + + Y_UP + + + + + + + + + + + + + + + + + + + -400 230.94 0 -400 230.94 200 4.54747e-13 461.88 200 6.25278e-13 461.88 0 6.25278e-13 461.88 0 4.54747e-13 461.88 200 400 230.94 200 400 230.94 0 400 230.94 0 400 230.94 200 400 -230.94 200 400 -230.94 0 400 -230.94 0 400 -230.94 200 -1.7053e-13 -461.88 200 2.27374e-13 -461.88 0 2.27374e-13 -461.88 0 -1.7053e-13 -461.88 200 -400 -230.94 200 -400 -230.94 0 450 259.808 200 -3.69482e-13 519.615 200 -2.27374e-13 519.615 0 450 259.808 0 -3.69482e-13 519.615 200 -450 259.808 200 -450 259.808 0 -2.27374e-13 519.615 0 -450 259.808 200 -450 -259.808 200 -450 -259.808 0 -450 259.808 0 -450 -259.808 200 -1.13687e-13 -519.615 200 -2.27374e-13 -519.615 0 -450 -259.808 0 -1.13687e-13 -519.615 200 450 -259.808 200 450 -259.808 0 -2.27374e-13 -519.615 0 450 -259.808 0 450 259.808 200 450 259.808 0 450 -259.808 200 -400 -230.94 0 -400 -230.94 200 -400 230.94 200 -400 230.94 0 -1.7053e-13 -461.88 200 -1.13687e-13 -519.615 200 -400 -230.94 200 -450 -259.808 200 -450 259.808 200 450 -259.808 200 400 -230.94 200 400 230.94 200 4.54747e-13 461.88 200 -3.69482e-13 519.615 200 -400 230.94 200 450 259.808 200 -2.27374e-13 -519.615 0 400 -230.94 0 2.27374e-13 -461.88 0 400 230.94 0 -2.27374e-13 519.615 0 6.25278e-13 461.88 0 450 -259.808 0 450 259.808 0 -400 230.94 0 -450 259.808 0 -400 -230.94 0 -450 -259.808 0 + + + + + + + + + + 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 1 0 0 1 0 0 1 0 0 1 0 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 + + + + + + + + + + + + + + +

0 2 1 0 3 2 4 6 5 4 7 6 8 10 9 8 11 10 12 14 13 12 15 14 16 18 17 16 19 18 20 22 21 22 20 23 24 26 25 26 24 27 28 30 29 30 28 31 32 34 33 34 32 35 36 38 37 38 36 39 40 42 41 40 41 43 44 46 45 44 47 46 48 50 49 50 52 51 53 54 49 55 57 56 48 49 54 57 52 58 58 56 57 53 59 55 57 55 59 53 55 54 49 50 51 52 50 58 60 62 61 63 65 64 63 67 66 68 69 64 66 61 63 64 67 63 60 61 66 60 70 62 69 70 71 60 71 70 64 65 68 69 68 70

+ + + + + + + + + + + + + + + + 0.603922 0.647059 0.686275 1 + + + + + + + + + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf b/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf new file mode 100644 index 00000000..330bd320 --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf @@ -0,0 +1,549 @@ + + + + 1 + + + -1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + -1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + -1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + + 3.5 0 -0.5 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.8 0.8 0.8 + + + 10 + + + + + + + + + + + + + 3.5 0 -0.5 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.8 0.8 0.8 + + + + + + + + + 1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + 1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + 1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + 1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + -1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + -1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + -1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + -1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + + 0 0 -0.3 0 0 -1.5708 + + + model://turtlebot3_world/meshes/wall.dae + 0.25 0.25 0.25 + + + 10 + + + + + + + + + + + + + 0 0 -0.3 0 0 -1.5708 + + + model://turtlebot3_world/meshes/wall.dae + 0.25 0.25 0.25 + + + + + + + + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config new file mode 100644 index 00000000..bc6a666f --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config @@ -0,0 +1,17 @@ + + + + TurtleBot3 World + 1.0 + model-1_4.sdf + model.sdf + + + Taehun Lim(Darby) + thlim@robotis.com + + + + World of TurtleBot3 with ROS symbol + + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf new file mode 100644 index 00000000..d733d00f --- /dev/null +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf @@ -0,0 +1,549 @@ + + + + 1 + + + -1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + -1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + -1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + -1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 0 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 0 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 -1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 0 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + 1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + 10 + + + + + + + + + + + + + 1.1 1.1 0.25 0 0 0 + + + 0.15 + 0.5 + + + + + + + + + + 3.5 0 -0.5 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.8 0.8 0.8 + + + 10 + + + + + + + + + + + + + 3.5 0 -0.5 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.8 0.8 0.8 + + + + + + + + + 1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + 1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + 1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + 1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + -1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + -1.8 2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + -1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + 10 + + + + + + + + + + + + + -1.8 -2.7 0 0 0 0 + + + model://turtlebot3_world/meshes/hexagon.dae + 0.55 0.55 0.55 + + + + + + + + + + 0 0 -0.3 0 0 -1.5708 + + + model://turtlebot3_world/meshes/wall.dae + 0.25 0.25 0.25 + + + 10 + + + + + + + + + + + + + 0 0 -0.3 0 0 -1.5708 + + + model://turtlebot3_world/meshes/wall.dae + 0.25 0.25 0.25 + + + + + + + + + diff --git a/open_manipulator_x_description/CHANGELOG.rst b/open_manipulator_x_description/CHANGELOG.rst new file mode 100644 index 00000000..d8aa8eb5 --- /dev/null +++ b/open_manipulator_x_description/CHANGELOG.rst @@ -0,0 +1,105 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_description/CMakeLists.txt b/open_manipulator_x_description/CMakeLists.txt new file mode 100644 index 00000000..6a4b55c1 --- /dev/null +++ b/open_manipulator_x_description/CMakeLists.txt @@ -0,0 +1,31 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(open_manipulator_x_description) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +################################################################################ +# Find and load build settings from external packages +################################################################################ +find_package(ament_cmake REQUIRED) + +################################################################################ +# Install +################################################################################ +install(DIRECTORY launch meshes rviz urdf gazebo ros2_control + DESTINATION share/${PROJECT_NAME} +) + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in") +################################################################################ +# Macro for ament package +################################################################################ +ament_package() diff --git a/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in b/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in new file mode 100644 index 00000000..cd4ca905 --- /dev/null +++ b/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;GAZEBO_MODEL_PATH;share \ No newline at end of file diff --git a/open_manipulator_x_description/gazebo/materials.xacro b/open_manipulator_x_description/gazebo/materials.xacro new file mode 100644 index 00000000..5ce0e021 --- /dev/null +++ b/open_manipulator_x_description/gazebo/materials.xacro @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/open_manipulator_x_description/gazebo/open_manipulator_x.gazebo.xacro b/open_manipulator_x_description/gazebo/open_manipulator_x.gazebo.xacro new file mode 100644 index 00000000..2854c9b0 --- /dev/null +++ b/open_manipulator_x_description/gazebo/open_manipulator_x.gazebo.xacro @@ -0,0 +1,57 @@ + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + PositionJointInterface + + + + + + + 1000000.0 + 100.0 + 30.0 + 30.0 + 1.0 + 0.001 + Gazebo/DarkGrey + + + + + + + + + + + + + + Gazebo/Red + + + + + + + + + + + + diff --git a/open_manipulator_x_description/launch/.DS_Store b/open_manipulator_x_description/launch/.DS_Store new file mode 100644 index 0000000000000000000000000000000000000000..8024f307f168e1589f2b2044b6bea2249d771535 GIT binary patch literal 6148 zcmeHKu}TCn5Pi`r4s5Q}E~S;AtxY)2!rBi|?-UeP_c(4vt@1m5rM{Pm;-1@RAtE!7 zyva;vCV8+)7C@MGSBJm^z?e-Kqzs76gRZSQSr|%=bKKz)_qfG!AkkkmY3+Mxx!!B8 z`}6ucG~COYz7_2&JaAXfIH2Vb>(i!P%$s)3Xi;~}w*Oj$8W6vc@rXH9sbl>GZa7-v zX(Rv3M*dR7J7>Tda0Z+KXW&N+@XnTucMQFE2AlzB;DZ6350OonDrUoSbg2L2fXVKh6NO}VLhw|?24?%K$9!6wnXEQLb7_XyxX g=g4kxnmxvGj@ukFD1$7kQitm`;`;$Zco zF!&OF^)m?WaFRL`>}g6eI-vxyo4OgBFpF8*$h?VXaU*SJ{=~P~1KgUlEN%jBPudpy zfIE|p#jUiP^(H-w+i5==Oa_2Ey!(?gS>rw4hknS(i8CI&0XELjn3&;rQI*e5UkEjm zNvV?}2j%7qF-s#U>bb9ETxBBHFwyyDeo*8*S=u!ZiY$wA4m173B7DKE%ofE;J9oJH#&}HXHhIpPJzS>sos4AfD8Wt z{AvpXC$AmtyduBQE9X}t+nk<&L*a_-XtBGmF4g`#k=c*1&psFD$xLV|#Qro=Vn2fe z)59OeuAzl8tOQ=h6BminYD^?Nrt_O4`_R1|Skt}Kx@xbyG^0d?=TVyQP#0mXevFKK z5R7&SBaF&57(5`)K!MR>$^Q;OJJ*itXu`=gA&#c8a}BSfT~4p@S}u2?Cz|Po_Bi8> z8xn&h_h8)QO+Y?x#bCod-oA1`!du5JSVQkE;MLLMwX3Q2IpqVs#)mg9V3&{hy2UNN z0d4b&Ca^z$kH3Gzbc=66dvxW-6f_@L%{JfG?JH#P;f)(Nkn8xPqs90j%7eVnLG6fO z9-WCm7E{n9dGIn)~F z!{m9gShVXWwwUhhEOw49)1a6K`cwq3AYgR>p&q13eik5EHKwM=#XPD~jloa}(|JHt z%StSEURL>XwbGFmL0L`HM4bv5!2Q8(T$oMEJhb!S{k$30>eW3x$WkHlF*D7wP7%`< z_ep2kWh8-_7E&3nsB~Fr)4(e|UC<|d%wzyTCoxh@D_Ul?lDjy5WN-Li4){i6cXiq9 zKK(m3z)ho+;2Mc_PBu|AP+%fR4+RFibWyZWU=9G0A>t7T)4~S}=SeEe8d4461Sl9* zW3#Uo1GL?aMJ}L{h-J>L2F52~pd<*7_}HjVDD6{9DDkYMxAYxt;&|EqV(;k2-5g;5 zZid~`Mvd_yn$)I+WJ0JB@~jrwmdsiy! z6|=F_tChm2YIj;sTJNG`&8W5vWuf-&wnp!ot(A#~+t_Eoie$;II2H?>wBGeUha9L< W@dXxHTU`B77mC?RgOTrcTmJyn9_Baz literal 0 HcmV?d00001 diff --git a/open_manipulator_x_description/launch/model.launch.py b/open_manipulator_x_description/launch/model.launch.py new file mode 100644 index 00000000..d484f965 --- /dev/null +++ b/open_manipulator_x_description/launch/model.launch.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Darby Lim, Sungho Woo + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command +from launch.substitutions import FindExecutable +from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def is_valid_to_launch(): + # Path includes model name of Raspberry Pi series + path = '/sys/firmware/devicetree/base/model' + if os.path.exists(path): + return False + else: + return True + + +def generate_launch_description(): + if not is_valid_to_launch(): + print('Can not launch fake robot in Raspberry Pi') + return LaunchDescription([]) + + prefix = LaunchConfiguration('prefix') + use_gui = LaunchConfiguration('use_gui') + + urdf_file = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_description'), + 'urdf', + 'open_manipulator_x_robot.urdf.xacro' + ] + ), + ' ', + 'prefix:=', + prefix, + ' ', + 'use_fake_hardware:=', + 'False', + ] + ) + + rviz_config_file = PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_description'), + 'rviz', + 'model.rviz' + ] + ) + + return LaunchDescription([ + DeclareLaunchArgument( + 'prefix', + default_value='""', + description='Prefix of the joint and link names'), + + DeclareLaunchArgument( + 'use_gui', + default_value='true', + description='Run joint state publisher gui node'), + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': urdf_file}], + output='screen'), + + Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config_file], + output='screen'), + + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(use_gui)), + ]) \ No newline at end of file diff --git a/open_manipulator_x_description/meshes/Plate.stl b/open_manipulator_x_description/meshes/Plate.stl new file mode 100644 index 0000000000000000000000000000000000000000..d496b76b9debdc4767872e96865bc3fe5faa162f GIT binary patch literal 354484 zcmb@Pef;HPo&P_W!MMK}tfq34+>V)sFqrwCbKLCINtVCP1DO4hS%2MraYb%wl z6eSX+l#MdqvLV#`}1j<|G)qDof%ED`h{0t|L&*G>Zi)>k+YlO(fe;aSn&D<*HNFhQhUQ@kN)4_ zYj5=bUMJM08kL&nq36$8^yQO1Lu*lk82iqtnNcIurP?!IamS3wB~#9@46Q{C;wxvqG&tsBX4D9EsrHODhYKeEH2rkT z&|1_W4p{xA!O1^oMvYLHYR_2fttU^O@Xb>#Lu*lkc;#na9Q<_JCiYoqni`=l)t)hb zzw;&^zVZE*p|z+%9JbkugZ=ksMvYLHYR~xl&s;LO@$n~FhSs76G2>e=4DS5`Girpo zRC~r-KXu*YCKtTNGPD*oh;Pq(VX*Ko%%~CSQtcTNuf1dPZwDW58Cr`P#J+d@ZE(t) z{x05+8lf)Lp7Fvd4^M9XU&mR7)}jWn(J_A;Y&Xe_8lf)Lp7Fu?ODCI?kG2f0MGfM> z=avm#yVIs|95q5+sy$=rom2W}|N3yt&|1_W=6q_|;E1c3Q6to)+B1&cb)EjdJaC9* zXf0|G56xUQIChtp#c|XKb*c7@e?4xK{#GX*U>RDA8pKzA`ut$+8<|lf)TP=p-n0AW z{g=M6z%sNJHHc6A^Yeps-Z~?WqeiGpwP*bBC)@Tf_)ym}v=%jpUu^LFV2fqUs1fQ? z?HR9}Hm85r<8v)TYf*!^=10#BmR$7mIF1^jF4dm#zki%5N7W5CR-hr6$1 z8Cr`P#P9cdcJSmIHjj)Np)S>)vE{E0??1eFie+dmY7hrcd3G@KWm`l>jZl|r&$#EL zqx&08q~FCfzS3IMAojm)>EOFhGNVSQOSNY_|ApiFXa4iUmZ7z%K`c3c>0rH^w~XVc z5$aOy8UK~ePs?_>!!ooMHHd#acIn`EUwswd^9rFZ)t+(RXW!Gm?@QNNhSs76amn1J zgGFa=6&W=`U8+6f$#wvkaI)TP=pR$cXs{@bsfVHsMB8pLNVUox1x(RPthBh;nZ zGddqSvp;d*Zx-P@sI{m;y!%T_2Ggc)9~m`5U8+5!d&*h;hp#-xGPD*oh<`tS$>5eH z%%~CSQtcUMr0cXZ=3RX~jzeovgLvXoO9szBx9@401YEou;W|9^Dt zEN1wp1VmeNl|+0JCvY5Eiy8zz$FnY*9T`H{ClTrDQtihP-zdw_TGSx$eZ2iMuZavH z+M25*rhVho{tF*D*)p^iH3wmK1 z(r^2$voEm>twjw2zw^5;+&MCYXlt&Lh%=65Xf0|GIA5*1!)qf$h_>b`i8$90`YkhrXlt&Lh>?tCXf0|G7zf?_y9TVx1fBQm6`OSOLoFGyq2&hG12hSs76fpO_Y3+G0L5N*v> z67$lSde0*_u?($64FcoqS!?ee8A7x*S4qT3-!iloH3(cM9QqSx2+`JDB@tIFmZ7z% zLEw7l+B5fv;}D{)xk@6gyeva&QG>vB+e#nyVzfk*+!Se|)ZGXf0|Gxc*$c zm>EK}HCIV&m9Cvb`iFNnby#L{EEU*l%MGXS;0gp|a z9~nZlHCIVIc8EJJHigTOpf|BK8JqOG|~B4)fSLu*lkzvrNeglKE7l8Bi*%g|cXATaND z)@jTTqOG|~B4!{hLu*lk!2IRq_c23=w&p5{m`Sw^twjw2^RS!l+>O7L5N*v>5-}re z8Cr`P1m=@3ZJ8lNTXU5}%xqhR)}jW1dGT#;WQGuJ%~cXHLvI;ciy8#x_uChlAw*kq zmBeZ{FPQw%^wTXvYf*#1I>o1F_Tv2zqOG|~V!hjDOul038J3~7s6k-8WuJqYAw*kq zmBcrnesIypPdn2xv=%i8tQ&pe1ZD`))?6iV(bMNF`p(H`S%%i427&djgHC6L5N*v> z5;G^SzWyCgd4|@a27z_F^IK*J(bilgal=opo_B0yXf0|Gm!8lJ^Kl4aD~d>0mueqh zJ@@-_<`3gw}h| zwWvYNTz%zXK7R<&)?6i#&Nvfkeb6$r7Bz@tx+@R!c~FS9<|>JF=A20DgO;JSs6l-G zgDVg7`CN#$<|>Ia0+>kagO;JSs6kxyt(Av)Tp~nUbCtx&Z&*69{fS3ghSs76@gEPb zJj~-OA=;X&B+`hilhy|b`i8Qk8r1e3|&|1_W_I=$d!#v&< zqOG|~V(Af^bWVJ~kEyj5HHgPgU1gZZ?LxFQS4nKM*XErUFZOE#twjyu>C09b=Iak3 z+M25*(iKaG*Em{>8pIp!S!I~7ql9Q{u98SsUL9VmX)S6H_cc?7`Fc=@w&p5{FFZE4 z^P#oo*n6(Es6l*Y`zgbGT`NRebCtw?r*%6|+`6r0Xf0|GhrMyiFkhbw(bilgk*?@F zX?@T#v=%jpjZd92%<~LFv^7^rq?w5h=Q6YwHHiKfrwsFai4bkgRT622rNcQHtwjxD zuSHXadEQ5ew&p5{G*i?`>x1^U(puCYK6(F?VV=JdqOG|~BF%Vp()ysyk!dYz5a&HV zWtivTglKE7l1MXmowPn^8Cr`P#Gccq4)c7X5N*v>5@`mqlhy|Xf0|Gm%n4`FweIO(bilg@xT?QcHXtO&*^I|Y7j4;GIf~O4TNZGu98SA z4V|<;Xvd+os6pJ&P95g;40e%t5}N~ z1pXpS2c{KDCBXf0|G_~py{GK8?-E2OJSwP&QA9G%$v(P%Ad5cmbp`%8qd-(94u zOSNaj8OM%8Yf*#1nJDj*5yH+NNLQC?&xkXpWoRvG5ICde{XRn2c@XL9QtcTr0A^2s@u6U0td@BStcop|z+%V1$$RUkPF35~Qn3wP(bLtdH-Y)}jW1ky_qI zCWMWzkghJ(o{@H*bz-ktqqV3(V8ogC!wF&IM5L=rwP&Oqbe-69*Jv$j5EwbS2c%&^#duC=H^VCE(73m3xXOOUQE)t(VEMV6tp zs6k)`DerF=!sdODt}fM{5i?$vp|z+%U?wc@(-*?#uaK@T)t(VEcb1{Gs6k*xFZUY= zVe@cESC?wfh#5%B&|1_WFf*C^CWNs0M5L=rwP(aks%2;`Y7m%V&HW!j*t{sx)uq}q zVn*09v=%i8%oOK779nhY7wPI!?HMt%Z5djN8U$v%b3cv{HcyRob*c7@n4z}}twjw2 zGxxc#NC=y6N4mOHd&c#5EST8OdzTumMGXQg5V=1}2wOKmy1G<*##T4Zn0WJ4>%nTY z7BvX0q~ty?A#D8v>FQGL8FxSX;QW)km#ooR)F7}Tl>60$uyq`yt4pQe0)7x`MMWoRvG5Lhw&+=a}j5$aOy8L`4^8Cr`P z1Xg|zKARaeLS3poBUY*{Lu*lkzzX(Pj$%fQP?u`Yh!uCs&|1_WuoC~t+03XB>Qe0) z>6c-we~`6U7X)?`?6UNA_E{+V2SKPywP!5f4ai!o3j#YMzIQb>ZQnSK8lf)Lo)J6f?0c@Y zs6k+7-7|YKqeiGpwP(amK>sa~TC58KI|To7$v$x$H9}pgJtKBBT87r527#T9zn#jA z8lf)Lo)J4UEkkQjgTRi>HUDw%IF1^jF4dl~d^aF#u`USgJiX~VdqqZ#P?u`YSiT#O zwOAJfcF-R4w_aq_2z9CUjC4gm)<4KvtP27=f%oh%qeiGpwP(c6WqUug7BvX$X#UoR zyKx*fLS3poBX(e0hSs76ft}eKe!mkLH9}pgJtNH&jr9+*7VCn*KJ>dEnTU)Up)S>) zk!HNc`UhEybwOaK{DB)VqeiGpwP&Q6yRrU3)?!@{*zy18-RH+~)ChH{_KY+GIo3bO zTC58KoeM8Na$aQA2z9CUj5L!v)<4KvtP27i7<+$s&&a3|>Qe0)(J_KKOY7!fE!G8r zPLex6zei-$2z9CUjODulS&MZ+pd)6^cXp4A8lf)Lo)H~9b{txZ8U#9f?!9SlWYh?C zsrHPt(lFLP$Xcum0v$?w-n(05)ChH{_KdV*GS)xHTC58K{aCj?zH4OE2z9CUjODul zS&MZ+pkr+5vRxvhMyN}5WO#?0tuARTY7pp;+hdJUMukw9YR^D7VCvKx^$)TZ>w-WB z->RFwHjbl4s7rOuNIi<9{z2AaUH+@?-ezaX2*UaYL8wc$XGG_ut=Hn;&RWzU@ORsI zx1AzG2wIsR4FKZta7srHQcBu*s4`UhEybwS{B zeAioE6B$BS{~!o;srHQcMp=f|q6UHQ<7?kNJ2Hf@{z0UxOSNZ2XR&2yEou<>U3uUm zvm!$X>mNkAx>S3HbOSb8iy8!eA3I;zF*1a({z0UxOSNZ2r?(x4)}jW1-}&3FnHd>E zSpOi>)uq}q;*4V%T8kP4&R0+0y+dROVP_nqt4p7BvWrgLdCQe0) zF(T{WJE*m&L0~+$?2K(ALkR01M7p|Edq&)u!ZNfLH3*DL-*(M5ks*Zj4(5y;nIVMr4)uq}q-sATWvJ9<7 z4FdB4FI&20WC&sXgGg7GYR`x}8(D_dq6UF^jraW%Gla1IL8PlowP(Z(i)CmnY7m&8 zdDlZ*#Bm5={ewtXmuk<5nIg;3TGSvg&(u4X8A4e9Akx*P+B2jZu+du7ATVFH>rb4y{EE!aDd?W`+>fKZta7srHPxQ=w&O zEou;0r#Nx(E93nT!utnPy1G<*M%;1HGPD*o2&}hU|7~UnVf}+hSC?wfh&xAGhSs76 zfpw!(&u4}Z)<1}Jb*c7@xC5qTXf0|GSpWLYzc51x>mNkAx>S2c-22lqv=%i8tmFOj zSz{Sd;|Ie8p( zl|-!YS_WN6rv`!b;rzK0!q#i~e$Z7Cu~KarbRmNp1lGCp_d^I-%eg^(t(Nz-ZmwK#!khPS+K9GF=5W@Cl@OPIkWKiQ@+cfEnGuA)IT6BTHewKV5 z6vFnG@ccnnNu)F9SpOhvDS>@C`Ft*f?UUhokS=6U`*EZZz*zqvYtaP)`-}3pL^B_Z8_4P8j527!H2d3+^=?OWowgszfEBeJpnLDo_N`@QlwQ3%_A#qkwg$e{MW zRT^22^$)TZT_CV;EsuAFuzh44C(>0Cv4hUOgLEOC8U*&=<#D?ZwjYk;UAjslb^=-k zUC5vYfqjJe`a=lY*T-=?T_q7a8ZCn^WKe^^e#m?sC4}uyDwsUzD|!!2Z;HT`Pp`7v=RJUC5xuzqV;&r>q@^zCsNG`)u>| zxe&JRmDja&l|;ItAL}1vEhVsDIL|W(Vf%M^eNGoLsQox%=dvA#zCsNG`=0ZBi4e9A znez;El|<~owhX$EP7MP4xAVM@5VoJ1^CfhZM4BlY>mOt-C9n@Y&tC~)`@%WzLl-ir z{cn|KyvF(mS&J?Z*iWD5;e@dL?VP`&t0dCQ-B|x1Ybk-gfIOclgzeMkJRDufppN%^ zx&A@cq6-B28}htpnzLlbq4gVZK9R1HNHeKp{e!Hf#IdRWBhT*&VSN*v7o`gs)P5Y% zF=BI;`U*7&^qb^)Y9XxugY&y|m4tKyS|1B*DS^J3Jl`&a^|5fCnl5Be`*B1Ej~$1; zLJb1_KY86i2T649wQ>K|k+CD6ggHNB=0!usyGu0|I!sQoyi<8ahJ$XayqU&Xaa&q!U3 zwrW{(l|*z-VlCDB2U!cp0Sy9wH(9UsUP};m9Lu{Zgm-{ihSs76f%l(l#i^e&6ZU=} zU0tfjKD(1R4y{EE0-s~9d5=E3AnbF6bakorjQB=bhSs76f$t;N?wdvk`+gu@U8+4J zI*TnsYf*#1?@Hd!AcTF-k*+S)o*~_Ujn<+Df!{~&rDz%Qe0)(dli+p|z+% z;CG&TLYhVh``tylx>S2coN+8eYf*#1`HFj6nnnmae;{35sy!ploR*=rs6pVo%RM?x zBZQp?k*+S)o)IGe%g|cXATWO5UZbWF!p`SNSC?wfh>?tCXf0|G7zc6BRMQAy;}WE+ zOSNajh^&wApw^-Wf$<#oel?8{Hoii-x>S2c+?m2Mv=%i8j7#%=I3aADh;((S_Kdj0 zhGl3iY7iJ-=Y4%b*mxJ|>Qe0)G19jTtwjw2*9qKH*fc`exE<;0QtcUW#bOy+iy8#3 zcepolbX5exu0N2jF4dkP-GGhOq6UHMHtz9k8X@dD3hC-n?HO?g8aocHMGXSipLyS_ z5OzI?bakorjJT7IWoRvG5V(%c`*($~>sq9%OSNaj9f2%EYf*#1d_dlZEQDR3BVApp zJtOXHWEonE8U*Gw@_uR|Y@Pw>>Qe0)F~edRT8kP4=4ZH{5`4Xh7OLZKF&n;Po z)}jW1c_!{jZudEPFOa zv$ghq)LbPIGs2dkwWvX0KAF8IO(TTO?;>4Ys{J^m8?ez@)F3b~&K{bk5yIxFk*+S) zo)I(jb{txZ8U*I|*$ZSnKe&sa{jF-Ql88GMT87r527z^o+#e-`tsAhfh^~@|J1$xV zT{uE&5Lj=?eO^M?`Ule0rP}{iapy?O&|1_Wux^z5)r7Eh9HgsDwP(Z~FfBuCQG>wx zSMIwL!q%gZt}fM{5qI*m46Q{Cf_;7kVe4v0SC?uhv>R~Sb6)DQe~=o)Z96x;d>l1G zU8+6fBIzI0TGSxU-fE>@em`o2x>S2cdIuM!`-|dl)oLwj5C>0RxtBjzH9}pgJtKV* zC)52!EkkQjgSd6&ReJgRQ6to)+B1&6eZl1M-ox8!Eou;N|MM!n{5`J`>Qe0)-#g~y z$-iFizYJQ78pMz8ozly{S2aRisy!q9!cC_8i`w5xYf*zZVlcIre|Kwyx>S2c`lTNA z4{9xH5I_I=s=a*vs1fQ??HREn#g0R3QG>YR+|_#dJXj;trP?#nnRC=XsI{m;ocZ?E zd-;4`Bh;nZGtvlP)IX@Ts6pJn=d@lPm(&P#srHQb@3VArqxbpq2elS8h_|h~MlX-A zYJ|E}dq(WoviC!4QG@vM18em1II%{kOSNaDk>#j=P-{_xn1AV-y*%En5$aOy8IK>m zN&gck`SS<07Bz_5-nmvUkK1d6x>S3{jET+r^S|!TAJkgZApW$~+P!@JQ6to)+B38p zP-{_xc+;=f?&a&K8lf)Lo}t}~(wjy0%8BOSNab<^$dSmwx8YAJkgZAU=A>y1jgTUL(|{+B4D>{iuIXYf&3<+4S@J~cvJsy*Y*`ws6f z{+U02P-{_xIPBT=dwKq9o(fzv@`tt|17Bz_DPS~KA=izFEx>S2cnzS1x#%~yv;r-BB)F5X3!}MNWH>eTnQtcUOrD4=RsI{m;Z2$4;y}bTWBh;nZGj2)G z^Sq6YEMmD7899j8X9OSNaLdHR|C9dCcoR+qFEHHgh_ncmCmQ8hwcsy*ZT z>0UA)__IHMP-{_xc=Q+3dt3`^ni`=l)t+%c>L2{$KHsyIIITquV%GiBdtB3Nni`=l z)j8wq`+m>XAhi}X|5aR@^o$_dnyVzPNq?(9{>h&|h<`h4QG>wWEw9%K(bilg5%1vq z^!;eG7BvXG|6D6>8X?-6t0dx+IDrhUMGXR}}H7BvX`K5{Qb(+FX|aNN&8 zS4sR+`fWe{Q~vxxy6~x{27%xCyuU<7z}L|b!}M2z(9Z>6=UjY!uY+*4@lgSeUi(bilg5mzjpA+>P*!JUV=-pTtc zg=lLoWQ-GW}}H7BvW5 zN9X;!LbNqkNt`x+^Zw-5{TU-#iy8#x1M)s(A=;X&BtG}HP5Nt`+!T8kP4=F9RveIeSKt0ZFP&N8$XH3-c6u}8qx2QfQn$5C^YM9e^1hSs76f%!}J zI@tOkW;rdR<|>JpNwo~EMGXS;u z0_#S(UrmU%<|+y425htzH3+PK<-R*1Yz2$`YIK!^bOSbY;W(&4U>&dCKUi~BiFxmi z=MQQvY7p0d^@VOej`Xi0(bilgk&b-+m*e?^T8kRQ!ojj`em{h0Yp#;G^r;8u|69E0 zT8kRQg%3T~&7UhF+M25*Hvh?tiLK+atF@>>^rt-A&EF3p+M25*Qb+E@pW-{HwWvXy zw!@Nc{+|CGv=%jp+Yb9{H~(G<(bilg@skDTO?><&fBvA>q6Trn z$DZ!y-(4ZvnyVyYCx;z})}jV+)#RVM`TQY7TXU5}>|wDCtwjxD*$Yp0^LbE+w&p5{ z*jZy4T8kRQA$vd3&F6C=+M25*Vuz4rXf0|GpZoOV-8?Q4qOG|~B8_Aw()yrfXf0|G zpZw$RyLo&iL|b!}MC{nI46Q{CV)}uPb@Mn;h_>b`iP(8&8Cr`P#6621>E`jS5N*v> z5^03o;h0)$QG>XC?(e#J+%800bCpCI>34W-ptYz$oP5K>-F*EaL|b!}gmeQoT8kRQ zHx7NMo3EpUXlt&Lcz(*9&aHp)XT@tRY7iSg|C?^U9u%Ujxk@74fu^%~6@UJq)}jV+ z>J<-m^L4EdZOv5@=}Neh)(7o#rM0L*yy2f8=;rHlA=;X&Bx1*}WoRvG5DPo^ck?`h z5N*v>64DLWXf0|Gt895+H_w*{(bilgA>DwD)}jXSmyLei&GSA&v^7^rNH<`kwWvXy zKjYqRp1%^Jt+`4dc6LAmIMr%>?UzOK6k-@v3 z)?6jAeLC{9=A4W5+6MnB)}jW1zerxM6~c~#{eyIsM7)ERK^Lx_sX^eK&FjuW*!#i$ zLApvJK8ce!4!V#*4FaFgynZf(eXiI)NLNY3H_9^TLIyPmeADti03q!A!Tv$IN&?q{ zm@UF_(1i?Y5ctK&`x%6=?>YMi=_-l%g|iI0kUD^zq{-oqzf6;_}7kg12*~!H3*!E@;;e3N8!lr{K5V~x=JF>oc6b( z3+dD#a7NAheT1;{Ao~aDDv1~YSO#6lpay}FLEg6{gq_dXKS)pytVI_Hj5zatI3aAD z$o@gPN+bd|)vOr6u& z=TCDjgDzxHgTR$#-uEhmT@SK{FcD@0p!l|>s47Bx1(PGU!4EH3-av<$d}> z*!&gy2k9z_n7OkIx{yH)0yBEK-#`ePhhzUBT_q7Skd{FgGN?gdW-|9p2x0Sy>>s47 zBw{AjGU!4EH3-bG=Kc>MY+e-U>QaqLW8Hv_)}jW1nd02X5_8D-SK0h7`v>VNiI~~8 zbJln}OV!2UtHN@ByCW=tFq&mW`<>C_;wl9Kzpgs}Aw_7Bok62E-z!TBe8 zFIhtuGN?gdMJV^H31RCv>>s47BvOal==pO@P=mk<9ee)l*?=T$U5)*Nbd^M`LE5JsuA}$gcz_IQ5Lju<&rz%q>QaqLX>aB8 z=PXL=gO;JSs6k*|H9rrsMyN}*XT%DxWoRvG5Lo%m&$X-(>Qe0)Tlw0yWoRvG5LoBV z&*!WW>Qe0)vEptST8kP4R^sz>Mr(w+RC@-l1+kkPzi?WM8U%I}r?rV)umuk;Qztm&>gRI56Ah5$DKfks{s7tkHq%+Rw`GZ=E8U%K#mOt-)&+qbH~D$OH9}pgJtK_(#`*_Yi*-R@=TLqwa*a@zYR^a`nX&#s z)?!@{*a4NF?_49)rP?#nh-~!yL9Im%0z0|#bE<2Ex>S2c8d;9@53&~Pg20Zn{JiZN zp)S>)5j*JYJE*m&L11THer|Y;P?u`Yh@F6zp|z+%V25CS{&|g1muk<59gUWuwWvX0 zr(=GOdyP<+YR`zBnUqbR|62 zKge3F3j#Z6FP!qq$fyzOQtcV(ihitrkhNIXh_ny+#v7PXBh;nZGh*kmy&u?B%v#hS zu%r2qvp0+5s1fQ??HREH+cLBkH3;m?e)0{QM@EfMmuk;gz8jFWSQi9#n9qFK7LidS z)TP=pmhT2+E!G8ro$}v(k{LBZU8+4}`EEegVqFl}@xR{9TgGwJ2z9CUjODulS&MZ+ zpmX7OUwswdb9rK6%~cZ7n_)AlT8kP4IxrTUy;Wq?2z9CUq@s z88t#(sy!pkZ1>appk-(+Y7pp~x%0rSBcn#BOSNZ22ajcFEouusKnOPEn3)TP=p&<&V6^+x@Jti`$@ z(82foqdUZL)ChH{&KapkanwJ^TCB@|)y!Ybl#C#(e-MPaRC`8rPTE>3{_U(q4FZ3+ zKis-wWC&sXgCNwU+B4!Ev<$694Fd1~kFK3H{+@%dcM$37QtcV>Nt{T8^$)TZ>w>`N zc-CdJBSQ%59|WN;)t(XGD9g}V)FAMEy!|t;i3}mEe-P>FQtcVI7Q}23jzeovgTU{~ zR%gzM3?Zz45b5et?HTb4XBk?H8U%hHhsW&{8A4e9Akx*P+B2fl+cLBkH3w>@;_2Jcai3}mynyVz1 z?;m6>)&+s_!(P8-h7fiK0TAj^?SCuj25htzH3*D@ZvNh`aU4Qe{~*%UrP?#38_@a( zS&MZ+U_7_#M|X=1A*_E8gt}CFM%yJZ!!VDp-e-P>FQtcUW#o`%K3)dg0 z3j)_W*PgjY9ET9rKe)WBB%~X#(OT3XaNYLO%snGR2Ral{>H>^QU*H3(dP zE?&$GA*_E8>FQGL8F42a%g|cXAaEW1$Vv0!IE1kNL8PlowP(Z~fhvIz+=_}b zT8kP4=Kan(jTu6;HCIW*45VdfEou;$zr6fDW(d*NTqO}Rsg|L&s6k*JcGI1^@wXD9 zt+`4fW`r$6Yf*#1eDb9&GlXbsu9A>$z(#9PgTTD_wl^|E2Rdmc0Nb{txZ z8iaN5y~qq9tbY*c>QWuwLBF@5WoRvG5Ll=9^voW=gPHLD!IZ8p)t(V|T(k_WMGXS$ zE&CkI3?Zz45b5et?HO_BNXyV#)F7~K^obLgA%yi0B3)goJtOXbX&G9J8U)tA4mzC~ zLRkMG($%HfGvZF3mZ7z%L0}#4{FWI)v^7_iz>1<}Xf0|GSdYxd5v!IUtbY*c>QX)W zTVXBLGPD*o2&}8-_aoL)Eu-ctiCE#a46Q{C0_(&1b0vhW*Yf?Kt0ZEj+A`?E5mJM| zI(Pnl2x05a{9Msh60zcL8FV3o8iaN5jlSnX*!nrYA9R%jt_86J6ysgGkUfXqvsE5EouvKuRKl^!uDTrd_`ADq><(5`Ga&Jof-u8t>y8q5VnsD>FQGL#}PZ| z>^;|7)F80`E|1%Vu>EjISC?wfh@F6zp|z+%U>{+={t&|U^>N%zS4qTC_;w z(=lI131RyadHq3GNyN@f%b*Jx)F800GhYu1Vf!q39Yt43NH<_Z7c!_pV1H`9t`)-e zi}HGqu98Ss!ejk|tfd6@+2-qWA#C3(uWRW-2DQKE>56{z{6T$%8U*$W=XnMpZ2vB= z&*>_O*tu-)2VF>~27!IgdA>vl+lS0~2D(Zjc3@iuUC5vYf&JTg-bV=APtExfx=KR2 z0c~G6Ybk+!=$vyJJ-Y_~D%%&%c^|rvLGABH?1;DdM16%C1oqSCc{m|#e>>-|=qd^6 z25jg;IyDIN1#k|qX@szS`kaTOt0bfwu%QbX)F9B`z`4w(5yJWnIG;#YNu-(7(enrC zLIyR6hg1JYp5GP1`X-RBF4g{iM8}A|A6knV1o}<#Jhc$k|ABOMsrHOCvpv>7$Xcum z0(~=izFi3GW8pkCT_u6>8%AXq!_kFwY7prE$?FC}SU(Qu+vzHaw9+to{vch*pay|H zro8?kg!L71-GHu=NGm2|{e!Hf1p2YK_G0&FBw_tgT>qd88PxuML}!?-73nL~AkbHq z*Q126J}<80&{Y!A;bs|hA)Oio`s273)-*y`zZ%!0=qicm)U&lax{yH)0v&u@(`y3MHKZ6kVJx98_RQqwnFPvp)Eou<>ea!nZ zgs|T$q^nD{XGEvBWoRvG5cr+X`%8qd-(94uOSNaj8OJiT7BvW*ukt<_A?*Btbakor z4Cw}Jv=%i8oOiiLXLPQ`zsk;oNLQC?&ya3FJD;-_>w>`efqRWcV+PBpxk@5NGWNI9 zTGSvg4&t7v@#hcLTqPmhfQ{Co27&P$_kNAWaDDu(YOa!qJ5zwLaUyH6E(nZE^L{uX zY@7%}U8?>4h&yaphSs76f$=r>;-!1azW;#E^+#-C$5C^YM2z$;Lu*lkz;y!m6plZC zu;wa>xMHyktwjw2*E`%B*)&4f^#{_`rP}{i(hb;XEouM zY+e-U>Qe0)F(YglT8kP4=9AfbGS+cu^Sek_mukv|ID2TCMhKgyM!LFG zdq&LA+i_?uY7m&;&wWKg*nB(E)uq}q;!cH@p|z+%V4WiOM+sr;21r+zYR`x}E?S1x zq6UHWmfYthgsp!dU0td@Bkmk&8Cr`P1lEmmznT!Xj)Qb{srHPx1EytYEou;0*~)!) zLfCo~($%HfGvZF3mZ7z%L9oxSAgq57>FQGL#M`gAdfq?D`rxuh-#Q|oZ*7{vmWM9u zzV1H%?@c3wtq(5mDv2Ne<(&E7iuFOQMGfLL+br$wb}ci6Xlt&LIC#l}^G}cai)t-u z5Qn_@bobY1Fhhv8<|>J|-7;fhx46Hk)}jV+{=H9j-!PLILbNqkNnCi-f{7dA{-Roo z8pP9!{?PsYj}N!^scD30Yp#;m<+zh4Zjbdrtwjyuy;nZc-FF{m2+`JDCGqop&ztz4 zxWA~@q6RVbvWL2zZ~sFahY)SeRT4i-&zd+a-BSkRcCAGX;%iqu(EZ~|%n+ijxk}>b zPhU6j$mi{j){WMp265o6_jbS7c}pCJ5N*v>66>eczA0}!-i||SQG`lmNX zh7fJdRT5X6{P4udeuwc!Yf*!kGv}`Ef1Y+&WC+pLTqW_|v_7~=++S2{QG?jxoIAQ# zpL%Fy2+`JDC2?_DAN+5><9nmEs6l+>soT1rI_8kb5TdQQO5&N_*XeA2pWV5>(OT3X zo;~H3?hod^DKdm;Yp#;G<)}?MFFVEW*{`*zLCl?gWA}T1J2*0gXlt&LSbg5+oyRWq zXAfvCY7o!gc|&)LD-VhcA=;X&B>r^Uww+I$+O^NF)}jV+*X4`4mn=LmGK6Sru9CQ7 zwK<(#p78sNYAtFIum9Q)yN~^H|Hu%ct+`6#@F(YXKD4UeUsP*RgScnW54sl}vR`Bf z(bilgaZ~y`eKY;N@O!1Ts6pJn?EBqM4i-j+5NS==)77OK=k})gM0&r^Nbfr`v=%jp zYu|cx_r%R!9~nZlHCIWjck%(9Iq9A<$k1BUAg+4os_sT7E{F^v+M25*-kiRF7pLzb zGPD*oh`)XETit`cyKiI&(bilg@vHPZ@~-rIf()%i4dR-&UC~|k=-!bbL|b!}M3a6G zf1G|7k)gGyK`cG;^6t~??G+h9v^7^r9Fcz4*GT(6kfF7xL0o(GW!;y*#&@bTjSy|k zRT8gH=cjvj^81TwEou;tEdF};*nNFxMAHb-)?6iVXnJzQ=KpC=YHG9=HHg<8cuDu# zgMCMHdX9T0+M25*o_W_vopsK?#Qs)ViyFk2mwvUo)?vQGylI4JYp#;$r*Y3gfA9Af z)mqdbK6Ts0-M75Scl5+6(Bw~OB8>w{X08pKsUy{P-Sg}cV*N{F`RDv7_Q zap)BTdy-kBwWvXyv+OI~AJ6ku*|egRiMHk{iF49;`s01Szo^!t265t>ztVkZd++RN z8X?-6t0Wet>w?}YznPEkht{G7apl8b?moSdcR{7QuVtdGxk>`pH@Ip+hSs76@o!)H za`(BHyeq9~glKE7lE8By?!Vw_TOZU~)F8G$=gYP}=w|~V+L~*Hgsl&1Eo%O&{&t72 zY&A_}fM{#3l87U>wLAQ)Sc@72{vs<~>#I(qzm=`s)m$YJ@1SL9Eou;WXE*qouLzCa zbIYi?N+Ldqw*H~Ds6pTpI{Q<;S~L3Wg0RmO($%Hf-@*7sS%%i427zzdk!Noa-*X}C z`+;;1j;R5x0S8U%j% z9{Zrr+^1a+nXun0q^nD{XT&eH{jIbXH3Eu-cti5LM`hSs76fsw(=i#CeC zl@NA5N4mOH``;=?GM1sWs6k+a^ZcV5MureJEFQGL88Je(46Q{C0wd?Mzr9Xm2w~%0 zq^nD{XT(U~GPD*o2wVZYW39C#LkJtUBVAppJtMAIEJJHigTR%{n?AH=WC&r`A4pf1 zYR`x(FU!zc)F5z0w${>Vks*X#M6-JsPx$>swH7r9Tv;A;_Ue%#gk29J zU0td@W6n>v?Hq7w*N#JLQG>u0@?Bf378yd=buH4>rP?#%irzA`7BvW5>38p%8W}>^ z^*PejrP?!MX2LSG7BvXWSj_wEl*kam<{6N#F4dk9Gc1;&wWvX0=H+cCtP&YQ*nA1n z)uq}qVy4J4v=%i8%pmm+T{$v@uz4S(t4pbakor zjF`Ez46Q{C0yBDhzoUr^A#5HF>FQGL88HKC8Cr`P1ZF0`^YZ_@p&)EN5$Wnu?L^F^ zT87r527wvY^)7fZGK8>sQKYL&wP(bPuw`g1Y7m$y{-@3hks*Z5?;>4Ysy!oSwk<vY_ggpoTVx1f^VCRJmuk<58G6gmTGSvgbAQ(KWsxC-&9@_6U8+4JRvIirYf*#1 z3dBKsKOY%F*t!AI)uq}q&P(?feSO?tRBKU#z)H$%FM2LAgs}Awq^nD{XEe`0IR8Cy ze^ISP4FW4dAKZZ%LfASE($%HfGhzkH{#IIx8U$9h4twO;I1V9fJqqdSQtcTZy7cOK z$H)CewH7r9tPei(GiC^3>uN|>muk=0@bas#j|^MwV=dMNao*u8_40Al2z99*{jC)k-kxrX?@WCR$7Z1#OzC__44<;MyN}*XQW?-$+SLb z8Cr`P#8cm1vzLFbYJ|E}dq(<&n@sD2mZ7z%L45tYYxnZ+ZjDfvYR}l<{7WX&6`o~i zEou-4d~4laK7Z5*b*c7@`_8*=@|X+WV;NeD8bovP`n`M}tP$!`?HTD@JDJu8En}=! z)CFZ|USS?>pKu zv=%jp&+oNKFORQkgt}CFMjDaz)B2!gXf0|GkFB+7FOL&zgt}CFMjBc6)B2!gXf0|G zSKjurULNn(2z9CUj5I>-r}aV0&|1_W);j&=y*zHO5$aOy8TX}UlfUol3oJuxQG=PU8+4JU9t4z8mHA-)F5{I&St%Q9aSULrP?#jn>wezyFUw})mqdb&fa;8 zUcMfz5$aOy8R-hLpVkNMZ>6=ULCn8&%U-^&tr6-{?HTDxxS!SsEkkQjgSc+Bt$O+T zyhf-?wP&O&`hHp;v<$694Pw7Hzq*&_8ES;ORC`96ndqnWLCeru)F2Kzf9qbJFR2mg zQtcUOhNYj@2Q5QuQG?iia+_YB_o)%;QtcUOrl_CR2Q5QuQG?k3?rqck`Z#}8Bh;nZ zGt!J#Kdlei%$?Sv2C?HWw(I42xEi4@)t-@N?)qtc&@!|ZHHiIwynQdvC)NmcsrHOC zXW38dgO;JSs6p)V#T|NiUbIH2OSNaDxz~PLAG8duMGfNYBWL#V{BDg!Kdlei-%4vygLvd)vwC^`qeiGpwP&O?mVR0vv<$694dT7` z&g$iLoEo7n)t-^oiu!4N&{lr57Bz^wHk{qd>rpjAU8+3;*EhIo!8MN7q6YEsuCsew z3$rz^f>4)g&qynA{j@%4D{)$j8pNmfnr-(NT`{39)nkOML24~({;TpjXZ))`v^7^r z#NTRuA{zYLS&JG3{%(1_R*1IdDv5XpEkkQjgTVWr*PVs1cM$37Qtf}M_#{r?IJ6cu z2z-w7`neEo%~cZdjj{}_MGXSq$Gi_fh_>b`iL`T|&%Fj(iy8!eSMq)aA=;X&B+?Fu zKKD#$Eou<>ea!nZgs@*Yq^nD{zaQ~SZGS7RMGXSK^Lc-X5cW%rbakorj5y<1hSs76 zf%8?~CnH2#bCpD#IW0qLQG>vFH}CfmqOG|~B1Qm~p|z+%VEmBxEeX-qTqO}B8OzXG z)F3bp%KNW`Xlt&Lh!I%_-$AWK4Fco2ypK$Xw&p5{7+G3|)}jW1acSNUCq!Fwl|+n? zEkkQjgTVMY@9Ptyt+`4fM*5bawWvYhIw9{*6r!!UN+PaUEJJHigTVDp-e)O9TXU5} zTzOfB)}jW1>$bdKRETu1Nl#aoYX6?!k*+ykOnXN0xzbwHAaMPe_q__y)?6i#cFOj- zw^nOWgTQrk-oGn^T?r#yU8?;!;)>pmLu*lkzv| zM&3^?L|b!}M9i>QhSs76f%%!dFIJpDY6W$MGXS;OnHC15N*v>5;5at8Cr`P z1m?@~K7AqDnyVyY=FT#-7BvXW`{jNEA=;X&Bw_~AGPD*o2+U>Xz6l}PnyVyYCe<>= zYDHZTn1{{%A40Sw)TP=#yD=kd8Cr`P1m=@-ABzxe%~cXHvuzn#iy8#x#dAN75N*v> z5-~$>8Cr`P1m^d1Uy%@P%~cYy(qI`{iy8#hDRO_55N*v>5~-tVl08^jiy8#hTXLV5 z5N*v>5*z>J!A0jrFPYY&27z^>+^;5tt^6QeU8?1QN zU(*b)-OB%a(+JVlTqTi?eAGXvwWvY--JUBAf3koXLbNqkNu<@j`DuO7-Vd!s4dT!n zRvK>eQDz9y)?6iV@Q-Iq{A2VFYAtFIckZ+D@S4TU5TdQQN+NxuM*V|YiyFi}x2`FK zI1VA&nyVz<^0`YUK9%|bFox4w)F7rUoHE?#C1wcG)?6i#&Nvfkeb6$r7Bz^aH%}QJ zcf)FN9741;S4pHZ=cs>BYf*!^q&IbV&;_eUh7fJdRT9z-*k~hQhiOp6R5 z+M25*q#Ll&TGSwpn7!)ofRC*a8A7x*S4pH1*{FX|Yf*!E>f%+0@3}DD@7_A9glKE7 zlGt_rrT~18A7x*S4pHR$Wi~G)}jWn@9NWrM_;jFWC+pL zTqW_xbbryCZ}_@BSsI5LE2 zYp#+=GZUl!L9Im%;`UY682<4;|1L6wXlt&LNHZ*>{z0up4PvhY));>0fR{yv5N*v> z5^1Jr)IX@Ts6qVnGiwZ2-Sp*=Aw*kqmBg!0Il8lOckdt6TGSvqcds#A_s_403?bT@ zt0dCQ-Kc+1Yf*#v*t%;De|P6*ks(A|bCtv&4m!T`vfaIZP-{_xID6kUhhO^P7Lg%D zTXU7f2B}Z~i=XrUL9Im%V$%<-Iegu>Ulkcbv^7^rY;wX$oufYK{exPI8pOb`iRgT>c~PxJ4dUB3uQ|N@%55S;h_>b`iRj?546Q{C;$I(Kb9nqU+eL;D zZOv5@^HMk9)Sr9*pw^-W@!e*v;k+NGbzwW#3enbFC6QK4M*V|YiyFlAjn*1&cF&HH zAw*kqmBe?`b-|AxdC=C1v=%jpZ@y}+;ku8{jtn8%nyVyEO#OqWt#__vXf0|G@142U zaP^gTiVPvznyVx}lCIPKbqntw)LPUa4xhbN`n`H>WC+pLTq`872H9vWYW}OH&e?T5 z!&WV8u9Ao&UxW<&t5}N~1pXo~Etot0w*q0u0YY7>{gW8)pk-(+Y7ls5|8&G2ks*Y= zA4pf1YR`yI;v|kkYf*#1C-mpQX)Sjj{}_MGXSqw6A@5A~NDT2*SP} zNLQC?&xp=q%g|cXAn=QE+&SII5W>FaNLQC?&xj6b%g|cXAn?n#>Bskq3?c0I3hC-n z?HTb)Z5djN8U%j9Z#sLQ$PmJQcag3x)t(V&9LvyJ)F5ysdfx~CJ~D)`^9Rz^rP?#% z%xM`~iy8#ZsM{U$y2ud1&Vxu-muk<55rAcAEou-L8N9UD!pIQ9&gV#1muk<5k&I<% zEou-L;jFglevu)BjZ2WOF4dk9BeFiegIbFk1V(B{{_p;gA%u;ukghJ(o)IHU%g|cX zATZ+G^5O#{LkJruB3)goJtIcQmZ7z%L15&3#=!?ih7dO1MY_6Fdq#}(EkkQjgTNKQ z7ykOj$PmKD?MPRbYR`x(7R%6D)F5yr^R06ai3}m^`UC0eQtcUW#(?HMsMVHsMB8U$u6Zan_2ks*Z5 zGay}Esy!oSSS&+pQG>wD%U8}nA~J-q`4Xh7OSNajOp#@1Eou;$LF#|=$jA`F=6#T^ zF4dkP-GGhOq6UGPunVs|Dl&wy`75NWOSNZ6H=xbKu@>usz>MCa|N6(s5W?o+K&VT# zXT%Jo{jIbXH3-a1e&c;dM}`nKpNMpIsrHPRNwo~EMGXQotk3QIw#X2|=0%aNF4dk9 zGs2dkwWvX0rg*>KGD8TP-$lB*RC`9uY+Hubq6UE(?>Bwmm^cn0Y@QnF>Qe0)F+*<| zT8kP4X6^?|m?4DCwt=#9NMfdmM)lwr+rQb*c7@SEc^J{iAJ_+BZL~5882PEou;05!&Z*W(Z;HI7nBQYCn!x!LkgkMGXQg zTZgWFZ2YZ+u=OaUt4pQe0)>29G|gG5HG_IWMV z1##^g|8J0wqeiGp_2_Rk>IT$W)F9?9cxjN|kMy@H2z9CUj9B5d*GwWvWnG4pSO{5`J`>Qe0) zZ#?Sc$+_S3{z0up4dP$6T{g(SS2aRisy*Yb*Pk~zFZu_y7Bz@D+dMzWzq>U;U8+4J zb$U;x`-|G&N^4Prc*}Or4f6S;MyN}*XQVUEsDDsvQG+;Z_OpY09;^}SQtcV(%sJ{G z)LPUaUbV;4K|Y_?2z9CUj5Gq6O!pVHzm?Xa2664{mkc{-Tzl zwWvXS|EOmMd3;qP)TP=p(ui!-Kd7~+LHzrN{yNCx#2TS4)t-??mi@FoXvd+os6l-F z3x65p@otS!muk;QBjkQsAG8duMGfN2;nRaWZm$vQQtcUOq~A~LgO;JSs6ib6^izX; z{ZS*-rP?!MN26tEEouQe0)>B_60?k{Q?T8kRQ#5?{x$k&54LS3po zBV9p``UkZZHHe3<{L>&`*VYJisrHO?CEQQzgLWKRiyFjcFFZNO*XK1tU8+4JUD1#F z2elS8h{Fzfa**d4YJ|E}d&XBja6tdfZ}t8`twjyuzH6Qs)@yIU^>EAZt z{exPI8pLCpKQYMjJ~cvJsy$|De{Q265DvA0OoT#2TS4)t-@NAp2>35OaO4)}jV++)j@V z^1Nt`P?u`YNHeMZv_5DVT8kRQ9{=-)L7v~O5$aOy8EHnipVkL0Lu*lk*zoi}4Dvj6 zjZl|r&xpJk7Z~rY7lRE>i2`ZZcroCrP?zt-{|!I zLErQKL9Im%V*TrXKgjDJH9}pgJ>$43XY{YS-}?u(7Bz^yKlA%RUdO2s>Qe0)Y2~M% z)(35MNo!Gq*yIDhALR9@8lf)Lp7BKLA6)Am?;q4!)F3{6?C%F$3u~Ghp)S>)@s@O* zHfz1_*-D(&q6YEbNBw?~*9U8ax>S#4pc}B!TGaek<#o=;0MXW5B@stH9~twmW8Rk`L|b!}MEp`) zhSs76f#3POzeI?(<|>Ie<5-5)q6UHURo*8fL|b!}M4UM-Lu*lkz2mPj{XqqV3(U>ub9UkTCHTqO}BvJSq3T8kP4#&dZe znGkKwRT43>v<$694FcoRydO@8w&p5{7$IAR)}jW1@pazUCq!Fwl|+p6EkkQjgTQq{ z-k&H$TXU5}T(MY&)}jW1>z%yMQi!(ZDv7xAvJ9<74FcC~dB3O-ZOv5@X@_Y)-Cxu) zv=%i8Tz}?$uR^pnS4qU3bSy(_QG>vBbl$%!L|b!}L|oBZhSs76f%$;E4_Sz|<|>Jp znXnA4MGXS;8hJmp5N*v>5;4PK8Cr`P1m5u(TF6 z2&}i{J})8KnyVz@&XIN;T8kP4){Sz%nh>_~gLHMN_OVZ_U|ELNq6UHWuiSShL|b!} zM7moj?)_;QT8kP4JNW!xB}7|ul|-y4S_W#d7m*qS)+2Ktq!6~E$mb8zRT8l>Y8iCl z-%AYw>#Dh*QwUq1s1oltlzHK3FKLgU$rP?#38_@a(S&MZ+U>`{CKNrIGWq?qZYR^b# zoU#5v)?!@{*w2#t=!LNTB_PzL+B4FbbF6=mwOAJf_T}XJ0SICHWI(7(wP&Oe!07pd zT8kP4_7~;*8VF(geUPp$)t-??GNb1YYAtFI*e8|m&me^DTSB_JRC`7mk&X2avKH%t zz<#fMp9vvs{}l*zsrHOCvK;FlWG&VOfqiTFei=g8J~9yMQtcUOggn+i$Xcum0{ids zeLsY-{cs@ErP?#nNPnzw#?R?)TA#6W2($%HfGo%}^(OT3X zun#@o|4Ink7mjpwsrC%%2DJU{ti`$@u%ABP$4dy?-wr}usy!pk+>P}QvKH%tKwm(< zADIxgPalN3RC`96fgI}}WG&VOf&PYkUo|1D-vETVRC`96NgeASWG&VOfj*6Ve>fqm zZvuq6RC`965gzLwWG&VOfqs*GpF1I}{{w`&RC`8rzS!KW)}jW1zL|W#J|V1+h4;Cm zt0ba>$1>?yk8$(B_Z8_4PD5f27x}NeE&ottgi^^>Qe20E9nMo zv=%i8^ke1wKnh{~QAk&pYR^b3KV$uati`$@&{vl4XDNjBd4W)uYR`~vz(#9PgFt^A z@1>cZgqS-HtzT_)Kg)2HgmeR1-yLhQE(mn+J^$zq1OHbEVSRTX)TMfiz#63W53&~P z@?VwjlNuSeYFTrY#Pa=vtcBx%27$j@zTc}5b{xyQN+RCDQU4%ou`UR_|M|YPLfHEO zLS3r;ZzbJ;)<4KvtP294<9z>JA?$O-=MU0V64DK5-w)PO0^i4cA7LTv`@!cA(uE9a zKaS`up2T~uuTX=)?@GQOvJmz?=ko{YDv9WjwhX$EP7MOTkNLjNLfG#WpFc=fNyIO; zWzdBTY7qFH&-bSm!hU!8{6V@(BF;FLK^HQpLEwCq@3Sq0oj;JSF4g{i#F^7Fv=%i8 zoOkp6!iBK&Akx*P+B0GVU>RDA8U)4<`M&2u*!i5#AEc`!q#LlI3+dD#Fb>N1Zx_PG zC4BxMT_qvifHuBjEhR9X%lDxd!p2v8{vch*p!UC&bOYKrk+tXofpKZRpS}<_PUQ0k z=_(272DI@mYbk;8b$%{@5H{ZB^9Sid2DKkYjP(2Xp6e^rAaI?KpKl<9jobPBLApvJ zu2?LCE~Ha~!1YdkPJ(Bh$3?b}#kk22at0dx1I+j5fGN?h|Iyyi9LkPRBCY^Kpc*c^^K1kgk%5886GA3mMcPFkhCR^CN`KU-9{a zbd^NR+*t-)$e;#+dB6O;A|Y%Zj?W*Yt0ZCu(lY2m1~mxGU*_j731RbzeEuL^B@r{J zmO&RXs6k*JHa|Z~2%8t>^9SiF3F!uG=t2fH2+Swv=U@q8^SgZhAYCOP-GDYv&00!e zUOYd~O9-2%=JN;XLIySdwM`@4fHvRGT6BTH{C<8enGiPL&gT!(RT8n%VDATANT&vY zb&C9aH6d)>fX^SKt0dx%i%;kTRU_1;+B0IM+A_2jH3+O==kG_2P?u`YNZ+W@^9QvSH3+Q4=kIxqP?u`Y zSiT#OwOAJfb`S3{^4)-}#kwG{GlIXBO;aP(r8?e^<@yI%i*-R@hetkt zEZ;xK-(9*&BAs!@`UhD{3G7tK=fN6*x^$`b<49-DvHn5UQUW_}^7*_*pe|jiJtK_( z#`*_YO9|{8%Hxt6fx2|5_KY-=8S5WpEhVr6Dvz&f1nSbI+B4FKY^;BfwUoe4t~^ex z5vWU-YR^a`%d!4J)=~mH((-t>MxZWTsy!o(kjMH5SxX7*tjpu}8iBfWsrHOC(jV&| zWGyAILoi=|)Ckn2OSNajjz;@UqzkW5gTPM5d>vII)TP=pVrQmhXf0|G*s+S2cx}qQJA7m}o z1%aKwd7hz0s7tkHEZ+^tTC58KJDNGi(KIzeU8+4}`EEegVqFl}na#PHrl}F?QtcVb zcLTB(>w>@zbIv(6O^r~OYR_1{8<4eF7X)_7=Xtmqp)S>)k!J43`UhEybwQvnAkQb( z2z9CUj5Gr|)<4KvtP2923wd6&MyN}*XQY|bvHn5UVqFmEz{vBvH9}pgJtNHskM$3- z7VCmQCrO^Ct`X`|?HSSeg1J}gN?|S51%Zy3Jl|d;)TP=pqJzgWv=%i8boS(RgBqbO z)t<3@Hy~@VE(mleam}J>YJ|E}d&ctJfUL#3Akb;WwU?%;5$aOy8ENHbtbdTTSQi93 z#<+&mG&Mq9sy$=*Za~&zT@dJe<62nRKgjhcx=LdCZa~&j0v&u@(`%X zK@9@e3EWfIG(uSaAoumrRT6Q`ceppQX@s!;LGDkat0dye%QEOf1~mv= zw{eeW+Uep~cOa~Pkozp@Dv7uQjb+e<3~CU#{>=Mcg|PlX?iZ!2B;rmwmO&RXs6pU5 zI`7{V!ukif@0G5Sh%0)_pbHt)ATS@my~9l-g!K<{|1Mo65i=8(K^HQpL112kd!U;} z2rBh&xAG23^RY27z^>+^;5t^$)Voi>{K06)el33mMcPu>O_%?u4-ZLH4WB zRT6P0Ps^YS8Pp)Kj>n$ArV+yW2ibQ=S4m)8<^R|A&)#aK-U-KSYaDwTzxBuQHH*v#M}S8 zO7G;K*D}HVx3AtiX?cTrn$% zQHH*v#M{hq~|-aIU@;#3(~wQQ}R%Uc0x&vRa0^@HBI-z7@nMLtjzivmallx8$OiSB^tn zc-}f!%nD+Zp|2?M?%C`1_B`+vl?-*^x$#^vD~M5szM{lO?^w5Y;gnZaGSqe5YFqWV zVpb5N41HycIBvb(jW^UX)V1H6U)|%1*+-Wra)!L3#MaNO*E{6w%__&Cu7l3sy2ll> zf*57!D@x2hZT;SpZ`iz&p|0&Gx9M@ktRO}i`ic^VJ-dEy=F7IIWTFShG(#jGGk8TyJ68~tK~-g-B0Svd}M?f2vDdt5Oqh*5^V zqC|JM4ST=)>Z>Xl>e}Ut=_zliF90LJf*57!D@y#}KQ`;4UUJH7MOl?-*=J8j1vSIi1xl%cOEapPvwdv_kVbtOYxzxm3JJ+8hL#3(~w zQDVkFOz&;8$2OG=bsef836|;gEW#}tP%({Pi`XyrDU9z|SzqYTgPwzE*A8y^TlA$jA-MC^_5TguzMG1Tsest}uN`|`d{&U5w zAVwMbiW2xt&$?`OB|}~K9CP)pAVwMbiW2y4-u{`_R5H|s?;}^=3SyL@uPA}vn61v7 zQ^`;lepk5qRuH2MeMJfURt}Hbsgj{C{62E^tsq7j`ic_x&EIw5&Xo*x;dh>^Zv`>R z&{veexoh1WUR%jf7tU8)eJhAjhQ6W%&cP3_wo4^LT{!P@^{pUA8TyJ67)$K++ggUY zFn-{QSwW04^c5vACc621yH<`vT^I*(#jGGk8TyJ67~8G-(cLN;>cV)At8WD{%FtJo zz!>$Sg>x$z>cY5`t8WD{%FtJoz$kpy+PhaW)P?aiSKkU^l%cPT5$XEl(4W*Y)P?H= zuD;X^=t8WD{%FtJo zz_sY&#kCA|;rf%SZv`>R&{veeHT5GW&8r-Tx^NxM)whBeW#}tPU~b^CY4a-?>cV^g zSKkU^l%cOEfjN%zE~sUw3-cOWeJhAjhQ6W%=4w9w%8ANxs0;HmTzxBuQHH*v1m>Lj zU#w-Q3-e4|eJhAjhQ6W%=FVbD5XlSIbZr<}bPWRuH3%u~&q^ob0AMcPoD@t&4eBuD%t-C_`US0&~lkwzUj( zVLq9wZv`>R&{vee9Qn34)-u$Ed2z136~rh*Ur_>c{q2jj40U0CpQ~>LG0MI?Vw9n;D1kMkPn=N8P#4yXxcXKQ zqYQmTiQ9H=dIz0e%TU*Ohp*J*>RUmKGV~QCwms*i?)hykV|f=>-&RcME6^ma|LP0f z`f;dh%R`rSc^p+D9ZTESSCm*dSk^7x4%Co`2Ukq!D@q))_Y>XXd~O-)df+ej zba_5sF`=(0@wrbw-Yv!@mZ7dWbMESLT(V+9Us2+dfBb#77++b2y0$pyjxNVnD<U#dp8@jyySTUimDDjO$ALmpVk_Z_!|A2!3Vnoov-QHg$|s zh^Cd2y?kG)G?Wrjhd8B0>qngu62(8Bcg$z5dw%a&`7i6bvcLDrc<%Acxz=2Bj`5De za=&&p!4eX`^^`BpC%x@b4eF~bsJis3zn!~(zM5bOiTiK6be{4I;X&2=e)rtD^9-vA zmXO%{mS32sd`WmvbZBdLSpx!pPQ%r zRd`T!-ecchTT9!cRlJ&D35nD1^4WRH!-WS`SDbkE+Gy@LPGE1WV|*GNk>JM-Zzie zc8NF-mXOf3Fd26qJ*d*>?{ViY5gsfdp=)|Fem;6orR&(^=UpN^SVBVI&B;7~(Ss^| zA3YDCON0kYNa!9jna?nKP^J5d=QDJP@L&lE-76>aGDZ)obbs``j4lx#EFqzL{$zg1 z=s}h4^PXSQCBlOxB=qc>%##^CsM7Pw^JKb2c(8cv;rS_cXk#&ji zUWfb1y^bDK zssHJDuU#TMSVBU5>dE}w(Ss`WqdkAOON0kYNGLZjnTI@jP^El;=OK5A@L&lEMju;EFqy>&17Eq=s}h8GoBaTCBlOxB$RWS%x@n(s8XKE^V_>bc(8v2X8s+1S^ zdYmp19xNfDT>oTU(da>y^7~#_)Fr}$B_uRvFvUNQ*}sxa2S4s7>v#L^%{%8ol~$AcS8fwAda#7Vy+8hw_2<8^ z=RuWLllxb06ES+Qgv7g!`k(bj+^Xk6l~$AcS8fwAda#7Vq3`^k^{;>Iu+Do$l~$Ac zS8fwAda#7V!w>uE`U%hLc~GU*50;R4 z&^3RyUiRvFP^H!6{*~KAj2ma_P^H!6{*~KAj2%|q+}-n_N~_8JE4PUlJy=5Gu&@7o{oOCY4>k`9;*qKkoefY&o87`&D>QrQPv;zgkVOgv0@#_@#|>-wh9{w9CKm zyQ>M7khtSD_u5F$kMN*MzgFPq$7+HlBpx^4Ya=}e!-FdQ>Vltxs|l8nc;%n$wUM6Z z;X##tJ;KlP)dWjOob%4THd4DJJgCyIV7OhfnqUcumtVHmMrvP$2UYqt54W#Y6D%Qd z(;M!+k=lvjL6v@$#O=h@1WQQV`@VZ`r1oxjP^Dj2aeH?)!4eYReCFO8sofqPROwe@ z+-_e@u!O|--m>?G`wX?});9ZhvvJ?&-@0=iROwfg9`p7CL$#V< z35kb4bf1m4-R(9V532NQP^WA?I8>_%mXNsNGxyo}tU}ZSVH0n z$86cS=)602JgCyI3EuZzEfcbuUx4Xe=f+Zx*{{1Z* zUwrqB=d~+*HNg@RA1qroKK007 z?RZe7U)wzD!ggh>CRjq^TQAwNaqnY~?08V6UkyF%GwoAbO|XPS*|}xohIjw9jt5ox z_0t2s+1}CB1WQOfZTFUqPv7<~9S^GXE3Vi4rFJB%36_w!@D*F?o#@v)9#rXfX02LH zu!MxXf8*MBoq81Y?x>RAmLu;Lszk)?K?F-k=smpRDtHX{ld_;n@7vFgy+?R-3EhMi zEFqz5;UnK{M>rf%+_TDpDt-RndDL%&N0$f>mXOdjeXq~mefeIE=RuXO<9~hXQK9M* z;lUCT`fh&iL+xFdo(EO>KEC19t>Mun!h0hrIZh@aPiZ!4eX>=WjptUY+xxO85B>o%Wld>Js6>5)yiL-Q>jf zX-?0BDm|~B{wMbdk1i1&EFqz1@PFU)=+1dirRVPLA9vqSb&2p`2?@0&9(;p##Z1qG zDzzVecFg_5qf3MbOGv0q^y#nlJg8DTXx~E~5FTA3JXk_PZMy^ic3bDYny6~4@5Fz9 zV5qu8c(6n*Koe@CzVp;}q4?h@1Upi2GE_ni8O@aPiZ!4eYcQ-A-B zJrAnXkG}Jl9vL28B0N|^Lb-t--s|3-_lhdz13q}C6GGJ`!hi5E< zON0kYNGO;2cl)=ePZhRT(Rom%{N-Dp{+RIS5|t|pmXJ_R_BJQ=Jg8D0_Q!X5YmXJ`c|7Azq zFX$?4ucCLCD&_arFK-@QqH-}iVXTXU#w=d{=$;2v8mD;VqaPnf(j~%!B_uTVa@=V> z52`fY@{MzvN0$f>mXOdG(qFu`=RuXmjehf+&7(_%2TMq3EbPR0_B^Q4_}B6KKOx?$ zE)gCqA)yhvv(D{#Y*y`iNb~3t;lYwN^4AEBO-|0ENUp1dDvd|_JQ7g>y~=_mBs9i4 z`8DiIaXt1MVTLSx>O??;hb1XUX6 z_V*(Z70|2B#X(&pG{Jud-kX3C(Sp zJO_*9BB;`Q7C#3QQ31Wmf+Zw0M`!XpFOrL(O7n93JWoUg^ePLMkkDMCNxP&-E`ln} zFLJvi5f#v@ELcK9bEYQkt0K7wsx(i^?W;soK(Df32?@>pnzR#(Q#ceNN5h*q`g}t7eSTgt+~CMhzjUc7Azs5xp3g}f9EFp2;`+vMX z=?@mkMNp+*A98;%5f#v@ELcL~L4WY$^+~_BNG^gZ{fd(NwTY;JUiH3eyx}skz52-`js(faS~Ahy~=_mB#ztXN9&XP zRgqi-Rr<9w=dTh`0ln&PbD%B~54`A#^+_JCNG^gZ{py?ZaEYjZUS+`&5|{73VttZN zERu_$O21y`d}1OhpjTP2gv9SX;fnQ1UbILqf-3zApYx)LsDNH&!4eX0yhr^VcjkAC z0-57O|xaD^*U!UaL zi{v7x(yu!@-=2sH=v5XhA@N5SU%oyWHz<;eph~||>2ZTZR6wt?UyvSuBDn~v^sAvB$4Nv5^ePLMka+TIE?=LFM-|CM zP^Di#^>|bwDxgsHoDf z(OQ)VVL`BjguK%@XAo4$&*Pkl7(G}*LhoT3uMH2X^uBq#HW8x-OGxNiNaN1oL6ts# zk2@z~^k4}IUDIj&JUpnswRJy=3Q_n0)FAv~zk zeZ}(`5;1zPgoN&uX)Lh2UTi6c-~SXMh}*dP@5>t zzX}hk)DH6et3-?*EFqz`U7AN09#pA4=UHKi7(G}*LT%JEA1*woQoGdi;Sw==u!Mx# z+G$>2cu=MGwdeIEV)S4M3H2G${KW8}O8o@SPfWzm>iS=Y|PiLyQ&My%I z)%xe&Gf#wf##7cH?bla*TL0WC;e7`aF?z6sgmN`$ z|HSa1O8FV@pO}czgC!)Cb4vR_h6h#3GkG7#M2sFRA)(w^+RrjPs8YVn`&lMp^k4}I zE^!}oW7(G}*LOI#APilBjr97>%j(s+yaLr%o#!4eW0LrVKPhX+*} zH}byDi5NXtLPBF<{r#z_()gG6r%uG^!4eW0q3iFny;3} zk1kQqrL7-yT_iNdnm&&psM5HqKabS}OGs#}I9*raL6ydbeO;|4SVBT$-s$@h9#mAo8tRB8T% z@4KrBmXOe#ko5cr52`c|#Lth_1WQP0ZcBO&h6h!e&*JCcYJw#sG)E^r&%=W%&CBug zd^N!m5}Iq2+9lyZmF5?@U9y^B2?@=aO6{xgpi1+k+`d{(u!Mx>ex-I|cu=MJUT!C@ zCRjp3bJ$XQH$14)yfwFXR}(BDp}Bae-5wrPY5twt?W+lvkkFh$_Ze!{t^W}oRB0Ze z`w6QFmXOfg$o@XtRB1k>_t{=eu!Mx>c=q?%rb_cVz0dY)f+Zw0SGB*-HdUIR>V39X z6D%R2Ik)|NwyDxQTko^InqUbD%^mLVvrU!e3wxjK)dWjOXbyCLpKYo%@7eoouO?VR zLUXzM`)p5Cb(XjH*6pKYo%58eB0uO?VRLUZf;`)pIC`Sjjrdo{rl z5?UkB-)EaDtqbrz+p7tdkkDF({yy7OX?=tD*-?-*O|XQ7y!-oXQzbv|vz>@&YN$WR5)yh3`}=HDrT5MI zYEFqz`U4NhLiK@2xyw7%*2oIL11!zKT)c!u(RHhnwy9G4+WTyGiSS?v3H2HJ`)pICeuDSe?h@g_5)$hB^!M4OO8p)0 zv)v`agC!)?hwJaNO_lm>-ezXPYYZKfTX(mk1A*kWinxzt1*R>PLH@ z?Jf}>EFq!XK!2ZYs+14#KHFU)JXk_PIgb85+f*s9;eEEdM0l`-gmN|geYUAme#ZN3 zcZu*|2?^z#`ul8Cr96}O+3phI!4eY6o%Q$Grb_uT@3Y+{!hUhnUDA}XL)TR-T!NN5hims4Kk&}K@kz52-nh)uI zR3a*%S6e^mx=3h_=cGSaBo{%I=5@M1n1~AK)z%NXE)trnI_cLI$wg45`Kj*LCZYm* zwe^Fpi-hLfPWtCXauHN%o~`@miKu{HZT+C@BB8m%lRQI_Tm)5`FYG)+A}XL)TR-T! zNN5i9Bwtb_7eSTgJv(2LhzjV{)(^Ta5}M0B$@>(^MNp;r+s@)7q5^ug^@Fa9gyy7A z@>fN25mad&y7O0wsDNH={h;e2p}F;wJY11n1XY?(?>t;0Dxgn^3h33=54tWATI(>$ix$a6P^I+^&Wk3Z0(!OegRYB&)@)4jyG3#lRB4@t z^Sg887DUyqzO6##aj+2NA=+)K_x-Jr0V>TI&Dw2z!O6$rz9+ij+=+)K_x-Jr0 zE7u=aqe|=JJg%0AW>Gy@LPBf)Jo48LM~9@NqDt%htV)EiAXq{|-f5gO2&&}gan3}H z9xNfDcQ=jKh6h!8-#lKMh|z;3By^Reap&-$N}s>Sof9#7u!MxJ_B4JT9#rW%_V{@s zMh}*d(6=_t0|*bQ^nLU^fJBTQEFq!0PnypV9#rYR;`t1T7(G}*LU++LFC#pt(*4o% zG7>R*u!Mx325Ejtcu=MLyyurBV)S4M2|d-)JelyIO3y3LlS#zr!4eXBI;Z(Q;X#$2 zyPofph|z;3B-F}C^OnMcDzzUxZz&O@2TMq(wUp*xg$Gq?2YLQgB1R9EkWi~H%_9pB zs??tItgu9k9xNfD)@zy%7amlpUF!L8i5NXtLPD+ZG_Nl_s8ajd^ZF7oda#6qdJ}1W zVt7!ceuC#GCSvqp2?_Oz(mc!Xpi2E6&$CR#=)n>a>fNRJqTxZ6`fZ*snuyVZB_z~K zP4iyEgDUkuJ?}LUqX$b!sJETw?}i6eTn(5AdDK@~=$8q-N1J}J&^xgmchFvI{e9J_ zdd}%v*1WG;BAP|@Uu15`>G{k^k4~zx1YZ6T7O?Ps%B?hf6e=8=)n>ak3HbpYyExIsCwlw zH(K+)YKa&pl&Rio-17u|Tx`>G{k z^k4~zi~is`Yu*bj#HIdQ|0@fsuJfliS@XVXi3m&tOGtd@R{O8@_f@0nS8jT*x)Rio=nJ`>G{k^k4~zSG@STYyExIs5wE<2dwq?RioU_yUh}?cf3}%edZ2{FZGL#Ywf??pR6S((ZPvW6S|UadmXJ8&+1Fp|@2f`D;d^eo z=6%%?F?z6s#KV7d{k8tSYE(V&)3;mmzG{gWJy=5Gq(8jDT7O?Ps_t~j?bp1oS|Uad zmXJ93k{hh`_f@0n0T&*==6%%?F?z6s#Qg3DuJ!j-qw21&zQdaLRZGO^!4eXG|MCOZ z`unO;b=rw{T=Twai5NXtLgJW94_xc-t47u5u63t1@2i%G(Ss!<{_(Jb*82OZQT655 z-)YVJswHCdU;-EuX$g!M2sFRA#wPN4_fQ*t47t=UUcU*@2i%G z(Ss!G{k^k4}Iy@&mM)u__@=6%%?F?z6sgsz4DzG_tI^Y^}Li5NXtLPFPce_u7K zbRBzNwM2{_EFq!qW`AEbs`P#IzG{gWJy=3Q_n7{^YEEvw_f<>8=)n>aYD@I@RijGn2k)zvh|z;3B-AGA@2f_Y+Ckn|EfJ#!OGv0~ z*WXu-Dz)dluUaBT50;Qn8@0c$8dYkSdSA6fj2F=vXmHIp0S1l2v2TMq(57*yUjVkrqysug!Mh}*d zP+zpauNqbAe|lfFM2sFRA)!8X`d#Fr?>SZKNBeh?6ES+QgoJVf>Gz$(gDT|%{QJ&{ z7(G}*LOG7~JJsPqmGTo9_Uj*OzZ5L7TCT12%6gTDJ@{wWh~KVB#6Xn=OGqfUe8Tg3 z9#koxynbBsNW|#D5)#UhfA(HI52}a%JrAMdLC3MzyE-vnnxl= z50;S7n8njBzF8ch#LLfvDveW|ckw=_KjnEn zkIkxm4{08&36>}}@qxejpLzd0sJi9xSIvDMU80^#S+In}{nxLWr_Un@s($shSIqr+ ztR`4O;@jP8u;X&2Ie)_$+udCGrOGqsC`O!RmKf;5m-JkpR+~1GY1WQOf`wo}S z)Au|)sQTUo|26mbd^N!m5*I!E2lI5l3JzRX=*hhv#nZ zt|nMQ;sf{ik9lghhX+;nIOreeZnv){SVH2I4_-D;{g3dV>dH@iaPI!cYJw#s-g^93 z=cykR9#nnm>b>-;zn!~(zM5bOiTiK6be{4I;X&2=e)rtD z^9-vAmXO%{mS32sd`WmvbZBdLSpx! zpPQ%rRd`T!-eccBcm8TM!4eXu-{rIOl!prss;)Tk?78!Bs|l8nIQY^3Hc$D)@Sy5_ zr@du9<`Y*Etc%1aUVX_tnWy}2cu;lU2fuOd{O)Rk zB_uw2+-K$~PaPgqz3z&?nLAItnqUcuk6iTWdCIqk2UWW+I&1EH`)YzEBo2Axr{`(h zAUvo#=c9i;_qf4of+Zw=a^=PIH2x7DRK4)3*Uvrvv6^5BiI>0o;&~d!2@k5C_K4Td zJ&v=QU{SJWu0l;X&0)U-P=T$JJI7EFtlO z<1VguqFhuN5)yh3C*!qoNIEL2 z^uBq#woAl$u!MxJg~_<{=s}e}e~&wNiSS?v30>2Z@$=DxDqY7OKkpLZ!4eYsZcgR_ zj2=|!`{;Q9T_QYKLPGbL$$W;Js6>5)x_?P3B*X9#p9v5gsfdp+3W8e&Xms zmHG*upV%eBgC!)?_nFMI96hK~f5-DIyF_@fgoOHVllh{f2UY5~dA?|u2oIK!P+xR1 z?{)N`O8rmId+ie8!4eYcQ%~majviF0AMN?OT_QYKLPEKL$vouIgDT|%JP)}`ga=DV zD916GPd$22rM!mcQ+J8*US&uV% zP^G-M*W+}F@L&lE<@zVk5^IugDQ<1dG%SBs90IBgoMVzPJHM6mVGsu zga=g`|2lsECxog?ga=DVXoT*pb9)|}Rr?;&Ji0`9u%wOrH9}*Plk=$O(9VM@jYs-C z5>WxY>N*zGMS^SonpAQTRB@f(lz7&g&pql%-+kUTvF)gqqoEq-{K#y!&XLC13`8+O z72_ShR6aPHUHJVM9{tW!ng{E0EJB~GM=&Qu>8Ua?yYfp%KlRzqjq_k#jzw^!v7YXd zC_Pmsc6{j2t@qpVobX^>jzw^!aW(@{da6vk_Cv>R{op}=8Xm06u?UVd&SoG=PnC(g zo^;CAkDdL@@L*kzMR25XHUm+5s!aUry?1Q=S+mVmCQFyQ}$09h=IGceeJyj+iQNMp@)$gIsgLOF;!I8$< z3`FUvGV$4ZA9-TEpU8uCITpc@#@P%+>8Ua?tM|hT>wQrktjn(_q9W#PfP9E;#c<7@_^^i-L+UwuA(?$ zU|o(yaHMfI15tXaO#JYPf3)qoyWbcdtjn7NHR#k6_MbAWBb_iTl@n!P?$mK3d;F*5z0PM;d1{5T&Qeg!(t? z=g5O~ITpc@#@P%+>8UdDE+v zM3kN?6T_KbAP*gt>vAkY?}$e*$M;G^oQDXi7`Hy>@Ck+o>vAkYpR7kP$Im=Gda6te zSK=O>2kUYyLRY9qFvn|GL|j*Lr;2eqkKr2?9<0l;2z}E$f;oN%!=tCl#Ea^k;jf=~ zN_em?$0Bsc@CfE?1|q)aa;J)MJC8-ZCqCppJHmr?IToQipGPofGZ1mVk~>w5o5ygc z4iDDlScLB29>E;%?eSjqRGAo_IN`y%9E;GC$Rn8JGb%iKs!R+|&hTJejz#E+>JiNG zSsNZbRVIcOKzOh&$0F1+@CfGEW(bd-DicFXCOlY|V-afMcm%V4D_YwpJbJ233@x&S zzJsjGu?V%)Jc2p4;Y39HO72uKZr9b&vJ4N_}= z7NLxVM=-}+MtJm8nHVxG;la8bi%{mpBbZ}OCOmqoObnT#@L*kzMJR*h5zH~S6dpZQ zCWeexc(5+VB9sa92E6GP@MJXn`w5z6R!1ar*wg-1`7i6H|S9<0l;2xTTc zf;r|a!=tCl#E?l157y;agfgri!5nk1;n7oNV#o-G2kUYyLYZQZV2(ND@aU;BF=V#G zgLOF;p^UdjFvnbYc=S}67&7$X!MYraQ0Cqvm}5>qJbJ233?mKU!MYra&C2K4F|W>vAlDBbf1C zC4wr(&EtmO|I!7gx6iz=F2^FcQ|WBhBdB8BJnEIWXSj9?>vAlDJC(+@n+U2HH;?)q z+*8K~7BShWbk-xNV%$8wu;ctakNeA~h6n3%EP^|g z&SpJ=D#p#DK5O^X@xkzz)HUprDuO$e#9SeIiF+^IC$)QO;qar5}X(KlZ_{Y}S) z2kUYyf;*MgYqvvC#khIY9?N3r;}q8ASOj+}jXq8ysAAkaKECD1#ZSK-eYL{69E;#i zrL$R&po($xsC~{w9UqMMigh^_!JSH@&zT6S7&njFcV5)-!SG;Rjzw^%(&#%Uf-1(% zqvik>b$l>9SeIiF+^IBj0EwWAar3CTj71$E3=h`jSOj+}ja)_|sAAkaYEEWR#|OiM zbvYKnok}AolL)F9H;X^c%If-1(% zqmHpG>iA%Iur9|UxKnA2u_S^j#?7OS6)o!cV2u2*F2^FcQ)!G9C4wr(%|rd0+9wPT z*5z0PcPfoBuS8JAxOvpkzC|4$jFC9jF$}2TT4-uuO z%Ea(q9bJhTcRh71f;*Lt@0EzsQ)OcK%)^6qITpd4O2^MUJbJ234A<^fod@f3EP^|g zj@Pb;(ovAlDJC%;#!SLv*GErv^EIikMbvYKnol5J$FNxAqWunf2Sa{9^ z>vAlDJC(+q2@`RL(|T~K7`JP8xVOi9#kw4e;7+CEy*=Kmo+=Z=Gb%h-mtzs!sdRis zg-1`7iQ!or9<0l;2<}umK5N6Hr^>|8W(W_~2kUYyf;*Mg zZ$)b@g-1`7iJ=X*P2WM*hT9<0l;2<}umwnf9Er^>|8rVbC* zU`dpoDid|4?80+vS(jrG+^IC? z)|!Z(u=Wk2igCLq4n6vKuUMC35!|VC%mKuE)l+3+$Yq2F>vAlDJC%;PjPU5GGBM<2 z!h>}=7QvlL$DB-f^i-J`a!cXCx*Ut(PNidRDLi_rObi*X@L*kzMR2FmF-I02Jyj-# z%w2e}F2^D!JC!=u7al!TCWf44c(5+VBDhoOn6nIzo+=YVCN(@Jbq)KZir`MAW9~IP zOi;zReLsebaCopT$0E2>>C8Fg@aU;BG33I-gLOF;!JSISTzGi&RGAoZ`r*O49E;#i zrDIM%JbJ233}X}F!MYra;7+CE*hF~rRGFwNs`hvd7VB~>f;*MQ8Y~l~r^>{KF8|U6 z&l=W}u`b6VG;Xw9uLdGUezZ>}RgBwprM{r%L8@KCx*Utp_?LGM9ml-lz3Qnlap2kK ze&9*N+CJ9hScF&bwW9)2da6v+M$+Q=Jr-?@Po(QM(Dvee&zP3?YZ8CIugZSK6UqoWBq^8;SulC&wqa9 zTQ;jcdGph@_K7FG;MTYK`OlwqTYIQ3#UysFU4Q%1zv&VK^5cXaeVzY-FOx=unX&QEaGK*-(lmq zud+vvpo($xcvPK9{;msSCV639jzygD)x$S_>+$yJ5mYg59`CO+;;;2jF(baPF2^E% zdfwq1r{B>YJ%TF6&Er{hHtuuU%>Kf<9En|EbQ#oyQ7= z!nz!bc)~Gd-MpZ7&nhk)^!t?wUriybvYLCimPtF@!^-+qeoE1xOrS!S7yGv zuF%w-fps|+@w&5bzw!P@*rP{K#khIgu&#Vrze}uqDXhz}h_^iW_8YIgl|6a{Rg9a5 zR%U9}md=B9ITrExZ{2R=Mc=>H@R|1rsu(wq?I%BG+bPG!N~FTN9Ex=-ADS(jrGr|*BejZf{eM~|S2ar3zM=N`80AC8KZVTE-$7V*Sg zx7~RBdV-XMA|F#<^+|3?6f-1(%-g;Z9S6G*0 z5zl-6Z8ko3ZF}?xsu(wqSJV}UUu`Q63+r+$;vV0*^~NW@a?Rno>Jd~iZXPeMD<_X> zD<=!d#jDl@3Kdapo($x(7MzI zx0S4gbvYLClxyE=W8bIPqeoE1xOv>8e%|YCMQ&kTjz!%4QNOZr;Qj5nrxORI4Rg8!8m|wB=Hh&Z=)@%D}S(jrG zFZjcoZyfn4d+h2ER55NITu0Bk9E&*o4To(!_k4Tw2&x!2k7Mrj@vSF6Bz6!etjn>8 z_g-?dji3LGJ$eLHjGM=ccbvcVkLn6deFs^WV-e52#?3Z1UT%*bK^5cXv3tiGxBh+G z(V?&|$0Dx!wVQ5y;#7O|2&x!2kDd43vGotz&J=}pITmr+qi(Ws=cm}CM^MGMd8qxS zR+-)_*5z2li%&ar;~T$cj~+o4W6B{GTr!{rJ|~F09M3h*y65mp7hvAA9r& zsu(wqzx(P7kN#xqZ5P(%Sj2m-`^y_2ImRA6f-1(%L;W=M*YugQF2^EHJmN+hyVmT{ zBdB8BJl^`;b3gF(p-0cU9E;fUkQ?GWK$M;;6ZOn57~ZQH>vAmOu%{o4&jUp1sWMSN z!3&1Ze8##Qi#XuSgK%AeC_Pms>Xo=>xOQi(%dv=K|L#D1KR}e8Diie^wP*Ma&RCaY z5qJK=4e&h&QF^LO)H}nT;a)OhU5-WEWv?6Heg&fRRGFxExIM!?amKnFi}=7Du8;dJ zh|*JKqTZ?Z4EOdK>vAmO9*?{po*y7ePnC)K#Mv`Eqh_qjv5382d;p$8m%jhHc%FkOJyj-Z3t*3T>tS7vMf}@_~ zel3X7Q)Qy|gcn2KdB(aNi@5t2_eK94MCqwAQG4`@AqOyHU5-UO?znxCX8=)ps!Y_( z#A3*0%vhIW5fA*AYam|&qV!ams2P^Ukdv9QF2^E%^|)&w?*pRrRGFxmqQ#I~nz1g& zA|CR^Ey!PiC_PmsYQ}3Z8hu&l#;(G6mnX;_Qv4|6n*n1q)tFywEMCqwk5}NT_+gHoF9Q&wFdX~or2M-aoebor6 z7`Hy>aOM}tLr3Mh9E;F9a`X4t14NvM2&x!2kKq#x57y;agg)6f{HHxY#OEP*su(wq z;Y!@2^I%<$Md%8>x!nV%$82Cr)^Me3TzB})LB#Vz?o=^u9>bF}JXn`w5qhG& z;L-K~5zj%nQ^mM>3@w20U|o(ysAce~o%R3`&vUs`#khG4Et&9OU5-Vlg|lMvJ4N_)z8IAfkURcd8gSk0CP=9<0l;2xTk|I>8W41rYJmEmtzsiARY7<_5cxiAGuS-csP%iBMT4K^A=R55NIwJ*3wPaWNNS(jrG%1qwkEA{{p`9!%> z#khG4nbh!LU5-U4!}{nS+5<%7MdeNv26Cs0aq}2POu~b8IToRjl-J(P9w1`;L+(^D zZXTC>_DdIhdDyLobvYKH5ux`SWe*TBjw5%f7!T*M<@5_qZ#&S$c$DjMy&^QS^*eKW z{N9p?@hA~gF>W5iNL;*EtjnL+-#KXa?(;aG&mhnLqCh#2AZ=V4VQ z>XoRm?Rc-Ol80jv8s}bqKS0DtwXZ9yGBJ#}hlf@2a4bUO^~>)$h!}DA_rt18D6ga* zp6=~d$-}V-%{y4$uRz4i0e{b}%0#`xY0gA=SS1g~A~b(udEW&QGa!7wvMLkxPQBIl zcB|y!ScK+*_?|zTfryzLzVBLpMxM`Mv9*wR%JqYS3hg5 zl80jvnwPVDo`Z;)HGU3Sl?mmK)x*~l80jvnt!)!w}XfobZ+ljm5JKY*WAJIuu2||MQ9%3vi|`h zW&*n1ZdE2~k41AJ!^0|hI2NJ#kjs7)h?vpn{)bhWs68*uEXh?%nP*IJc{ z+N0MT;qb6Z9*#w5zVI^703v4mx_@p}CTeCvbDhJ(DtR~-p?S~CdWda6v+`?}Twga_+#EJEu7mia^wF|*%!IIA*IpHEuT5FS?P zJRFPA`i5m*6hy30a6ZwhOw>%O)@Fo~xh#EKE;cdg1q&1`F}On6u&562?3Zf2Qp2N5e@oTs)b6Se)OH9z5Dl{_4a z(E6X{xB-Y*!Q*_pRhg(G4O+Vt9#+Z2u?VeWT8@8!h?PhlH?S%bb;Lw#u)@PCc{moK z^;pYs91yXh%HtnaWulJ!Xf0Wc6^;XS@rN9(A1s!R-@U=Xa!u?T(se|C~R zdIVLB+j$IE;#M6M>vAkY*YSC;u}6=figEK8zER=9x*Utp_wm^m+oMNN#khG4cZTp_ zU5-WQzH}=7NPs&BevP2M^MGMc?@^z@L*kzMd&_%&u7@9 zM^MGMc??gS@L*kzMd*2T@3-2cM^MGMc??g^@L*kzMd-Qv_%GO_M^MGMc?>Op@L*kz zMX3Goto^S$eC9oZD#p!YXvu^J>vAkYEu5!qwMUPjigEK8T4dYw9b{dOMW{V@=cn4E zM^MGMc?>Pf@L*kzMW|i++Sk~lM^MGMc?>P&@L*kzMW}uK=6|+FkD!Wi^B7wC;la8b zi%>t|u9w@RM^MGMc?><4@L*kzMX0}Xl?RU5-VlAARK+_UI8*F>W41k3KwDmtzsi z2YmZHd-MpZ7&nh0GZ7xF%drUMHGX)BJ$eLHjGM=hVF?e`vAkY`Q#%{vqz7higEK8GTY(7x*Us8Ui?#M*rP{K#khG48T#;GU5-U4zkkSI z+oMNN#dvtHHmx6YU9MMz#wmXGHhTX~aSRqs`-A9P(LG_Jb*JV3-a zr_aMGc{mzLPa#bJz}%bhC5%|m%5_3-q0ur9|UH1A+}zXB2S z0OU>;z3eIBgKu?Wq}Sw7D}#5@_f zQ^mM>D1WRTo;+BWV-cEPv}~7vi1|Kpr;2g&7-l+!2kUYyLi42DCYsGa#JnZBQ^mM> z)E3!f{h;e|y&^Qp?Pb|_HJ#9dc|SPBNIUtk~m0vlKxUy!0^uFLg`(7J$SJ`qIB(-%P%E*RUS2=w z(KoAB5-|p8mE0Zss9t@##|JmBACx;)j3@6^jHT+>U6*4Kdf&eCAbV_HKPZAK#?52+ z1jB=MIToSM|HK>F14Mita;J)M^BAtgJ!(_4F2^Et9UuOcYY(|_5OH0}ohru7WB5je z2kUYyLf^-e&b9}L_vAkY_m$h6Vh<4UJ(oLGjGM=BhYJtZ^?9UiR9u?XGg-+WDbfQb99+^J&RJccJuc(5+VBJ{j^)u*pD zeC8nH`5||z7&njM$r&E3%drSOcTYXb9w6d5D0iwDH;O?@L*kzMW`Kg@>Y9*h<1tGsbbtbh8EdE-$B;pScKYh@42o$Kt%gW?o=^u z9z)AAJXn`w5o(tn^Ob83pE-zVC(4~F#?51BA%_R+ax6ma>wCV}9w4H3_berU|o(yC?D|sf3gRN=%34-D#p!Y$V`L>>vAkYd5v$q$sQmg z&mecI7&nh0!xA2>%drUMXTEm2JwQahMDA2EZXQFXC_Gq~V-d zIFC*12VIxz6`_3D^&V`GAxEa8iu{!bsu(wqA#)e!!MYraP~PvvyV(Oo|hoitjnW41CN(@*mtzsi!=AODJwQZWRPIzUZXUyqa^b+#khG48T#;GU5-U4 zzyIM&?Exb4?Q*Azaq}2P8p4BhIToRDim!gs9w1`eK<-pAZXUykNqDd>$09V|^7wzY z2Z$K|kULe3o5!$oWO%SH$09Ut^q>pv0V2k6p>e#E_t*nOjH`*DigEM6YQPvDbX~4jgvKN7J&r-@%wv2|1XYa3 z_ey)Q#(TxO9E;Gn>hklb@0E#IKj`zYDiie+)Sj~8VU^q+i_rM+^11>M>j(XLSe1!k zq&hsTl80jv8s}bqKS0F#L0?x^Wuktgw8w9FSS1g~A~X`e{GNk|^@IL?Se1!-XPB%X zbX_Dg?_hbq0uk#6{XMrz9*)~*Uhi;|^@FajzyI3C;Ifwi7|b`a!p^tdfW0;XF32A9P(-iO{^Y zWqWs+s}|?cQ)QyIkSFU0U6<<>q4{^qb~}hzKj`+ZRhg(Q{mJ@4*F{3}2$%g25V3yH z?RKl=;kdn5wZ}49Kj^xw5~2B!%YGDySU>3ghgF%VJ+I07LDxk>^E#LPK@hQi(ETW@ zat%8BGwPOKWJ4ZYEO8we$aK1&^+5^{~ScDA9TOgDtS0= z=TUp~ll6nH%PJ9?FTBh%fQa>j?w?zgiJF<1tRHk;BsA}NnJ)nm>j#}@uu2||+j-Os z%Vhna>#|CO=5H_aJ|JTKpz|eGWuj(^ChG@X7YWTnU*@ks#QH(!eXNp)<8~f3<26}7 z=(?;Dq51U7JRFFa5%2t!Rhg)nyUF@N*F{3>0+#ti5V3wxgzB}5aXXKift;)#bX_F0 zzG0ac1rh59olmq%9*&zw&7@A&54tX^L};DHGQSHV)(<)_YE>p`MtHJ*&~=f}dXr_I z8bqugbbi+=c{pz8Q8U|<^@FaYll6nH%PJ9C$Fv;(01@j4J#JuCChCaEWc{G)BBAwI%W)hKv3}6w zA6Ci3aXXJX@-taK=(?;Dp><`;@hA|nGR)&RR%K%I`a#!4LMz`q7B;Sl(+F0qAM|*X zRq}A$&SUfXLDyxK2(R-?M6A2>xSCa&s7Eur%0j;fdHnuQh~KqrzZ%)%Ke%AN?q4so zT7Mhy@UQ*ogK`(~!+&Y92>+(ylu&=bzq_LThkqaCch5O=qwm2I_3r#DA?i&of6Zg* zQWokjd;c5tYrWr&YTCJe;P&=j4Mfp-@JQ72@-M2;JM>jO3xX>32mSjfi3m&{EFr;j z8pMMK&rs*FYxrFe)-)xk(!1dIr%P0HdQ>Eo*YG<%dIX_NvwtB=nQi|f*XpZ88;n2#0B_uRHyj&+d$m^f7(CC79htMd1ccWlUp)ZXp z3#xSH-XkLs4X7S0A))thxr@gjuYVfh?}2iC{;nvuxcp5d5-d@c)4$FnZ`U;Hi#(`O z?$y5;)g{tVkx+Jc`Kwl=OZDKDq4&+d4cGOEqoPWmjeoJOOQfSBp|PpuZ|d==SfWuf z|GHn-BaVtHjotY-2fIW%DiRu9T>dH|k4j_z{>@U&HSlkw$|HS0;;5+774BbdO~ha` zy%I_AySY3jT_ybH$j>YKlSe%wy~=_rozo@5KFf({KqXj0LNgVXJ0T46@X)xt_mj~0 zx%aPNO`$K1DhsMK8tz>+648K7M@2&8+{=AG26_Ec78+6V4n`XJ@s3H!qclo)xTQ+t zINrx85rfV2y&|FS;Bu!a%XL(|hbtF;wa=(}WMx5>GW08-gSMGG)kx?(`gb3yG}7P} zK$oaK(|aNb-XDhpia%w+`;y)(|GHn-qsW6QeK!8h!7h=GiiEC(<*yS?F%yofUOo@lK9v@A^7!(#Dkq z`vYoUyPwb{iq4!Wwbi}rWg?nIC0Ig2t^VbHmV>uYH@hCuk>EkBhsrZs8XxO zdyplf0hM403ATd=ar~LSyF4e|cb9ujE}636XP{?^&z5wny#8Bs9#rXh^^3i`B-ra) z9+NzHN7i-a-^JBC(tk!#rR&(g*4rg?D)H{RE)wi5O^=GtZMCVrBPBnlAWGxPf-1G) zy-((9f+ZwYe;!n+XW{)m6EU33^lB%Ou9ERQG*;l=k8=9%N1brQwz01s)sF| z?f1M=WkHpC_r6PX&m#zykWj`VUF|_A$L@ZHa{BJE=sbKa%=D^iy~;vkMQ$f*7K=S9yKTnev34M|l2B=jxt(`P)Y zqNCzFs=TOssjO*AP^E0Gd%}t69u*1YlijBt#LX(cAO3b~6u@m_IjUKXD=**qv();Ed z8EYDeB7!R26P;m8L|~hbiiE~I-76kFYETv|(Ws^S(aED8QG2hb(io&Of{6&D%|}Io z*K~+S4?ZVEIM0wgMuI9Gp>r9D=pNN(!q?mMsHoEW=KPF2Cd9E`ln3Kb#9qL7u@f_mENBZgL(~-QHcQ)Zg)L zG>I6DT07Bokx40lE-*dR4I4jy@?WGn`piIs7R=PZofesf7(cc z-$jifc)XtP`jntb`904fSWU2mgmQqM=gutfPXUzMwmiYoQuz1L5dsAkhEk%aOA%N>En zQ?o z>($?Vs8UAQvxVqA^RHT*7>uSzMMB?s&$AxH@uy{LU81{$XM87*(ztdn zu}b53p3|O)!DxC^B$Si&O!+|^e_Ec}Ye{t1{{KG>c!$%G_?alb<*r;ym$J}v$@^vK z3Fmz+NRRrWdm>e8i+IP2E>Y2T<~|Y3T@Q`IHXQIXK|$~#bv z9zn1~&qVJb)%7TPuc*?~**ijYiF8yX^qluzPt&7fiP{g|t*Pq~M@5y|BHnwcOQfSB zp;nD|Rhk|ZOVo1mzDQk&%gd^b>j&-V}2IPyJL=j@-}0ktp)uH&q#Ww;9q=RdqWLsx%_tc1a=zqv=tR zNZ%+CMMuT&x$cS1r1IMw)mL>RsM0;kPnkpnCJ&a7(A~~YxIsL4=vs2#M^~kLLaZr> zQva?0wQJWZUHNV&CL*xSM@2&4dADRkTp>!Gqr%jZ?Tq zmORFzqDo^EZs{eWdsHO&eViT@znvOga2q#yj7LS4#wOgRPDJ;pNbspWJt}?=)P8U~ zF|l2Q)2l|n;sRv2YQOSKggPb7>|l7J*l13O+@#oNTeR*=s}fQ zGQU{XnFPmP#`CB_8?W_uO?Oe}{q&jlzvooxuI=1nBE~ak2?;$VobepQ?cgSN23?iT z!|Iy$n%E|7TzlfUE?s@jz0!Lmiq4!WUAxZQCZbtvx?hpt5sn@;XrstJUVT%Yn@%3} zh}!*%Dt*tL3r|EC$%7@E2|wpUJbF;2JCRq;s-_7s@o)(Vwr~b}@-@yU+2g-8;9tH*x!e4$edoLDjj>-MQWUokR?WR@-+OcETcK7HL z(LE{>CqHA?c4rUJhc0qUr^&*|FWNr9=!y)9Z=^ulzwgc<_55mHUIqV>~LV zvwi-_b5sFo|gbBLDjEr+qK;pmP9n55-cHc_^o$s zceZ5^$Dj6Ixm}`XiQ6UV%u5C8e`P_{Ntf-|?siEc!a)Q}NbGmXj_qza4dVDy7A(={ z>^2H(YSQ{|kq1?}O5Bo3L|~hbiiECEw`vA4JbcCJ2;E~z9`%UYQBkEccaJ3z-J>GG z=lt}j_=HrO!8w5BF&-6FYNNQ{pNQ^JkvRBOyQi&;(Ss_roZOE}#OT2i5_j9!x!rxL zK^%X|f+Yuie&=>)DOgjJ)_;pWbEGF*S@7Gbr;qyv$)i-D{#O=M>8a+7XClHu1WQP;y)ua7 zPy5V$HRxXJoE5L0CawP#c~GT0wR`l52uuV^Na!iy-u)nsKh>kQiamc%t!4L+Skp+9 z@PJA^q>3b$%A_5b^5)ypZ2XXu<(dYC#dg*azT)jse6;-c$${DNAqkB{&?*5W9 zR=ig%dDR=wxcbi{j*6;(Se&u?Gw&W1i5GwVj1|`|ORoIk8CU=Ph@+xvcEFjdzk}VQ zBC+FkXHM_!Bk?EaojE>V|Mx{_jy+Re3xjyLuBbZeOJ|Nf;Vx0p>2)KIdu=s&s_z*RCd5LPGC|d&twH zI`x#@V;lFxC+;3w$f~K|%Ao3IkKR4Dyc01P)hiKok@&~sc8{(8=~3xga;896rSlA| zDTvaz`uu=O-xX)M5-}J}kBWr8o6fKe;`mb*EP40q&m1#%$)hx`EU3D}OU@kgeu)^2 zrbk8MhfhCqY@H3__)``vIrxO?G5n@M@+gg~9#Fk%?aZ-dnTWw?dQ>EIhjSZs5XYao zKO9TyUDUnCw$B zg*J}fX%qUK`uz{yEkAv5{XW*;RMZ%~-m7%qjiaLK;kVg6W+u8sJ+k%*x-Jr*zw7QX z*E^g-{3#2TJp4CzkJ;g_M;sMZ`fj_I+9lFakH$5ueU-j8ICnJyk zy`3t3{_Z;^VlbNC8A#|FbH8nRR4mbT?EYZ#D2*!%s`OoPZ!!_xqawlQ%JitzdUcLn z?OkW(lgD^eRH=Rai;V)1;26^MsCduN)8J~eAN9z}f+{_IoRi^QY3jie5^SSR3AQG5 zCAuf9_saK}DM6L4eD}6j6D-+G^sim2^nG;hEq#K+p-rz{5^NJq&x21E^*=mHk~~V| z%7QBOcYd)^84|ogr$?nL-1{)-3ipl<2Y>z*+xvSVDO=+mNR(mo-Xh9a^a-kz_wz0; zU80_SSx}|iqW5}9#Bee#Ut)I>8bk8F7Sl7=H^=)pDKp_!>Z{M3D)q2Odh>fV9u?~% zp}xBJESesb#&NtGiAI6E8;Q=`D|JVr=)F=_*88w1XXSlZxE*g!(03=|70$Pg&@j;}zt3?t1+>Ynl?gUe$*8=gfD# zRo=MP(ziw$9;Ik#6JDjiMsfV&S-oZ-QA@4eruH90(`d`ZwS*6^PcO6PZ z7>VG!sJ@zaTS)JfJnF0UQmxWG*n2f3q5+#eK@z$jdrkcyjz4vmG<<4mYk4<;uUZ6$!OIycT^B$Dh{6@$N}#Q+qEQezHy4xat9w+VI|cClQ0u^eQ2t zo{abU8N|&h`Z!}r?Y($!qU2E;*ZMeCsmJ2IkPK&}o zl~^iK|7#{OC+KJ zo33^eyekdj_|tmyz5}T3F%L~Vy-4}P{9b?|k}YhfgcK0&H9n&t0GBAUgf z>xzU%w)_nq#KX1A=c}H(ekRIe@~o{#)UI}_G|R$!Qzc?@RIZDJ=2Cc{s_9X&M58_4 zk1BbLM@5y!io6?1BDzOKLgQ54nPhrYEYbLv_ZCSW<55wiv2E{|m5A<9kOSFuh!DHO6_*H z020vv-LKFtA;Ir|tGppKtA_7^MtJ=k)S0{GJQ77mMU}>DeaA^ev)J?uTK_HjJgCyW#O>%r1SWzdB-qOs#DfRFYnnOY9kW6Lz(lZw zg!-as*E-q6+z;;$r|*jQhht4qeLbRfR8;Bv=>6dm5!mLVBB3W-+8<6f(OU8oK<#!v zKjbm#-Hje>J1Y<8{hbm~8q)H+Rw*av{m51mESV6vk45dH2UW`Ad6zWa`P%XLs8~XR zIgTm8Z>OGcZds;lS4UO`XxlvVNX)hIvXU%Tmg#8FYD7VDDU^zM!x>g)USP!8b#e=ng*nTeJ6D|y6GQKh_w z?{?ku5Hz`8kWN5y_)ep#Ik~cUI{+=QLj*2SvqgS3s_ozsy-R^f}dQ>b?OaE#=k2or-)c;udnRkzh zgnBZ*dZtIk66Kbz_TC;xMU}?uS3WuC2>?#sm0+QWS6K% zHoe+O@Qym1Li{NU-d~lobPKub5l2Opa-?pBcZqaVB$Sg)edi$fE-GhvwQbQjs)?%R zNLRLZYyEmn-z#NRO(=iWZ)Na!m3wp6pnK*;9#koF=gdTxNJq7q@RLZ#uRBk>`%tCa zqH`qjm=L1sYx-0ppd>xUXE8sp9Fc40+FrPaMwY2B`K3tBljd9SLv5mafVzcUkwsDNH=J*Vp;p_RVHRSwSB?v+e@)^#da&-70TRWpUcQ_uJU<;zL4p$RF>waj#Pk zT}>3dyHwrrto=5=@h3N3O;nq*pz2-!wBN>So_*MAB0Q-2KVRQ( z|6Koz``@ik1SW!Yk+}W`{&W4pqkg?}R8skD<>&W`HH}B51B;`wO6TOyBN5|KQS}?2 z{I7L?=7}f`DGRDj`n7Ma`-(}#=)n>akAB@Z*Z=LcH(H+g|D)_oz~w5+^xu#K2oQpp zfPflNWQT~zCXntsM!KY-oAy{(SAx>bK4| zm7ZD49z^^LLsflb<~k$hTM&kU&@D=6El6Vf2}AYwky-ycPOo(TTSLyl-ns6KI?;HA zJ%915dZxk|e8=ANEM4qJh)DbkLsg-~+VOdRva zCq5%y#vik0uRCHLENleGL8$85SFAf?-7bg-;FBLKB~IRM-4WXrm)dtEbjyOd>&9`S zw^xOY00Xe0D$r~P3&KrI@=6JK$Tm2MlMU=S+jAT@j*wuZ81I@_s=}y`BeH_XykcHT zV8qE0VD6P}!AO>4#==JQ$d8q(Fj{6mSP)gOlt5cTZ%y3TP!&cm+B;`NV?*;&0^=a| z^GWQchG;oYn)3#$+h~JlpR|vqHK&FMC?%>wJAcsid$$u24MSC7?DOnz2knH}P!+~n zkF7tyoiH2fuM#@KiNfX+uerPU@o~1CA3x(y<7{`4Q!zJ)To|hAt4BXE&Q`lh1Q1#e zwB)0d6gG^Qtm1ht2<(XwxNW!8AXN3b@2@^OXV$ZIA_Dm2UMcb3{iclG`ocdO z?OOkitFO1j_xpa}a4X?g^{0)l`}-qSQra=H_}zc(Zz_yYK6B_B+lc_LRE2TSC&u?{ zCu~2oMk%2!M!p|9#yR1!N5?s4sMZg&p{lb-kBxIIQV_{dKI&6KYjf_E*3_Hd@Z|Wh zZ(6O{^P&!#SE_o&YnP7?-r1=WRj-sdXYNzuUwQH0D(76cym{-V$JhVVUrkl)d2l0P zsHz>ed}jRbyML(@Rj-uz@K(=`XC<+*p*3g0O`TC|9Rv~oV#^?^7wxgysI{I)0H3^m zD4})JrS=^OtvQ?haLR~#uJykm0&J*i`hHVKJh}zpCMJ2M#PZvxj_PMn`&2|JTKlNz zksm8nAri8}eh^UhoNXmA7G=MicENt)b5iBpKD^zZ$6MR000Xe0s$cB%m+{s%(JX+_ zqo{wDBmLH~PxQ#W zQWa(v964!QncL7UN?<0#HaI7A3nDtLZF}RDsxWtA&!Dz*8@fda%)+kP??7wqassXB z4W}PwTQCkZ-+DU9b-ctYXYHu-r(avAeeRKPs2iQQ{PC`9FtKI#w z=ak#fe9_~qaLiyfRD~XnBM!Bf+t4jaXj#Y!JyYo0H~Zp2E7@~ZVf?_-r?zt&xC<bDoHq?D)%J>1@} zTLFQc&w5TJ+G~{VAx0dnHsW6U(Nz^j`YYUz!Yd^(%c!4o-GUKP?MTP0%_H|pRTy9K z9O$_*B7Nn@N(qeZ_Wfn;4C)q)8K3>FtEx5v&WEZnS7W|a0=tu)K_%MD72?rXo;k0N zIVaZDjz6zwY1qg{`puq$IN_$PO@+wzp5NZKPQ-n{$JbI%dH?H8h4{>O&wZVxF)rcC zuS+2Zp(;d;HojnfoiGdp)>tn6#%89%ipjoTeubsm_XAsK9R$5XPV_cqZ$Dv((d3#N zzp?NHW1^$}Yk&K{OXGKdSGtE72fh4?15H&W0A)5X_I+;teucf{W-ul>m>x5yG zy;1_vulkKoRoEjqFd?4P+8ilATk5w^l zk7_ShszUwX`6vh&ij-e#Kox2i>qkLE^+X+{3MGTJs~{qPPd>*{0%h#Ke=^68f@}TT zqnuwkU=!Pd_~jZKzqmP8JhM5WEe=-Y=KNy&b|SzlRbl0C<{##^6VX^+u9U#);##Y{ z+UVRX-GUX&H8y_DN^GbK^t2s!SqY)_LkW2J+&;TiywWW=3jcTHYgb}JRXEZ=J#o*K z5bBi@+OOotO50aNn%Q0zbugYieEeDist~7Un^+JIq%+96rUc3uTks^dpD@%dh`qBt zFKh(oyzEki_&(eAf=GtET+u5f5FI@0;Dak?5HqjK$BwiusCT3>ry5>+)C2*q;$Pn5 zV77JH7mqeol>lVp_8orpW>aC*&-O~~MWN;IHdKX~Fk8EVFl@3{N?;cLrSlKX_XDPe zx&^cQi~n?psR|pxJ@a)=st`5c5iN+SS4wD4oy7LzYZI(%Si57bg^jGe3eM$(s<39k z`k33$e6hO7wnS-nCw>PUD^-DJ8&wd=P~K82fwdR5k4cy)az=o>OUa==@ zx1n2<&@pPhAKC(7KUj{7wj_FlPkjHmsIX4KT2v4YL_)VHq2rY#wjW>Z`_2h(wc~^M z6JlIARL^70nr37LpY`c=!P1%~)>;7<@d4)O7VKd%gX_PCrk1lo2 zul=A`h%IoRszkuiRTUx=w{5?mZo^GPLi17rQ4zMTxmP-`!FZ12Qq8F$%qvx)PH^5< z5LK@x6IuO;`FpKZ73v^I0I-n}nGJ4HqP>OFnu?OY=z$IG=%SP`r<8s2N>%7N|98=g z+6nB6m3HQ(1p0Q4I2#*AAYL;5k9?aR#H>2~}Z^%TahiL;#<>|4;(++WOJenGoWatP{o2 z4Q3+RP!(dNtb+yNKqPdF5{Qel&L^?`grT-Fi1pt3uWxJ0RlO}y72?UCKjoM@QT0j* z#IK*;>Fq{O@=97Y#DLb^cm*w7!1+)W;!Le=V&j#TI3*Cv%F0#DAD=MPEr^%3mfZk3 z2vs2_#~P)!qqh0vrCkZc{a80m9vlio-GbQSoBqjbaA5;P{PUGjst~_?;{$#U3c^i9 zLboV^Sa1Cd>bXVqls2@4TK3T+e8Nywh`rLof^Z-bx8u|G;U*>>DitqqZje|e9CcID*rrkkoDk|7^0(FWS6H+*m1!d?<58>&JJ zxBt}j+6g$8^@B>Fb^Z05W=`x!>$lqou%Rl9CO6({gLb0uN(qchIi61r zr9H=pjK>GFVjj2RoJWuRe5eYuU!JvsNQUxWO$n4;p3TN9n6c{s-ni#9+gsGZZ+z3! zsGBS?4G}=73Uz+`;5Am^SSf)vX7+PV=QgxdV;;>TTI_lB$d8q(FhA!REQn+%KUPX0 zHoy{-d!dTFdslu+ey>Dzrq$-?nC*sCuOYTE5~~Maox^xdpv5ZM2_3Rbe#1 z{iqUVL$@fQtwh>|bUySbVg|vQ+J3B5h4}|thJvUbDyZ&@C7<+;rE}dMSw>xmT*f7-h>3wGdUWl)#wi2m3hP zcm*@rHAT9&S{955kb_Va<_X-7f-nq(Zczdwfcmjge=+L2dGnS%Z@f~Kk1|_`#w*uN*$8eae^(>!0Oq@vkj}2&0TKWF$rM!K z+%R1bZXyxPO9`a;e-p?2-G-_*d&kt~O|b>h*w8IXyy&K>`8#qmg1y&CM_k_Hy%}G8 z=+iykb&H#I!@o*nu;&%gP|4t?I?hO*1E=~Cz7vtGV`Pmfo5-v9V7db~anUS&iw6o$La z`cAKQwenN%{a%lE6*M*i&pGq@;G=i+c)vlFh-etT;mU9Hc*of{old53YSbbc+%)u)<W{redaJ)mEWYE49`6~?z0xhHMZ6bXb7~0lN>!+XtWgDF*yJ;) zgtozrjhG0-ta+NxxB|~&kMo~P4-HOL;w0%kN1U` z7&hW3ujkMFu*iaQ(ZT24p(Rih*J6|}r&wi;O zV*bA8s(NUbxqbG71<}~hElMnzKex|*H;L^h-Wx7kK(G(joLuT$7^(^$ax791$&epi z_8&@UZ+p*>s@+fTfL?x^kh zEI;>5-@eb1h&^ZN$q7~c`tt4jEbUbyz$;Z9G-roC#|c#;8q3SB5?Vq_uh=?cpV+py zpFvgae95+b_8A2c_W_?URMi{5y-lBeR6#h9_JgHFiS6&+y3al_iK7A7(E5!2s`W0@ zsCGhC=m&MQTt9=_mMDQfnD@;zUcrn#8IO5gG=HN$uhakCx*PX-m48m0yVGWUj+S=) z%A7vi-QrjU=i;-~MfHl8ytL1@y`9j0V(gwrdc0RgYk}ns8}Tm;RrT1d|I_3BI0a!C z2;HK@$zS|UuYT8-ZkhJE2Yb9fN^`R8qet$Qs;<25{vPj-Du}9AO6bTriIWYC`glhT z#*D4^$~0c73S&{;9|Nyg78)Bd5wBa4((uJQHtF+T5S+n+h<3tIRqOuqi~GE?ze<=5 z-J%3a4DTe!z0xfxW4xz9b7~0lN>wPwyo0Ao6kaK@*VQlY^Nyj$h7r2umNhr;^XOJ> zm{+QL;}Ngy^9)vr!Yd^X8r!1JvXFbFTeexYWuJGoRc)A8s@nhOTlIMtU6m-jQUYya zz4g&8Xhqq=Rc)A8szR&HRZ}Q+!7^(_=BIg4I zk*wsS3?~b{ncf8RIOqAc9*8Lsg+1b0%C6$+dhxl+a!x ziR~v0wFf{w=j^_)5u6J{RiRzsyuTolq1-Da(2g-4ki@AK)gBUaT$c8tO@vqR&u79^ zVeZTtRS?NgKGIi0Tks@KHnep{%z-UVVIz9vUa1Q68n$o+QT0j*%#hfkHeSJu^#(kO z7&CD6TAYvITo|hABmX$N&k=G#Bt!WbRN|J8y|B-yQ4-rvEW5hpiWl@5XDVz2=i-}z zM0LQIX7<@T7eq3Yd!+=9ZqYJCJ7K7*&t5;H-#TV!Y^dtdFR#<*Sft&Cs^(rjy}xQ> zA0>|d)S7*cwUR^j6Qi}V<*f6k^*M`g_exb4oHC`qYEc6v7A{z=&nQLXRlw23sE_w^ zVa&MlyS7w?$i&L;+R_m{dNszyic%6i!Y2$>ga{KR$6@8`tP~HwVr$dws;)vwJfFgUXaEH%nT7V;QcWev9{j36g_gURE6;_?|gz+*_EtV9!;B zUX<4b7lfOLglFVx#cRPa3gewx?IlrF7{jgFoncC7i;;H0oy&S>^bEY;9c?@BWX}o2 z4Y>u;Iud%+b3#>!_3>W)DiLt3RE5|g@AR({(U_kPmLDasuEz3~d!?;D&Ob*rdS;D? z9^n&)szRycc&8v7NM5o0C;_iH4oYJC2}2w!_Rcuc?2+4Ts0#62_Ra+n+>)>MQH6Li zTc3hRuH~g&3B(52J|?mKl4*pYgL8u1ItxG zBt!XFR0%ElNt|pTddh3)aeP?D5J~0r^hSjE9slx@h;cNpp)UwvM&Ky&jx)7^I12A8 zgBa@42I3{Wf2<%t#g9kNIbuazoFm!doCgCUp(@11**h161CdY_V&NPC6hvb~Rfw2# z>{}3x4c(#yRfS9S1Je?3OKqLdGi`-jNy6Jb(H{QE4*wOutLMT2T($@ zW7XJD6;^?GheDMw8@fdal=k9|5hHX9>Id&i(VXCreFFAFRj7}=^QB5eEN|hIK>g>D zb_4z$hT6iR|6nPp+AyzFg1IcH--^HKu+AZM>hoNQ9BkK8L&VW!7^uplz8SUZ)_HYWE<+YiiG zSw9LJjaRC|teADMAgW#|fhbPBo@-r0Pt7(F=bZbO?}w_euEG|zN<=iT=St{^qp@Lx zjt4Oo;cUAoiNQHvE20YdGDcPq$xvQ)m4JsF;UuyB_&;n zx&={gj@xzrEc*aC2vs5WzG|Z-B@pZ9=*(=SFJFD-7OcuJF4pdqs;~;QYH>Ivu*!Mq#7zN>zyUaU@d^ z$xuFzQUY;Cj)`)w5X<2Gz!-nF-T~Tpr7DbGS9q6cG?sg%1V+xhUp4niM|xO;TytZ; zQ?I(`(KAMbU4@dd@^PZ(r3A_tN1TmUwjZcbyz3M7k#~buTWa%4RjB9O+bUr;bc+&b zV|b(+ugr#?Tf~z%`_=t3BC5T#s|wL5&V&oXu*qjo2`xQIoNVa1#hi-As<6>`r7FzN zc%%!W>Xi~&|8uYO+@d8~VLgwYg`uj@4z{)>Rj-uLGn;#*Dzv+7yQ)M)^AUg&T0$Ee zMrip#-^Y4hwGm)LRp>d{N>qu$DTAm9Ncs%ZPW-= zY1(akbJjn1r{BJh+0eY02GP4_;ot*z(MF9>m8RXs8P}ZNJ?n(i%!cO0G>HGYZsFjT z=`+&#s1d5twA;A+_V;xU>Mb-Inita`Zv6Vf!4X~Bs1d5twA=XEyH4#sGu~(;$w2XyM?D8SABUUL#bcX}2*r z`Izo$`z$vbnita`?)u}x!AbLIqeiGo({5wm`xkVN|KZdw>Y(PuG>8MATR7O^MB1nk zs?xOE*!!MCyWiMzJ+q;CF>PMWSTwloQrf5ys?xOEnE&1bx{Hq9$oE6?x+|qY9QM*h zgR6f)8#O{znsyt9T)j_se9xDe4b6*b5a@${Ha>ZJulwK~TbT{bi)j$( zwRfB|GnK0vp(;(gjSC;ytvi2>9nFU3#WVzgp#^VJWnita`Za90<;F%?~Q6p5PX}7UkJm2H-yu*g( z#WaXB#ug18xrsJvgsL>{HqLnaM%|a4dVtx`yqE^@+_j4aPuxu#H9}RIb{mU#U9Wrd zeTSM2&5LOeyNnhMo_&%wYJ{pZ?KVF5)2ZE!?p%({2c;7=Q@pUQ5HP?e_B ze)N{l+v3z?%!cO0G>CO`}lc`*&*Ro%sdd8g1ujZl@Q-NvOC zT{Uk(95-M;G%uz>T=AC0gB{PMjT)gUO}mXn`!1aK?(e?OY-nCggE;xUiwAcup^X}$ zDowi$jNdRS!+vO9OoLdsaPeT{&(KDVP?e_Bu^R0&?{{%LYUN7uV!j|AzjpE9xoc_T z(h8v}O}mY^-M;C(ejHcBhUUdI2+YFIx{)?&gsL>{HtznBeh3k2s*;F%erq~b9nFhr5ceIt zM9zm0p{6Q{cm{7x=e(nNF%9C=?^z<{N{CQXl|(Fw%Tn3xXkJW%*!81Jq<#nyYO0cm zHELO^gB{I_X%H7*wM6Q<5TT|jiP$nMOKnL<^I{srH-<~3y%HkSR3#BxxMitL>}Xz0 zgSg<{CDQH+5o)TEh%NQ9)V6mtFQ!2}^TZPAKZFQ1RY}AiXIbi_I+_>LAUZRbN zsHsXK_MFR7U)#y@O8J6VxYbhW&xHsiRHbRJ=WztEER7jDnita`F77XtafuM2rYeaz zl3A9UAwo@65^;n)Ok?Vf=EXFKuUx)V#_d9cnyMt?NPn2-1|7|dX%O$fX{pRV zga|cNNyHh;FwJo~nita`-gn1RnMVl`YO0cmGp}Kqt93Llra@fstEDm@6e84AB@t(k z!!+mYXkJW%c=WNQGOraP)Kn!AXTrlYckXCjOoMpK)G?W#3lVCnl87_ z{CxJ9#2JJLHC0K($iy(kGCGp{6Q{7!e+(7;;DRVj9FhojE3PY9T^RRT43>JxsCij^@QQh}VC3OyccA zgqo@(;`nWd86M7u=EXFK|M`DovTh(msHsXKt~3nO+C)e5Vj9F-7mdmKhY+ErDv7va zGE8eM9nFhr5SNUN$vTb@p{6Q{xbibhYegN+i)j#_y=Y9 zA%vX|SXY&%-9{>j%dj7s7t6{B;<3w0jm8RWB8X^0BNM0DpB3}>~Iq!ZWZ3toG-N~wwNF#l-p?NV4 z0yBUwe1tZHuyH%At4hgzX}6JP^kzfzVj2Wy`j6jE8$#Ip9M)B(X}6Ig6J|s6Vj2V@77u@xHiWP^ z1FWk`({3Y0Sj>jz#WaW~V%+C9=hKD|7B7KyRcYF7q)3t3(7c!ife6yW@1hMMEbasA zs?v1Y508_YF+HH(2ylUCkPdL?VXkJW%K&1G~&#aN!b|Eaj3+t-VwA)CLZL^_yF%1F{?-_rf z4IwN}4eP4XwA)A#db6Qv3^Th%tla#WV=42p#Zk z+7QClabR6lnsyssd+@wle-`J2u%US|4FW4$pBvJK5Vjr#>#EYU+ej;MW<&F08U$A8 zPF_YELfE<*tgA}XZX>NInhnj1X%JYCoY)T`Y<&>cRi)|XSnb7QrFk(80_&<1=OZ2~ z61FnR{h+EO;u+kF=bS27XBq_7hbPLF5Vpe0^FdWf#FB`$Z97&}!3NVHu+BYEKZLNA zYL+XiN+Q-MtkIhds$hd@5LmCDsOLi1iaYBERV9JA5@vWPyHvpj(;#r&!9;r{M5w7s zBDQe2X2NV}UQC0)^%E2At`K$w1ludBN+Pz@^Vqgih5cX}1g-;_=s$!AHC0K(9tYR3 z*nVhUOoPy?u{#|h?1~ijA5@h@>^bMLucZq6!8C~cDsLe|O;r+zA7h4xbFO(Y4FcB} zO^i!~uq%YvpHo#5=}IRrSCW?!xK3(fd?kck>BMmfRj|P{j!35yM`XCh%WUWtra|C( zuZeM@5TT|ji8!*vwQ6QV^I{qVu3MWJ?+OuWs*;E!WL$G+HZ(7$LE!qkiE+CSp{6Q{ zIMT;Cjf3d6W=#MI+}QRFy=WdEr`4 zvq2R~1JfXIUFXDnPzbv+lk+I5N+Ql6aZRe(pb9pa27&8SC+4+6*cGOn4^mYUaVCsw zYt05#u)#D4TxUBmKNlj@R3#B-^teXYY-nCggTVE|6LAJ1LQPc?F*1Q`oy~^k#WV<9 z_dF3V5hB!7B@rVmxMtgIXkJW%!1db`aUUT**)ra6;G>@r++lRT8mX$Gre%gDR9Ora|DofQfjb5TT|j zi5P*zJq>0<^I{qV?r)fgiwY5Hs*;G2RNR|kHZ(7$LEt`(iTJJ%p{6Q{7!k%jCT2tP zVj2YQH<^f23lVCnl8BLQ+$&=?G%uz>;J%rOc)JjxrYeaze#1RKW<&F08U*hDnOHXv zBGgnR5my>;?~>WjyqE@o`I# z(;#@CA8a@G_SxE9O;r*YANnVBkD9wtyf<{sfqAmti$>p{@97JVebML}_br`||5I)E zD0b)HI(78O-@R-cTZWfR9UXe^97}`1{gDk3A{YNcS5+IHK6Uh++g@5HlA+Mmyp%Zn zkg21~raGO(_Tx4_v)^t_zGvLMWCTw@?=WL``@c)$cYs&0K^5{n><6wY2*bD^w1K0r z#)oz->?Lusp{h^ZH+6LTjk~lHa4hpmiG|Nh9nF7W=X^hm!26bu*?&XZGUvB{8^7`p zPapL3U+tM^&G~5dV~<(WM(f}H^2QUMwez=|Hu~>>dqr*|&?~CmF@M_V)^j&ERYAap z5o!azU;oHvg*}L&c0yIy^X)ciAwb1XUP_eE66#w24&4*xtTsCK3O_zS`mfIDx-(aH ztn|2JpB`G%&w-Yn#zufws>1P^x$SG3J!g5#3C(LV!J}?O%-@fds&GDb*>9)DMn(XO zROo8WS-Q>C(H;lBq)tS&6RP@UXX@z5&%C%!L;#=sd?@jEznC)G?Ac9?b}i%}bjxF( zn=)GOt*+AgZ$wl(p{hTeK4moHJDaS6S4!-?@08Iyc6Qoq;48o@-SUzRr;LugZsWR* zsCFV#J+%7hHLq?VB7jeRtS%Jdx-YFh+IrcGD#uEC?8cEf0Tm+Gw}gTU)xlw5#g2Fm3eREw-u? z(Ib5FI;g~7-#cxz_|Y^drxhfN#3`X63dCnA7P?v)bS z@4D8%Ll>p*bqDt?UzFpIzT4A!o=h9!cl--oRiUmN^v^AXVf2dSM+wxyvEXzP+fV3j z`N`ErTYaZzUd{Z;YNM-zrwgy353r#sv^X2@v;qQ9Hd<0bYZ|iW$9tT+?%Kn=LLc?q zcAjqcN>%6+Z$A6A%^BnoVgC5dy;4F)nK_~L;M<>^Hp1JJF>Y%w?WzL(y(hh{6htzV zmn$W-rZrv#?1#4cI5+$6-_n0X^~4#Z3g`ca(^o(M%0~K1XzLnSKKMyXoEo=>9c?d8I0h=YDhQo=s_Ad5uy6<44x!#zstp zu8xY)ijIzdbz_4)krB}&FITEUtG)TzJJt!qU_JdFbk8tQbPM(6UY4V3Ei8%`SdtP_XqsL)8m{Q;7L>UW2LHP zFM4{MbMk_46VZllQDWY|KQ+#HfV<;J=<1e-&wFZ|aR$w)Ap-V8RllGA)HtIQ1>q(p zd8Nd`Q=b~={IjtU6QQeH_P=BKIOny6jp&h|K~+6DwtSp3?}9Kxu%TO&c-uRcj~DZE z^Ge5@2dwqncx#*3c%`bBF8%8`TWW19lSB5Cx1vfMKIgCFZ1Y{}Tv^DpC^ zM-@clm2OdD?;rnpob$HEE12o3zo*{yf zPmXhZ?Na+rUa=j-(Pb-I*a#$4g|o@XL_s(($*ajk)*Cc7RE09e2n%dvL}No$s8NjK z6hvb~Rj9#?y%j`bL$@e_7K3rWB(@)KOSlEC595iNlS`coT~(pAV>GfLk|96mJSIw@ zg!2E5jet_3Dzw3jo)$!7L$@e_cAinyBz99>)IrV%(3WtXpgFlTeg}A^Dzsf3$rOZP zAasio#keHq?>1D07IlT=D?r)&LkXRcC5P-MuY(V6_v|<$P?!JknQ_LlTbb8b)&$+g@oB_3Y<PJ^q2R{AmIAeWP0%o%L zhZ4n}M~~bqRqeR=*>Of73nBve4G1@@J24VFcDy-+t4U zme#pwViMO2v=GTqeyo&0i*eOKuQz*1Y(KuzaN`a8 z+7?7McV6!4;(UY%#6MrnpbBx(Pt0v8i6G*~W5_J8STR|7X;&3iXjoSYuiV6>^PvR# zl7r8650lt_e65JR42aXmT6&z|oUav81rNXS;T9qp^7FyoO9|BA_nhN&65EfTj|Jz< zvGajdhu{9l)7rW=M8MHi6;^wW*!*QiSBZ$`y_ynOWBuTHPB%7;&@EV3-R030*f6g$ zRa{r?zr*{h=jRo!10bQjUGA0Ug?hKccAMLtqdu;Fm#1?=`y%uVcigmH`?0e9P!)Qg z?#0`;6VX_Hbd^AFcmD@>FuL)|Y+wZoZ!pK$2kTL+KiR8KB_{= z-{SZdA{olPQUdFEZ25Apv{l2p(d9?4poI(Ab5&t&iY?CbJ61~Q$RPJhM|$WPHd(%c zQJ;CGD)c^^e&QAF%dQeyyK=9zuAvv*_k*u&w_#qX3Zvy~ckwa2*5=%XZc##K z06C#`4J%Er`Nz2{@k&)#X}a^xuWBcvvHVymft9Ac_u0|t#;X8AYbsRV|HCfrHjGde zZ0vE*t}7w*SWPB4QZuh&{$9^jg`;rKcXw-SWQ1Ydb8b;W$MZ>SKRyCre}|Fm%6m># zp-!;hMU7&Qn%mGVN}zVJpU(+BQ#dyp?)vIx&kGxYXOJqi!9U+}r*0M|8tKOj7uV#@3|6acR9kzZ6J!n zaXVsGEMth(a13um^vKJust}XmxV=gM>Z%Py=s73XoN^ng!ioUr=LKOHZ0MHBL{_c> zTmqpgtZZ;rgq$*>v7sugt8lJX5RDC0VO56n!GdUP=oTfgB9zU9VIzKgOwGOz5uKIy zoT|dA!^&q9D7#I`5CY!v|LzW4f>)X^R!=y4DG0+r=pJIlj8OwUBDoD!VO5TiiGpZs z=oTfgZpcVTV;-#XZ+%pSs66LU1!0C@L$@e__&(=txmUUcYY2?>Xig0gU_(`i z$}_f95LK^~(E6XmZpzl9SP!sL$?-01)a#(Cu+GVOZ9!ySF)t;s#>#ng?v-xA+AU|^ zg^l1`?43mgnxnpgsCuOYJmmN>_e!_mC~yW**l4^`6!MH{e+mDYjxCLz#<0XZS;GBhH|fz&|~QC#P5JJs4BFz6I^>mA`N42LlYi zhN{rczwG)J!c9!_N(t?Qlh}Sj7j5m?xA@a2&}#qf-#o3g*`>~T+fEgFhS7yBL^71u zK_$>r|Kv=klh{plb<0kxZ!p^7JuNnZbD^uM9e%mN=yz{xA(EloD<$^%+6zakA6D~9 zx1dg(`^FXL8uD-?j?t}5D~y9_eu$^&93$D(A72^N8$Z9dwbqqqf~_>eedEHA{oldt`Zod ztoa`4KN>=}U>x+q{aS1U?uV){)|#<(3(?rnJyZf?-!;}2uV5qo#gGDZ6fpmd#@dySViy;_wp z8>+hT?w2+_XO%D;xd3j9HD|C&L=^Md ziKD2*oBnUJyw)Z|p^JGxdzp(Kc~R4=X->IUs`}gyH*R{)Dp7c)#EGBZq@{P(Ef<`- zX)^+-+6dTlRh@g}iyB_(9rlqbDg;z>w zy=lBM8@v7AE1Mn}^@II6Y;cxgL{xi?QWa_!XTJqu7|Rv&QUYFa=AFd$6Ytx;X`5!g zyyU@cnrKA34XxYQC(fO!$11=pRh{#}Et>qWi!vL z+AyzFb^eU4nt5%ND7;eQogdt)nMdbd>6UvR-l~bJRc)A8s`}i4TaPZ=+@l6nqVP(I zPhYe3=$l8>A`^O^KQU$7CVmAQEIqa#(IYR3s`~Y&+cq(>f-sC{ka;O__BPu#(Y7SE zpE!n-El1AXwuxC5HbMmAU+Ag|^hCsS(tenQ$zCY|4>{9I;$%Z>4(d50UhQ6~3hfG` zjs;QmN(r=MjCkf=slPZkoYxjMLgeCKykkUEIRBg#7eq4Tb&zLJ3A7j-!zZ!*gf9A5 z#wpQLGYYIR4wuI70IyVq_M9=~f-nq(s?Zy7oLCSsf48A3^ggYl*T#nCr389Aj+T?y zenMBbpyy=dMssp${0`U;RiUR|wMdZ?x*}jU(pTte3-HNjw{9YkIOo~C7DRwos(Q!# zZJNktJE2=96D&REP|V+LsOrDNHcjmH`IST^wC^$-_zKt$Eu$bfOD$|fwG*m>hn(XU zLxU9JhKyyo7IF}}1?Pq_vcg7GJE1DH?Toz@LLC$N? zi*i;}*obN;RE6G|k%EGV06w`_O6Zu#wUC2Qf6Z3r!Pwtfx?QMywFni(3PeHheXhXLsp)I(( z<4EWtmceJ;Bl^K-(qk5$J#C?}f!X%XPpTQHOM^Ih*=$SaHdKZ9%t!ZW zAsQQqr0sU=`j!_WX(ui7H1;pE(GZ9_@BR51Mj&Q=+%G-d?iJ?9e0v1u&V0*5vF9Ng ziGQK1D$LdSPOO4xywWX7AU43ab|ta>_zExIGl~`7lb5ZtlB26C#HR7CSDc$-&oi%h z6qP`f`)}K?YqY!Lh{yDKbTwZ@>vw;zAL*RHnPT}tPsVrU=n-j%fTOD_oO8axry$&^ zNaz+NPS3yS8{5CMd$5Yy+oy$ZrG5V}PPEpLsDnD9suO9Ntk zd}o~I)DQuLst{Y`o9qh0u*qI2q4mG95fi>1#WzS|rHN1cFKk4Qynd((t4n;3WE=w&q0@Y}|Bd3(?rn zyp%w+{sVW_S_Z9aSUutUs`Mx{UIloiD#Wb$_Nju%ykc9T1mf3xuT*0rCW5W8uqQ*0 z!?&OmHljy`t zJT3^sCVQoXma!zZAHQaT=N9&EnZB}~Q&p%FM|4|=%qx~rC5rx|A+!%i%W%dFA8}yM znUn8;BcYqAVtaMcycVLdp?RV0B7vCx4ktRD#BR#YIjy5@?;Pdh67&-DPVsb3po~6u zt#3iuJ?qAnkxYnO{PT4js!&%J-O@rNLq6)GS4!w8%EU2$d_q@i4pwORhQy*=1;|0D z3ac`F4_ZM)0H3^mD1mhx_Q5W-??`CP!3quE`&-xuFaR5>!m144MqChXVv<)%;GDC! zOJe)+evqXB#ATPZl&k0wKJhwzQNhEjf6ziWki6mA9{TYkqqVMLkVpk zlh}TI4U(k~s~Ieni2G)x1p0tur7Embu;jNBxuMqL#SOoDhj(w^n(Y!*ujU!f+c8+&JaO7+@qS}wHs!+Q) zS}q8~uphcb3G4?WNJ(rz9t-DjMf+yEr^A3B%?VsM8L69 z6(Xq|2UUr}D zsLId3N7bqX9Lu~?0y#0Fm)p>qgO-6&9N4JWT2-MXVx*`bnmyMoN}%Oq%&GCp_Cs47 ztTZr^THW)2Jy#W0HW(?a5@tiUD1ns}_QAPV+9P94hqF}8sUgfORbkzTbKELXc%_7v zvD}89Ta+=zfT}jkD^;PqGCEWx3a^xaSBx6vUf~Qf_J(@L$Q^9d%ay87YdI4th-4`5 z4U|BO!C6@D6l#+^f-(T6+~1!p(>0M7>z23 z2;h_Nxe^$0F#6?M$U*2aK^x3@ZDAv-olq6pcE*zmA_Dm2UMZm?1J^xke_&+E z$aY~Ps+~|3MzV~a7eoZ`$-PoS%Ythm2ce}Jb&wG+*eJ&BMyLvPo>8@SLbpsN>h(ia zXo(obdwzSa1X?&opz}S~Eoc)N*)D9vz4l|JDzw3jix)&Pl%EeJw8hB1(tZLXeXe;G zHX5%~g_#A{)e55Ol@eM)8?Ry_biaSc&wG6CvEKFh!-Id(;}idl2#|wN)k6>eqQ|HF z7ldITHhkvSJw9#t^e_CX$7dBIr>vAT#Q8h@XOB(WCUiLiwhQOZJ-uZ!j;+s8896g#S?X-b?;u|~)0$8L%7d49S zs~Ow$o*v(DgLBTDAm%6DQ6=a0@!9wGTHjCsB*2EMV1w_scz)$d39bM1$ZhCR#L;bi zzd`iK302{2w!T9l8OqC*5_+Uf3>)ziy5E^ItIs#iZ2Fd2eV$F!`RwQh10wO;yJz)T zmganKR-YxYolw<-U!2uv2`>mY5pC!eCAPU>R-d&iiS5S;wozZd@P&Q0?Y&)J*k}8g z6CXN${XW}|5B+#%pREsUGbhj7xzJVBKhB-mXKzpt$x!Z<5}&+ay*~Su+^g%B{jtZV z?rZ7$)JGoc@!cbhR{>{GRd*czryk!*QV@+-YUA0zJksNP0W_z^E3=`hLkAD{_*Q`` z5z*W$CH`f}A9{S(MPtK=U)=he9-qMft=;~+$0yfUZJ1Z8I_S6E2<~j5~|P=wY~u?0{G;$RtfZ1^|y>^JwUBxAEouiGq>+(Lsh8r961$) z1Ch`zlZkr$P}ThVUfAa-?D^FXCA0=7hkSRsTAyd%y+NPjtHMTbF1}qwRL8%2gFZ)R z1(6KpwN{D4pL#)`Jwp=PPw0O0o~eD-yW{>irO)^L6gGl$@p>OoeSh^SeZIA)Ad;cn zDQ!Md}BzJD7;ca zTfQjtPr!cY_zL4v_SCRZ^v*`83iFy(dl#J&m>cljcgd&d61>tam`(8Qe(heV3bU70 zdz+vVIvO;uqDS}yYZ>SX+7J4UvRgk^szU3-V^t7tBHGX`O6Vxc-Ekyd=@dsu9JgWQ z#8IE-)DQvtp{gD3o7LyYyCB@eB(Idvky;YFsjjwUb3gEcK1ax~QE%H-wZ#+b_c?be zh|DYIrG$>Va<6n$^rm}f^f_xRYy{`xHQS>4zs+a#IRYq%s#i+f@ek|vIpWB@(k**k zy-uHFl)^^im8w2{|Jr@F3&5sl@os1iELG_PX*_{8<|IIiV8Sux(_J6W;k^*X33jIa4l)`D;olS;c1C|C8- zlFn-|a^lRiuo18m*bh}<{=r#lK~%j`LhFAL+fTfnLC)=cdra?hR16!eH!gK9Ufm|D zvyWS=&rx_mBt!X_L5VM%zhstO}O_BaLMCMK0DB{1S-zmmlE6S{g7fBM-q`kc2F z=OcQAPw1-Z$nj}?&gcrlf#elSgAzL8^voTJvs9@EXm>gPDE2%=BL0Q0s?g4J_EHdT zV$!ivLeGizChV*A|2sfDJ7_n9;Sq z=?^yIU+Ag|^TF1)3BpGFB4KQak~rDWEr_tO z?J8_UkK8L&AzH`xkrzbODzn$HxjdS!dADvT5P+~O)>`=Kg~OIPjL(Mn((%x6{S`+eMC9vMc=f&qP<$IWy{loPq6!(ug3im1u|nuQFAfQ?WHD2saT4J?BdG(#5>$X zh3F|;pMpq+@=~G%;y!HclGsgIM3`F;DPqf6*a*(W=g^A^$BHd=K_o-DS4!X*vS&zQ z`|;>GYYw6x?4t@B!8u=7qYBX$_JakH4CP)afv5p{)Fif_yw+lt!ZAZ(BRJ;~NUAW$ zVF@pYWGMGa3CyopyOP*`eCEY1n76Uk7B+%&KD(m|^Fh}6f=GsPuav-Cln20#H}CD1li9ZRa*rg_#B44OkF{!3H7)%nK0&61q=G zoc2Rih)nPef>pxyL$@e_s0iQT*Vr&Zw^%GZy{|}fYKVaSP!;B9d?!+sFdMo>3C#5B zZ}ZTe1+!wll|*yOy;2os+bl~}qVP%y?YnXtm`(7VY8bKV8`-knAiw~Qm8vk_ECiy1P@SVIIHT~%Qw%(7GvjaORY5T)RoXsV^eY^Vy65WelEN<=&P zxI_sZIVFcuDbevcW-lBKRBf17s=^G3z$TL zj0QO3G#ly5$0e+t80oWJQ9AcZRhU`ud=x}uL$@f=KC;viD@JFWL26F9SE|C;kTb7> zsCuOYMwuMdCb66HYl>3KfY=**vcg7i&R1oqLgbD;XF()Gd0V1{_U%b*KTfdxU>3%= z!(txA64Oqos_-gAF8=wInam3^= zc2l;Z$fJlpk@LaAMuUl#enMBbV1&#!dKNaKNA8uXFe2xhLJJ}p%DqxT&r=fHkIz4N z6ft|@`<)9LAp-HwuMnaNvm(A7x**H~2;HItW@P_z*&FPA>n4u*c#3 zLJmSz7_qXH6og@uy;1`072m_$*nk;Z;pJJwY@6q~un|3SuT+H?z^auiB@ky|IX17- zS9~6_)FO28R;>;yfe|2UT4N(7e9S<9G0I>&-|m&FF!ous_DTuFuUON}tMujPoX<#s zC%ozj^CiL}NqqLYhSN*;Hm@vZ|G6Y-nEk3^F`rHG2j`%-`#Ws`M$6 z1<}~hGlewYSO;&}qnfy}FP$7hhL(sV%pvS;aW3-Tp_J?DEn zkv;OCI962Y+fNH3QC_Y{;3zEaZ#clSadn>{p0F`l)k-urG%ut{=suZ^j8K)P>1|GE zUPzO`lXTh%RcX3TXkJK@zcL9@XvlL^u;RU6!*Pl|vI{=c-rElTKfl5=94 zS1um#iDH+mvv|PggY9?4U;Fho@rJV(4fqVIufA*1fKOtp5&=h7RTmt#Xuv12RSE2T z_o#P`4fwv2mmWPf;G0ioZ*tL~c%p+5f7^MRn5BQ|9+=gyB`Mrw=d{&s|loP7D z;w_5@d_G`1p<9&Dve4KFb_ONiS9KQ;_}sxN5z)}qElOyM(bzCTOWbqUE*kL3c$!l~ z1lUm38DonEd@^2@FdMo>32l`c8x6r;1}zca?ueF%?}juYI2XE~y=ZK}H-BF;Ha6fp zy-_N&UacWi_0~mW1HQ+*AlwA%hvcQi|NOtP0pD4j#P$=qs0VzrG3pB6wWW0>C(iiS zu>nVrUSj-tWCanNW36=+Xg>3-Ad(^954Olk;281=Z6 z8+~)HbPL*fzN=AlY6$ZxQ^nqZ?}Mxog;(eeNT6&+=pU~AZ`uY!#4fw`%oXvXuP}M)3Io8-H2sbgQeoQ7<-rOBWLRVENOWY?d zV<~$Ftqw9TC3N(W6FL_8&cS1Yk32rz<_3k0;9R`AO;m@!ZfwAJtQSNwl-DRF?%jE8 z!1t}^UTJHv`}{=%KGhQ~9G~iGL~t%X&+^_SP5rn&)*`kN&pffDsVfZ;sW=K3RcGeX zrhXKJn}~$k_{MO_fbWpba)LVM?_&m4U3}G&0pB5A5TN2G_ezOfKf0u8C0y&@@p<90 z<ouv9R>D6S_r-YL8qnJ{JCygFN@zcy@45E+ z7+_6F=OTE{==6p9PlZtE{)#-`=Kf=`2}GZ2;FkwC#lxUyhXdDJ(Mn1n?bIQF^6~>8t`g=iCy;1_TnNR6XVmFn=eQ<6V>(jm~ z_exbbAB+?h#DrH8RZ{|Iknw=rE0lJ|5Yb*S0;REmh6p%=s`}~NMNK@ZozN|liDFh{ zHdJ-kOBW6JJauhd8?R!*BP`5IiMwVj8t^&mIiXt)d~RWL)(RWZBX8kUb=MykHl?H> zlA+uyC60e+VN)t|uP}q;%pEgG){k}@Xhk{RRTbJwj(yEWh~M!q_X=$y$GZh#7znk2 zR)Vn%wP)g(zuQoi_Q(a%*w8IXXq+jxp(^de3!<@sbHG_6&H?9~YP+!!6LHkHVCjHw zBYonOr31b<8g-E6EhiAU;TyA|TGU^+nb+25^r`~Q)i<<08PV*AZc##`ZAt9Tb+y0y zuj>{L_>|_Nj|vfpf1#@?lteyrxgg8}2;HIt%5?F}Wtg#e3Ag;{j)en0b6In;?4w8S zm8#acd*Oi3TrLR1CVQm>&Oe{hp2YSOx@Z~r40W_6d|r5ABSawn#dkZ23T+pk7hVu% zVX{|BXv`sr?Z=NUqY;`fMg~P>!n{%y`Vx+03Zn5!d-`oZy0jUE>&U>}iQi^JRZqnF z!MIpKG&XdL5~yj6uqCnmcpc=Hi~Dhvi=R8XGWU z^_*`gz>%h^un|3)I3H9!5!NX$XO%GF<14q^q4rr@AwzGszTZ2 zs$4-B212(ep)-IawjUp5uy*1+Gg72Exio$U*iaQp3D-Od!m!C+DWN0hB(@)KMY+y} z_K_=Cg^lQud!;J0iCm*Ch-4@~R!V3MPGbAXTL$#)98(uILImQUuYXX5nFYt~1z{E@ zA1ftbk28QIwx4`I(EFS|nqhMTobv@wdAcD2Y~Tzon={i?Xq~_JMo$-BMYZpTs?fF{ zc6tku4CP)afqsSY%Otj+&_#5aZ|XtRneW|cueGW|l$-B|FNnq~RU!7y_t6(bh)Ddy zXlX))`2NamXkJQS4T1Y)4#7tJ#P`0&^%}l+2O|rebr`BM;vfzT~V zpvPfb+1P*?8^f_B!w8TuM9rxo0ti(hQnYF@OC@w>VK&lN=pqis`5D%MIlI$QZSIw- z5Z~k6upk;6x7ev)7B`{a#`f%e_!1>TN z1JRb3UGGtY_Ij==j9#w%eGAcerFkiVR`l*Uwbn2caso66@{WLPP)`pLuaqtOQz#O*VAewf-GfEoD52>vGttwYOKQ!nHrE z)`}{D>xF#6NEnsX`0Ky)6hgG3lHufi{sx+Qcz`e7sFO@G-AzXzdn_dm1eU$3!lL z9E7US!m*|ngkd0bixOxP*-A7vV8-SK+=5X*TOZA-Ap!_hVWiLbazPk2*()V5H{i^> zu@MuYi`J4OVT}4YrY>wmkNjAv3L|}v+Y7=BP4-F&T{ko_Y{XBDkx74tEA}`}Xzy!P zg{uNrZCs*6Hs3XO;1axob*@KYJ)(8ZE2^DP6;?A^*VQ6`Pd+YD0!Nou8@m*85URp@ z3)k@q!Y~lJMG36$a4eC;$%gh`h>NjCReK!sN>zxeu|5_=O__Dj#NO;iG9mpz=j zW8cv$otYqh!4XGcBVZ@6AF9IoDX;!32saT4-I5Wl?XId&yBK?Wer<^osA-J*nL}|O z@bR%I+dhmkIX^FK1js?C3L{I#016@k_~gBT656`D)V?F3{-Opm%30V5FaR5>Lak+t zv>@EXB(Ib}J!gIFbdGrRobK}VZ`>1h7}NcJr!&qp?p|g*8UL>ns?xL*cb|4n_n2Rw zw-;<^UQB~PbO$S2?S!f{?ZjapIJ>(^=htRK^I{qV*Sy*ZRcYFZBd49!{qnarornF< zyqE^T2z@)DDos1_%~}84oqqd1W<&F08U$Aa+6h%@+KDr+IlX(<38$G2&5LOeT(f8= zRHbPrF2DVK-Gh1y&4%X1Gzdhzu#(bFs7ljL{Onz)b{`tM%4}#}OoQNBYCEASO*?Vl zMJIMITyl%q(7c!ifq5cUd)o_SbzSED*hUUdI2+St22Hj4mO4Cl9_OHix zzi`?^W<&F08U)w8+X+=^+KIu*$8=BIXSvzXyqE^Tb+vXvRho8U-}@JIkN@G+F6yA> z#WV=6Xt-T7ns#FAnE%z$4$770#WVAb4G8JE1B~JF#0l-{bMT!-nR?Gzdg@aQ8?%p(;%~amL#>>b~sM1I&iz#WV=s zt+jkLRcYFZ&;4|2ccXh3m<`Q~X%L7r;aa?Q zLRFehUiFsG+v3z?%!cO0GziROaE)I(p(;%~fp#73JI=Z0#WV=sKip2JO4Clv-{Z&g zPCn=av!Qu04FWSH-22>4s7ljLT(Iz#d8dBrM6;oJF%1GUFWiycPN+)LPK+(QYTnmR zIMr-uUQB~P3=nsKw-c(;v=fW=T{!RE-+iCi(7c!ifv6$wD{CiIrD-QHe#1Bp=Unq* z8U$jCxZACrP?e^gxOBA7yx+y~sFf?pi}`{;WD@t(w-c(;v=eW;ebagUIIe~b&5LOe zytl8NP?e^g*!?fR+UvMDKY$I*i)j#uc;fEEc0yH}cH(Ofp10S};yed7G%uz>Ad-r^ zDBB5DY1)a;pMArgr^NXhY-nCggWz48?S!f{?Zo#!c*D0&b{iedi)j#8AD-9`Awo@6 z67g8wnvPXR^I{qV*4rk|hY+ErDv5Z`Z%yaCqj@n60x@em8^ZToh)`3NL@c|@QrYcj zUQB~P%zC1J2oY+kl8AM1S*n8_&5LOeh?h>(b0I=aRT8l+S(e(8j^@QQ2t=tS+AASK zO;r-HOkgnK1KsHsXKj^T!B4A;@TmW6XSLvLQPc?ac(e7 zbAyiN#WV;+ig9na?}refrYeaz#~G$MPDk@%8U$j?c#@La5F*r6B@yRp!!%dxXkJW% zKx`Rz6}Sx{LQPc?an3nRbIy+D#WaXVW6TLfct`VM8U$9Ia3_K9hY+ErDv3CL8>X0kNAqGD1lFc-?}6J8BGgnR5!WV$ zX>FpTc`*$FYhJj=$ZZG_YO0cmYb?XG#?sNem-l_uy&VM4p3DRsmC!JR3R^> zL7*qX^L69fi)j!Tso@F; zw;_a$uXuG6RV9%|mS%%0lRv4XThA(;zSdz?C&_LkJtU^STwPN+Qiz%m!7+i)j#;$>7=_w;_bh zKX_daRV9&TUS@+TjiE&3C9;lRyScLBMRaKrIM#TL&73!!eF-9iB1C??Si_nOL>c!K; zh!|&}rX5u!#+Xcapi(Yk5&!jZAJw6!hY>MeLbW=oN{o@B@Ia+p#3D3;q{{X5Fe1i% z)>ZIBRf#d;6&{|8ScFExRNp?A>} z9#tj!z2HrK>*%_RO1X$d(EpVlM#Oj`^_!?FF-B6u16Aa*?lXAA~aI0D$PQ~_%0Qgs46i=w!;IJauJKrh_`A-)5C}ur=~^} zRVBsGOS2oF@sMJz%y5UTP|4ku?RXa(!+>0Q&pn>_EBwA9#^BAL%GN+f}fl9fEMevNI^f02$RFxRL zeKB|Exrjy3=a(KvM5i9}YN#r)vvYd??*~uEHBqyDyf2YhdDWx1>0UM><_AT%%47Mh zRv#WavDbeng~zGg!tEfW<)O4jjN9aaqYb~_FTobl8Q`8T)%x`?cwV|ER`?qYGp*5 zsVc#?(*Hddv52?6ef$>NE%odVy@yqwNJXsuq4$jP_aNFJnyWl-AvzZOy!j_i=*_fq z4_Enm%m^)AzPVS=bkb46c2lhV!QZ7+H*R+1ZmYB;GotMa>;GbZkV`brkW%dL$Zh+d ze}DJn?e{<3^G_7>&RS^sMe6;7zfSBu@bRUmw^h6~wi4&h@APA9wen9CJHa|FJg#o5 zcx$Zwz9i0m{maSDu8%~v?lz&LM;`i@+e>>;+egO|@A~z4N%Zsl8|`NZU#I0B?AMY* zJapc%4~XTVbCXPQWeB2Q84lgU^Dc>gWx%7kidf6OB>I&hh<;@_^iP~%33JW+d5*_= z59$+NbC$Lciz5-0)|@yA-yi-7u?T-Vbnd>fb(^bvZx&*`&;1i8bpErSmL9I+DABSn ziT;gq)W4(RJaiSY)591W-{(bxLu)pKw6eX?q(v?Rn3oWWX;cJ$i) zcMf-7{^4L-^~5KaUzrmZpZ)d92hY411i3^z!5N$$fBbmo3QM)E!edFu1ApyL0|HPmBK&&NPt^Nn_XNnjA literal 0 HcmV?d00001 diff --git a/open_manipulator_description/meshes/chain_link_grip_l.stl b/open_manipulator_x_description/meshes/gripper_left_palm.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link_grip_l.stl rename to open_manipulator_x_description/meshes/gripper_left_palm.stl diff --git a/open_manipulator_description/meshes/chain_link_grip_r.stl b/open_manipulator_x_description/meshes/gripper_right_palm.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link_grip_r.stl rename to open_manipulator_x_description/meshes/gripper_right_palm.stl diff --git a/open_manipulator_description/meshes/chain_link1.stl b/open_manipulator_x_description/meshes/link1.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link1.stl rename to open_manipulator_x_description/meshes/link1.stl diff --git a/open_manipulator_description/meshes/chain_link2.stl b/open_manipulator_x_description/meshes/link2.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link2.stl rename to open_manipulator_x_description/meshes/link2.stl diff --git a/open_manipulator_description/meshes/chain_link3.stl b/open_manipulator_x_description/meshes/link3.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link3.stl rename to open_manipulator_x_description/meshes/link3.stl diff --git a/open_manipulator_description/meshes/chain_link4.stl b/open_manipulator_x_description/meshes/link4.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link4.stl rename to open_manipulator_x_description/meshes/link4.stl diff --git a/open_manipulator_description/meshes/chain_link5.stl b/open_manipulator_x_description/meshes/link5.stl similarity index 100% rename from open_manipulator_description/meshes/chain_link5.stl rename to open_manipulator_x_description/meshes/link5.stl diff --git a/open_manipulator_description/package.xml b/open_manipulator_x_description/package.xml similarity index 53% rename from open_manipulator_description/package.xml rename to open_manipulator_x_description/package.xml index fc8f01f9..dc0a7d9e 100644 --- a/open_manipulator_description/package.xml +++ b/open_manipulator_x_description/package.xml @@ -1,23 +1,28 @@ - - open_manipulator_description - 2.0.2 + + + open_manipulator_x_description + 2.3.0 - OpenManipulator 3D model description for visualization and simulation + open_manipulator_x_description ROS 2 package. - Apache 2.0 Darby Lim Hye-Jong KIM Ryan Shim Yong-Ho Na + Will Son Will Son + Apache 2.0 http://wiki.ros.org/open_manipulator_description http://emanual.robotis.com/docs/en/platform/openmanipulator https://github.com/ROBOTIS-GIT/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator/issues - catkin - joint_state_publisher - robot_state_publisher - xacro - urdf + ament_cmake + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz2 + + ament_cmake + diff --git a/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro b/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro new file mode 100644 index 00000000..f1c86a9d --- /dev/null +++ b/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro @@ -0,0 +1,188 @@ + + + + + + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + fake_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + dynamixel_hardware_interface/DynamixelHardware + ${port_name} + 1000000 + 0.2 + /param/dxl_model + 5 + 5 + + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1 + + + 1, 0, 0, 0, 0, + 0, 1, 0, 0, 0, + 0, 0, 1, 0, 0, + 0, 0, 0, 1, 0, + 0, 0, 0, 0, 1 + + 200 + dynamixel_hardware_interface/dxl_state + dynamixel_hardware_interface/get_dxl_data + dynamixel_hardware_interface/set_dxl_data + dynamixel_hardware_interface/reboot_dxl + dynamixel_hardware_interface/set_dxl_torque + + + 1 + dxl5 + -1.52 + 0.92 + ${prefix}gripper_left_joint + 0.019 + -0.01 + + + + + + + ${-pi*0.1} + ${pi*0.1} + + + + + + + + + ${-pi*0.57} + ${pi*0.5} + + + + + + + + + ${-pi*0.3} + ${pi*0.44} + + + + + + + + + ${-pi*0.57} + ${pi*0.65} + + + + + + + + + -0.01 + 0.019 + + + + + + + + dxl + 11 + + + + + + + 800 + 100 + 100 + 0 + + + + dxl + 12 + + + + + + + 800 + 100 + 100 + 0 + + + + dxl + 13 + + + + + + + 800 + 100 + 100 + 0 + + + + dxl + 14 + + + + + + + 800 + 100 + 100 + 0 + + + + dxl + 15 + + + + + + + + + + + + + diff --git a/open_manipulator_x_description/rviz/open_manipulator_x.rviz b/open_manipulator_x_description/rviz/open_manipulator_x.rviz new file mode 100644 index 00000000..6c8b9ce1 --- /dev/null +++ b/open_manipulator_x_description/rviz/open_manipulator_x.rviz @@ -0,0 +1,256 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 549 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + dummy_mimic_fix: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + dummy_mimic_fix: + Value: true + end_effector_link: + Value: true + gripper_left_link: + Value: true + gripper_right_link: + Value: true + link1: + Value: true + link2: + Value: true + link3: + Value: true + link4: + Value: true + link5: + Value: true + world: + Value: true + Marker Scale: 0.3499999940395355 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + link1: + link2: + link3: + link4: + link5: + dummy_mimic_fix: + {} + end_effector_link: + {} + gripper_left_link: + {} + gripper_right_link: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.3669214248657227 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.09866781532764435 + Y: -0.062461238354444504 + Z: -0.023534774780273438 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.84539794921875 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.155397653579712 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 130 + Y: 71 diff --git a/open_manipulator_description/urdf/materials.xacro b/open_manipulator_x_description/urdf/materials.xacro similarity index 71% rename from open_manipulator_description/urdf/materials.xacro rename to open_manipulator_x_description/urdf/materials.xacro index 74abea43..30509889 100644 --- a/open_manipulator_description/urdf/materials.xacro +++ b/open_manipulator_x_description/urdf/materials.xacro @@ -25,12 +25,4 @@ - - - - - - - - diff --git a/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro b/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro new file mode 100644 index 00000000..2735f7db --- /dev/null +++ b/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro @@ -0,0 +1,266 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro b/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro new file mode 100644 index 00000000..0ad9f979 --- /dev/null +++ b/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find open_manipulator_x_bringup)/config/gazebo_controller_manager.yaml + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/open_manipulator_x_gui/CMakeLists.txt b/open_manipulator_x_gui/CMakeLists.txt new file mode 100644 index 00000000..5ca84e3c --- /dev/null +++ b/open_manipulator_x_gui/CMakeLists.txt @@ -0,0 +1,113 @@ +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ +cmake_minimum_required(VERSION 3.5) +project(open_manipulator_x_gui) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(moveit_msgs REQUIRED) +find_package(moveit_ros_planning REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(Qt5 COMPONENTS Widgets REQUIRED) + +################################################################################ +# Define project sources and headers +################################################################################ +set(PROJECT_SOURCES + src/main.cpp + src/main_window.cpp + src/qnode.cpp +) + +set(PROJECT_HEADERS + include/${PROJECT_NAME}/main_window.hpp + include/${PROJECT_NAME}/qnode.hpp +) + +set(PROJECT_UI + ui/main_window.ui +) + +################################################################################ +# Qt specific configurations +################################################################################ +set(CMAKE_AUTOUIC ON) +set(CMAKE_AUTOUIC_SEARCH_PATHS ui) +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +qt5_wrap_cpp(PROJECT_MOCS ${PROJECT_HEADERS}) +qt5_wrap_ui(PROJECT_UI_HEADERS ${PROJECT_UI}) + +################################################################################ +# Include directories +################################################################################ +include_directories( + include + ${EIGEN3_INCLUDE_DIR} + ${CMAKE_CURRENT_BINARY_DIR} +) + +################################################################################ +# Build +################################################################################ +add_executable(${PROJECT_NAME}_node + ${PROJECT_SOURCES} + ${PROJECT_MOCS} + ${PROJECT_UI_HEADERS} +) + +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + geometry_msgs + std_msgs + sensor_msgs + moveit_msgs + moveit_ros_planning + moveit_ros_planning_interface + Eigen3 + Qt5Widgets +) + +################################################################################ +# Install +################################################################################ +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY launch ui + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + + +################################################################################ +# Export information +################################################################################ +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}_node) + +################################################################################ +# Macro for ament package +################################################################################ +ament_package() diff --git a/open_manipulator_control_gui/include/open_manipulator_control_gui/main_window.hpp b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp similarity index 55% rename from open_manipulator_control_gui/include/open_manipulator_control_gui/main_window.hpp rename to open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp index 8dbb1596..e4afa2ab 100644 --- a/open_manipulator_control_gui/include/open_manipulator_control_gui/main_window.hpp +++ b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2018 ROBOTIS CO., LTD. +* Copyright 2024 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,30 +14,25 @@ * limitations under the License. *******************************************************************************/ -/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na */ +/* Authors: Ryan Shim, Sungho Woo */ -#ifndef open_manipulator_control_gui_MAIN_WINDOW_H -#define open_manipulator_control_gui_MAIN_WINDOW_H - -/***************************************************************************** -** Includes -*****************************************************************************/ +#ifndef OPEN_MANIPULATOR_X_GUI_MAIN_WINDOW_HPP +#define OPEN_MANIPULATOR_X_GUI_MAIN_WINDOW_HPP #include #include "ui_main_window.h" #include "qnode.hpp" #include #include +#include +#include +#include +#include +#include +#include -/***************************************************************************** -** Namespace -*****************************************************************************/ - -namespace open_manipulator_control_gui { +namespace open_manipulator_x_gui { -/***************************************************************************** -** Interface [MainWindow] -*****************************************************************************/ class MainWindow : public QMainWindow { Q_OBJECT @@ -58,23 +53,22 @@ public Q_SLOTS: void on_btn_read_kinematic_pose_clicked(void); void on_btn_send_kinematic_pose_clicked(void); void on_btn_set_gripper_clicked(void); - void on_btn_actuator_enable_clicked(void); - void on_btn_actuator_disable_clicked(void); - void on_btn_get_manipulator_setting_clicked(void); - void on_radio_drawing_line_clicked(void); - void on_radio_drawing_circle_clicked(void); - void on_radio_drawing_rhombus_clicked(void); - void on_radio_drawing_heart_clicked(void); - void on_btn_send_drawing_trajectory_clicked(void); - + void on_btn_save_pose_clicked(void); + void on_btn_play_clicked(void); + void on_btn_reset_task_clicked(void); + void on_btn_stop_clicked(void); + void on_btn_read_task_clicked(void); void tabSelected(); + void on_check_CheckBox_Toggled(bool checked); private: Ui::MainWindowDesign ui; QNode qnode; QTimer *timer; + QTableWidget *tableWidget; + std::string csv_file_path_; }; -} // namespace open_manipulator_control_gui +} // namespace open_manipulator_x_gui -#endif // open_manipulator_control_gui_MAIN_WINDOW_H +#endif // OPEN_MANIPULATOR_X_GUI_MAIN_WINDOW_HPP diff --git a/open_manipulator_x_gui/include/open_manipulator_x_gui/qnode.hpp b/open_manipulator_x_gui/include/open_manipulator_x_gui/qnode.hpp new file mode 100644 index 00000000..9a39c819 --- /dev/null +++ b/open_manipulator_x_gui/include/open_manipulator_x_gui/qnode.hpp @@ -0,0 +1,84 @@ +/******************************************************************************* +* Copyright 2024 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Ryan Shim, Sungho Woo */ + +#ifndef OPEN_MANIPULATOR_X_GUI_QNODE_HPP_ +#define OPEN_MANIPULATOR_X_GUI_QNODE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace open_manipulator_x_gui { + +class QNode : public QThread { + Q_OBJECT +public: + QNode(int argc, char** argv); + virtual ~QNode(); + bool init(); + void run(); + + std::vector getPresentJointAngle(); + std::vector getPresentKinematicsPosition(); + bool setJointSpacePath(std::vector joint_angle); + bool setTaskSpacePath( + std::vector kinematics_pose, + bool position_only, + double position_tol, + double orientation_tol); + bool setToolControl(std::vector joint_angle); + bool isMotionComplete(); + void stopMotion(); + bool isStopRequested() ; + void resetStopRequest(); + bool sendTorqueSrv(bool checked); + std::string getCSVPath() const; + +private: + int init_argc; + char** init_argv; + rclcpp::Node::SharedPtr node_; + std::shared_ptr executor_; + std::thread spinner_thread_; + rclcpp::Client::SharedPtr torque_client_; + + std::shared_ptr move_group_; + std::shared_ptr move_group2_; + + std::vector present_joint_angle_; + std::vector present_kinematics_position_; + + void updateRobotState(); + std::atomic stop_requested_; + std::string csv_file_path_; + + +Q_SIGNALS: + void rosShutdown(); +}; + +} // namespace open_manipulator_x_gui + +#endif /* OPEN_MANIPULATOR_X_GUI_QNODE_HPP_ */ diff --git a/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py b/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py new file mode 100644 index 00000000..df81a8d9 --- /dev/null +++ b/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py @@ -0,0 +1,24 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + csv_file_path = os.path.join( + get_package_share_directory('open_manipulator_x_gui'), + 'config', 'robot_joint_log.csv') + + gui_node = Node( + package='open_manipulator_x_gui', + executable='open_manipulator_x_gui_node', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'csv_path': csv_file_path} + ] + ) + + return LaunchDescription([ + gui_node + ]) diff --git a/open_manipulator_x_gui/package.xml b/open_manipulator_x_gui/package.xml new file mode 100644 index 00000000..7c2419ea --- /dev/null +++ b/open_manipulator_x_gui/package.xml @@ -0,0 +1,32 @@ + + + + open_manipulator_x_gui + 0.0.0 + TODO: Package description + yunwonho + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + qtbase5-dev + qt5-qmake + libqt5core5a + libqt5gui5 + eigen3_cmake_module + moveit_msgs + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/open_manipulator_x_gui/src/main.cpp b/open_manipulator_x_gui/src/main.cpp new file mode 100644 index 00000000..47e89e50 --- /dev/null +++ b/open_manipulator_x_gui/src/main.cpp @@ -0,0 +1,14 @@ +#include +#include +#include "../include/open_manipulator_x_gui/main_window.hpp" +#include "open_manipulator_x_gui/qnode.hpp" + +int main(int argc, char **argv) { + QApplication app(argc, argv); + open_manipulator_x_gui::MainWindow w(argc, argv); + w.show(); + app.connect(&app, SIGNAL(lastWindowClosed()), &app, SLOT(quit())); + + int result = app.exec(); + return result; +} diff --git a/open_manipulator_x_gui/src/main_window.cpp b/open_manipulator_x_gui/src/main_window.cpp new file mode 100644 index 00000000..b52ed211 --- /dev/null +++ b/open_manipulator_x_gui/src/main_window.cpp @@ -0,0 +1,571 @@ +/******************************************************************************* + * Copyright 2024 ROBOTIS CO., LTD. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ + +/* Authors: Ryan Shim, Sungho Woo */ + +#include +#include +#include +#include "../include/open_manipulator_x_gui/main_window.hpp" + +namespace open_manipulator_x_gui +{ + + using namespace Qt; + + MainWindow::MainWindow(int argc, char **argv, QWidget *parent) + : QMainWindow(parent), qnode(argc, argv) + { + ui.setupUi(this); + tableWidget = new QTableWidget(this); + tableWidget->setColumnCount(6); + QStringList headers; + headers << "Joint 1" << "Joint 2" << "Joint 3" << "Joint 4" << "Gripper" << "Status"; + tableWidget->setHorizontalHeaderLabels(headers); + tableWidget->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + + ui.verticalLayout_19->addWidget(tableWidget); + + QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); + connect(ui.tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabSelected())); + QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close())); + connect(ui.checkBox_2, &QCheckBox::toggled, this, &MainWindow::on_check_CheckBox_Toggled); + + ui.btn_init_pose->setEnabled(false); + ui.btn_home_pose->setEnabled(false); + ui.btn_gripper_open->setEnabled(false); + ui.btn_gripper_close->setEnabled(false); + ui.btn_read_joint_angle->setEnabled(false); + ui.btn_send_joint_angle->setEnabled(false); + ui.btn_read_kinematic_pose->setEnabled(false); + ui.btn_send_kinematic_pose->setEnabled(false); + ui.btn_set_gripper->setEnabled(false); + ui.btn_save_pose->setEnabled(false); + ui.btn_play->setEnabled(false); + ui.btn_stop->setEnabled(false); + ui.btn_reset_task->setEnabled(false); + ui.btn_read_task->setEnabled(false); + ui.tabWidget->setEnabled(false); + + ui.btn_timer_start->setEnabled(true); + + qnode.init(); + } + + MainWindow::~MainWindow() {} + + void MainWindow::timerCallback() + { + std::vector joint_angle = qnode.getPresentJointAngle(); + ui.txt_j1->setText(QString::number(joint_angle.at(0), 'f', 3)); + ui.txt_j2->setText(QString::number(joint_angle.at(1), 'f', 3)); + ui.txt_j3->setText(QString::number(joint_angle.at(2), 'f', 3)); + ui.txt_j4->setText(QString::number(joint_angle.at(3), 'f', 3)); + // ui.txt_j5->setText(QString::number(joint_angle.at(4), 'f', 3)); + // ui.txt_j6->setText(QString::number(joint_angle.at(5), 'f', 3)); + ui.txt_grip->setText(QString::number(joint_angle.at(4), 'f', 3)); + + std::vector position = qnode.getPresentKinematicsPosition(); + + if (position.size() != 7) + return; + + ui.txt_x->setText(QString::number(position.at(0), 'f', 3)); + ui.txt_y->setText(QString::number(position.at(1), 'f', 3)); + ui.txt_z->setText(QString::number(position.at(2), 'f', 3)); + ui.txt_q_x->setText(QString::number(position.at(3), 'f', 3)); + ui.txt_q_y->setText(QString::number(position.at(4), 'f', 3)); + ui.txt_q_z->setText(QString::number(position.at(5), 'f', 3)); + ui.txt_q_w->setText(QString::number(position.at(6), 'f', 3)); + } + void MainWindow::tabSelected() + { + if (ui.tabWidget->currentIndex() == 0) + on_btn_read_joint_angle_clicked(); + if (ui.tabWidget->currentIndex() == 1) + on_btn_read_kinematic_pose_clicked(); + } + + void MainWindow::writeLog(QString str) + { + ui.plainTextEdit_log->moveCursor(QTextCursor::End); + ui.plainTextEdit_log->appendPlainText(str); + } + + void MainWindow::on_btn_timer_start_clicked(void) + { + timer = new QTimer(this); + connect(timer, SIGNAL(timeout()), this, SLOT(timerCallback())); + timer->start(100); + writeLog("QTimer start : 100ms"); + ui.btn_timer_start->setEnabled(false); + ui.btn_init_pose->setEnabled(true); + ui.btn_home_pose->setEnabled(true); + ui.btn_gripper_open->setEnabled(true); + ui.btn_gripper_close->setEnabled(true); + ui.btn_read_joint_angle->setEnabled(true); + ui.btn_send_joint_angle->setEnabled(true); + ui.btn_read_kinematic_pose->setEnabled(true); + ui.btn_send_kinematic_pose->setEnabled(true); + ui.btn_set_gripper->setEnabled(true); + ui.btn_play->setEnabled(true); + ui.btn_stop->setEnabled(true); + ui.btn_reset_task->setEnabled(true); + ui.btn_read_task->setEnabled(true); + ui.tabWidget->setEnabled(true); + + csv_file_path_ = qnode.getCSVPath(); + + qnode.start(); + } + + void MainWindow::on_btn_init_pose_clicked(void) + { + std::thread([this]() + { + std::vector joint_angle = {0.0, 0.0, 0.0, 0.0}; + + if (!qnode.setJointSpacePath(joint_angle)) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[ERR!!] Failed to send service"); + }, Qt::QueuedConnection); + return; + } + + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send joint angle to initial pose"); + }, Qt::QueuedConnection); + }).detach(); + } + + void MainWindow::on_btn_home_pose_clicked(void) + { + std::thread([this]() + { + std::vector joint_angle = {0.0, -1.0, 0.7, 0.3}; + + if (!qnode.setJointSpacePath(joint_angle)) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[ERR!!] Failed to send service"); + }, Qt::QueuedConnection); + return; + } + + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send joint angle to home pose"); + }, Qt::QueuedConnection); + }).detach(); + } + + void MainWindow::on_btn_gripper_open_clicked(void) + { + std::thread([this]() + { + std::vector joint_angle = {0.019}; + + if (!qnode.setToolControl(joint_angle)) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[ERR!!] Failed to send service"); + }, Qt::QueuedConnection); + return; + } + + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send gripper open"); + }, Qt::QueuedConnection); + }).detach(); + } + + void MainWindow::on_btn_gripper_close_clicked(void) + { + std::thread([this]() + { + std::vector joint_angle = {-0.01}; + + if (!qnode.setToolControl(joint_angle)) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[WARN!!] Contacted with a object"); + }, Qt::QueuedConnection); + return; + } + + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send gripper close"); + }, Qt::QueuedConnection); + }).detach(); + } + + void MainWindow::on_btn_read_joint_angle_clicked(void) + { + std::vector joint_angle = qnode.getPresentJointAngle(); + ui.doubleSpinBox_j1->setValue(joint_angle.at(0)); + ui.doubleSpinBox_j2->setValue(joint_angle.at(1)); + ui.doubleSpinBox_j3->setValue(joint_angle.at(2)); + ui.doubleSpinBox_j4->setValue(joint_angle.at(3)); + + ui.doubleSpinBox_gripper->setValue(joint_angle.at(4)); + + writeLog("Read joint angle"); + } + + void MainWindow::on_btn_send_joint_angle_clicked(void) + { + std::vector joint_angle; + + joint_angle.push_back(ui.doubleSpinBox_j1->value()); + joint_angle.push_back(ui.doubleSpinBox_j2->value()); + joint_angle.push_back(ui.doubleSpinBox_j3->value()); + joint_angle.push_back(ui.doubleSpinBox_j4->value()); + + std::thread([this, joint_angle]() + { + bool success = qnode.setJointSpacePath(joint_angle); + if (!success) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[ERR!!] Failed to send service"); + }, Qt::QueuedConnection); + } + else + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send joint angle"); + }, Qt::QueuedConnection); + } + }).detach(); + } + + void MainWindow::on_btn_read_kinematic_pose_clicked(void) + { + std::vector position = qnode.getPresentKinematicsPosition(); + ui.doubleSpinBox_x->setValue(position.at(0)); + ui.doubleSpinBox_y->setValue(position.at(1)); + ui.doubleSpinBox_z->setValue(position.at(2)); + ui.doubleSpinBox_q_x->setValue(position.at(3)); + ui.doubleSpinBox_q_y->setValue(position.at(4)); + ui.doubleSpinBox_q_z->setValue(position.at(5)); + ui.doubleSpinBox_q_w->setValue(position.at(6)); + + writeLog("Read task pose"); + } + + void MainWindow::on_btn_send_kinematic_pose_clicked(void) + { + std::vector kinematics_pose; + Eigen::Quaterniond temp_orientation; + + bool position_only = ui.checkBox->isChecked(); + double position_tol = ui.doubleSpinBox_position_tol->value(); + double orientation_tol = ui.doubleSpinBox_orientation_tol->value(); + + kinematics_pose.push_back(ui.doubleSpinBox_x->value()); + kinematics_pose.push_back(ui.doubleSpinBox_y->value()); + kinematics_pose.push_back(ui.doubleSpinBox_z->value()); + kinematics_pose.push_back(ui.doubleSpinBox_q_x->value()); + kinematics_pose.push_back(ui.doubleSpinBox_q_y->value()); + kinematics_pose.push_back(ui.doubleSpinBox_q_z->value()); + kinematics_pose.push_back(ui.doubleSpinBox_q_w->value()); + + std::thread([this, kinematics_pose, position_only, position_tol, orientation_tol]() + { + bool success = qnode.setTaskSpacePath(kinematics_pose, position_only, position_tol, orientation_tol); + if (!success) + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("[ERR!!] Failed to send service"); + }, Qt::QueuedConnection); + } + else + { + QMetaObject::invokeMethod(this, [this]() { + writeLog("Send task pose"); + }, Qt::QueuedConnection); + } + }).detach(); + } + + void MainWindow::on_btn_set_gripper_clicked(void) + { + std::vector joint_angle; + joint_angle.push_back(ui.doubleSpinBox_gripper->value()); + if (!qnode.setToolControl(joint_angle)) + { + writeLog("[WARN!!] Contacted with a object"); + return; + } + writeLog("Send gripper value"); + } + + void MainWindow::on_btn_save_pose_clicked(void) + { + std::vector joint_angle = qnode.getPresentJointAngle(); + std::ofstream file; + file.open(csv_file_path_, std::ios::app); + + if (file.is_open()) + { + file << joint_angle.at(0) << "," << joint_angle.at(1) << "," + << joint_angle.at(2) << "," << joint_angle.at(3) << "," + << joint_angle.at(4) << std::endl; + file.close(); + + writeLog("Pose saved to CSV."); + + int row = tableWidget->rowCount(); + tableWidget->insertRow(row); + for (int i = 0; i < 5; ++i) + { + QTableWidgetItem *item = new QTableWidgetItem(QString::number(joint_angle.at(i), 'f', 3)); + tableWidget->setItem(row, i, item); + } + QTableWidgetItem *statusItem = new QTableWidgetItem("Saved"); + tableWidget->setItem(row, 5, statusItem); + } + else + { + writeLog("[ERR!!] Unable to open file."); + } + } + + void MainWindow::on_btn_read_task_clicked(void) + { + tableWidget->setRowCount(0); + std::ifstream file(csv_file_path_); + std::string line; + + if (file.is_open()) + { + int row = 0; + while (std::getline(file, line)) + { + std::vector values; + std::stringstream ss(line); + std::string value; + + while (std::getline(ss, value, ',')) + { + values.push_back(value); + } + + if (values.size() == 5) + { + tableWidget->insertRow(row); + for (int col = 0; col < 5; ++col) + { + double doubleValue = std::stod(values.at(col)); + QTableWidgetItem *item = new QTableWidgetItem(QString::number(doubleValue, 'f', 3)); + tableWidget->setItem(row, col, item); + } + QTableWidgetItem *statusItem = new QTableWidgetItem("Loaded"); + tableWidget->setItem(row, 5, statusItem); + ++row; + } + } + file.close(); + writeLog("Task data loaded and displayed."); + } + else + { + writeLog("[ERR!!] Unable to open file."); + } + ui.btn_save_pose->setEnabled(true); + } + + void MainWindow::on_btn_play_clicked(void) + { + if (tableWidget->rowCount() == 0) + { + writeLog("[ERR!!] No tasks available in the table."); + return; + } + + qnode.resetStopRequest(); + + int repeatCount = ui.txt_rap->value(); + + std::thread([this, repeatCount]() + { + for (int repeat = 0; repeat < repeatCount; ++repeat) + { + if (qnode.isStopRequested()) + { + QMetaObject::invokeMethod(this, [this]() + { writeLog("Play operation stopped by user."); }, Qt::QueuedConnection); + break; + } + + QMetaObject::invokeMethod(this, [this, repeat, repeatCount]() + { writeLog(QString("Executing task repetition %1 of %2").arg(repeat + 1).arg(repeatCount)); }, Qt::QueuedConnection); + + int rowCount = tableWidget->rowCount(); + for (int row = 0; row < rowCount; ++row) + { + for (int col = 0; col < 6; ++col) + { + QMetaObject::invokeMethod(this, [this, row, col]() + { tableWidget->item(row, col)->setBackground(Qt::white); }, Qt::QueuedConnection); + } + QMetaObject::invokeMethod(this, [this, row]() + { tableWidget->item(row, 5)->setText("Pending"); }, Qt::QueuedConnection); + } + + std::ifstream file(csv_file_path_); + std::string line; + std::vector current_joint_angles = qnode.getPresentJointAngle(); + double previous_gripper_value = current_joint_angles.back(); + + if (file.is_open()) + { + int row = 0; + while (std::getline(file, line)) + { + if (qnode.isStopRequested()) + { + QMetaObject::invokeMethod(this, [this]() + { writeLog("Play operation stopped by user."); }, Qt::QueuedConnection); + break; + } + + std::vector joint_angle; + std::stringstream ss(line); + std::string value; + + while (std::getline(ss, value, ',')) + { + joint_angle.push_back(std::stod(value)); + } + + if (joint_angle.size() == 5) + { + double current_gripper_value = joint_angle.back(); + + QMetaObject::invokeMethod(this, [this, row]() + { + for (int col = 0; col < 6; ++col) + { + tableWidget->item(row, col)->setBackground(Qt::yellow); + } + tableWidget->item(row, 5)->setText("Executing..."); }, Qt::QueuedConnection); + + if (!qnode.setJointSpacePath(joint_angle)) + { + QMetaObject::invokeMethod(this, [this]() + { writeLog("[ERR!!] Failed to send joint space path service"); }, Qt::QueuedConnection); + break; + } + + if (std::abs(current_gripper_value - previous_gripper_value) > 0.01) + { + std::vector gripper_only = {current_gripper_value}; + if (!qnode.setToolControl(gripper_only)) + { + QMetaObject::invokeMethod(this, [this]() + { writeLog("[ERR!!] Failed to send gripper control service"); }, Qt::QueuedConnection); + break; + } + + QMetaObject::invokeMethod(this, [this]() + { writeLog("Gripper adjusted."); }, Qt::QueuedConnection); + } + + QMetaObject::invokeMethod(this, [this]() + { writeLog("Playing saved pose."); }, Qt::QueuedConnection); + + QMetaObject::invokeMethod(this, [this, row]() + { + for (int col = 0; col < 6; ++col) + { + tableWidget->item(row, col)->setBackground(Qt::green); + } + tableWidget->item(row, 5)->setText("Done"); }, Qt::QueuedConnection); + + previous_gripper_value = current_gripper_value; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + row++; + } + file.close(); + } + else + { + QMetaObject::invokeMethod(this, [this]() + { writeLog("[ERR!!] Unable to open file."); }, Qt::QueuedConnection); + } + } + + QMetaObject::invokeMethod(this, [this]() + { + writeLog("All repetitions completed. Resetting table."); + int rowCount = tableWidget->rowCount(); + for (int row = 0; row < rowCount; ++row) + { + for (int col = 0; col < 6; ++col) + { + tableWidget->item(row, col)->setBackground(Qt::white); + } + tableWidget->item(row, 5)->setText("Done"); + } }, Qt::QueuedConnection); }) + .detach(); + } + + + void MainWindow::on_btn_stop_clicked(void) + { + qnode.stopMotion(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + int rowCount = tableWidget->rowCount(); + for (int row = 0; row < rowCount; ++row) + { + for (int col = 0; col < 6; ++col) + { + tableWidget->item(row, col)->setBackground(Qt::white); + } + tableWidget->item(row, 5)->setText("Stopped"); + } + writeLog("Robot motion stopped and state reset."); + } + + void MainWindow::on_btn_reset_task_clicked(void) + { + std::ofstream file(csv_file_path_, std::ios::trunc); + if (file.is_open()) + { + file.close(); + writeLog("CSV file and log cleared."); + } + else + { + writeLog("[ERR!!] Unable to reset file."); + } + tableWidget->setRowCount(0); + + ui.plainTextEdit_log->clear(); + writeLog("Reset completed."); + } + + void MainWindow::on_check_CheckBox_Toggled(bool checked) + { + writeLog(checked ? "Torque ON" : "Torque OFF"); + if (!qnode.sendTorqueSrv(checked)) + writeLog("[ERR!!] Failed to send torque service."); + } + +} // namespace open_manipulator_x_gui diff --git a/open_manipulator_x_gui/src/qnode.cpp b/open_manipulator_x_gui/src/qnode.cpp new file mode 100644 index 00000000..dc5195b9 --- /dev/null +++ b/open_manipulator_x_gui/src/qnode.cpp @@ -0,0 +1,302 @@ +/******************************************************************************* +* Copyright 2024 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +/* Authors: Ryan Shim, Sungho Woo */ + +#include "open_manipulator_x_gui/qnode.hpp" + +namespace open_manipulator_x_gui { + +QNode::QNode(int argc, char** argv) + : QThread(), + init_argc(argc), + init_argv(argv) +{ +} + +QNode::~QNode() +{ + if (spinner_thread_.joinable()) { + rclcpp::shutdown(); + spinner_thread_.join(); + } +} + +bool QNode::init() +{ + rclcpp::init(init_argc, init_argv); + + std::string node_name = "open_manipulator_x_gui"; + node_ = std::make_shared(node_name); + + if (!rclcpp::ok()) { + return false; + } + + node_->declare_parameter("csv_path", "robot_joint_log.csv"); + node_->get_parameter("csv_path", csv_file_path_); + RCLCPP_INFO(node_->get_logger(), "CSV Path set to: %s", csv_file_path_.c_str()); + + std::string planning_group_name = "arm"; + move_group_ = std::make_shared(node_, planning_group_name); + + std::string planning_group_name2 = "gripper"; + move_group2_ = std::make_shared(node_, planning_group_name2); + + if (!move_group_ || !move_group2_) { + RCLCPP_ERROR(node_->get_logger(), "Failed to initialize move groups"); + return false; + } + executor_ = std::make_shared(); + executor_->add_node(node_); + + spinner_thread_ = std::thread([this]() { executor_->spin(); }); + + torque_client_ = node_->create_client("dynamixel_hardware_interface/set_dxl_torque"); + + return true; +} + +void QNode::run() +{ + rclcpp::Rate loop_rate(10); + while (rclcpp::ok()) { + updateRobotState(); + loop_rate.sleep(); + } + RCLCPP_INFO(node_->get_logger(), "ROS shutdown, proceeding to close the GUI."); + Q_EMIT rosShutdown(); +} + +void QNode::updateRobotState() +{ + std::vector jointValues = move_group_->getCurrentJointValues(); + std::vector jointValues2 = move_group2_->getCurrentJointValues(); + std::vector temp_angle; + + if (jointValues.size() >= 4) { + temp_angle.push_back(jointValues.at(0)); + temp_angle.push_back(jointValues.at(1)); + temp_angle.push_back(jointValues.at(2)); + temp_angle.push_back(jointValues.at(3)); + } else { + RCLCPP_ERROR(node_->get_logger(), "jointValues does not have enough elements"); + return; + } + + if (jointValues2.size() >= 1) { + temp_angle.push_back(jointValues2.at(0)); + } else { + RCLCPP_ERROR(node_->get_logger(), "jointValues2 does not have enough elements"); + return; + } + + present_joint_angle_ = temp_angle; + + geometry_msgs::msg::Pose current_pose = move_group_->getCurrentPose().pose; + std::vector temp_position; + temp_position.push_back(current_pose.position.x); + temp_position.push_back(current_pose.position.y); + temp_position.push_back(current_pose.position.z); + temp_position.push_back(current_pose.orientation.x); + temp_position.push_back(current_pose.orientation.y); + temp_position.push_back(current_pose.orientation.z); + temp_position.push_back(current_pose.orientation.w); + + present_kinematics_position_ = temp_position; +} + +std::string QNode::getCSVPath() const { + return csv_file_path_; +} + +std::vector QNode::getPresentJointAngle() +{ + return present_joint_angle_; +} + +std::vector QNode::getPresentKinematicsPosition() +{ + return present_kinematics_position_; +} + +bool QNode::setJointSpacePath(std::vector joint_angle) +{ + const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup("arm"); + + moveit::core::RobotStatePtr current_state = move_group_->getCurrentState(); + + std::vector joint_group_positions; + current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); + + joint_group_positions[0] = joint_angle.at(0); // radians + joint_group_positions[1] = joint_angle.at(1); // radians + joint_group_positions[2] = joint_angle.at(2); // radians + joint_group_positions[3] = joint_angle.at(3); // radians + move_group_->setJointValueTarget(joint_group_positions); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + if (!success) + return false; + + move_group_->move(); + while (!isMotionComplete()){ + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + return true; +} + +bool QNode::isMotionComplete() +{ + if (isStopRequested()) { + RCLCPP_INFO(node_->get_logger(), "Motion stopped by user."); + return true; + } + + moveit::core::RobotStatePtr current_state = move_group_->getCurrentState(); + + const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup("arm"); + std::vector current_joint_positions; + current_state->copyJointGroupPositions(joint_model_group, current_joint_positions); + + std::vector target_joint_positions; + move_group_->getJointValueTarget(target_joint_positions); + + const double tolerance = 0.03; + for (size_t i = 0; i < current_joint_positions.size(); ++i) + { + if (std::abs(current_joint_positions[i] - target_joint_positions[i]) > tolerance) + { + return false; + } + } + RCLCPP_INFO(node_->get_logger(), "Motion completed"); + return true; +} + +bool QNode::setTaskSpacePath( + std::vector kinematics_pose, + bool position_only, + double position_tol, + double orientation_tol) +{ + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = kinematics_pose.at(0); + target_pose.position.y = kinematics_pose.at(1); + target_pose.position.z = kinematics_pose.at(2); + target_pose.orientation.x = kinematics_pose.at(3); + target_pose.orientation.y = kinematics_pose.at(4); + target_pose.orientation.z = kinematics_pose.at(5); + target_pose.orientation.w = kinematics_pose.at(6); + + if (position_only) { + move_group_->setPositionTarget( + target_pose.position.x, + target_pose.position.y, + target_pose.position.z); + move_group_->setGoalPositionTolerance(position_tol); + move_group_->setGoalOrientationTolerance(orientation_tol); + } else { + move_group_->setGoalPositionTolerance(position_tol); + move_group_->setGoalOrientationTolerance(orientation_tol); + move_group_->setPoseTarget(target_pose); + } + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + if (!success) + return false; + + move_group_->move(); + + return true; +} + +bool QNode::setToolControl(std::vector joint_angle) +{ + const moveit::core::JointModelGroup* joint_model_group = move_group2_->getCurrentState()->getJointModelGroup("gripper"); + + moveit::core::RobotStatePtr current_state = move_group2_->getCurrentState(); + + std::vector joint_group_positions; + current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); + + joint_group_positions[0] = joint_angle.at(0); // radians + move_group2_->setJointValueTarget(joint_group_positions); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group2_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + if (!success) + return false; + + std::atomic move_completed(false); + + std::thread move_thread([&]() { + move_group2_->move(); + move_completed.store(true); + }); + + std::this_thread::sleep_for(std::chrono::seconds(2)); + + if (!move_completed.load()) { + move_group2_->stop(); + move_thread.join(); + return true; + } + move_thread.join(); + return true; +} + +void QNode::stopMotion() +{ + stop_requested_ = true; + if (move_group_) { + move_group_->stop(); + } + if (move_group2_) { + move_group2_->stop(); + } + RCLCPP_INFO(node_->get_logger(), "Robot motion stopped."); +} + +bool QNode::isStopRequested() +{ + return stop_requested_; +} + +void QNode::resetStopRequest() +{ + stop_requested_ = false; +} + +bool QNode::sendTorqueSrv(bool checked) +{ + if (!torque_client_->wait_for_service(std::chrono::seconds(5))) + { + RCLCPP_ERROR(node_->get_logger(), "Torque service not available."); + return false; + } + + auto request = std::make_shared(); + request->data = checked; + + auto future = torque_client_->async_send_request(request); + RCLCPP_INFO(node_->get_logger(), "Torque service succeeded: %s", checked ? "ON" : "OFF"); + return true; +} + +} // namespace open_manipulator_x_gui diff --git a/open_manipulator_x_gui/ui/main_window.ui b/open_manipulator_x_gui/ui/main_window.ui new file mode 100644 index 00000000..c009d948 --- /dev/null +++ b/open_manipulator_x_gui/ui/main_window.ui @@ -0,0 +1,1305 @@ + + + MainWindowDesign + + + true + + + + 0 + 0 + 900 + 759 + + + + + 0 + 0 + + + + + 793 + 550 + + + + + 900 + 800 + + + + OpenManipulator control GUI + + + + :/images/icon.png:/images/icon.png + + + + + + + + + + + + + + Logging + + + + + + + true + + + + + + + Task + + + + + + + + + + + + + + Timer Start + + + + + + + + + + + + + + Joint 2 + + + + + + + Gripper + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + rad + + + + + + + Joint 4 + + + + + + + rad + + + + + + + rad + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + rad + + + + + + + Joint 1 + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + true + + + Qt::LeftToRight + + + 0.00 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + Joint 3 + + + + + + + rad + + + + + + + + + + + + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + Q.x + + + + + + + X + + + + + + + Z + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + m + + + + + + + Q.y + + + + + + + Y + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + Q.z + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + Q.w + + + + + + + true + + + Qt::LeftToRight + + + 0.0 + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + true + + + + + + + m + + + + + + + m + + + + + + + + + + + + + + + false + + + Init pose + + + + + + + false + + + Home pose + + + + + + + + + + + false + + + Gripper open + + + + + + + false + + + Gripper close + + + + + + + + + + + + 348 + 348 + + + + 0 + + + + true + + + false + + + Joint space + + + + + 10 + 10 + 321 + 162 + + + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + -3.140000000000000 + + + 3.140000000000000 + + + 0.050000000000000 + + + + + + + rad + + + + + + + rad + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + -3.140000000000000 + + + 3.140000000000000 + + + 0.050000000000000 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + -3.140000000000000 + + + 3.140000000000000 + + + 0.050000000000000 + + + + + + + Joint 3 + + + + + + + rad + + + + + + + rad + + + + + + + Joint 2 + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + -3.140000000000000 + + + 3.140000000000000 + + + 0.050000000000000 + + + + + + + Joint 4 + + + + + + + Joint 1 + + + + + + + + + + + false + + + Read joint angle + + + + + + + false + + + Send + + + + + + + + + + + Task space + + + + + 10 + 10 + 321 + 222 + + + + + + + + + X + + + + + + + Y + + + + + + + m + + + + + + + m + + + + + + + Q.x + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + Z + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + Q.y + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + Q.z + + + + + + + Q.w + + + + + + + 3 + + + -1.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + m + + + + + + + + + + + + + + + + + + + + false + + + position_tol + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + orientation_tol + + + + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + 0.000000000000000 + + + 1.000000000000000 + + + 0.010000000000000 + + + 0.050000000000000 + + + + + + + position only + + + true + + + + + + + + + + + false + + + Read kinematic pose + + + + + + + false + + + Send + + + + + + + + + + + + + Task constructor + + + + + 10 + 10 + 321 + 231 + + + + + + + Save pose + + + + + + + + + + + + + Iters + + + + + + + 999 + + + 1 + + + + + + + + + Play + + + + + + + + + + + true + + + Read task + + + + + + + Stop + + + + + + + + + + + Reset task + + + + + + + + + 10 + 250 + 191 + 31 + + + + + 75 + true + + + + Torque on/off + + + true + + + + + + 140 + 250 + 191 + 73 + + + + Qt::LeftToRight + + + Caution: Torque off may cause collision; torque on may cause sudden pose return. + + + + + + + + + + false + + + true + + + + + + + + + + + Gripper + + + + + + + + 0 + 4 + + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + 3 + + + -1.135000000000000 + + + 1.135000000000000 + + + 0.002000000000000 + + + 0.000000000000000 + + + + + + + rad + + + + + + + false + + + Set gripper + + + + + + + + + + + + + + + 0 + 0 + 900 + 22 + + + + + &App + + + + + + + + + + + + + &Quit + + + Ctrl+Q + + + Qt::ApplicationShortcut + + + + + &Preferences + + + + + &About + + + + + About &Qt + + + + + + + action_Quit + triggered() + MainWindowDesign + close() + + + -1 + -1 + + + 399 + 299 + + + + + diff --git a/open_manipulator_x_moveit_config/.setup_assistant b/open_manipulator_x_moveit_config/.setup_assistant new file mode 100644 index 00000000..70083b6e --- /dev/null +++ b/open_manipulator_x_moveit_config/.setup_assistant @@ -0,0 +1,25 @@ +moveit_setup_assistant_config: + urdf: + package: open_manipulator_x_description + relative_path: urdf/open_manipulator_x_robot.urdf.xacro + srdf: + relative_path: config/open_manipulator_x.srdf + package_settings: + author_name: yunwonho + author_email: sirius584@naver.com + generated_timestamp: 1732003006 + control_xacro: + command: + - position + state: + - position + - velocity + modified_urdf: + xacros: + - control_xacro + control_xacro: + command: + - position + state: + - position + - velocity \ No newline at end of file diff --git a/open_manipulator_x_moveit_config/CMakeLists.txt b/open_manipulator_x_moveit_config/CMakeLists.txt new file mode 100644 index 00000000..c8ce8a11 --- /dev/null +++ b/open_manipulator_x_moveit_config/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.22) +project(open_manipulator_x_moveit_config) + +find_package(ament_cmake REQUIRED) + +ament_package() + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/open_manipulator_x_moveit_config/config/initial_positions.yaml b/open_manipulator_x_moveit_config/config/initial_positions.yaml new file mode 100644 index 00000000..eb06942d --- /dev/null +++ b/open_manipulator_x_moveit_config/config/initial_positions.yaml @@ -0,0 +1,8 @@ +# Default initial positions for open_manipulator_x's ros2_control fake system + +initial_positions: + gripper_left_joint: 0 + joint1: 0 + joint2: 0 + joint3: 0 + joint4: 0 diff --git a/open_manipulator_x_moveit_config/config/joint_limits.yaml b/open_manipulator_x_moveit_config/config/joint_limits.yaml new file mode 100644 index 00000000..b8c7d327 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/joint_limits.yaml @@ -0,0 +1,42 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + gripper_left_joint: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 + has_position_limits: true + + gripper_right_joint: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 + joint1: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 + joint2: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 + joint3: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 + joint4: + has_velocity_limits: true + max_velocity: 4.7999999999999998 + has_acceleration_limits: false + max_acceleration: 0 diff --git a/open_manipulator_x_moveit_config/config/kinematics.yaml b/open_manipulator_x_moveit_config/config/kinematics.yaml new file mode 100644 index 00000000..62de4e92 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/kinematics.yaml @@ -0,0 +1,6 @@ +arm: + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 + kinematics_solver_attempts: 3 + position_only_ik: False diff --git a/open_manipulator_x_moveit_config/config/moveit.rviz b/open_manipulator_x_moveit_config/config/moveit.rviz new file mode 100644 index 00000000..f31651ed --- /dev/null +++ b/open_manipulator_x_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: world + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: world + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/open_manipulator_x_moveit_config/config/moveit_controllers.yaml b/open_manipulator_x_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 00000000..eef405c0 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,21 @@ +# MoveIt uses this configuration for controller management + + controller_names: + - arm_controller + - gripper_controller + + arm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + gripper_controller: + type: GripperCommand + joints: + - gripper_left_joint + action_ns: gripper_cmd + default: true diff --git a/open_manipulator_x_moveit_config/config/moveit_servo.yaml b/open_manipulator_x_moveit_config/config/moveit_servo.yaml new file mode 100644 index 00000000..317f68a4 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/moveit_servo.yaml @@ -0,0 +1,69 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +# use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. + joint: 0.5 + +## Properties of outgoing commands +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: true +publish_joint_accelerations: false + +## Incoming Joint State properties +low_pass_filter_coeff: 2.0 # Larger --> trust the filtered data more, trust the measurements less. + +## MoveIt properties +move_group_name: arm # Often 'manipulator' or 'arm' +planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: end_effector_link # The name of the end effector link, used to return the EE pose +robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. + +## Topic names +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: true # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: threshold_distance +# Parameters for "threshold_distance"-type collision checking +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/open_manipulator_x_moveit_config/config/moveit_teleop.rviz b/open_manipulator_x_moveit_config/config/moveit_teleop.rviz new file mode 100644 index 00000000..4d8f5dc3 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/moveit_teleop.rviz @@ -0,0 +1,236 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 746 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_scan: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + caster_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + caster_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wheel_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.9508342742919922 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 975 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000375fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000375000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000375fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000375000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000050f0000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 175 diff --git a/open_manipulator_x_moveit_config/config/ompl_planning.yaml b/open_manipulator_x_moveit_config/config/ompl_planning.yaml new file mode 100644 index 00000000..12ae9905 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/ompl_planning.yaml @@ -0,0 +1,203 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar + FMTkConfigDefault: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMTkConfigDefault: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDSTkConfigDefault: + type: geometric::PDST + STRIDEkConfigDefault: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRTkConfigDefault: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRTkConfigDefault: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiESTkConfigDefault: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjESTkConfigDefault: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRMkConfigDefault: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstarkConfigDefault: + type: geometric::LazyPRMstar + SPARSkConfigDefault: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwokConfigDefault: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 + TrajOptDefault: + type: geometric::TrajOpt + +arm: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +arm_hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault +hand: + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + - FMTkConfigDefault + - BFMTkConfigDefault + - PDSTkConfigDefault + - STRIDEkConfigDefault + - BiTRRTkConfigDefault + - LBTRRTkConfigDefault + - BiESTkConfigDefault + - ProjESTkConfigDefault + - LazyPRMkConfigDefault + - LazyPRMstarkConfigDefault + - SPARSkConfigDefault + - SPARStwokConfigDefault + - TrajOptDefault diff --git a/open_manipulator_x_moveit_config/config/open_manipulator_x.ros2_control.xacro b/open_manipulator_x_moveit_config/config/open_manipulator_x.ros2_control.xacro new file mode 100644 index 00000000..519fbb68 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/open_manipulator_x.ros2_control.xacro @@ -0,0 +1,49 @@ + + + + + + + + + mock_components/GenericSystem + + + + + ${initial_positions['joint1']} + + + + + + + ${initial_positions['joint2']} + + + + + + + ${initial_positions['joint3']} + + + + + + + ${initial_positions['joint4']} + + + + + + + ${initial_positions['gripper_left_joint']} + + + + + + + diff --git a/open_manipulator_x_moveit_config/config/open_manipulator_x.srdf b/open_manipulator_x_moveit_config/config/open_manipulator_x.srdf new file mode 100644 index 00000000..5c2a72da --- /dev/null +++ b/open_manipulator_x_moveit_config/config/open_manipulator_x.srdf @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/open_manipulator_x_moveit_config/config/open_manipulator_x.urdf.xacro b/open_manipulator_x_moveit_config/config/open_manipulator_x.urdf.xacro new file mode 100644 index 00000000..57986217 --- /dev/null +++ b/open_manipulator_x_moveit_config/config/open_manipulator_x.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/open_manipulator_x_moveit_config/config/pilz_cartesian_limits.yaml b/open_manipulator_x_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 00000000..b2997caf --- /dev/null +++ b/open_manipulator_x_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/open_manipulator_x_moveit_config/launch/demo.launch.py b/open_manipulator_x_moveit_config/launch/demo.launch.py new file mode 100644 index 00000000..cfc48b17 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("open_manipulator_x", package_name="open_manipulator_x_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/open_manipulator_x_moveit_config/launch/move_group.launch.py b/open_manipulator_x_moveit_config/launch/move_group.launch.py new file mode 100644 index 00000000..6077a102 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/move_group.launch.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM, Sungho Woo + +import os +import yaml +import xacro + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + # Robot description + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("open_manipulator_x_description"), + "urdf", + "open_manipulator_x_robot.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + # Robot description Semantic config + robot_description_semantic_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "open_manipulator_x.srdf", + ) + with open(robot_description_semantic_path, "r") as file: + robot_description_semantic_config = file.read() + + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + # kinematics yaml + kinematics_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "kinematics.yaml", + ) + with open(kinematics_yaml_path, "r") as file: + kinematics_yaml = yaml.safe_load(file) + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization \ + default_planner_request_adapters/FixWorkspaceBounds \ + default_planner_request_adapters/FixStartStateBounds \ + default_planner_request_adapters/FixStartStateCollision \ + default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "ompl_planning.yaml", + ) + with open(ompl_planning_yaml_path, "r") as file: + ompl_planning_yaml = yaml.safe_load(file) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + # Moveit Controllers + moveit_simple_controllers_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "moveit_controllers.yaml", + ) + with open(moveit_simple_controllers_yaml_path, "r") as file: + moveit_simple_controllers_yaml = yaml.safe_load(file) + + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": + "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + # Planning Scene Monitor Parameters + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + "publish_robot_description": True, + "publish_robot_description_semantic": True + } + + ld = LaunchDescription() + use_sim = LaunchConfiguration('use_sim') + declare_use_sim = DeclareLaunchArgument( + 'use_sim', + default_value='true', + description='Start robot in Gazebo simulation.') + ld.add_action(declare_use_sim) + + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + {'use_sim_time': use_sim}, + ], + ) + + ld.add_action(move_group_node) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/moveit_core.launch.py b/open_manipulator_x_moveit_config/launch/moveit_core.launch.py new file mode 100644 index 00000000..be82b0c8 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/moveit_core.launch.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM, Sungho Woo + + +import os + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + ld = LaunchDescription() + launch_dir = os.path.join( + get_package_share_directory( + 'open_manipulator_x_moveit_config'), 'launch') + + # RViz + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([launch_dir, '/moveit_rviz.launch.py']) + ) + ld.add_action(rviz_launch) + + # move_group + move_group_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([launch_dir, '/move_group.launch.py']) + ) + ld.add_action(move_group_launch) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py b/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py new file mode 100644 index 00000000..e9de161a --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.substitutions import FindPackageShare +from ament_index_python.packages import get_package_share_directory + + +def generate_launch_description(): + + ld = LaunchDescription() + launch_dir = os.path.join( + get_package_share_directory( + 'open_manipulator_x_moveit_config'), 'launch') + bringup_launch_dir = os.path.join( + get_package_share_directory( + 'open_manipulator_x_bringup'), 'launch') + + # RViz + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([launch_dir, '/moveit_rviz.launch.py']) + ) + ld.add_action(rviz_launch) + + # move_group + move_group_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([launch_dir, '/move_group.launch.py']), + launch_arguments={ + 'use_sim': 'true', + }.items(), + ) + ld.add_action(move_group_launch) + + # gazebo_control with robot_state_publisher + rviz_arg = DeclareLaunchArgument( + 'start_rviz', + default_value='false', + description='Whether execute rviz2') + ld.add_action(rviz_arg) + + empty_world_path = PathJoinSubstitution( + [ + FindPackageShare('open_manipulator_x_bringup'), + 'worlds', + 'empty_world.model' + ] + ) + + gazebo_control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([bringup_launch_dir, '/gazebo.launch.py']), + launch_arguments={ + 'world': empty_world_path, + 'x_pose': '0.0', + 'y_pose': '0.0', + 'z_pose': '0.0', + 'roll': '0.0', + 'pitch': '0.0', + 'yaw': '0.0', + }.items(), + ) + ld.add_action(gazebo_control_launch) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py b/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 00000000..bc27b09a --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,118 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +import os +import xacro +import yaml + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + # Rviz config save file + rviz_config = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "moveit.rviz" + ) + + # Robot description + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("open_manipulator_x_description"), + "urdf", + "open_manipulator_x_robot.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + # Robot description Semantic config + robot_description_semantic_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "open_manipulator_x.srdf", + ) + with open(robot_description_semantic_path, "r") as file: + robot_description_semantic_config = file.read() + + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization \ + default_planner_request_adapters/FixWorkspaceBounds \ + default_planner_request_adapters/FixStartStateBounds \ + default_planner_request_adapters/FixStartStateCollision \ + default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "ompl_planning.yaml", + ) + with open(ompl_planning_yaml_path, "r") as file: + ompl_planning_yaml = yaml.safe_load(file) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # kinematics yaml + kinematics_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "kinematics.yaml", + ) + with open(kinematics_yaml_path, "r") as file: + kinematics_yaml = yaml.safe_load(file) + + ld = LaunchDescription() + + warehouse_ros_config = { + # For warehouse_ros_sqlite + "warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection", + "warehouse_host": "/home/robotis/warehouse_db.sqlite", # change to your path + "port": 33829, + "scene_name": "", # If scene name is empty, all scenes will be used + "queries_regex": ".*", + } + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config], + parameters=[ + robot_description, + robot_description_semantic, + ompl_planning_pipeline_config, + kinematics_yaml, + warehouse_ros_config, + ] + ) + + ld.add_action(rviz_node) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/rsp.launch.py b/open_manipulator_x_moveit_config/launch/rsp.launch.py new file mode 100644 index 00000000..12a00b87 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/rsp.launch.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +import os +import xacro + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + ld = LaunchDescription() + publish_frequency = LaunchConfiguration("publish_frequency") + ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0")) + + # Robot description + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("open_manipulator_x_description"), + "urdf", + "open_manipulator_x_robot.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + # Given the published joint states, publish tf for the robot links and the robot description + rsp_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + respawn=True, + output="screen", + parameters=[{"publish_frequency": publish_frequency}, robot_description] + ) + + ld.add_action(rsp_node) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/servo.launch.py b/open_manipulator_x_moveit_config/launch/servo.launch.py new file mode 100644 index 00000000..46d98929 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/servo.launch.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +import os +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import xacro + + +def generate_launch_description(): + + ld = LaunchDescription() + + use_sim = LaunchConfiguration('use_sim') + declare_use_sim = DeclareLaunchArgument( + 'use_sim', + default_value='true', + description='Start robot in Gazebo simulation.') + ld.add_action(declare_use_sim) + + # Robot description + robot_description_config = xacro.process_file( + os.path.join( + get_package_share_directory("open_manipulator_x_description"), + "urdf", + "open_manipulator_x_robot.urdf.xacro", + ) + ) + robot_description = {"robot_description": robot_description_config.toxml()} + + # Robot description Semantic config + robot_description_semantic_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "open_manipulator_x.srdf", + ) + try: + with open(robot_description_semantic_path, "r") as file: + robot_description_semantic_config = file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + robot_description_semantic = { + "robot_description_semantic": robot_description_semantic_config + } + + # kinematics yaml + kinematics_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "kinematics.yaml", + ) + with open(kinematics_yaml_path, "r") as file: + kinematics_yaml = yaml.safe_load(file) + + # Get parameters for the Servo node + servo_yaml_path = os.path.join( + get_package_share_directory("open_manipulator_x_moveit_config"), + "config", + "moveit_servo.yaml", + ) + try: + with open(servo_yaml_path, "r") as file: + servo_params = {"moveit_servo": yaml.safe_load(file)} + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + # Launch as much as possible in components + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + parameters=[ + {'use_gazebo':use_sim}, + servo_params, + robot_description, + robot_description_semantic, + kinematics_yaml, + ] + ) + ld.add_action(servo_node) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py b/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 00000000..f18f35c7 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("open_manipulator_x", package_name="open_manipulator_x_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py b/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 00000000..71e9d0e6 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +from launch import LaunchDescription +from launch.actions import ExecuteProcess + + +def generate_launch_description(): + ld = LaunchDescription() + + # Load controllers + load_controllers = [] + for controller in [ + "arm_controller", + "gripper_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner.py {}".format(controller)], + shell=True, + output="screen", + ) + ] + ld.add_action(load_controllers) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py b/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 00000000..0900f2db --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + + ld = LaunchDescription() + + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "odom", "base_footprint"], + ) + ld.add_action(static_tf) + + return ld diff --git a/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py b/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 00000000..7f225a16 --- /dev/null +++ b/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,56 @@ +#!/usr/bin/env python3 +# +# Copyright 2020 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Authors: Hye-jong KIM + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + ld = LaunchDescription() + + # The default DB port for moveit (not default MongoDB port to avoid potential conflicts) + ld.add_action(DeclareLaunchArgument("moveit_warehouse_port", default_value="33829")) + + # The default DB host for moveit + ld.add_action( + DeclareLaunchArgument("moveit_warehouse_host", default_value="localhost") + ) + + # Load warehouse parameters + db_parameters = [ + { + "overwrite": False, + "warehouse_port": LaunchConfiguration("moveit_warehouse_port"), + "warehouse_host": LaunchConfiguration("moveit_warehouse_host"), + "warehouse_exec": "mongod", + "warehouse_plugin": "warehouse_ros_mongo::MongoDatabaseConnection", + }, + ] + + # Run the DB server + db_node = Node( + package="warehouse_ros_mongo", + executable="mongo_wrapper_ros.py", + parameters=db_parameters, + ) + ld.add_action(db_node) + + return ld diff --git a/open_manipulator_x_moveit_config/package.xml b/open_manipulator_x_moveit_config/package.xml new file mode 100644 index 00000000..2a585303 --- /dev/null +++ b/open_manipulator_x_moveit_config/package.xml @@ -0,0 +1,52 @@ + + + + open_manipulator_x_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the open_manipulator_x with the MoveIt Motion Planning Framework + + yunwonho + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + yunwonho + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + open_manipulator_x_description + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + xacro + + + + ament_cmake + + diff --git a/open_manipulator_x_playground/CMakeLists.txt b/open_manipulator_x_playground/CMakeLists.txt new file mode 100644 index 00000000..bb881a9c --- /dev/null +++ b/open_manipulator_x_playground/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(open_manipulator_x_playground) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) + + +add_executable(hello_moveit src/hello_moveit.cpp) +target_include_directories(hello_moveit PUBLIC + $ + $) +target_compile_features(hello_moveit PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + hello_moveit + "moveit_ros_planning_interface" + "rclcpp" +) + +install(TARGETS hello_moveit + DESTINATION lib/${PROJECT_NAME}) + + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/open_manipulator_x_playground/package.xml b/open_manipulator_x_playground/package.xml new file mode 100644 index 00000000..2525dbee --- /dev/null +++ b/open_manipulator_x_playground/package.xml @@ -0,0 +1,20 @@ + + + + open_manipulator_x_playground + 0.0.0 + TODO: Package description + yunwonho + TODO: License declaration + + ament_cmake + moveit_ros_planning_interface + rclcpp + ament_lint_auto + ament_lint_common + + + + ament_cmake + + diff --git a/open_manipulator_x_playground/src/hello_moveit.cpp b/open_manipulator_x_playground/src/hello_moveit.cpp new file mode 100644 index 00000000..eed52e2d --- /dev/null +++ b/open_manipulator_x_playground/src/hello_moveit.cpp @@ -0,0 +1,57 @@ +// Modify the MoveIt tutorial as much as you like using the basic example. +// Author: Sungho Woo + +#include + +#include +#include + +int main(int argc, char * argv[]) +{ + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared( + "hello_moveit", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) + ); + + // Create a ROS logger + auto const logger = rclcpp::get_logger("hello_moveit"); + +// Create the MoveIt MoveGroup Interface +using moveit::planning_interface::MoveGroupInterface; +auto move_group_interface = MoveGroupInterface(node, "arm"); + +// Set a target Pose + auto const target_pose = []{ + geometry_msgs::msg::Pose msg; + msg.orientation.x = 0.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = 1.0; + msg.position.x = 0.163; + msg.position.y = 0.0; + msg.position.z = 0.200; + return msg; + }(); + move_group_interface.setPoseTarget(target_pose); + move_group_interface.setGoalPositionTolerance(0.01); + move_group_interface.setGoalOrientationTolerance(0.01); + + // Create a plan to that target pose + auto const [success, plan] = [&move_group_interface]{ + moveit::planning_interface::MoveGroupInterface::Plan msg; + auto const ok = static_cast(move_group_interface.plan(msg)); + return std::make_pair(ok, msg); + }(); + + // Execute the plan + if(success) { + move_group_interface.execute(plan); + } else { + RCLCPP_ERROR(logger, "Planing failed!"); + } + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/open_manipulator_x_teleop/CMakeLists.txt b/open_manipulator_x_teleop/CMakeLists.txt new file mode 100644 index 00000000..624b97a0 --- /dev/null +++ b/open_manipulator_x_teleop/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(open_manipulator_x_teleop) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(open_manipulator_x_bringup REQUIRED) +find_package(open_manipulator_x_moveit_config REQUIRED) +find_package(open_manipulator_x_description REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(rclcpp_action REQUIRED) + + +add_executable(open_manipulator_x_teleop src/open_manipulator_x_teleop.cpp) +target_include_directories(open_manipulator_x_teleop PUBLIC + $ + $) +ament_target_dependencies( + open_manipulator_x_teleop + "rclcpp" + "open_manipulator_x_bringup" + "open_manipulator_x_moveit_config" + "open_manipulator_x_description" + "geometry_msgs" + "nav_msgs" + "control_msgs" + "sensor_msgs" + "std_srvs" + "rclcpp_action" +) + +install(TARGETS open_manipulator_x_teleop + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/open_manipulator_x_teleop/include/open_manipulator_x_teleop/open_manipulator_x_teleop.hpp b/open_manipulator_x_teleop/include/open_manipulator_x_teleop/open_manipulator_x_teleop.hpp new file mode 100644 index 00000000..93c3e6e9 --- /dev/null +++ b/open_manipulator_x_teleop/include/open_manipulator_x_teleop/open_manipulator_x_teleop.hpp @@ -0,0 +1,129 @@ +#ifndef OPEN_MANIPULATOR_Y_TELEOP__OPEN_MANIPULATOR_Y_TELEOP_HPP_ +#define OPEN_MANIPULATOR_Y_TELEOP__OPEN_MANIPULATOR_Y_TELEOP_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +// Define used keys +#define KEYCODE_1 0x31 +#define KEYCODE_2 0x32 +#define KEYCODE_3 0x33 +#define KEYCODE_4 0x34 +#define KEYCODE_Q 0x71 +#define KEYCODE_W 0x77 +#define KEYCODE_E 0x65 +#define KEYCODE_R 0x72 + +#define KEYCODE_O 0x6F +#define KEYCODE_P 0x70 + +#define KEYCODE_ESC 0x1B + +// Some constants used in the Servo Teleop demo +const char ARM_JOINT_TOPIC[] = "/servo_node/delta_joint_cmds"; +const char GRIPPER_TOPIC[] = "/gripper_controller/gripper_cmd"; + +const size_t ROS_QUEUE_SIZE = 10; + +const char BASE_FRAME_ID[] = "link1"; + +const double ARM_JOINT_VEL = 3.0; // rad/s + +// A class for reading the key inputs from the terminal +class KeyboardReader +{ +public: + KeyboardReader(); + void readOne(char * c); + void shutdown(); + +private: + int kfd; + struct termios cooked; +}; + +// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller +class KeyboardServo +{ +public: + KeyboardServo(); + ~KeyboardServo(); + int keyLoop(); + + void connect_moveit_servo(); + void start_moveit_servo(); + void stop_moveit_servo(); + void send_goal(float position); +private: + rclcpp_action::Client::SharedPtr client_; + + void spin(); + void pub(); + + rclcpp::Node::SharedPtr nh_; + + rclcpp::Client::SharedPtr servo_start_client_; + rclcpp::Client::SharedPtr servo_stop_client_; + + rclcpp::Publisher::SharedPtr joint_pub_; + + control_msgs::msg::JointJog joint_msg_; + control_msgs::msg::GripperCommand gripper_cmd_; + + bool publish_joint_; + + bool publish_gripper_; + void goal_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& result) + { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + break; + case rclcpp_action::ResultCode::ABORTED: + break; + case rclcpp_action::ResultCode::CANCELED: + break; + default: + break; + } + } +}; + +KeyboardReader input; + +void quit(int sig) +{ + (void)sig; + input.shutdown(); + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + KeyboardServo keyboard_servo; + + signal(SIGINT, quit); + + int rc = keyboard_servo.keyLoop(); + + input.shutdown(); + rclcpp::shutdown(); + + return rc; +} + +#endif // OPEN_MANIPULATOR_Y_TELEOP__OPEN_MANIPULATOR_Y_TELEOP_HPP_ diff --git a/open_manipulator_x_teleop/package.xml b/open_manipulator_x_teleop/package.xml new file mode 100644 index 00000000..812ccca0 --- /dev/null +++ b/open_manipulator_x_teleop/package.xml @@ -0,0 +1,27 @@ + + + + open_manipulator_x_teleop + 0.0.0 + TODO: Package description + yunwonho + TODO: License declaration + + ament_cmake + + rclcpp + open_manipulator_x_bringup + open_manipulator_x_moveit_config + open_manipulator_x_description + geometry_msgs + nav_msgs + control_msgs + sensor_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/open_manipulator_x_teleop/src/open_manipulator_x_teleop.cpp b/open_manipulator_x_teleop/src/open_manipulator_x_teleop.cpp new file mode 100644 index 00000000..6245ce2d --- /dev/null +++ b/open_manipulator_x_teleop/src/open_manipulator_x_teleop.cpp @@ -0,0 +1,272 @@ +// Copyright 2024 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hye-jong KIM, Sungho Woo + +#include +#include + +#include "open_manipulator_x_teleop/open_manipulator_x_teleop.hpp" + +// KeyboardReader +KeyboardReader::KeyboardReader() +: kfd(0) +{ + // get the console in raw mode + tcgetattr(kfd, &cooked); + struct termios raw; + memcpy(&raw, &cooked, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(kfd, TCSANOW, &raw); +} + +void KeyboardReader::readOne(char * c) +{ + int rc = read(kfd, c, 1); + if (rc < 0) { + throw std::runtime_error("read failed"); + } +} + +void KeyboardReader::shutdown() +{ + tcsetattr(kfd, TCSANOW, &cooked); +} + +// KeyboardServo + +KeyboardServo::KeyboardServo() +: publish_joint_(false), publish_gripper_(false) +{ + nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); + + servo_start_client_ = + nh_->create_client("/servo_node/start_servo"); + servo_stop_client_ = + nh_->create_client("/servo_node/stop_servo"); + + joint_pub_ = nh_->create_publisher(ARM_JOINT_TOPIC, ROS_QUEUE_SIZE); + client_ = rclcpp_action::create_client(nh_, "gripper_controller/gripper_cmd"); +} + +KeyboardServo::~KeyboardServo() +{ + stop_moveit_servo(); +} + +int KeyboardServo::keyLoop() +{ + char c; + + // Ros Spin + std::thread{std::bind(&KeyboardServo::spin, this)}.detach(); + connect_moveit_servo(); + start_moveit_servo(); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("Joint Control Keys:"); + puts(" 1/q: Joint1 +/-"); + puts(" 2/w: Joint2 +/-"); + puts(" 3/e: Joint3 +/-"); + puts(" 4/r: Joint4 +/-"); + puts("Use o|p to open/close the gripper."); + puts("'ESC' to quit."); + + std::thread{std::bind(&KeyboardServo::pub, this)}.detach(); + + bool servoing = true; + while (servoing) { + // get the next event from the keyboard + try { + input.readOne(&c); + } catch (const std::runtime_error &) { + perror("read():"); + return -1; + } + + RCLCPP_INFO(nh_->get_logger(), "value: 0x%02X", c); + + // Use read key-press + joint_msg_.joint_names.clear(); + joint_msg_.velocities.clear(); + + switch (c) { + case KEYCODE_1: + joint_msg_.joint_names.push_back("joint1"); + joint_msg_.velocities.push_back(ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint1 +"); + break; + case KEYCODE_2: + joint_msg_.joint_names.push_back("joint2"); + joint_msg_.velocities.push_back(ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint2 +"); + break; + case KEYCODE_3: + joint_msg_.joint_names.push_back("joint3"); + joint_msg_.velocities.push_back(ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint3 +"); + break; + case KEYCODE_4: + joint_msg_.joint_names.push_back("joint4"); + joint_msg_.velocities.push_back(ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint4 +"); + break; + case KEYCODE_Q: + joint_msg_.joint_names.push_back("joint1"); + joint_msg_.velocities.push_back(-ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint1 -"); + break; + case KEYCODE_W: + joint_msg_.joint_names.push_back("joint2"); + joint_msg_.velocities.push_back(-ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint2 -"); + break; + case KEYCODE_E: + joint_msg_.joint_names.push_back("joint3"); + joint_msg_.velocities.push_back(-ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint3 -"); + break; + case KEYCODE_R: + joint_msg_.joint_names.push_back("joint4"); + joint_msg_.velocities.push_back(-ARM_JOINT_VEL); + publish_joint_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint4 -"); + break; + case KEYCODE_O: + send_goal(0.019); + publish_gripper_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Open"); + break; + case KEYCODE_P: + send_goal(-0.01); + publish_gripper_ = true; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Gripper Close"); + break; + case KEYCODE_ESC: + RCLCPP_INFO_STREAM(nh_->get_logger(), "quit"); + servoing = false; + break; + default: + RCLCPP_WARN_STREAM(nh_->get_logger(), "Unassigned input : " << c); + break; + } + } + + return 0; +} + +void KeyboardServo::send_goal(float position) +{ + auto goal_msg = control_msgs::action::GripperCommand::Goal(); + goal_msg.command.position = position; + goal_msg.command.max_effort = 100.0; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.result_callback = std::bind(&KeyboardServo::goal_result_callback, this, std::placeholders::_1); + + RCLCPP_INFO(nh_->get_logger(), "Sending goal"); + client_->async_send_goal(goal_msg, send_goal_options); +} + +void KeyboardServo::connect_moveit_servo() +{ + for (int i = 0; i < 10; i++) { + if (servo_start_client_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNECT SERVO START SERVER"); + break; + } + RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNECT SERVO START SERVER"); + if (i == 9) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), + "fail to connect moveit_servo." << + "please launch 'servo.launch' at 'open_manipulator_x_moveit_configs' pkg."); + } + } + for (int i = 0; i < 10; i++) { + if (servo_stop_client_->wait_for_service(std::chrono::seconds(1))) { + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS TO CONNECT SERVO STOP SERVER"); + break; + } + RCLCPP_WARN_STREAM(nh_->get_logger(), "WAIT TO CONNECT SERVO STOP SERVER"); + if (i == 9) { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), + "fail to connect moveit_servo." << + "please launch 'servo.launch' at 'open_manipulator_x_moveit_configs' pkg."); + } + } +} + +void KeyboardServo::start_moveit_servo() +{ + RCLCPP_INFO_STREAM(nh_->get_logger(), "call 'moveit_servo' start srv."); + auto future = servo_start_client_->async_send_request( + std::make_shared()); + auto result = future.wait_for(std::chrono::seconds(1)); + if (result == std::future_status::ready) { + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS to start 'moveit_servo'"); + future.get(); + } else { + RCLCPP_ERROR_STREAM( + nh_->get_logger(), "FAIL to start 'moveit_servo', execute without 'moveit_servo'"); + } +} + +void KeyboardServo::stop_moveit_servo() +{ + RCLCPP_INFO_STREAM(nh_->get_logger(), "call 'moveit_servo' END srv."); + auto future = servo_stop_client_->async_send_request( + std::make_shared()); + auto result = future.wait_for(std::chrono::seconds(1)); + if (result == std::future_status::ready) { + RCLCPP_INFO_STREAM(nh_->get_logger(), "SUCCESS to stop 'moveit_servo'"); + future.get(); + } +} + +void KeyboardServo::pub() +{ + while (rclcpp::ok()) { + if (publish_joint_) { + joint_msg_.header.stamp = nh_->now(); + joint_msg_.header.frame_id = BASE_FRAME_ID; + joint_pub_->publish(joint_msg_); + publish_joint_ = false; + RCLCPP_INFO_STREAM(nh_->get_logger(), "Joint PUB"); + } + if (publish_gripper_) { + publish_gripper_ = false; + } + rclcpp::sleep_for(std::chrono::milliseconds(10)); + } +} + +void KeyboardServo::spin() +{ + while (rclcpp::ok()) { + rclcpp::spin_some(nh_); + } +} From 447e0b7d631901b33d9b2af42c38ccf4c5433371 Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Fri, 6 Dec 2024 18:21:34 +0900 Subject: [PATCH 02/20] Refactored code style Signed-off-by: Sungho Woo --- .github/workflows/ros-ci.yml | 21 +--- .vscode/settings.json | 7 ++ README.md | 8 +- open_manipulator/CHANGELOG.rst | 8 +- open_manipulator/package.xml | 6 +- open_manipulator_x_bringup/CHANGELOG.rst | 110 ++++++++++++++++++ open_manipulator_x_bringup/CMakeLists.txt | 2 +- .../config/gazebo_controller_manager.yaml | 1 - .../launch/base.launch.py | 2 +- .../launch/fake.launch.py | 3 +- .../launch/gazebo.launch.py | 3 +- .../launch/hardware.launch.py | 2 +- open_manipulator_x_bringup/package.xml | 19 ++- .../worlds/turtlebot3_world/model.config | 2 +- open_manipulator_x_description/CHANGELOG.rst | 8 +- .../open_manipulator_x_description.dsv.in | 2 +- .../launch/model.launch.py | 2 +- open_manipulator_x_description/package.xml | 15 +-- .../urdf/open_manipulator_x.urdf.xacro | 3 +- .../urdf/open_manipulator_x_robot.urdf.xacro | 4 +- open_manipulator_x_gui/CHANGELOG.rst | 110 ++++++++++++++++++ .../open_manipulator_x_gui/main_window.hpp | 2 +- .../launch/open_manipulator_x_gui.launch.py | 18 +++ open_manipulator_x_gui/package.xml | 17 ++- open_manipulator_x_gui/src/main_window.cpp | 16 ++- open_manipulator_x_gui/ui/main_window.ui | 53 +-------- .../config/kinematics.yaml | 4 +- .../launch/demo.launch.py | 18 +++ .../launch/move_group.launch.py | 2 +- .../launch/moveit_core.launch.py | 3 +- .../launch/moveit_gazebo.launch.py | 2 +- .../launch/moveit_rviz.launch.py | 4 +- .../launch/rsp.launch.py | 4 +- .../launch/servo.launch.py | 4 +- .../launch/setup_assistant.launch.py | 18 +++ .../launch/spawn_controllers.launch.py | 4 +- .../launch/static_virtual_joint_tfs.launch.py | 4 +- .../launch/warehouse_db.launch.py | 4 +- open_manipulator_x_moveit_config/package.xml | 26 ++--- open_manipulator_x_playground/CHANGELOG.rst | 110 ++++++++++++++++++ open_manipulator_x_playground/CMakeLists.txt | 2 - open_manipulator_x_playground/package.xml | 16 ++- .../src/hello_moveit.cpp | 24 +++- open_manipulator_x_teleop/CHANGELOG.rst | 110 ++++++++++++++++++ open_manipulator_x_teleop/package.xml | 16 ++- 45 files changed, 649 insertions(+), 170 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 open_manipulator_x_bringup/CHANGELOG.rst create mode 100644 open_manipulator_x_gui/CHANGELOG.rst create mode 100644 open_manipulator_x_playground/CHANGELOG.rst create mode 100644 open_manipulator_x_teleop/CHANGELOG.rst diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index b224b490..70b20be0 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -3,9 +3,9 @@ name: ros-ci # Controls when the action will run. Triggers the workflow on push or pull request on: push: - branches: [ master, develop, melodic-devel, noetic-devel ] + branches: [ master, develop, noetic-devel ] pull_request: - branches: [ master, develop, melodic-devel, noetic-devel ] + branches: [ master, develop, noetic-devel ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: @@ -15,22 +15,11 @@ jobs: fail-fast: false matrix: ros_distribution: - # - kinetic - - melodic - noetic include: - # Kinetic Kame (May 2016 - May 2021) - # - docker_image: ubuntu:xenial - # ros_distribution: kinetic - # ros_version: 1 - # Melodic Morenia (May 2018 - May 2023) - - docker_image: ubuntu:bionic - ros_distribution: melodic - ros_version: 1 - # Noetic Ninjemys (May 2020 - May 2025) - docker_image: ubuntu:focal - ros_distribution: noetic - ros_version: 1 + ros_distribution: noetic + ros_version: 1 container: image: ${{ matrix.docker_image }} steps: @@ -49,4 +38,4 @@ jobs: with: package-name: open_manipulator target-ros1-distro: ${{ matrix.ros_distribution }} - vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/open_manipulator/develop/openmanipulator.repos" \ No newline at end of file + vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/open_manipulator/develop/openmanipulator.repos" diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..ec6235a4 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,7 @@ +{ + "files.associations": { + "*.tcc": "cpp", + "string": "cpp", + "string_view": "cpp" + } +} \ No newline at end of file diff --git a/README.md b/README.md index d4cea515..c577c3f6 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,9 @@ -[![kinetic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/kinetic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/kinetic-devel) -[![melodic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/melodic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/melodic-devel) -[![noetic-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/noetic-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/noetic-devel) -[![dashing-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/dashing-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/dashing-devel) -[![foxy-devel Status](https://github.com/ROBOTIS-GIT/open_manipulator/workflows/foxy-devel/badge.svg)](https://github.com/ROBOTIS-GIT/open_manipulator/tree/foxy-devel) - # OpenMANIPULATOR-X +- Active Branches: noetic, humble, main +- Legacy Branches: *-devel # ROBOTIS e-Manual for OpenMANIPULATOR-X - [http://emanual.robotis.com/docs/en/platform/openmanipulator/](http://emanual.robotis.com/docs/en/platform/openmanipulator/) diff --git a/open_manipulator/CHANGELOG.rst b/open_manipulator/CHANGELOG.rst index 1ea5bcd1..7bff3569 100644 --- a/open_manipulator/CHANGELOG.rst +++ b/open_manipulator/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package open_manipulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + 2.3.0 (2021-10-06) ------------------ * ROS2 Foxy Fitzroy supported @@ -10,7 +14,7 @@ Changelog for package open_manipulator 2.2.0 (2019-11-13) ------------------ -* Applied robotis coding style guide +* Applied robotis coding style guide * Contributors: Ryan Shim 2.1.0 (2019-08-31) @@ -77,7 +81,7 @@ Changelog for package open_manipulator * added function to support protocol 1.0 * modified color, xacro server, mu1, mu2, collision range, joint limit * modified joint_state_publisher, joint_states_publisher -* modified params of inertial, xacro, gazebo, collision, friction +* modified params of inertial, xacro, gazebo, collision, friction * modified urdf file names and collision geometry * modified motor id, msg names * modified description and package tree diff --git a/open_manipulator/package.xml b/open_manipulator/package.xml index 8010d294..ea1f6b27 100644 --- a/open_manipulator/package.xml +++ b/open_manipulator/package.xml @@ -2,11 +2,11 @@ open_manipulator - 2.3.0 + 3.0.0 OpenMANIPULATOR-X meta ROS 2 package. - Will Son + pyo Apache 2.0 http://wiki.ros.org/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator @@ -16,6 +16,8 @@ Ryan Shim Yong-Ho Na Will Son + Sungho Woo + ament_cmake open_manipulator_x_controller open_manipulator_x_description diff --git a/open_manipulator_x_bringup/CHANGELOG.rst b/open_manipulator_x_bringup/CHANGELOG.rst new file mode 100644 index 00000000..7902186b --- /dev/null +++ b/open_manipulator_x_bringup/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_bringup/CMakeLists.txt b/open_manipulator_x_bringup/CMakeLists.txt index c1d93e12..ac311386 100644 --- a/open_manipulator_x_bringup/CMakeLists.txt +++ b/open_manipulator_x_bringup/CMakeLists.txt @@ -29,4 +29,4 @@ ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.d ################################################################################ # Macro for ament package ################################################################################ -ament_package() \ No newline at end of file +ament_package() diff --git a/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml b/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml index 0d725bbc..f7cd2f37 100644 --- a/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml +++ b/open_manipulator_x_bringup/config/gazebo_controller_manager.yaml @@ -21,7 +21,6 @@ arm_controller: - joint3 - joint4 - interface_name: position command_interfaces: diff --git a/open_manipulator_x_bringup/launch/base.launch.py b/open_manipulator_x_bringup/launch/base.launch.py index 8b2d81dd..f0b604d3 100644 --- a/open_manipulator_x_bringup/launch/base.launch.py +++ b/open_manipulator_x_bringup/launch/base.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Darby Lim, Sungho Woo +# Author: Wonho Yoon, Sungho Woo from launch import LaunchDescription from launch.actions import DeclareLaunchArgument diff --git a/open_manipulator_x_bringup/launch/fake.launch.py b/open_manipulator_x_bringup/launch/fake.launch.py index 45ecd956..bea7f1ce 100644 --- a/open_manipulator_x_bringup/launch/fake.launch.py +++ b/open_manipulator_x_bringup/launch/fake.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Darby Lim, Sungho Woo +# Author: Wonho Yoon, Sungho Woo import os @@ -34,7 +34,6 @@ def is_valid_to_launch(): else: return True - def generate_launch_description(): if not is_valid_to_launch(): print('Can not launch fake robot in Raspberry Pi') diff --git a/open_manipulator_x_bringup/launch/gazebo.launch.py b/open_manipulator_x_bringup/launch/gazebo.launch.py index 1887304c..1b46c9b0 100644 --- a/open_manipulator_x_bringup/launch/gazebo.launch.py +++ b/open_manipulator_x_bringup/launch/gazebo.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Darby Lim, Sungho Woo +# Author: Wonho Yoon, Sungho Woo import os @@ -38,7 +38,6 @@ def is_valid_to_launch(): else: return True - def generate_launch_description(): if not is_valid_to_launch(): print('Can not launch fake robot in Raspberry Pi') diff --git a/open_manipulator_x_bringup/launch/hardware.launch.py b/open_manipulator_x_bringup/launch/hardware.launch.py index 5d30f2a3..e9dc717b 100644 --- a/open_manipulator_x_bringup/launch/hardware.launch.py +++ b/open_manipulator_x_bringup/launch/hardware.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Darby Lim, Sungho Woo +# Author: Wonho Yoon, Sungho Woo import os diff --git a/open_manipulator_x_bringup/package.xml b/open_manipulator_x_bringup/package.xml index 3b57bda7..d0209cac 100644 --- a/open_manipulator_x_bringup/package.xml +++ b/open_manipulator_x_bringup/package.xml @@ -2,10 +2,21 @@ open_manipulator_x_bringup - 0.0.0 - TODO: Package description - yunwonho - TODO: License declaration + 3.0.0 + + OpenMANIPULATOR-X bringup ROS 2 package. + + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues + Darby Lim + Hye-Jong KIM + Ryan Shim + Yong-Ho Na + Will Son + Sungho Woo ament_cmake gazebo_ros diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config index bc6a666f..387bb6e6 100644 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config +++ b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config @@ -4,7 +4,7 @@ TurtleBot3 World 1.0 model-1_4.sdf - model.sdf + model.sdf Taehun Lim(Darby) diff --git a/open_manipulator_x_description/CHANGELOG.rst b/open_manipulator_x_description/CHANGELOG.rst index d8aa8eb5..960cad37 100644 --- a/open_manipulator_x_description/CHANGELOG.rst +++ b/open_manipulator_x_description/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package open_manipulator_x_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + 2.3.0 (2021-10-06) ------------------ * ROS2 Foxy Fitzroy supported @@ -9,7 +13,7 @@ Changelog for package open_manipulator_x_description 2.2.0 (2019-11-13) ------------------ -* Applied robotis coding style guide +* Applied robotis coding style guide * Contributors: Ryan Shim 2.1.0 (2019-08-31) @@ -76,7 +80,7 @@ Changelog for package open_manipulator_x_description * added function to support protocol 1.0 * modified color, xacro server, mu1, mu2, collision range, joint limit * modified joint_state_publisher, joint_states_publisher -* modified params of inertial, xacro, gazebo, collision, friction +* modified params of inertial, xacro, gazebo, collision, friction * modified urdf file names and collision geometry * modified motor id, msg names * modified description and package tree diff --git a/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in b/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in index cd4ca905..c9db3254 100644 --- a/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in +++ b/open_manipulator_x_description/env-hooks/open_manipulator_x_description.dsv.in @@ -1 +1 @@ -prepend-non-duplicate;GAZEBO_MODEL_PATH;share \ No newline at end of file +prepend-non-duplicate;GAZEBO_MODEL_PATH;share diff --git a/open_manipulator_x_description/launch/model.launch.py b/open_manipulator_x_description/launch/model.launch.py index d484f965..6954183c 100644 --- a/open_manipulator_x_description/launch/model.launch.py +++ b/open_manipulator_x_description/launch/model.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Author: Darby Lim, Sungho Woo +# Author: Wonho Yoon, Sungho Woo import os diff --git a/open_manipulator_x_description/package.xml b/open_manipulator_x_description/package.xml index dc0a7d9e..3bd93001 100644 --- a/open_manipulator_x_description/package.xml +++ b/open_manipulator_x_description/package.xml @@ -2,21 +2,22 @@ open_manipulator_x_description - 2.3.0 + 3.0.0 open_manipulator_x_description ROS 2 package. + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues Darby Lim Hye-Jong KIM Ryan Shim Yong-Ho Na Will Son - Will Son - Apache 2.0 - http://wiki.ros.org/open_manipulator_description - http://emanual.robotis.com/docs/en/platform/openmanipulator - https://github.com/ROBOTIS-GIT/open_manipulator - https://github.com/ROBOTIS-GIT/open_manipulator/issues + Sungho Woo + ament_cmake joint_state_publisher joint_state_publisher_gui diff --git a/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro b/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro index 2735f7db..c5b05b8a 100644 --- a/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro +++ b/open_manipulator_x_description/urdf/open_manipulator_x.urdf.xacro @@ -8,7 +8,6 @@ name="meshes_file_direction" value="package://open_manipulator_x_description/meshes"/> - @@ -263,4 +262,4 @@ - \ No newline at end of file + diff --git a/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro b/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro index 0ad9f979..680d6f23 100644 --- a/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro +++ b/open_manipulator_x_description/urdf/open_manipulator_x_robot.urdf.xacro @@ -42,6 +42,4 @@ - - - \ No newline at end of file + diff --git a/open_manipulator_x_gui/CHANGELOG.rst b/open_manipulator_x_gui/CHANGELOG.rst new file mode 100644 index 00000000..5d064ae1 --- /dev/null +++ b/open_manipulator_x_gui/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_gui +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp index e4afa2ab..07a368a0 100644 --- a/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp +++ b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp @@ -59,7 +59,7 @@ public Q_SLOTS: void on_btn_stop_clicked(void); void on_btn_read_task_clicked(void); void tabSelected(); - void on_check_CheckBox_Toggled(bool checked); + // void on_check_CheckBox_Toggled(bool checked); private: Ui::MainWindowDesign ui; diff --git a/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py b/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py index df81a8d9..62f36049 100644 --- a/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py +++ b/open_manipulator_x_gui/launch/open_manipulator_x_gui.launch.py @@ -1,3 +1,21 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Wonho Yoon, Sungho Woo + import os from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription diff --git a/open_manipulator_x_gui/package.xml b/open_manipulator_x_gui/package.xml index 7c2419ea..45de0e66 100644 --- a/open_manipulator_x_gui/package.xml +++ b/open_manipulator_x_gui/package.xml @@ -2,10 +2,19 @@ open_manipulator_x_gui - 0.0.0 - TODO: Package description - yunwonho - TODO: License declaration + 3.0.0 + OpenMANIPULATOR-X gui ROS 2 package. + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues + Darby Lim + Hye-Jong KIM + Ryan Shim + Yong-Ho Na + Will Son + Sungho Woo ament_cmake diff --git a/open_manipulator_x_gui/src/main_window.cpp b/open_manipulator_x_gui/src/main_window.cpp index b52ed211..7b52605a 100644 --- a/open_manipulator_x_gui/src/main_window.cpp +++ b/open_manipulator_x_gui/src/main_window.cpp @@ -42,7 +42,7 @@ namespace open_manipulator_x_gui QObject::connect(ui.actionAbout_Qt, SIGNAL(triggered(bool)), qApp, SLOT(aboutQt())); connect(ui.tabWidget, SIGNAL(currentChanged(int)), this, SLOT(tabSelected())); QObject::connect(&qnode, SIGNAL(rosShutdown()), this, SLOT(close())); - connect(ui.checkBox_2, &QCheckBox::toggled, this, &MainWindow::on_check_CheckBox_Toggled); + // connect(ui.checkBox_2, &QCheckBox::toggled, this, &MainWindow::on_check_CheckBox_Toggled); ui.btn_init_pose->setEnabled(false); ui.btn_home_pose->setEnabled(false); @@ -74,8 +74,6 @@ namespace open_manipulator_x_gui ui.txt_j2->setText(QString::number(joint_angle.at(1), 'f', 3)); ui.txt_j3->setText(QString::number(joint_angle.at(2), 'f', 3)); ui.txt_j4->setText(QString::number(joint_angle.at(3), 'f', 3)); - // ui.txt_j5->setText(QString::number(joint_angle.at(4), 'f', 3)); - // ui.txt_j6->setText(QString::number(joint_angle.at(5), 'f', 3)); ui.txt_grip->setText(QString::number(joint_angle.at(4), 'f', 3)); std::vector position = qnode.getPresentKinematicsPosition(); @@ -561,11 +559,11 @@ namespace open_manipulator_x_gui writeLog("Reset completed."); } - void MainWindow::on_check_CheckBox_Toggled(bool checked) - { - writeLog(checked ? "Torque ON" : "Torque OFF"); - if (!qnode.sendTorqueSrv(checked)) - writeLog("[ERR!!] Failed to send torque service."); - } + // void MainWindow::on_check_CheckBox_Toggled(bool checked) + // { + // writeLog(checked ? "Torque ON" : "Torque OFF"); + // if (!qnode.sendTorqueSrv(checked)) + // writeLog("[ERR!!] Failed to send torque service."); + // } } // namespace open_manipulator_x_gui diff --git a/open_manipulator_x_gui/ui/main_window.ui b/open_manipulator_x_gui/ui/main_window.ui index c009d948..0b9b4014 100644 --- a/open_manipulator_x_gui/ui/main_window.ui +++ b/open_manipulator_x_gui/ui/main_window.ui @@ -533,7 +533,7 @@ - 0 + 2 @@ -1118,57 +1118,6 @@ - - - - 10 - 250 - 191 - 31 - - - - - 75 - true - - - - Torque on/off - - - true - - - - - - 140 - 250 - 191 - 73 - - - - Qt::LeftToRight - - - Caution: Torque off may cause collision; torque on may cause sudden pose return. - - - - - - - - - - false - - - true - - diff --git a/open_manipulator_x_moveit_config/config/kinematics.yaml b/open_manipulator_x_moveit_config/config/kinematics.yaml index 62de4e92..754e4def 100644 --- a/open_manipulator_x_moveit_config/config/kinematics.yaml +++ b/open_manipulator_x_moveit_config/config/kinematics.yaml @@ -1,6 +1,6 @@ arm: - kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 kinematics_solver_attempts: 3 - position_only_ik: False + position_only_ik: True diff --git a/open_manipulator_x_moveit_config/launch/demo.launch.py b/open_manipulator_x_moveit_config/launch/demo.launch.py index cfc48b17..2afa53cc 100644 --- a/open_manipulator_x_moveit_config/launch/demo.launch.py +++ b/open_manipulator_x_moveit_config/launch/demo.launch.py @@ -1,3 +1,21 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Wonho Yoon, Sungho Woo + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_demo_launch diff --git a/open_manipulator_x_moveit_config/launch/move_group.launch.py b/open_manipulator_x_moveit_config/launch/move_group.launch.py index 6077a102..06aa54c9 100644 --- a/open_manipulator_x_moveit_config/launch/move_group.launch.py +++ b/open_manipulator_x_moveit_config/launch/move_group.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM, Sungho Woo +# Author: Wonho Yoon, Sungho Woo import os import yaml diff --git a/open_manipulator_x_moveit_config/launch/moveit_core.launch.py b/open_manipulator_x_moveit_config/launch/moveit_core.launch.py index be82b0c8..22e53782 100644 --- a/open_manipulator_x_moveit_config/launch/moveit_core.launch.py +++ b/open_manipulator_x_moveit_config/launch/moveit_core.launch.py @@ -14,8 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM, Sungho Woo - +# Author: Wonho Yoon, Sungho Woo import os diff --git a/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py b/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py index e9de161a..5a114470 100644 --- a/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py +++ b/open_manipulator_x_moveit_config/launch/moveit_gazebo.launch.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo import os diff --git a/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py b/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py index bc27b09a..9e0da0b4 100644 --- a/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py +++ b/open_manipulator_x_moveit_config/launch/moveit_rviz.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo import os import xacro diff --git a/open_manipulator_x_moveit_config/launch/rsp.launch.py b/open_manipulator_x_moveit_config/launch/rsp.launch.py index 12a00b87..bc139126 100644 --- a/open_manipulator_x_moveit_config/launch/rsp.launch.py +++ b/open_manipulator_x_moveit_config/launch/rsp.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo import os import xacro diff --git a/open_manipulator_x_moveit_config/launch/servo.launch.py b/open_manipulator_x_moveit_config/launch/servo.launch.py index 46d98929..2475fc20 100644 --- a/open_manipulator_x_moveit_config/launch/servo.launch.py +++ b/open_manipulator_x_moveit_config/launch/servo.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo import os import yaml diff --git a/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py b/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py index f18f35c7..cddcd615 100644 --- a/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py +++ b/open_manipulator_x_moveit_config/launch/setup_assistant.launch.py @@ -1,3 +1,21 @@ +#!/usr/bin/env python3 +# +# Copyright 2024 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Wonho Yoon, Sungho Woo + from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_setup_assistant_launch diff --git a/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py b/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py index 71e9d0e6..523daf18 100644 --- a/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py +++ b/open_manipulator_x_moveit_config/launch/spawn_controllers.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo from launch import LaunchDescription from launch.actions import ExecuteProcess diff --git a/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py b/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py index 0900f2db..d9309575 100644 --- a/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py +++ b/open_manipulator_x_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo from launch import LaunchDescription from launch_ros.actions import Node diff --git a/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py b/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py index 7f225a16..320334c0 100644 --- a/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py +++ b/open_manipulator_x_moveit_config/launch/warehouse_db.launch.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 # -# Copyright 2020 ROBOTIS CO., LTD. +# Copyright 2024 ROBOTIS CO., LTD. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. # -# Authors: Hye-jong KIM +# Author: Wonho Yoon, Sungho Woo from launch import LaunchDescription from launch.actions import DeclareLaunchArgument diff --git a/open_manipulator_x_moveit_config/package.xml b/open_manipulator_x_moveit_config/package.xml index 2a585303..5d37424a 100644 --- a/open_manipulator_x_moveit_config/package.xml +++ b/open_manipulator_x_moveit_config/package.xml @@ -2,19 +2,21 @@ open_manipulator_x_moveit_config - 0.3.0 + 3.0.0 An automatically generated package with all the configuration and launch files for using the open_manipulator_x with the MoveIt Motion Planning Framework - yunwonho - - BSD - - http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - yunwonho + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues + Darby Lim + Hye-Jong KIM + Ryan Shim + Yong-Ho Na + Will Son + Sungho Woo ament_cmake @@ -26,10 +28,6 @@ joint_state_publisher_gui tf2_ros xacro - - - controller_manager moveit_configs_utils moveit_ros_move_group diff --git a/open_manipulator_x_playground/CHANGELOG.rst b/open_manipulator_x_playground/CHANGELOG.rst new file mode 100644 index 00000000..bbd84fcc --- /dev/null +++ b/open_manipulator_x_playground/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_playground +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_playground/CMakeLists.txt b/open_manipulator_x_playground/CMakeLists.txt index bb881a9c..3921fb48 100644 --- a/open_manipulator_x_playground/CMakeLists.txt +++ b/open_manipulator_x_playground/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(ament_cmake REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(rclcpp REQUIRED) - add_executable(hello_moveit src/hello_moveit.cpp) target_include_directories(hello_moveit PUBLIC $ @@ -25,7 +24,6 @@ ament_target_dependencies( install(TARGETS hello_moveit DESTINATION lib/${PROJECT_NAME}) - # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) diff --git a/open_manipulator_x_playground/package.xml b/open_manipulator_x_playground/package.xml index 2525dbee..a0e8255b 100644 --- a/open_manipulator_x_playground/package.xml +++ b/open_manipulator_x_playground/package.xml @@ -2,18 +2,22 @@ open_manipulator_x_playground - 0.0.0 - TODO: Package description - yunwonho - TODO: License declaration + 3.0.0 + + OpenMANIPULATOR-X teleop ROS 2 package. + + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues + Sungho Woo ament_cmake moveit_ros_planning_interface rclcpp ament_lint_auto ament_lint_common - - ament_cmake diff --git a/open_manipulator_x_playground/src/hello_moveit.cpp b/open_manipulator_x_playground/src/hello_moveit.cpp index eed52e2d..82d9b0f7 100644 --- a/open_manipulator_x_playground/src/hello_moveit.cpp +++ b/open_manipulator_x_playground/src/hello_moveit.cpp @@ -1,5 +1,21 @@ -// Modify the MoveIt tutorial as much as you like using the basic example. -// Author: Sungho Woo + +/******************************************************************************* + * Copyright 2024 ROBOTIS CO., LTD. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *******************************************************************************/ + +/* Authors: Wonho Yoon, Sungho Woo */ #include @@ -35,8 +51,8 @@ auto move_group_interface = MoveGroupInterface(node, "arm"); return msg; }(); move_group_interface.setPoseTarget(target_pose); - move_group_interface.setGoalPositionTolerance(0.01); - move_group_interface.setGoalOrientationTolerance(0.01); + move_group_interface.setGoalPositionTolerance(0.02); + move_group_interface.setGoalOrientationTolerance(0.02); // Create a plan to that target pose auto const [success, plan] = [&move_group_interface]{ diff --git a/open_manipulator_x_teleop/CHANGELOG.rst b/open_manipulator_x_teleop/CHANGELOG.rst new file mode 100644 index 00000000..bbd84fcc --- /dev/null +++ b/open_manipulator_x_teleop/CHANGELOG.rst @@ -0,0 +1,110 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_playground +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.0.0 (2024-12-06) +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_teleop/package.xml b/open_manipulator_x_teleop/package.xml index 812ccca0..f4ad02b6 100644 --- a/open_manipulator_x_teleop/package.xml +++ b/open_manipulator_x_teleop/package.xml @@ -2,10 +2,16 @@ open_manipulator_x_teleop - 0.0.0 - TODO: Package description - yunwonho - TODO: License declaration + 3.0.0 + + OpenMANIPULATOR-X teleop ROS 2 package. + + pyo + Apache 2.0 + http://wiki.ros.org/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator + https://github.com/ROBOTIS-GIT/open_manipulator/issues + Sungho Woo ament_cmake @@ -20,7 +26,7 @@ ament_lint_auto ament_lint_common - + ament_cmake From 1f83a382b543a193075caf2b6713b909cbed1864 Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Tue, 10 Dec 2024 11:10:42 +0900 Subject: [PATCH 03/20] Modified codes based on reviews Signed-off-by: Sungho Woo --- .../scripts/create_udev_rules | 0 .../launch/model.launch.py | 2 +- ...en_manipulator_x_system.ros2_control.xacro | 49 ++-- .../config/robot_joint_log.csv | 5 + open_manipulator_x_gui/ui/main_window.ui | 2 +- .../config/kinematics.yaml | 2 +- .../config/moveit.rviz | 236 +++++++++++++++++- 7 files changed, 258 insertions(+), 38 deletions(-) mode change 100644 => 100755 open_manipulator_x_bringup/scripts/create_udev_rules create mode 100644 open_manipulator_x_gui/config/robot_joint_log.csv diff --git a/open_manipulator_x_bringup/scripts/create_udev_rules b/open_manipulator_x_bringup/scripts/create_udev_rules old mode 100644 new mode 100755 diff --git a/open_manipulator_x_description/launch/model.launch.py b/open_manipulator_x_description/launch/model.launch.py index 6954183c..f7ec152e 100644 --- a/open_manipulator_x_description/launch/model.launch.py +++ b/open_manipulator_x_description/launch/model.launch.py @@ -71,7 +71,7 @@ def generate_launch_description(): [ FindPackageShare('open_manipulator_x_description'), 'rviz', - 'model.rviz' + 'open_manipulator_x.rviz' ] ) diff --git a/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro b/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro index f1c86a9d..50090ca1 100644 --- a/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro +++ b/open_manipulator_x_description/ros2_control/open_manipulator_x_system.ros2_control.xacro @@ -24,21 +24,22 @@ 1000000 0.2 /param/dxl_model - 5 + 6 5 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, - 0, 0, 0, 0, 1 + 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0 - 1, 0, 0, 0, 0, - 0, 1, 0, 0, 0, - 0, 0, 1, 0, 0, - 0, 0, 0, 1, 0, - 0, 0, 0, 0, 1 + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0, + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0 200 dynamixel_hardware_interface/dxl_state @@ -61,50 +62,44 @@ - - ${-pi*0.1} - ${pi*0.1} - + - - ${-pi*0.57} - ${pi*0.5} - + - - ${-pi*0.3} - ${pi*0.44} - + - - ${-pi*0.57} - ${pi*0.65} - + - - -0.01 - 0.019 - + + + + + + + + ${prefix}gripper_left_joint + 1 + diff --git a/open_manipulator_x_gui/config/robot_joint_log.csv b/open_manipulator_x_gui/config/robot_joint_log.csv new file mode 100644 index 00000000..1f7e817a --- /dev/null +++ b/open_manipulator_x_gui/config/robot_joint_log.csv @@ -0,0 +1,5 @@ +0,-0.998621,0.699837,0.299272,0.0188927 +0,-0.268447,0.20105,0.0828754,0.0188927 +0,-0.934194,0.644587,0.274717,0.0188927 +0,-1.00476,0.699837,0.300807,-0.00990052 +0,-1.00169,0.699837,0.300807,0.0189474 diff --git a/open_manipulator_x_gui/ui/main_window.ui b/open_manipulator_x_gui/ui/main_window.ui index 0b9b4014..9e68fdfb 100644 --- a/open_manipulator_x_gui/ui/main_window.ui +++ b/open_manipulator_x_gui/ui/main_window.ui @@ -1161,7 +1161,7 @@ - rad + m diff --git a/open_manipulator_x_moveit_config/config/kinematics.yaml b/open_manipulator_x_moveit_config/config/kinematics.yaml index 754e4def..0f40b29b 100644 --- a/open_manipulator_x_moveit_config/config/kinematics.yaml +++ b/open_manipulator_x_moveit_config/config/kinematics.yaml @@ -1,5 +1,5 @@ arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05 kinematics_solver_attempts: 3 diff --git a/open_manipulator_x_moveit_config/config/moveit.rviz b/open_manipulator_x_moveit_config/config/moveit.rviz index f31651ed..8388215f 100644 --- a/open_manipulator_x_moveit_config/config/moveit.rviz +++ b/open_manipulator_x_moveit_config/config/moveit.rviz @@ -1,51 +1,271 @@ Panels: - Class: rviz_common/Displays + Help Height: 75 Name: Displays Property Tree Widget: Expanded: - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 147 - Class: rviz_common/Help Name: Help - Class: rviz_common/Views + Expanded: + - /Current View1 Name: Views + Splitter Ratio: 0.5 Visualization Manager: + Class: "" Displays: - - Class: rviz_default_plugins/Grid + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: true + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + dummy_mimic_fix: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + dummy_mimic_fix: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true Global Options: + Background Color: 48; 48; 48 Fixed Frame: world + Frame Rate: 30 + Name: root Tools: - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true - Class: rviz_default_plugins/MoveCamera - Class: rviz_default_plugins/Select + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0 + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: -0.1 + X: -0.10000000149011612 Y: 0.25 - Z: 0.30 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.43000003695487976 Target Frame: world - Yaw: -0.623 + Value: Orbit (rviz_default_plugins) + Yaw: 5.495185852050781 + Saved: ~ Window Geometry: + Displays: + collapsed: false Height: 975 - QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000372fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004600fffffffb000000100044006900730070006c006100790073010000003e00000122000000da00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001900000016b00fffffffb0000000800480065006c0070000000029a0000006e0000007500fffffffb0000000a0056006900650077007301000002fc000000b4000000b100ffffff000001f60000037200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false Width: 1200 + X: 1970 + Y: 50 From d7a539285d595884b4d39c21ed514a9cf727a74f Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Tue, 10 Dec 2024 13:32:08 +0900 Subject: [PATCH 04/20] Added practice code for controlling a robotic arm using MoveIt 2 API Signed-off-by: Sungho Woo --- .../src/hello_moveit.cpp | 74 ++++++++++++++----- 1 file changed, 57 insertions(+), 17 deletions(-) diff --git a/open_manipulator_x_playground/src/hello_moveit.cpp b/open_manipulator_x_playground/src/hello_moveit.cpp index 82d9b0f7..711c3e42 100644 --- a/open_manipulator_x_playground/src/hello_moveit.cpp +++ b/open_manipulator_x_playground/src/hello_moveit.cpp @@ -21,53 +21,93 @@ #include #include +#include +#include int main(int argc, char * argv[]) { - // Initialize ROS and create the Node + // Initialize the ROS2 node rclcpp::init(argc, argv); + + // Create a shared pointer for the node and enable automatic parameter declaration auto const node = std::make_shared( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) ); - // Create a ROS logger + // Create a logger for logging messages auto const logger = rclcpp::get_logger("hello_moveit"); -// Create the MoveIt MoveGroup Interface -using moveit::planning_interface::MoveGroupInterface; -auto move_group_interface = MoveGroupInterface(node, "arm"); + // Create the MoveIt MoveGroup Interface for the "arm" planning group + using moveit::planning_interface::MoveGroupInterface; + auto move_group_interface = MoveGroupInterface(node, "arm"); -// Set a target Pose + // Define the target pose for the robot arm auto const target_pose = []{ geometry_msgs::msg::Pose msg; - msg.orientation.x = 0.0; - msg.orientation.y = 0.0; - msg.orientation.z = 0.0; - msg.orientation.w = 1.0; - msg.position.x = 0.163; - msg.position.y = 0.0; - msg.position.z = 0.200; + msg.orientation.x = 0.0; // Orientation (quaternion x) + msg.orientation.y = 0.0; // Orientation (quaternion y) + msg.orientation.z = 0.0; // Orientation (quaternion z) + msg.orientation.w = 1.0; // Orientation (quaternion w) + msg.position.x = 0.163; // Position in x + msg.position.y = 0.0; // Position in y + msg.position.z = 0.2; // Position in z return msg; }(); + + // Set the target pose for the arm move_group_interface.setPoseTarget(target_pose); + + // Set tolerances for goal position and orientation move_group_interface.setGoalPositionTolerance(0.02); move_group_interface.setGoalOrientationTolerance(0.02); - // Create a plan to that target pose + // Plan the motion for the arm to reach the target pose auto const [success, plan] = [&move_group_interface]{ moveit::planning_interface::MoveGroupInterface::Plan msg; auto const ok = static_cast(move_group_interface.plan(msg)); return std::make_pair(ok, msg); }(); - // Execute the plan + // If planning succeeds, execute the planned motion if(success) { move_group_interface.execute(plan); + std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for 2 seconds after execution + } else { + RCLCPP_ERROR(logger, "Planning failed for the arm!"); // Log an error if planning fails + } + + // Create the MoveIt MoveGroup Interface for the "gripper" planning group + auto gripper_interface = MoveGroupInterface(node, "gripper"); + + // Set the "close" position for the gripper and move it + gripper_interface.setNamedTarget("close"); + if (gripper_interface.move()) { + RCLCPP_INFO(logger, "Gripper closed successfully"); // Log success + std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for 2 seconds } else { - RCLCPP_ERROR(logger, "Planing failed!"); + RCLCPP_ERROR(logger, "Failed to close the gripper"); // Log an error if it fails } - // Shutdown ROS + + // Move the arm back to the "home" position + move_group_interface.setNamedTarget("init"); + if (move_group_interface.move()) { + RCLCPP_INFO(logger, "Arm moved back to init position"); // Log success + std::this_thread::sleep_for(std::chrono::seconds(2)); // Wait for 2 seconds + + // Open the gripper + gripper_interface.setNamedTarget("open"); + if (gripper_interface.move()) { + RCLCPP_INFO(logger, "Gripper opened successfully"); // Log success + } else { + RCLCPP_ERROR(logger, "Failed to open the gripper"); // Log an error if it fails + } + + } else { + RCLCPP_ERROR(logger, "Failed to move the arm back to home position"); // Log an error if it fails + } + + // Shut down the ROS2 node rclcpp::shutdown(); return 0; } From 30fff3a6d2a147ef7b1cedc0166c04579b549dd2 Mon Sep 17 00:00:00 2001 From: wonho yun Date: Tue, 10 Dec 2024 15:17:17 +0900 Subject: [PATCH 05/20] Modified codes based on reviews-2 --- .vscode/settings.json | 7 ------ open_manipulator/package.xml | 6 ++++-- open_manipulator_x.repos | 25 ---------------------- open_manipulator_x_gui/src/main_window.cpp | 5 ++++- open_manipulator_x_gui/ui/main_window.ui | 6 +++--- 5 files changed, 11 insertions(+), 38 deletions(-) delete mode 100644 .vscode/settings.json delete mode 100644 open_manipulator_x.repos diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index ec6235a4..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,7 +0,0 @@ -{ - "files.associations": { - "*.tcc": "cpp", - "string": "cpp", - "string_view": "cpp" - } -} \ No newline at end of file diff --git a/open_manipulator/package.xml b/open_manipulator/package.xml index ea1f6b27..c81492cb 100644 --- a/open_manipulator/package.xml +++ b/open_manipulator/package.xml @@ -19,9 +19,11 @@ Sungho Woo ament_cmake - open_manipulator_x_controller + open_manipulator_x_bringup open_manipulator_x_description - open_manipulator_x_libs + open_manipulator_x_gui + open_manipulator_x_moveit_config + open_manipulator_x_playground open_manipulator_x_teleop ament_cmake diff --git a/open_manipulator_x.repos b/open_manipulator_x.repos deleted file mode 100644 index 9d396e1d..00000000 --- a/open_manipulator_x.repos +++ /dev/null @@ -1,25 +0,0 @@ -repositories: - DynamixelSDK: - type: git - url: https://github.com/ROBOTIS-GIT/DynamixelSDK.git - version: ros2-devel - dynamixel-workbench: - type: git - url: https://github.com/ROBOTIS-GIT/dynamixel-workbench.git - version: ros2-devel - open_manipulator: - type: git - url: https://github.com/ROBOTIS-GIT/open_manipulator.git - version: ros2-devel - open_manipulator_msgs: - type: git - url: https://github.com/ROBOTIS-GIT/open_manipulator_msgs.git - version: ros2-devel - open_manipulator_dependencies: - type: git - url: https://github.com/ROBOTIS-GIT/open_manipulator_dependencies.git - version: ros2-devel - robotis_manipulator: - type: git - url: https://github.com/ROBOTIS-GIT/robotis_manipulator.git - version: ros2-devel diff --git a/open_manipulator_x_gui/src/main_window.cpp b/open_manipulator_x_gui/src/main_window.cpp index 7b52605a..59ce2608 100644 --- a/open_manipulator_x_gui/src/main_window.cpp +++ b/open_manipulator_x_gui/src/main_window.cpp @@ -400,6 +400,7 @@ namespace open_manipulator_x_gui std::thread([this, repeatCount]() { + ui.btn_reset_task->setEnabled(false); for (int repeat = 0; repeat < repeatCount; ++repeat) { if (qnode.isStopRequested()) @@ -520,7 +521,9 @@ namespace open_manipulator_x_gui tableWidget->item(row, col)->setBackground(Qt::white); } tableWidget->item(row, 5)->setText("Done"); - } }, Qt::QueuedConnection); }) + } + ui.btn_reset_task->setEnabled(true); + }, Qt::QueuedConnection); }) .detach(); } diff --git a/open_manipulator_x_gui/ui/main_window.ui b/open_manipulator_x_gui/ui/main_window.ui index 9e68fdfb..a1e82e68 100644 --- a/open_manipulator_x_gui/ui/main_window.ui +++ b/open_manipulator_x_gui/ui/main_window.ui @@ -172,7 +172,7 @@ - rad + m @@ -230,7 +230,7 @@ Qt::LeftToRight - 0.00 + 0.000 Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -533,7 +533,7 @@ - 2 + 0 From f3d9b52b87486f9758c0c1f6c1c421a177337f1b Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Thu, 12 Dec 2024 13:51:21 +0900 Subject: [PATCH 06/20] Refactored code Signed-off-by: Sungho Woo --- README.md | 2 + open_manipulator_x_bringup/package.xml | 8 +- .../worlds/turtlebot3_world.model | 53 -- .../turtlebot3_world/meshes/hexagon.dae | 76 --- .../worlds/turtlebot3_world/meshes/wall.dae | 76 --- .../worlds/turtlebot3_world/model-1_4.sdf | 549 ------------------ .../worlds/turtlebot3_world/model.config | 17 - .../worlds/turtlebot3_world/model.sdf | 549 ------------------ open_manipulator_x_description/package.xml | 6 +- open_manipulator_x_gui/package.xml | 8 +- open_manipulator_x_moveit_config/package.xml | 7 +- open_manipulator_x_playground/package.xml | 3 +- 12 files changed, 11 insertions(+), 1343 deletions(-) delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world.model delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model.config delete mode 100644 open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf diff --git a/README.md b/README.md index c577c3f6..ffa8d964 100644 --- a/README.md +++ b/README.md @@ -2,6 +2,8 @@ +The 4-DOF Open Manipulator-X now supports MoveIt 2, enabling enhanced motion planning and control for advanced robotic applications. This update also brings significant improvements to the teleoperation features, example use cases, and the graphical user interface (GUI), providing a more seamless and user-friendly experience for developers and researchers. + - Active Branches: noetic, humble, main - Legacy Branches: *-devel diff --git a/open_manipulator_x_bringup/package.xml b/open_manipulator_x_bringup/package.xml index d0209cac..b391a182 100644 --- a/open_manipulator_x_bringup/package.xml +++ b/open_manipulator_x_bringup/package.xml @@ -6,16 +6,12 @@ OpenMANIPULATOR-X bringup ROS 2 package. - pyo + Pyo Apache 2.0 http://wiki.ros.org/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator/issues - Darby Lim Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son Sungho Woo ament_cmake @@ -30,4 +26,4 @@ ament_cmake - \ No newline at end of file + diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world.model b/open_manipulator_x_bringup/worlds/turtlebot3_world.model deleted file mode 100644 index 11bfcc22..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world.model +++ /dev/null @@ -1,53 +0,0 @@ - - - - - - model://ground_plane - - - - model://sun - - - - false - - - - - 0.319654 -0.235002 9.29441 0 1.5138 0.009599 - orbit - perspective - - - - - 1000.0 - 0.001 - 1 - - - quick - 150 - 0 - 1.400000 - 1 - - - 0.00001 - 0.2 - 2000.000000 - 0.01000 - - - - - - 1 - - model://turtlebot3_world - - - - diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae deleted file mode 100644 index c548e5f1..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/hexagon.dae +++ /dev/null @@ -1,76 +0,0 @@ - - - 2017-03-30T01:06:40 - 2017-03-30T01:06:40 - - Y_UP - - - - - - - - - - - - - - - - - - - -28.8675 50 100 -57.735 9.66338e-13 0 -57.735 1.13687e-12 100 -28.8675 50 0 -57.735 1.13687e-12 100 -28.8675 -50 0 -28.8675 -50 100 -57.735 9.66338e-13 0 -28.8675 -50 100 28.8675 -50 0 28.8675 -50 100 -28.8675 -50 0 28.8675 -50 100 57.735 1.7053e-12 0 57.735 1.36424e-12 100 28.8675 -50 0 57.735 1.36424e-12 100 28.8675 50 2.498e-13 28.8675 50 100 57.735 1.7053e-12 0 28.8675 50 100 -28.8675 50 0 -28.8675 50 100 28.8675 50 2.498e-13 -57.735 1.13687e-12 100 57.735 1.36424e-12 100 -28.8675 50 100 -28.8675 -50 100 28.8675 -50 100 28.8675 50 100 -57.735 9.66338e-13 0 28.8675 -50 0 -28.8675 -50 0 57.735 1.7053e-12 0 -28.8675 50 0 28.8675 50 2.498e-13 - - - - - - - - - - -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 -0.866025 -0.5 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 5.51091e-16 -1 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 -0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 0.866025 0.5 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 -4.28626e-16 1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 - - - - - - - - - - - - - - -

0 1 2 1 0 3 4 5 6 5 4 7 8 9 10 9 8 11 12 13 14 13 12 15 16 17 18 17 16 19 20 21 22 21 20 23 24 25 26 24 27 28 28 25 24 26 25 29 30 31 32 31 30 33 33 34 35 33 30 34

- - - - - - - - - - - - - - - - 0.392157 0.976471 0.121569 1 - - - - - - - - - - diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae b/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae deleted file mode 100644 index aa738ce7..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/meshes/wall.dae +++ /dev/null @@ -1,76 +0,0 @@ - - - 2017-03-30T02:35:53 - 2017-03-30T02:35:53 - - Y_UP - - - - - - - - - - - - - - - - - - - -400 230.94 0 -400 230.94 200 4.54747e-13 461.88 200 6.25278e-13 461.88 0 6.25278e-13 461.88 0 4.54747e-13 461.88 200 400 230.94 200 400 230.94 0 400 230.94 0 400 230.94 200 400 -230.94 200 400 -230.94 0 400 -230.94 0 400 -230.94 200 -1.7053e-13 -461.88 200 2.27374e-13 -461.88 0 2.27374e-13 -461.88 0 -1.7053e-13 -461.88 200 -400 -230.94 200 -400 -230.94 0 450 259.808 200 -3.69482e-13 519.615 200 -2.27374e-13 519.615 0 450 259.808 0 -3.69482e-13 519.615 200 -450 259.808 200 -450 259.808 0 -2.27374e-13 519.615 0 -450 259.808 200 -450 -259.808 200 -450 -259.808 0 -450 259.808 0 -450 -259.808 200 -1.13687e-13 -519.615 200 -2.27374e-13 -519.615 0 -450 -259.808 0 -1.13687e-13 -519.615 200 450 -259.808 200 450 -259.808 0 -2.27374e-13 -519.615 0 450 -259.808 0 450 259.808 200 450 259.808 0 450 -259.808 200 -400 -230.94 0 -400 -230.94 200 -400 230.94 200 -400 230.94 0 -1.7053e-13 -461.88 200 -1.13687e-13 -519.615 200 -400 -230.94 200 -450 -259.808 200 -450 259.808 200 450 -259.808 200 400 -230.94 200 400 230.94 200 4.54747e-13 461.88 200 -3.69482e-13 519.615 200 -400 230.94 200 450 259.808 200 -2.27374e-13 -519.615 0 400 -230.94 0 2.27374e-13 -461.88 0 400 230.94 0 -2.27374e-13 519.615 0 6.25278e-13 461.88 0 450 -259.808 0 450 259.808 0 -400 230.94 0 -450 259.808 0 -400 -230.94 0 -450 -259.808 0 - - - - - - - - - - 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -0.5 -0.866025 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -1 -6.12323e-17 -0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -0.5 0.866025 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -1 -5.21642e-16 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 -0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 0.5 -0.866025 0 1 0 0 1 0 0 1 0 0 1 0 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 1 1.83697e-16 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 -0 -0 -1 - - - - - - - - - - - - - - -

0 2 1 0 3 2 4 6 5 4 7 6 8 10 9 8 11 10 12 14 13 12 15 14 16 18 17 16 19 18 20 22 21 22 20 23 24 26 25 26 24 27 28 30 29 30 28 31 32 34 33 34 32 35 36 38 37 38 36 39 40 42 41 40 41 43 44 46 45 44 47 46 48 50 49 50 52 51 53 54 49 55 57 56 48 49 54 57 52 58 58 56 57 53 59 55 57 55 59 53 55 54 49 50 51 52 50 58 60 62 61 63 65 64 63 67 66 68 69 64 66 61 63 64 67 63 60 61 66 60 70 62 69 70 71 60 71 70 64 65 68 69 68 70

- - - - - - - - - - - - - - - - 0.603922 0.647059 0.686275 1 - - - - - - - - - - diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf b/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf deleted file mode 100644 index 330bd320..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/model-1_4.sdf +++ /dev/null @@ -1,549 +0,0 @@ - - - - 1 - - - -1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - -1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - -1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - - 3.5 0 -0.5 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.8 0.8 0.8 - - - 10 - - - - - - - - - - - - - 3.5 0 -0.5 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.8 0.8 0.8 - - - - - - - - - 1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - 1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - 1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - 1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - -1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - -1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - -1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - -1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - - 0 0 -0.3 0 0 -1.5708 - - - model://turtlebot3_world/meshes/wall.dae - 0.25 0.25 0.25 - - - 10 - - - - - - - - - - - - - 0 0 -0.3 0 0 -1.5708 - - - model://turtlebot3_world/meshes/wall.dae - 0.25 0.25 0.25 - - - - - - - - - diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config deleted file mode 100644 index 387bb6e6..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.config +++ /dev/null @@ -1,17 +0,0 @@ - - - - TurtleBot3 World - 1.0 - model-1_4.sdf - model.sdf - - - Taehun Lim(Darby) - thlim@robotis.com - - - - World of TurtleBot3 with ROS symbol - - diff --git a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf b/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf deleted file mode 100644 index d733d00f..00000000 --- a/open_manipulator_x_bringup/worlds/turtlebot3_world/model.sdf +++ /dev/null @@ -1,549 +0,0 @@ - - - - 1 - - - -1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - -1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - -1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - -1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 0 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 0 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 -1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 0 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - 1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - 10 - - - - - - - - - - - - - 1.1 1.1 0.25 0 0 0 - - - 0.15 - 0.5 - - - - - - - - - - 3.5 0 -0.5 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.8 0.8 0.8 - - - 10 - - - - - - - - - - - - - 3.5 0 -0.5 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.8 0.8 0.8 - - - - - - - - - 1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - 1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - 1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - 1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - -1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - -1.8 2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - -1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - 10 - - - - - - - - - - - - - -1.8 -2.7 0 0 0 0 - - - model://turtlebot3_world/meshes/hexagon.dae - 0.55 0.55 0.55 - - - - - - - - - - 0 0 -0.3 0 0 -1.5708 - - - model://turtlebot3_world/meshes/wall.dae - 0.25 0.25 0.25 - - - 10 - - - - - - - - - - - - - 0 0 -0.3 0 0 -1.5708 - - - model://turtlebot3_world/meshes/wall.dae - 0.25 0.25 0.25 - - - - - - - - - diff --git a/open_manipulator_x_description/package.xml b/open_manipulator_x_description/package.xml index 3bd93001..ccc6a5ac 100644 --- a/open_manipulator_x_description/package.xml +++ b/open_manipulator_x_description/package.xml @@ -6,16 +6,12 @@ open_manipulator_x_description ROS 2 package. - pyo + Pyo Apache 2.0 http://wiki.ros.org/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator/issues - Darby Lim Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son Sungho Woo ament_cmake diff --git a/open_manipulator_x_gui/package.xml b/open_manipulator_x_gui/package.xml index 45de0e66..a447e06a 100644 --- a/open_manipulator_x_gui/package.xml +++ b/open_manipulator_x_gui/package.xml @@ -3,17 +3,15 @@ open_manipulator_x_gui 3.0.0 - OpenMANIPULATOR-X gui ROS 2 package. + + The OpenMANIPULATOR-X GUI ROS 2 package enables users to explore Joint Space, + Task Space, and even work with the Task Constructor functionality. pyo Apache 2.0 http://wiki.ros.org/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator/issues - Darby Lim Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son Sungho Woo ament_cmake diff --git a/open_manipulator_x_moveit_config/package.xml b/open_manipulator_x_moveit_config/package.xml index 5d37424a..d25de50e 100644 --- a/open_manipulator_x_moveit_config/package.xml +++ b/open_manipulator_x_moveit_config/package.xml @@ -6,16 +6,12 @@ An automatically generated package with all the configuration and launch files for using the open_manipulator_x with the MoveIt Motion Planning Framework - pyo + Pyo Apache 2.0 http://wiki.ros.org/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator https://github.com/ROBOTIS-GIT/open_manipulator/issues - Darby Lim Hye-Jong KIM - Ryan Shim - Yong-Ho Na - Will Son Sungho Woo ament_cmake @@ -43,7 +39,6 @@ warehouse_ros_mongo xacro - ament_cmake diff --git a/open_manipulator_x_playground/package.xml b/open_manipulator_x_playground/package.xml index a0e8255b..9644d582 100644 --- a/open_manipulator_x_playground/package.xml +++ b/open_manipulator_x_playground/package.xml @@ -4,7 +4,8 @@ open_manipulator_x_playground 3.0.0 - OpenMANIPULATOR-X teleop ROS 2 package. + This package provides an example for utilizing the MoveIt API with the OpenMANIPULATOR-X, + allowing users to practice and experiment freely. pyo Apache 2.0 From d0d33726686547c790256ec0ab7c5bd5f6753a48 Mon Sep 17 00:00:00 2001 From: Sungho Woo Date: Thu, 12 Dec 2024 13:56:31 +0900 Subject: [PATCH 07/20] Added ROS 2 Humble support to the ros-ci workflow Signed-off-by: Sungho Woo --- .github/workflows/ros-ci.yml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 70b20be0..52cc7680 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -16,10 +16,14 @@ jobs: matrix: ros_distribution: - noetic + - humble include: - docker_image: ubuntu:focal - ros_distribution: noetic - ros_version: 1 + ros_distribution: noetic + ros_version: 1 + - docker_image: ubuntu:jammy + ros_distribution: humble + ros_version: 2 container: image: ${{ matrix.docker_image }} steps: From e838952f254aefb9fc13f99ed03b0d64b61768b0 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 12 Dec 2024 18:44:11 +0900 Subject: [PATCH 08/20] Modified the CI settings for ROS 2 humble Signed-off-by: Pyo --- .github/workflows/ros-ci.yml | 28 +++++++++++++--------------- 1 file changed, 13 insertions(+), 15 deletions(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 52cc7680..1cce03a8 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -3,27 +3,29 @@ name: ros-ci # Controls when the action will run. Triggers the workflow on push or pull request on: push: - branches: [ master, develop, noetic-devel ] + branches: [ main, humble ] pull_request: - branches: [ master, develop, noetic-devel ] + branches: [ main, humble ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: - ros-ci: + ros2-ci: runs-on: ubuntu-latest strategy: fail-fast: false matrix: ros_distribution: - - noetic - humble + # - rolling include: - - docker_image: ubuntu:focal - ros_distribution: noetic - ros_version: 1 - - docker_image: ubuntu:jammy + # Humble + - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble ros_version: 2 + # # Rolling + # - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest + # ros_distribution: rolling + # ros_version: 2 container: image: ${{ matrix.docker_image }} steps: @@ -33,13 +35,9 @@ jobs: uses: actions/checkout@v2 with: path: ros_ws/src - - name: Setup ROS environment - uses: ros-tooling/setup-ros@0.2.1 - with: - required-ros-distributions: ${{ matrix.ros_distribution }} - name: Build and Test - uses: ros-tooling/action-ros-ci@v0.2 + uses: ros-tooling/action-ros-ci@0.2.6 with: package-name: open_manipulator - target-ros1-distro: ${{ matrix.ros_distribution }} - vcs-repo-file-url: "https://raw.githubusercontent.com/ROBOTIS-GIT/open_manipulator/develop/openmanipulator.repos" + target-ros2-distro: ${{ matrix.ros_distribution }} + vcs-repo-file-url: "" From bfcde745efe288bb096f0ab253375e07cb68789a Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 12 Dec 2024 18:45:20 +0900 Subject: [PATCH 09/20] Modified the CI settings for ROS 2 humble Signed-off-by: Pyo --- .github/workflows/ros-ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 1cce03a8..721082e9 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -3,9 +3,9 @@ name: ros-ci # Controls when the action will run. Triggers the workflow on push or pull request on: push: - branches: [ main, humble ] + branches: [ master, main, humble ] pull_request: - branches: [ main, humble ] + branches: [ master, main, humble ] # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: From 744726a4ca1eab8bcc6ac8ab3979a9a6c62229d7 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 12 Dec 2024 18:54:24 +0900 Subject: [PATCH 10/20] Modified the CI settings for ROS 2 humble and action version Signed-off-by: Pyo --- .github/workflows/ros-ci.yml | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 721082e9..49382454 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -1,13 +1,11 @@ name: ros-ci -# Controls when the action will run. Triggers the workflow on push or pull request on: push: branches: [ master, main, humble ] pull_request: branches: [ master, main, humble ] -# A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: ros2-ci: runs-on: ubuntu-latest @@ -16,27 +14,21 @@ jobs: matrix: ros_distribution: - humble - # - rolling include: - # Humble - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest ros_distribution: humble ros_version: 2 - # # Rolling - # - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-rolling-ros-base-latest - # ros_distribution: rolling - # ros_version: 2 container: image: ${{ matrix.docker_image }} steps: - name: Setup directories run: mkdir -p ros_ws/src - name: checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 with: path: ros_ws/src - name: Build and Test - uses: ros-tooling/action-ros-ci@0.2.6 + uses: ros-tooling/action-ros-ci@0.3 with: package-name: open_manipulator target-ros2-distro: ${{ matrix.ros_distribution }} From 3ec9f2a4a7ee9a9a5968b25c288f511019e3a7b3 Mon Sep 17 00:00:00 2001 From: Pyo Date: Thu, 12 Dec 2024 18:55:51 +0900 Subject: [PATCH 11/20] Modified the CI settings for ROS 2 humble and action version Signed-off-by: Pyo --- .github/workflows/ros-ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index 49382454..5958f9ef 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -28,7 +28,7 @@ jobs: with: path: ros_ws/src - name: Build and Test - uses: ros-tooling/action-ros-ci@0.3 + uses: ros-tooling/action-ros-ci@v0.3 with: package-name: open_manipulator target-ros2-distro: ${{ matrix.ros_distribution }} From f703e37ce90dff7ff423c011bc82985f2a408e2a Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 11:37:46 +0900 Subject: [PATCH 12/20] Modified the readme file for related doc, code and video Signed-off-by: Pyo --- README.md | 38 ++++---------------------------------- 1 file changed, 4 insertions(+), 34 deletions(-) diff --git a/README.md b/README.md index ffa8d964..c107812a 100644 --- a/README.md +++ b/README.md @@ -10,47 +10,17 @@ The 4-DOF Open Manipulator-X now supports MoveIt 2, enabling enhanced motion pla # ROBOTIS e-Manual for OpenMANIPULATOR-X - [http://emanual.robotis.com/docs/en/platform/openmanipulator/](http://emanual.robotis.com/docs/en/platform/openmanipulator/) -# Wiki for open_manipulator Packages -- http://wiki.ros.org/open_manipulator (metapackage) -- http://wiki.ros.org/open_manipulator_control_gui -- http://wiki.ros.org/open_manipulator_controller -- http://wiki.ros.org/open_manipulator_description -- http://wiki.ros.org/open_manipulator_libs -- http://wiki.ros.org/open_manipulator_moveit -- http://wiki.ros.org/open_manipulator_teleop - # Open Source related to OpenMANIPULATOR-X -- [robotis_manipulator](https://github.com/ROBOTIS-GIT/robotis_manipulator) -- [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) -- [open_manipulator_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_msgs) -- [open_manipulator_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_simulations) -- [open_manipulator_perceptions](https://github.com/ROBOTIS-GIT/open_manipulator_perceptions) -- [open_manipulator_processing](https://github.com/ROBOTIS-GIT/open_manipulator_processing) -- [open_manipulator_friends](https://github.com/ROBOTIS-GIT/open_manipulator_friends) -- [open_manipulator_with_tb3](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3) -- [open_manipulator_with_tb3_msgs](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_msgs) -- [open_manipulator_with_tb3_simulations](https://github.com/ROBOTIS-GIT/open_manipulator_with_tb3_simulations) - [open_manipulator](https://github.com/ROBOTIS-GIT/open_manipulator) -- [turtlebot3_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_msgs) -- [turtlebot3_simulations](https://github.com/ROBOTIS-GIT/turtlebot3_simulations) -- [turtlebot3_applications](https://github.com/ROBOTIS-GIT/turtlebot3_applications) -- [turtlebot3_applications_msgs](https://github.com/ROBOTIS-GIT/turtlebot3_applications_msgs) -- [turtlebot3_autorace](https://github.com/ROBOTIS-GIT/turtlebot3_autorace) -- [turtlebot3_deliver](https://github.com/ROBOTIS-GIT/turtlebot3_deliver) -- [hls_lfcd_lds_driver](https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver) -- [manipulator_h](https://github.com/ROBOTIS-GIT/ROBOTIS-MANIPULATOR-H) +- [open_manipulator_y](https://github.com/ROBOTIS-GIT/open_manipulator_y) +- [open_manipulator_p](https://github.com/ROBOTIS-GIT/open_manipulator_p) - [dynamixel_sdk](https://github.com/ROBOTIS-GIT/DynamixelSDK) - [dynamixel_workbench](https://github.com/ROBOTIS-GIT/dynamixel-workbench) -- [OpenCR-Hardware](https://github.com/ROBOTIS-GIT/OpenCR-Hardware) -- [OpenCR](https://github.com/ROBOTIS-GIT/OpenCR) +- [dynamixel_hardware_interface](https://github.com/ROBOTIS-GIT/dynamixel_hardware_interface) # Documents and Videos related to OpenMANIPULATOR-X - [ROBOTIS e-Manual for OpenMANIPULATOR-X](http://emanual.robotis.com/docs/en/platform/openmanipulator/) - [ROBOTIS e-Manual for OpenMANIPULATOR-P](https://emanual.robotis.com/docs/en/platform/openmanipulator_p/overview/) - [ROBOTIS e-Manual for DYNAMIXEL SDK](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/overview/) - [ROBOTIS e-Manual for DYNAMIXEL Workbench](http://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_workbench/) -- [e-Book for open_manipulator and OpenManipulator](https://community.robotsource.org/t/download-the-ros-robot-programming-book-for-free/51/) - -https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb - -https://www.youtube.com/playlist?list=PLRG6WP3c31_XI3wlvHlx2Mp8BYqgqDURU +- [YouTube Play List for OpenMANIPULATOR](https://www.youtube.com/playlist?list=PLRG6WP3c31_WpEsB6_Rdt3KhiopXQlUkb) From f3ede4fe2ee1fee2670c06921ef14f774341beb5 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 12:08:08 +0900 Subject: [PATCH 13/20] Modified the changelog files Signed-off-by: Pyo --- open_manipulator/CHANGELOG.rst | 1 + open_manipulator_x_bringup/CHANGELOG.rst | 1 + open_manipulator_x_description/CHANGELOG.rst | 1 + open_manipulator_x_gui/CHANGELOG.rst | 1 + .../.setup_assistant | 6 +- .../CHANGELOG.rst | 111 ++++++++++++++++++ open_manipulator_x_playground/CHANGELOG.rst | 1 + open_manipulator_x_teleop/CHANGELOG.rst | 7 +- 8 files changed, 123 insertions(+), 6 deletions(-) create mode 100644 open_manipulator_x_moveit_config/CHANGELOG.rst diff --git a/open_manipulator/CHANGELOG.rst b/open_manipulator/CHANGELOG.rst index 7bff3569..fd738e3a 100644 --- a/open_manipulator/CHANGELOG.rst +++ b/open_manipulator/CHANGELOG.rst @@ -3,6 +3,7 @@ Changelog for package open_manipulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo diff --git a/open_manipulator_x_bringup/CHANGELOG.rst b/open_manipulator_x_bringup/CHANGELOG.rst index 7902186b..33f8fa14 100644 --- a/open_manipulator_x_bringup/CHANGELOG.rst +++ b/open_manipulator_x_bringup/CHANGELOG.rst @@ -3,6 +3,7 @@ Changelog for package open_manipulator_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo diff --git a/open_manipulator_x_description/CHANGELOG.rst b/open_manipulator_x_description/CHANGELOG.rst index 960cad37..dfe972db 100644 --- a/open_manipulator_x_description/CHANGELOG.rst +++ b/open_manipulator_x_description/CHANGELOG.rst @@ -3,6 +3,7 @@ Changelog for package open_manipulator_x_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo diff --git a/open_manipulator_x_gui/CHANGELOG.rst b/open_manipulator_x_gui/CHANGELOG.rst index 5d064ae1..8df9b77a 100644 --- a/open_manipulator_x_gui/CHANGELOG.rst +++ b/open_manipulator_x_gui/CHANGELOG.rst @@ -3,6 +3,7 @@ Changelog for package open_manipulator_x_gui ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo diff --git a/open_manipulator_x_moveit_config/.setup_assistant b/open_manipulator_x_moveit_config/.setup_assistant index 70083b6e..2c6ee017 100644 --- a/open_manipulator_x_moveit_config/.setup_assistant +++ b/open_manipulator_x_moveit_config/.setup_assistant @@ -5,8 +5,8 @@ moveit_setup_assistant_config: srdf: relative_path: config/open_manipulator_x.srdf package_settings: - author_name: yunwonho - author_email: sirius584@naver.com + author_name: Sungho Woo + author_email: wsh@robotis.com generated_timestamp: 1732003006 control_xacro: command: @@ -22,4 +22,4 @@ moveit_setup_assistant_config: - position state: - position - - velocity \ No newline at end of file + - velocity diff --git a/open_manipulator_x_moveit_config/CHANGELOG.rst b/open_manipulator_x_moveit_config/CHANGELOG.rst new file mode 100644 index 00000000..5cf67d2d --- /dev/null +++ b/open_manipulator_x_moveit_config/CHANGELOG.rst @@ -0,0 +1,111 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_moveit_config +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +3.0.0 (2024-12-06) +------------------ +* Refactored OM-X for compatibility with MoveIt 2 +* Contributors: Wonho Yoon, Sungho Woo + +2.3.0 (2021-10-06) +------------------ +* ROS2 Foxy Fitzroy supported +* OpenMANIPULATOR Teleop developed in python +* Contributors: Will Son + +2.2.0 (2019-11-13) +------------------ +* Applied robotis coding style guide +* Contributors: Ryan Shim + +2.1.0 (2019-08-31) +------------------ +* Added support for ROS2 +* Contributors: Ryan Shim + +2.0.1 (2019-02-18) +------------------ +* added dependency option for open_manipulator_control_gui package +* Contributors: Pyo + +2.0.0 (2019-02-08) +------------------ +* updated the CHANGELOG and version to release binary packages +* added new packages (open_manipulator_control_gui, *_controller, *_libs, *_teleop) +* deleted unused packages (open_manipulator_dynamixel_ctrl, open_manipulator_position_ctrl) +* - open_manipulator_control_gui - +* updated function name, UI +* added group names and gripper args +* added position only client +* modified topic names, end-effector name +* - open_manipulator_controller - +* added jointspace path serv, moveit params +* added moveit config and controller +* added kinematic pose pub +* added mimic param and end effector point +* added execute permission +* added usb rules +* added cdc rules +* removed warn message +* renamed open_manipulator lib files +* changed math function name, namespace +* changed openManipulatorProcess() to processOpenManipulator() +* updated start_state after execution on MoveIt +* updated thread time, dynamixel profiling control method +* updated drawing line +* updated flexible node +* updated tool control +* updated chain to open_manipulator +* updated new kinematics +* used robot_name on joint_state_publisher's source_list +* - open_manipulator_description - +* deleted model.launch +* modified gripper origin +* modified end_effector origin +* modified link2 and joint2 position +* updated inertia +* changed calculated inertia param +* changed gripper link name +* changed axis for grip_joint +* - open_manipulator_moveit - +* added moveit config and controller +* updated moveit rviz +* Updated start_state after execution on Moveit `#83 `_ +* changed control period 40mm to 100mm +* Contributors: Darby Lim, Hye-Jong KIM, Yong-Ho Na, Ryan Shim, Guilherme de Campos Affonso, Pyo + +1.0.0 (2018-06-01) +------------------ +* package reconfiguration for OpenManipulator +* added new stl files +* added urdf, rviz param, gazebo params, group +* added function to support protocol 1.0 +* modified color, xacro server, mu1, mu2, collision range, joint limit +* modified joint_state_publisher, joint_states_publisher +* modified params of inertial, xacro, gazebo, collision, friction +* modified urdf file names and collision geometry +* modified motor id, msg names +* modified description and package tree +* deleted unnecessary packages +* merged pull request `#34 `_ `#33 `_ `#32 `_ `#31 `_ `#27 `_ `#26 `_ `#25 `_ +* Contributors: Darby Lim, Pyo + +0.1.1 (2018-03-15) +------------------ +* modified build setting for using yaml-cpp +* Contributors: Pyo + +0.1.0 (2018-03-14) +------------------ +* added meta package for OpenManipulator +* updated dynamixel controller +* modified joint control +* modified gripper topic +* modified URDF +* modified description +* modified messages +* modified moveit set and gripper control +* modified gazebo and moveit setting +* modified cmake, package files for release +* refactoring for release +* Contributors: Darby Lim, Pyo diff --git a/open_manipulator_x_playground/CHANGELOG.rst b/open_manipulator_x_playground/CHANGELOG.rst index bbd84fcc..206b53a5 100644 --- a/open_manipulator_x_playground/CHANGELOG.rst +++ b/open_manipulator_x_playground/CHANGELOG.rst @@ -3,6 +3,7 @@ Changelog for package open_manipulator_x_playground ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo diff --git a/open_manipulator_x_teleop/CHANGELOG.rst b/open_manipulator_x_teleop/CHANGELOG.rst index bbd84fcc..cd95e7b1 100644 --- a/open_manipulator_x_teleop/CHANGELOG.rst +++ b/open_manipulator_x_teleop/CHANGELOG.rst @@ -1,8 +1,9 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_x_playground -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_teleop +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) +------------------ * Refactored OM-X for compatibility with MoveIt 2 * Contributors: Wonho Yoon, Sungho Woo From 7d3582ef017e660a418ccd99f74706da9555580b Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 12:12:51 +0900 Subject: [PATCH 14/20] Modified the rules file Signed-off-by: Pyo --- open_manipulator_x_bringup/99-open-manipulator-cdc.rules | 2 -- 1 file changed, 2 deletions(-) diff --git a/open_manipulator_x_bringup/99-open-manipulator-cdc.rules b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules index a23da3d6..6ac7da16 100644 --- a/open_manipulator_x_bringup/99-open-manipulator-cdc.rules +++ b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules @@ -1,3 +1 @@ -#http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2 - KERNEL=="ttyUSB*", DRIVERS=="ftdi_sio", MODE="0666", ATTR{device/latency_timer}="1" From c9c52ec1e6f3aba9d0725f32a36f159ae569021f Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 12:13:51 +0900 Subject: [PATCH 15/20] Modified the rules file Signed-off-by: Pyo --- open_manipulator_x_bringup/99-open-manipulator-cdc.rules | 2 ++ 1 file changed, 2 insertions(+) diff --git a/open_manipulator_x_bringup/99-open-manipulator-cdc.rules b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules index 6ac7da16..a23da3d6 100644 --- a/open_manipulator_x_bringup/99-open-manipulator-cdc.rules +++ b/open_manipulator_x_bringup/99-open-manipulator-cdc.rules @@ -1 +1,3 @@ +#http://linux-tips.org/t/prevent-modem-manager-to-capture-usb-serial-devices/284/2 + KERNEL=="ttyUSB*", DRIVERS=="ftdi_sio", MODE="0666", ATTR{device/latency_timer}="1" From 1398d40fe54c6486df64da7ad31a704c68983658 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 13:05:28 +0900 Subject: [PATCH 16/20] Added a newline at of file Signed-off-by: Pyo --- open_manipulator_x_description/launch/model.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/open_manipulator_x_description/launch/model.launch.py b/open_manipulator_x_description/launch/model.launch.py index f7ec152e..b8340bd6 100644 --- a/open_manipulator_x_description/launch/model.launch.py +++ b/open_manipulator_x_description/launch/model.launch.py @@ -102,4 +102,4 @@ def generate_launch_description(): package="joint_state_publisher_gui", executable="joint_state_publisher_gui", condition=IfCondition(use_gui)), - ]) \ No newline at end of file + ]) From a39e3410bd9301301b4fdc42cdd62f6e7cab5da5 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 13:05:46 +0900 Subject: [PATCH 17/20] Modified the package name Signed-off-by: Pyo --- open_manipulator_x_bringup/CHANGELOG.rst | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/open_manipulator_x_bringup/CHANGELOG.rst b/open_manipulator_x_bringup/CHANGELOG.rst index 33f8fa14..d67db9e4 100644 --- a/open_manipulator_x_bringup/CHANGELOG.rst +++ b/open_manipulator_x_bringup/CHANGELOG.rst @@ -1,6 +1,6 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package open_manipulator_bringup -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package open_manipulator_x_bringup +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 3.0.0 (2024-12-06) ------------------ From 6fd8cb434ebb613a3ba3f9ea57eae6be743d7ac1 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 13:10:26 +0900 Subject: [PATCH 18/20] Added gitignore Signed-off-by: Pyo --- .gitignore | 535 +++++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 518 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index 259148fa..95e39316 100644 --- a/.gitignore +++ b/.gitignore @@ -1,32 +1,533 @@ -# Prerequisites +############################## +# C +############################## + +## Prerequisites *.d -# Compiled Object files -*.slo -*.lo +## Object files *.o +*.ko *.obj +*.elf + +## Linker output +*.ilk + +### (reserved) +*.map +*.exp -# Precompiled Headers +## Precompiled Headers *.gch *.pch -# Compiled Dynamic libraries +## Libraries +*.lib + +### (reserved) +*.a +*.la +*.lo + +## Shared objects (inc. Windows DLLs) +*.dll *.so +*.so.* *.dylib -*.dll -# Fortran module files -*.mod +## Executables +*.exe +*.out +*.app +*.i*86 +*.x86_64 +*.hex + +## Debug files +*.dSYM/ +*.su +*.idb +*.pdb + +## Kernel Module Compile Results +*.mod* +*.cmd +.tmp_versions/ +modules.order +Module.symvers +Mkfile.old +dkms.conf + + +############################## +# C++ +############################## + +## Prerequisites +*.dpp + +## Compiled Object files +*.slo + +## Fortran module files *.smod -# Compiled Static libraries +## Compiled Static libraries *.lai -*.la -*.a -*.lib -# Executables -*.exe -*.out -*.app + +############################## +# Python +############################## + +## Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +## Distribution / packaging +.Python +build/ +develop-eggs/ +dist/ +downloads/ +eggs/ +.eggs/ + +### (reserved) lib/ +### (reserved) lib64/ +parts/ +sdist/ +var/ +wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +## PyInstaller +## Usually these files are written by a python script from a template +## before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +## Installer logs +pip-log.txt +pip-delete-this-directory.txt + +## Unit test / coverage reports +htmlcov/ +.tox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +.hypothesis/ +.pytest_cache/ + +## Translations +*.mo +*.pot + +## Django stuff: +*.log +local_settings.py +db.sqlite3 + +## Flask stuff: +instance/ +.webassets-cache + +## Scrapy stuff: +.scrapy + +## Sphinx documentation +docs/_build/ + +## PyBuilder +target/ + +## Jupyter Notebook +.ipynb_checkpoints + +## pyenv +.python-version + +## celery beat schedule file +celerybeat-schedule + +## SageMath parsed files +*.sage.py + +## Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +## Spyder project settings +.spyderproject +.spyproject + +## Rope project settings +.ropeproject + +## mkdocs documentation +/site + +## mypy +.mypy_cache/ + + +############################## +# Java +############################## + +## Compiled class file +*.class + +## Log file +*.log + +## BlueJ files +*.ctxt + +## Mobile Tools for Java (J2ME) +.mtj.tmp/ + +## Package Files +*.jar +*.war +*.nar +*.ear +*.zip +*.tar.gz +*.rar + +## virtual machine crash logs, see http://www.java.com/en/download/help/error_hotspot.xml +hs_err_pid* + + +############################## +# Android +############################## + +## Built application files +*.apk +*.ap_ + +## Files for the ART/Dalvik VM +*.dex + +## Generated files +bin/ +gen/ +out/ + +## Gradle files +.gradle/ +build/ + +## Local configuration file (sdk path, etc) +local.properties + +## Proguard folder generated by Eclipse +proguard/ + +## Android Studio Navigation editor temp files +.navigation/ + +## Android Studio captures folder +captures/ + +## IntelliJ +*.iml +.idea/workspace.xml +.idea/tasks.xml +.idea/gradle.xml +.idea/assetWizardSettings.xml +.idea/dictionaries +.idea/libraries +.idea/caches + +## Keystore files +## Uncomment the following line if you do not want to check your keystore files in. +*.jks + +## External native build folder generated in Android Studio 2.2 and later +.externalNativeBuild + +## Google Services (e.g. APIs or Firebase) +google-services.json + +## Freeline +freeline.py +freeline/ +freeline_project_description.json + +## fastlane +fastlane/report.xml +fastlane/Preview.html +fastlane/screenshots +fastlane/test_output +fastlane/readme.md + + +############################## +# CUDA +############################## +*.i +*.ii +*.gpu +*.ptx +*.cubin +*.fatbin + + +############################## +# CMAKE +############################## +CMakeCache.txt +CMakeFiles +CMakeScripts +Testing +cmake_install.cmake +install_manifest.txt +compile_commands.json +CTestTestfile.cmake + + +############################## +# QT +############################## + +## Qt-es +object_script.*.Release +object_script.*.Debug +*_plugin_import.cpp +/.qmake.cache +/.qmake.stash +*.pro.user +*.pro.user.* +*.qbs.user +*.qbs.user.* +*.moc +moc_*.cpp +moc_*.h +qrc_*.cpp +ui_*.h +*.qmlc +*.jsc + +### (reserved) +Makefile* +*build-* + +## Qt unit tests +target_wrapper.* + +## QtCreator +*.autosave + +## QtCreator Qml +*.qmlproject.user +*.qmlproject.user.* + +## QtCreator CMake +CMakeLists.txt.user* + + +############################## +# Node.js +############################## + +## Logs +logs +*.log +npm-debug.log* +yarn-debug.log* +yarn-error.log* +lerna-debug.log* + +## Diagnostic reports (https://nodejs.org/api/report.html) +report.[0-9]*.[0-9]*.[0-9]*.[0-9]*.json + +## Runtime data +pids +*.pid +*.seed +*.pid.lock + +## Directory for instrumented libs generated by jscoverage/JSCover +lib-cov + +## Coverage directory used by tools like istanbul +coverage +*.lcov + +## nyc test coverage +.nyc_output + +## Grunt intermediate storage (https://gruntjs.com/creating-plugins#storing-task-files) +.grunt + +## Bower dependency directory (https://bower.io/) +bower_components + +## node-waf configuration +.lock-wscript + +## Compiled binary addons (https://nodejs.org/api/addons.html) +build/Release + +## Dependency directories +node_modules/ +jspm_packages/ + +## TypeScript v1 declaration files +typings/ + +## TypeScript cache +*.tsbuildinfo + +## Optional npm cache directory +.npm + +## Optional eslint cache +.eslintcache + +## Microbundle cache +.rpt2_cache/ +.rts2_cache_cjs/ +.rts2_cache_es/ +.rts2_cache_umd/ + +## Optional REPL history +.node_repl_history + +## Output of 'npm pack' +*.tgz + +## Yarn Integrity file +.yarn-integrity + +## dotenv environment variables file +.env +.env.test + +## parcel-bundler cache (https://parceljs.org/) +.cache + +## Next.js build output +.next + +## Nuxt.js build / generate output +.nuxt +dist + +## Gatsby files +.cache/ + +## vuepress build output +.vuepress/dist + +## Serverless directories +.serverless/ + +## FuseBox cache +.fusebox/ + +## DynamoDB Local files +.dynamodb/ + +## TernJS port file +.tern-port + +## Yarn +.yarn/* +!.yarn/releases +!.yarn/plugins +!.yarn/sdks +.pnp.* + + +############################## +# Quasar +############################## + +.DS_Store +.thumbs.db +node_modules + +# Quasar core related directories +.quasar +/dist + +# Cordova related directories and files +/src-cordova/node_modules +/src-cordova/platforms +/src-cordova/plugins +/src-cordova/www + +# Capacitor related directories and files +/src-capacitor/www +/src-capacitor/node_modules + +# BEX related directories and files +/src-bex/www +/src-bex/js/core + +# Log files +npm-debug.log* +yarn-debug.log* +yarn-error.log* + +# Editor directories and files +.idea +*.suo +*.ntvs* +*.njsproj +*.sln + + +############################## +# ROS +############################## + +## Colcon/Catkin ignore files +CATKIN_IGNORE +AMENT_IGNORE +COLCON_IGNORE + + +############################## +# ETC. +############################## + +## Ignore generated docs +*.dox +*.wikidoc + +## doxygen +doc/ + +## IDE: vscode +.vscode/ + +## IDE: Emacs +.#* + +## IDE: VIM +*.swp + +## Setting files +*.settings + +## GurumDDS +*.ar From ed7044724dcaff28cc3553c9de89c88a991d1ef3 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 13:12:09 +0900 Subject: [PATCH 19/20] Removed system files Signed-off-by: Pyo --- open_manipulator_x_description/launch/.DS_Store | Bin 6148 -> 0 bytes .../__pycache__/model.launch.cpython-310.pyc | Bin 1782 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 open_manipulator_x_description/launch/.DS_Store delete mode 100644 open_manipulator_x_description/launch/__pycache__/model.launch.cpython-310.pyc diff --git a/open_manipulator_x_description/launch/.DS_Store b/open_manipulator_x_description/launch/.DS_Store deleted file mode 100644 index 8024f307f168e1589f2b2044b6bea2249d771535..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHKu}TCn5Pi`r4s5Q}E~S;AtxY)2!rBi|?-UeP_c(4vt@1m5rM{Pm;-1@RAtE!7 zyva;vCV8+)7C@MGSBJm^z?e-Kqzs76gRZSQSr|%=bKKz)_qfG!AkkkmY3+Mxx!!B8 z`}6ucG~COYz7_2&JaAXfIH2Vb>(i!P%$s)3Xi;~}w*Oj$8W6vc@rXH9sbl>GZa7-v zX(Rv3M*dR7J7>Tda0Z+KXW&N+@XnTucMQFE2AlzB;DZ6350OonDrUoSbg2L2fXVKh6NO}VLhw|?24?%K$9!6wnXEQLb7_XyxX g=g4kxnmxvGj@ukFD1$7kQitm`;`;$Zco zF!&OF^)m?WaFRL`>}g6eI-vxyo4OgBFpF8*$h?VXaU*SJ{=~P~1KgUlEN%jBPudpy zfIE|p#jUiP^(H-w+i5==Oa_2Ey!(?gS>rw4hknS(i8CI&0XELjn3&;rQI*e5UkEjm zNvV?}2j%7qF-s#U>bb9ETxBBHFwyyDeo*8*S=u!ZiY$wA4m173B7DKE%ofE;J9oJH#&}HXHhIpPJzS>sos4AfD8Wt z{AvpXC$AmtyduBQE9X}t+nk<&L*a_-XtBGmF4g`#k=c*1&psFD$xLV|#Qro=Vn2fe z)59OeuAzl8tOQ=h6BminYD^?Nrt_O4`_R1|Skt}Kx@xbyG^0d?=TVyQP#0mXevFKK z5R7&SBaF&57(5`)K!MR>$^Q;OJJ*itXu`=gA&#c8a}BSfT~4p@S}u2?Cz|Po_Bi8> z8xn&h_h8)QO+Y?x#bCod-oA1`!du5JSVQkE;MLLMwX3Q2IpqVs#)mg9V3&{hy2UNN z0d4b&Ca^z$kH3Gzbc=66dvxW-6f_@L%{JfG?JH#P;f)(Nkn8xPqs90j%7eVnLG6fO z9-WCm7E{n9dGIn)~F z!{m9gShVXWwwUhhEOw49)1a6K`cwq3AYgR>p&q13eik5EHKwM=#XPD~jloa}(|JHt z%StSEURL>XwbGFmL0L`HM4bv5!2Q8(T$oMEJhb!S{k$30>eW3x$WkHlF*D7wP7%`< z_ep2kWh8-_7E&3nsB~Fr)4(e|UC<|d%wzyTCoxh@D_Ul?lDjy5WN-Li4){i6cXiq9 zKK(m3z)ho+;2Mc_PBu|AP+%fR4+RFibWyZWU=9G0A>t7T)4~S}=SeEe8d4461Sl9* zW3#Uo1GL?aMJ}L{h-J>L2F52~pd<*7_}HjVDD6{9DDkYMxAYxt;&|EqV(;k2-5g;5 zZid~`Mvd_yn$)I+WJ0JB@~jrwmdsiy! z6|=F_tChm2YIj;sTJNG`&8W5vWuf-&wnp!ot(A#~+t_Eoie$;II2H?>wBGeUha9L< W@dXxHTU`B77mC?RgOTrcTmJyn9_Baz From 4db1027ddc538648f4706fc4a1d4f73c0ffeee84 Mon Sep 17 00:00:00 2001 From: Pyo Date: Fri, 13 Dec 2024 13:17:10 +0900 Subject: [PATCH 20/20] Modified the authors Signed-off-by: Pyo --- .../include/open_manipulator_x_gui/main_window.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp index 07a368a0..d93bec62 100644 --- a/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp +++ b/open_manipulator_x_gui/include/open_manipulator_x_gui/main_window.hpp @@ -1,5 +1,5 @@ /******************************************************************************* -* Copyright 2024 ROBOTIS CO., LTD. +* Copyright 2018 ROBOTIS CO., LTD. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,7 +14,7 @@ * limitations under the License. *******************************************************************************/ -/* Authors: Ryan Shim, Sungho Woo */ +/* Authors: Darby Lim, Hye-Jong KIM, Ryan Shim, Yong-Ho Na, Sungho Woo */ #ifndef OPEN_MANIPULATOR_X_GUI_MAIN_WINDOW_HPP #define OPEN_MANIPULATOR_X_GUI_MAIN_WINDOW_HPP