From 16cf499d3ae3dcb2996b3ac71f15e874069e6275 Mon Sep 17 00:00:00 2001 From: Aron Svastits Date: Wed, 25 Oct 2023 13:15:14 +0200 Subject: [PATCH] separate examples --- kuka_driver_examples/eci_demo/CMakeLists.txt | 20 ++++- .../eci_demo/include/moveit_example.h | 15 ++-- .../src/MoveitBasicPlannersExample.cpp | 25 +----- .../src/MoveitCollisionAvoidanceExample.cpp | 73 +++++++++++++++++ .../src/MoveitConstrainedPlanningExample.cpp | 79 +++++++++++++++++++ 5 files changed, 181 insertions(+), 31 deletions(-) create mode 100755 kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp create mode 100755 kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp diff --git a/kuka_driver_examples/eci_demo/CMakeLists.txt b/kuka_driver_examples/eci_demo/CMakeLists.txt index 67cd2676..bda91866 100755 --- a/kuka_driver_examples/eci_demo/CMakeLists.txt +++ b/kuka_driver_examples/eci_demo/CMakeLists.txt @@ -31,6 +31,24 @@ ament_target_dependencies(moveit_basic_planners_example kuka_driver_interfaces ) +add_executable(moveit_collision_avoidance_example src/MoveitCollisionAvoidanceExample.cpp) +ament_target_dependencies(moveit_collision_avoidance_example + moveit_ros_planning_interface + rclcpp + rviz_visual_tools + moveit_visual_tools + kuka_driver_interfaces +) + +add_executable(moveit_constrained_planning_example src/MoveitConstrainedPlanningExample.cpp) +ament_target_dependencies(moveit_constrained_planning_example + moveit_ros_planning_interface + rclcpp + rviz_visual_tools + moveit_visual_tools + kuka_driver_interfaces +) + add_executable(moveit_depalletizing_example src/MoveitDepalletizingExample.cpp) ament_target_dependencies(moveit_depalletizing_example moveit_ros_planning_interface @@ -40,7 +58,7 @@ ament_target_dependencies(moveit_depalletizing_example kuka_driver_interfaces ) -install(TARGETS moveit_basic_planners_example moveit_depalletizing_example +install(TARGETS moveit_basic_planners_example moveit_collision_avoidance_example moveit_constrained_planning_example moveit_depalletizing_example EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} ) diff --git a/kuka_driver_examples/eci_demo/include/moveit_example.h b/kuka_driver_examples/eci_demo/include/moveit_example.h index e8370750..7fe159d8 100755 --- a/kuka_driver_examples/eci_demo/include/moveit_example.h +++ b/kuka_driver_examples/eci_demo/include/moveit_example.h @@ -59,13 +59,15 @@ class MoveitExample : public rclcpp::Node planning_scene_diff_publisher_ = this->create_publisher( "planning_scene", 10); - move_group_interface_->setMaxVelocityScalingFactor(1.0); - move_group_interface_->setMaxAccelerationScalingFactor(1.0); + move_group_interface_->setMaxVelocityScalingFactor(0.1); + move_group_interface_->setMaxAccelerationScalingFactor(0.1); // Create collision box subscriber - collision_box_subscriber_ = - this->create_subscription("collision_box", 10, std::bind(&MoveitExample::addCollisionBox, this, std::placeholders::_1)); - + collision_box_subscriber_ = + this->create_subscription( + "collision_box", 10, std::bind( + &MoveitExample::addCollisionBox, this, + std::placeholders::_1)); } moveit_msgs::msg::RobotTrajectory::SharedPtr drawCircle() @@ -349,7 +351,8 @@ class MoveitExample : public rclcpp::Node protected: std::shared_ptr move_group_interface_; rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; - rclcpp::Subscription::SharedPtr collision_box_subscriber_; + rclcpp::Subscription::SharedPtr + collision_box_subscriber_; std::shared_ptr moveit_visual_tools_; const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan"); const std::string PLANNING_GROUP = "lbr_iisy_arm"; diff --git a/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp index eb2115e3..733d75c2 100755 --- a/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp +++ b/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp @@ -80,7 +80,7 @@ int main(int argc, char * argv[]) box_msg.size.x = 0.1; box_msg.size.y = 0.4; box_msg.size.z = 0.1; - + example_node->addCollisionBox(std::make_shared(box_msg)); example_node->addBreakPoint(); @@ -106,29 +106,6 @@ int main(int argc, char * argv[]) } example_node->addBreakPoint(); - // Plan with collision avoidance - planned_trajectory = example_node->planToPoint(standing_pose, "ompl", "RRTConnectkConfigDefault"); - if (planned_trajectory != nullptr) { - example_node->drawTrajectory(*planned_trajectory); - example_node->addBreakPoint(); - example_node->moveGroupInterface()->execute(*planned_trajectory); - } - example_node->addBreakPoint(); - - geometry_msgs::msg::Quaternion q; - q.x = 0; - q.y = 0; - q.z = 0; - q.w = 1; - example_node->setOrientationConstraint(q); - // Plan with collision avoidance - planned_trajectory = example_node->planToPoint(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; diff --git a/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp new file mode 100755 index 00000000..d26e37a1 --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp @@ -0,0 +1,73 @@ +// 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 + +#include "moveit_example.h" + +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(); + + // Add collision object + kuka_driver_interfaces::msg::CollisionBox box_msg; + box_msg.position.x = 0.25; + box_msg.position.y = -0.075; + box_msg.position.z = 0.675; + + box_msg.size.x = 0.1; + box_msg.size.y = 0.4; + box_msg.size.z = 0.1; + + example_node->addCollisionBox(std::make_shared(box_msg)); + example_node->addBreakPoint(); + + auto standing_pose = Eigen::Isometry3d( + Eigen::Translation3d( + 0.1, 0, + 0.8) * + Eigen::Quaterniond::Identity()); + + // Plan with collision avoidance + auto planned_trajectory = example_node->planToPoint( + standing_pose, "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; +} diff --git a/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp new file mode 100755 index 00000000..81408a46 --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp @@ -0,0 +1,79 @@ +// 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 + +#include "moveit_example.h" + +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(); + + // Add collision object + kuka_driver_interfaces::msg::CollisionBox box_msg; + box_msg.position.x = 0.25; + box_msg.position.y = -0.075; + box_msg.position.z = 0.675; + + box_msg.size.x = 0.1; + box_msg.size.y = 0.4; + box_msg.size.z = 0.1; + + example_node->addCollisionBox(std::make_shared(box_msg)); + example_node->addBreakPoint(); + + auto cart_goal = Eigen::Isometry3d( + Eigen::Translation3d( + 0.4, -0.15, + 0.55) * + Eigen::Quaterniond::Identity()); + + geometry_msgs::msg::Quaternion q; + q.x = 0; + q.y = 0; + q.z = 0; + q.w = 1; + + example_node->setOrientationConstraint(q); + // Plan with collision avoidance + auto planned_trajectory = + example_node->planToPoint(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; +}