From 5c9b14c298e514c6183b5f89d4b3dbad80b6c342 Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Tue, 12 Nov 2024 17:27:13 +0100 Subject: [PATCH] start migrating to common package - first example worked, second not yet --- examples/moveit_examples/CMakeLists.txt | 26 +++-- .../moveit_basic_planners_example.hpp | 94 ++++++++++++++++++ .../moveit_collision_avoidance_example.hpp} | 34 +++---- ...> iiqka_moveit_planning_example.launch.py} | 6 +- ... => kss_moveit_planning_example.launch.py} | 2 +- .../iiqka/moveit_basic_planners_example.cpp | 36 +++++++ .../moveit_collision_avoidance_example.cpp | 36 +++++++ .../src/kss/moveit_basic_planners_example.cpp | 36 +++++++ .../moveit_collision_avoidance_example.cpp | 36 +++++++ .../src/moveit_basic_planners_example.cpp | 99 ------------------- 10 files changed, 277 insertions(+), 128 deletions(-) create mode 100644 examples/moveit_examples/include/moveit_examples/moveit_basic_planners_example.hpp rename examples/moveit_examples/{src/moveit_collision_avoidance_example.cpp => include/moveit_examples/moveit_collision_avoidance_example.hpp} (61%) rename examples/moveit_examples/launch/{moveit_planning_example.launch.py => iiqka_moveit_planning_example.launch.py} (93%) rename examples/moveit_examples/launch/{moveit_rsi_planning_example.launch.py => kss_moveit_planning_example.launch.py} (98%) create mode 100644 examples/moveit_examples/src/iiqka/moveit_basic_planners_example.cpp create mode 100644 examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp create mode 100644 examples/moveit_examples/src/kss/moveit_basic_planners_example.cpp create mode 100644 examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp delete mode 100644 examples/moveit_examples/src/moveit_basic_planners_example.cpp diff --git a/examples/moveit_examples/CMakeLists.txt b/examples/moveit_examples/CMakeLists.txt index da52ba45..00d9bcbb 100755 --- a/examples/moveit_examples/CMakeLists.txt +++ b/examples/moveit_examples/CMakeLists.txt @@ -12,14 +12,26 @@ find_package(moveit_visual_tools REQUIRED) include_directories(include) -add_executable(moveit_basic_planners_example src/moveit_basic_planners_example.cpp) -ament_target_dependencies(moveit_basic_planners_example +add_executable(iiqka_moveit_basic_planners_example src/iiqka/moveit_basic_planners_example.cpp) +ament_target_dependencies(iiqka_moveit_basic_planners_example moveit_ros_planning_interface moveit_visual_tools ) -add_executable(moveit_collision_avoidance_example src/moveit_collision_avoidance_example.cpp) -ament_target_dependencies(moveit_collision_avoidance_example +add_executable(kss_moveit_basic_planners_example src/kss/moveit_basic_planners_example.cpp) +ament_target_dependencies(kss_moveit_basic_planners_example + moveit_ros_planning_interface + moveit_visual_tools +) + +add_executable(iiqka_moveit_collision_avoidance_example src/iiqka/moveit_collision_avoidance_example.cpp) +ament_target_dependencies(iiqka_moveit_collision_avoidance_example + moveit_ros_planning_interface + moveit_visual_tools +) + +add_executable(kss_moveit_collision_avoidance_example src/kss/moveit_collision_avoidance_example.cpp) +ament_target_dependencies(kss_moveit_collision_avoidance_example moveit_ros_planning_interface moveit_visual_tools ) @@ -37,8 +49,10 @@ ament_target_dependencies(moveit_depalletizing_example ) install(TARGETS - moveit_basic_planners_example - moveit_collision_avoidance_example + iiqka_moveit_basic_planners_example + kss_moveit_basic_planners_example + iiqka_moveit_collision_avoidance_example + kss_moveit_collision_avoidance_example moveit_constrained_planning_example moveit_depalletizing_example EXPORT export_${PROJECT_NAME} diff --git a/examples/moveit_examples/include/moveit_examples/moveit_basic_planners_example.hpp b/examples/moveit_examples/include/moveit_examples/moveit_basic_planners_example.hpp new file mode 100644 index 00000000..628fac91 --- /dev/null +++ b/examples/moveit_examples/include/moveit_examples/moveit_basic_planners_example.hpp @@ -0,0 +1,94 @@ +// Copyright 2024 Gergely Kovács +// +// 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. + +#ifndef MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_ +#define MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_ + +#include + +#include + +#include "moveit_examples/moveit_example.hpp" + +void moveItBasicPlannersExample(std::shared_ptr example_node, + const geometry_msgs::msg::Vector3 & standing_pose_coords, + const geometry_msgs::msg::Vector3 & lin_goal_coords, + const geometry_msgs::msg::Vector3 & collision_box_pos, + const geometry_msgs::msg::Vector3 & collision_box_size) +{ + example_node->initialize(); + example_node->addBreakPoint(); + + // Add robot platform + example_node->addRobotPlatform(); + + // Pilz PTP planner + auto standing_pose = + Eigen::Isometry3d(Eigen::Translation3d(standing_pose_coords.x, standing_pose_coords.y, standing_pose_coords.z) * Eigen::Quaterniond::Identity()); + + auto planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); + if (planned_trajectory != nullptr) + { + example_node->drawTrajectory(*planned_trajectory); + example_node->addBreakPoint(); + example_node->moveGroupInterface()->execute(*planned_trajectory); + } + example_node->addBreakPoint(); + + // Pilz LIN planner + auto cart_goal = + Eigen::Isometry3d(Eigen::Translation3d(lin_goal_coords.x, lin_goal_coords.y, lin_goal_coords.z) * Eigen::Quaterniond::Identity()); + planned_trajectory = + example_node->planToPoint(cart_goal, "pilz_industrial_motion_planner", "LIN"); + if (planned_trajectory != nullptr) + { + example_node->drawTrajectory(*planned_trajectory); + example_node->addBreakPoint(); + example_node->moveGroupInterface()->execute(*planned_trajectory); + } + example_node->addBreakPoint(); + + // Add collision object + example_node->addCollisionBox(collision_box_pos, collision_box_size); + example_node->addBreakPoint(); + + // Try moving back with Pilz LIN + planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "LIN"); + if (planned_trajectory != nullptr) + { + example_node->drawTrajectory(*planned_trajectory); + } + else + { + example_node->drawTitle("Failed planning with Pilz LIN"); + } + example_node->addBreakPoint(); + + // Try moving back with Pilz PTP + planned_trajectory = + example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); + if (planned_trajectory != nullptr) + { + example_node->drawTrajectory(*planned_trajectory); + } + else + { + example_node->drawTitle("Failed planning with Pilz PTP"); + } + example_node->addBreakPoint(); +} + +#endif // MOVEIT_EXAMPLES__MOVEIT_BASIC_PLANNERS_EXAMPLE_HPP_ diff --git a/examples/moveit_examples/src/moveit_collision_avoidance_example.cpp b/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp similarity index 61% rename from examples/moveit_examples/src/moveit_collision_avoidance_example.cpp rename to examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp index 581114e2..614c4a8e 100644 --- a/examples/moveit_examples/src/moveit_collision_avoidance_example.cpp +++ b/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Áron Svastits +// Copyright 2024 Gergely Kovács // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,42 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_ +#define MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_ + #include #include #include "moveit_examples/moveit_example.hpp" -int main(int argc, char * argv[]) +void moveItCollisionAvoidanceExample(std::shared_ptr example_node, + const std::vector & start_coords, + const geometry_msgs::msg::Vector3 & standing_pose_coords, + const geometry_msgs::msg::Vector3 & collision_box_pos, + const geometry_msgs::msg::Vector3 & collision_box_size) { - // Setup - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto const example_node = std::make_shared(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(example_node); - std::thread([&executor]() { executor.spin(); }).detach(); - example_node->initialize(); // Add robot platform example_node->addRobotPlatform(); // Go to correct position for the example - auto init_trajectory = example_node->planToPosition( - std::vector{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587}); + auto init_trajectory = example_node->planToPosition(start_coords); if (init_trajectory != nullptr) { example_node->moveGroupInterface()->execute(*init_trajectory); } // Add collision object - example_node->addCollisionBox( - geometry_msgs::build().x(0.125).y(0.15).z(0.5), - geometry_msgs::build().x(0.1).y(1.0).z(0.1)); + example_node->addCollisionBox(collision_box_pos, collision_box_size); example_node->addBreakPoint(); auto standing_pose = - Eigen::Isometry3d(Eigen::Translation3d(0.1, 0, 0.8) * Eigen::Quaterniond::Identity()); + Eigen::Isometry3d(Eigen::Translation3d(standing_pose_coords.x, standing_pose_coords.y, standing_pose_coords.z) * Eigen::Quaterniond::Identity()); // Plan with collision avoidance auto planned_trajectory = @@ -58,8 +54,6 @@ int main(int argc, char * argv[]) example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); } - - // Shutdown ROS - rclcpp::shutdown(); - return 0; } + +#endif // MOVEIT_EXAMPLES__MOVEIT_COLLISION_AVOIDANCE_EXAMPLE_HPP_ \ No newline at end of file diff --git a/examples/moveit_examples/launch/moveit_planning_example.launch.py b/examples/moveit_examples/launch/iiqka_moveit_planning_example.launch.py similarity index 93% rename from examples/moveit_examples/launch/moveit_planning_example.launch.py rename to examples/moveit_examples/launch/iiqka_moveit_planning_example.launch.py index e8591da7..1ad89912 100644 --- a/examples/moveit_examples/launch/moveit_planning_example.launch.py +++ b/examples/moveit_examples/launch/iiqka_moveit_planning_example.launch.py @@ -58,8 +58,10 @@ def launch_setup(context, *args, **kwargs): get_package_share_directory("kuka_lbr_iisy_moveit_config") + f"/urdf/{robot_model.perform(context)}.srdf" ) - .robot_description_kinematics(file_path="config/kinematics.yaml") - .trajectory_execution(file_path="config/moveit_controllers.yaml") + .robot_description_kinematics(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config") + + f"/config/kinematics.yaml") + .trajectory_execution(file_path=get_package_share_directory("kuka_lbr_iisy_moveit_config") + + f"/config/moveit_controllers.yaml") .planning_scene_monitor( publish_robot_description=True, publish_robot_description_semantic=True ) diff --git a/examples/moveit_examples/launch/moveit_rsi_planning_example.launch.py b/examples/moveit_examples/launch/kss_moveit_planning_example.launch.py similarity index 98% rename from examples/moveit_examples/launch/moveit_rsi_planning_example.launch.py rename to examples/moveit_examples/launch/kss_moveit_planning_example.launch.py index 687f562e..16dcc5b7 100644 --- a/examples/moveit_examples/launch/moveit_rsi_planning_example.launch.py +++ b/examples/moveit_examples/launch/kss_moveit_planning_example.launch.py @@ -107,7 +107,7 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): launch_arguments = [] launch_arguments.append(DeclareLaunchArgument( - "robot_model", default_value="kr6_r700_sixx")) + "robot_model", default_value="kr6_r900_sixx")) launch_arguments.append(DeclareLaunchArgument( "namespace", default_value="")) launch_arguments.append(DeclareLaunchArgument("x", default_value="0")) diff --git a/examples/moveit_examples/src/iiqka/moveit_basic_planners_example.cpp b/examples/moveit_examples/src/iiqka/moveit_basic_planners_example.cpp new file mode 100644 index 00000000..eb411d62 --- /dev/null +++ b/examples/moveit_examples/src/iiqka/moveit_basic_planners_example.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 Gergely Kovács +// +// 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. + +#include "moveit_examples/moveit_basic_planners_example.hpp" + +int main(int argc, char * argv[]) +{ + // Setup + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const example_node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(example_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + moveItBasicPlannersExample(example_node, + geometry_msgs::build().x(0.1).y(0).z(0.8), + geometry_msgs::build().x(0.4).y(-0.15).z(0.55), + geometry_msgs::build().x(0.25).y(-0.075).z(0.675), + geometry_msgs::build().x(0.1).y(0.4).z(0.1)); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp b/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp new file mode 100644 index 00000000..b89874a4 --- /dev/null +++ b/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp @@ -0,0 +1,36 @@ +// Copyright 2022 Áron Svastits +// +// 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. + +#include "moveit_examples/moveit_collision_avoidance_example.hpp" + +int main(int argc, char * argv[]) +{ + // Setup + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const example_node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(example_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + moveItCollisionAvoidanceExample(example_node, + std::vector{0.3587, 0.3055, -1.3867, 0.0, -0.4896, -0.3587}, + geometry_msgs::build().x(0.1).y(0).z(0.8), + geometry_msgs::build().x(0.125).y(0.15).z(0.5), + geometry_msgs::build().x(0.1).y(1.0).z(0.1)); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/examples/moveit_examples/src/kss/moveit_basic_planners_example.cpp b/examples/moveit_examples/src/kss/moveit_basic_planners_example.cpp new file mode 100644 index 00000000..027a47d9 --- /dev/null +++ b/examples/moveit_examples/src/kss/moveit_basic_planners_example.cpp @@ -0,0 +1,36 @@ +// Copyright 2024 Gergely Kovács +// +// 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. + +#include "moveit_examples/moveit_basic_planners_example.hpp" + +int main(int argc, char * argv[]) +{ + // Setup + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const example_node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(example_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + moveItBasicPlannersExample(example_node, + geometry_msgs::build().x(0.1).y(0).z(1.2), + geometry_msgs::build().x(0.7).y(-0.15).z(0.75), + geometry_msgs::build().x(0.3).y(-0.075).z(1), + geometry_msgs::build().x(0.1).y(0.4).z(0.1)); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp b/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp new file mode 100644 index 00000000..555c7da1 --- /dev/null +++ b/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp @@ -0,0 +1,36 @@ +// Copyright 2022 Áron Svastits +// +// 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. + +#include "moveit_examples/moveit_collision_avoidance_example.hpp" + +int main(int argc, char * argv[]) +{ + // Setup + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const example_node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(example_node); + std::thread([&executor]() { executor.spin(); }).detach(); + + moveItCollisionAvoidanceExample(example_node, + std::vector{0.2111, -0.9106, 0.0, -1.8677, 1.2075, -0.2111}, + geometry_msgs::build().x(0.1).y(0).z(1.2), + geometry_msgs::build().x(0.3).y(-0.075).z(1), + geometry_msgs::build().x(0.1).y(1.0).z(0.1)); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/examples/moveit_examples/src/moveit_basic_planners_example.cpp b/examples/moveit_examples/src/moveit_basic_planners_example.cpp deleted file mode 100644 index d47877a3..00000000 --- a/examples/moveit_examples/src/moveit_basic_planners_example.cpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2022 Áron Svastits -// -// 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. - -#include - -#include - -#include "moveit_examples/moveit_example.hpp" - -int main(int argc, char * argv[]) -{ - // Setup - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto const example_node = std::make_shared(); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(example_node); - std::thread([&executor]() { executor.spin(); }).detach(); - - example_node->initialize(); - example_node->addBreakPoint(); - - // Add robot platform - example_node->addRobotPlatform(); - - // Pilz PTP planner - auto standing_pose = - Eigen::Isometry3d(Eigen::Translation3d(0.25, 0, 0.85) * Eigen::Quaterniond::Identity()); - - auto planned_trajectory = - example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); - if (planned_trajectory != nullptr) - { - example_node->drawTrajectory(*planned_trajectory); - example_node->addBreakPoint(); - example_node->moveGroupInterface()->execute(*planned_trajectory); - } - example_node->addBreakPoint(); - - // Pilz LIN planner - auto cart_goal = - Eigen::Isometry3d(Eigen::Translation3d(0.47, 0, 0.75) * Eigen::Quaterniond::Identity()); - planned_trajectory = - example_node->planToPoint(cart_goal, "pilz_industrial_motion_planner", "LIN"); - if (planned_trajectory != nullptr) - { - example_node->drawTrajectory(*planned_trajectory); - example_node->addBreakPoint(); - example_node->moveGroupInterface()->execute(*planned_trajectory); - } - example_node->addBreakPoint(); - - // // Add collision object - // example_node->addCollisionBox( - // geometry_msgs::build().x(0.225).y(-0.075).z(0.6), - // geometry_msgs::build().x(0.1).y(0.4).z(0.1)); - // example_node->addBreakPoint(); - - // // Try moving back with Pilz LIN - // planned_trajectory = - // example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "LIN"); - // if (planned_trajectory != nullptr) - // { - // example_node->drawTrajectory(*planned_trajectory); - // } - // else - // { - // example_node->drawTitle("Failed planning with Pilz LIN"); - // } - // example_node->addBreakPoint(); - - // // Try moving back with Pilz PTP - // planned_trajectory = - // example_node->planToPoint(standing_pose, "pilz_industrial_motion_planner", "PTP"); - // if (planned_trajectory != nullptr) - // { - // example_node->drawTrajectory(*planned_trajectory); - // } - // else - // { - // example_node->drawTitle("Failed planning with Pilz PTP"); - // } - // example_node->addBreakPoint(); - - // Shutdown ROS - rclcpp::shutdown(); - return 0; -}