From 40ccd97e24f76e6196f88c2340cc5c787ae25aad Mon Sep 17 00:00:00 2001 From: Gergely Kovacs Date: Mon, 25 Nov 2024 18:24:39 +0100 Subject: [PATCH] change from axis positions to points, restructure constrained_planning examples --- examples/moveit_examples/CMakeLists.txt | 19 ++++++--- .../moveit_collision_avoidance_example.hpp | 6 ++- .../moveit_constrained_planning_example.hpp} | 41 +++++++++---------- .../moveit_examples/moveit_example.hpp | 8 ++-- .../moveit_collision_avoidance_example.cpp | 6 +-- .../moveit_constrained_planning_example.cpp | 37 +++++++++++++++++ .../moveit_depalletizing_example.cpp | 0 .../moveit_collision_avoidance_example.cpp | 4 +- .../moveit_constrained_planning_example.cpp | 37 +++++++++++++++++ 9 files changed, 119 insertions(+), 39 deletions(-) rename examples/moveit_examples/{src/moveit_constrained_planning_example.cpp => include/moveit_examples/moveit_constrained_planning_example.hpp} (57%) create mode 100644 examples/moveit_examples/src/iiqka/moveit_constrained_planning_example.cpp rename examples/moveit_examples/src/{ => iiqka}/moveit_depalletizing_example.cpp (100%) create mode 100644 examples/moveit_examples/src/kss/moveit_constrained_planning_example.cpp diff --git a/examples/moveit_examples/CMakeLists.txt b/examples/moveit_examples/CMakeLists.txt index 00d9bcbb..6802594f 100755 --- a/examples/moveit_examples/CMakeLists.txt +++ b/examples/moveit_examples/CMakeLists.txt @@ -36,14 +36,20 @@ ament_target_dependencies(kss_moveit_collision_avoidance_example moveit_visual_tools ) -add_executable(moveit_constrained_planning_example src/moveit_constrained_planning_example.cpp) -ament_target_dependencies(moveit_constrained_planning_example +add_executable(iiqka_moveit_constrained_planning_example src/iiqka/moveit_constrained_planning_example.cpp) +ament_target_dependencies(iiqka_moveit_constrained_planning_example moveit_ros_planning_interface moveit_visual_tools ) -add_executable(moveit_depalletizing_example src/moveit_depalletizing_example.cpp) -ament_target_dependencies(moveit_depalletizing_example +add_executable(kss_moveit_constrained_planning_example src/kss/moveit_constrained_planning_example.cpp) +ament_target_dependencies(kss_moveit_constrained_planning_example + moveit_ros_planning_interface + moveit_visual_tools +) + +add_executable(iiqka_moveit_depalletizing_example src/iiqka/moveit_depalletizing_example.cpp) +ament_target_dependencies(iiqka_moveit_depalletizing_example moveit_ros_planning_interface moveit_visual_tools ) @@ -53,8 +59,9 @@ install(TARGETS kss_moveit_basic_planners_example iiqka_moveit_collision_avoidance_example kss_moveit_collision_avoidance_example - moveit_constrained_planning_example - moveit_depalletizing_example + iiqka_moveit_constrained_planning_example + kss_moveit_constrained_planning_example + iiqka_moveit_depalletizing_example EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) diff --git a/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp b/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp index 614c4a8e..4d6b2332 100644 --- a/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp +++ b/examples/moveit_examples/include/moveit_examples/moveit_collision_avoidance_example.hpp @@ -21,7 +21,7 @@ #include "moveit_examples/moveit_example.hpp" void moveItCollisionAvoidanceExample(std::shared_ptr example_node, - const std::vector & start_coords, + const geometry_msgs::msg::Vector3 & 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) @@ -32,7 +32,9 @@ void moveItCollisionAvoidanceExample(std::shared_ptr example_node example_node->addRobotPlatform(); // Go to correct position for the example - auto init_trajectory = example_node->planToPosition(start_coords); + auto start_pos = + Eigen::Isometry3d(Eigen::Translation3d(start_coords.x, start_coords.y, start_coords.z) * Eigen::Quaterniond::Identity()); + auto init_trajectory = example_node->planToPoint(start_pos); if (init_trajectory != nullptr) { example_node->moveGroupInterface()->execute(*init_trajectory); diff --git a/examples/moveit_examples/src/moveit_constrained_planning_example.cpp b/examples/moveit_examples/include/moveit_examples/moveit_constrained_planning_example.hpp similarity index 57% rename from examples/moveit_examples/src/moveit_constrained_planning_example.cpp rename to examples/moveit_examples/include/moveit_examples/moveit_constrained_planning_example.hpp index 4818be70..a1985d6d 100644 --- a/examples/moveit_examples/src/moveit_constrained_planning_example.cpp +++ b/examples/moveit_examples/include/moveit_examples/moveit_constrained_planning_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,41 @@ // See the License for the specific language governing permissions and // limitations under the License. +#ifndef MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_ +#define MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_ + #include #include #include "moveit_examples/moveit_example.hpp" -int main(int argc, char * argv[]) +void moveItConstrainedPlanningExample(std::shared_ptr example_node, + const geometry_msgs::msg::Vector3 & start_coords, + const geometry_msgs::msg::Vector3 & end_goal, + const geometry_msgs::msg::Vector3 & collision_box_pos, + const geometry_msgs::msg::Vector3 & collision_box_size, + double constraint_tolerance) { - // 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.0017, -2.096, 1.514, 0.0012, -0.9888, -0.0029}); + auto start_pos = + Eigen::Isometry3d(Eigen::Translation3d(start_coords.x, start_coords.y, start_coords.z) * Eigen::Quaterniond::Identity()); + auto init_trajectory = example_node->planToPoint(start_pos); 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 cart_goal = - Eigen::Isometry3d(Eigen::Translation3d(0.4, -0.15, 0.55) * Eigen::Quaterniond::Identity()); + Eigen::Isometry3d(Eigen::Translation3d(end_goal.x, end_goal.y, end_goal.z) * Eigen::Quaterniond::Identity()); geometry_msgs::msg::Quaternion q; q.x = 0; @@ -57,18 +56,16 @@ int main(int argc, char * argv[]) example_node->moveGroupInterface()->setPlanningTime(30.0); - example_node->setOrientationConstraint(q); + example_node->setOrientationConstraint(q, constraint_tolerance); // Plan with collision avoidance auto planned_trajectory = - example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTkConfigDefault"); + example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTConnectkConfigDefault"); if (planned_trajectory != nullptr) { example_node->drawTrajectory(*planned_trajectory); example_node->addBreakPoint(); example_node->moveGroupInterface()->execute(*planned_trajectory); } - - // Shutdown ROS - rclcpp::shutdown(); - return 0; } + +#endif // MOVEIT_EXAMPLES__MOVEIT_CONSTRAINED_PLANNING_EXAMPLE_HPP_ \ No newline at end of file diff --git a/examples/moveit_examples/include/moveit_examples/moveit_example.hpp b/examples/moveit_examples/include/moveit_examples/moveit_example.hpp index 87220e06..efbd1bc9 100644 --- a/examples/moveit_examples/include/moveit_examples/moveit_example.hpp +++ b/examples/moveit_examples/include/moveit_examples/moveit_example.hpp @@ -255,16 +255,16 @@ class MoveitExample : public rclcpp::Node planning_scene_diff_publisher_->publish(planning_scene); } - void setOrientationConstraint(const geometry_msgs::msg::Quaternion & orientation) + void setOrientationConstraint(const geometry_msgs::msg::Quaternion & orientation, double tolerance) { moveit_msgs::msg::OrientationConstraint orientation_constraint; moveit_msgs::msg::Constraints constraints; orientation_constraint.header.frame_id = move_group_interface_->getPlanningFrame(); orientation_constraint.link_name = move_group_interface_->getEndEffectorLink(); orientation_constraint.orientation = orientation; - orientation_constraint.absolute_x_axis_tolerance = 0.2; - orientation_constraint.absolute_y_axis_tolerance = 0.2; - orientation_constraint.absolute_z_axis_tolerance = 0.2; + orientation_constraint.absolute_x_axis_tolerance = tolerance; + orientation_constraint.absolute_y_axis_tolerance = tolerance; + orientation_constraint.absolute_z_axis_tolerance = tolerance; orientation_constraint.weight = 1.0; constraints.orientation_constraints.emplace_back(orientation_constraint); diff --git a/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp b/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp index b89874a4..dd049dcd 100644 --- a/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp +++ b/examples/moveit_examples/src/iiqka/moveit_collision_avoidance_example.cpp @@ -25,10 +25,10 @@ int main(int argc, char * argv[]) 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.4).y(-0.15).z(0.55), 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)); + geometry_msgs::build().x(0.25).y(-0.075).z(0.675), + geometry_msgs::build().x(0.1).y(0.8).z(0.1)); // Shutdown ROS rclcpp::shutdown(); diff --git a/examples/moveit_examples/src/iiqka/moveit_constrained_planning_example.cpp b/examples/moveit_examples/src/iiqka/moveit_constrained_planning_example.cpp new file mode 100644 index 00000000..acb36411 --- /dev/null +++ b/examples/moveit_examples/src/iiqka/moveit_constrained_planning_example.cpp @@ -0,0 +1,37 @@ +// 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_constrained_planning_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(); + + moveItConstrainedPlanningExample(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.8).z(0.1), + 0.2); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/examples/moveit_examples/src/moveit_depalletizing_example.cpp b/examples/moveit_examples/src/iiqka/moveit_depalletizing_example.cpp similarity index 100% rename from examples/moveit_examples/src/moveit_depalletizing_example.cpp rename to examples/moveit_examples/src/iiqka/moveit_depalletizing_example.cpp diff --git a/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp b/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp index 395fc375..098ae1a7 100644 --- a/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp +++ b/examples/moveit_examples/src/kss/moveit_collision_avoidance_example.cpp @@ -25,10 +25,10 @@ int main(int argc, char * argv[]) std::thread([&executor]() { executor.spin(); }).detach(); moveItCollisionAvoidanceExample(example_node, - std::vector{0.2111, -0.9106, 1.2075, 0.0, -1.8677, -0.2111}, + geometry_msgs::build().x(0.7).y(-0.15).z(0.75), 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)); + geometry_msgs::build().x(0.1).y(0.8).z(0.1)); // Shutdown ROS rclcpp::shutdown(); diff --git a/examples/moveit_examples/src/kss/moveit_constrained_planning_example.cpp b/examples/moveit_examples/src/kss/moveit_constrained_planning_example.cpp new file mode 100644 index 00000000..43d84a8b --- /dev/null +++ b/examples/moveit_examples/src/kss/moveit_constrained_planning_example.cpp @@ -0,0 +1,37 @@ +// 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_constrained_planning_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(); + + moveItConstrainedPlanningExample(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.8).z(0.1), + 0.2); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} \ No newline at end of file