diff --git a/kuka_driver_examples/eci_demo/CMakeLists.txt b/kuka_driver_examples/eci_demo/CMakeLists.txt index 24e55cc1..80286dd1 100644 --- a/kuka_driver_examples/eci_demo/CMakeLists.txt +++ b/kuka_driver_examples/eci_demo/CMakeLists.txt @@ -19,14 +19,6 @@ find_package(moveit_visual_tools REQUIRED) find_package(rviz_visual_tools REQUIRED) find_package(moveit_msgs REQUIRED) -add_executable(moveit_circular src/MoveitCircular.cpp) -ament_target_dependencies(moveit_circular - moveit_ros_planning_interface - rclcpp - rviz_visual_tools - moveit_visual_tools -) - add_executable(moveit_basic_plan src/MoveitBasicPlan.cpp) ament_target_dependencies(moveit_basic_plan moveit_ros_planning_interface @@ -35,10 +27,6 @@ ament_target_dependencies(moveit_basic_plan moveit_visual_tools ) -install(TARGETS moveit_circular - EXPORT export_${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) install(TARGETS moveit_basic_plan EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME} diff --git a/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp b/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp index e3fee928..ac72feb8 100644 --- a/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp +++ b/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp @@ -23,6 +23,8 @@ #include "moveit_msgs/msg/collision_object.hpp" #include "moveit_visual_tools/moveit_visual_tools.h" +#define CIRCLE false + int main(int argc, char * argv[]) { @@ -77,27 +79,76 @@ int main(int argc, char * argv[]) planning_scene_interface.addCollisionObjects(collision_objects); // End Collision Objects define - std::vector waypoints; - moveit_msgs::msg::RobotTrajectory trajectory; - // Motion start - - // First circle faceing down - // Move to origin point - geometry_msgs::msg::Pose msg; - msg.orientation.x = 0.0; - msg.orientation.y = -sqrt(2.0) / 2.0; - msg.orientation.z = 0.0; - msg.orientation.w = sqrt(2.0) / 2.0; - msg.position.x = 0.1; - msg.position.y = 0.0; - msg.position.z = 0.8; - waypoints.push_back(msg); + // Add waypoints for planning + if (!CIRCLE) { + // Move to point near candle + geometry_msgs::msg::Pose msg; + msg.orientation.x = 0.0; + msg.orientation.y = -sqrt(2.0) / 2.0; + msg.orientation.z = 0.0; + msg.orientation.w = sqrt(2.0) / 2.0; + msg.position.x = 0.1; + msg.position.y = 0.0; + msg.position.z = 0.8; + waypoints.push_back(msg); + } else { + // First circle faceing down + // Move to origin point + 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.35; + msg.position.y = 0.0; + msg.position.z = 0.4; + waypoints.push_back(msg); + // Move circle + for (int i = 1; i < 63; i++) { + msg.position.y = 0.0 + sin(0.1 * i) * 0.18; + msg.position.x = 0.35 + cos(0.1 * i) * 0.18; + waypoints.push_back(msg); + } + + // Second circle faceing forward + // Move to origin point + msg.orientation.x = 0.0; + msg.orientation.y = -sqrt(2.0) / 2.0; + msg.orientation.z = 0.0; + msg.orientation.w = sqrt(2.0) / 2.0; + msg.position.x = 0.4; + msg.position.y = -0.2; + msg.position.z = 0.2; + waypoints.push_back(msg); + // Move circle + for (int i = 63; i > 0; i--) { + msg.position.y = -0.2 + sin(0.1 * i) * 0.15; + msg.position.z = 0.2 + cos(0.1 * i) * 0.15; + waypoints.push_back(msg); + } + + // Third cirlce faceing right + // Move to origin point + msg.orientation.x = sqrt(2.0) / 2.0; + msg.orientation.y = 0.0; + msg.orientation.z = 0.0; + msg.orientation.w = sqrt(2.0) / 2.0; + msg.position.x = 0.2; + msg.position.y = 0.2; + msg.position.z = 0.38; + waypoints.push_back(msg); + // Move circle + for (int i = 63; i > 0; i--) { + msg.position.x = 0.2 + sin(0.1 * i) * 0.18; + msg.position.z = 0.38 + cos(0.1 * i) * 0.25; + waypoints.push_back(msg); + } + } RCLCPP_INFO(logger, "Start planning"); - // Planning // move_group_interface.setPlannerId(""); double fraction = move_group_interface.computeCartesianPath(waypoints, 0.005, 0.0, trajectory); diff --git a/kuka_driver_examples/eci_demo/src/MoveitCircular.cpp b/kuka_driver_examples/eci_demo/src/MoveitCircular.cpp deleted file mode 100644 index 23a69f32..00000000 --- a/kuka_driver_examples/eci_demo/src/MoveitCircular.cpp +++ /dev/null @@ -1,150 +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 "rclcpp/rclcpp.hpp" -#include "moveit/move_group_interface/move_group_interface.h" -#include "moveit/planning_scene_interface/planning_scene_interface.h" -#include "moveit_msgs/msg/collision_object.hpp" -#include "moveit_visual_tools/moveit_visual_tools.h" - - -int main(int argc, char * argv[]) -{ - // Setup - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto const node = std::make_shared( - "moveit_circle", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) - ); - - // Create a ROS logger - auto const logger = rclcpp::get_logger("moveit_circle"); - - // Create Planning group - static const std::string PLANNING_GROUP = "iisy_arm"; - - // Next step goes here - // Create the MoveIt MoveGroup Interface - using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); - - // Create Planning Scene Interface, witch is for adding collision boxes - using moveit::planning_interface::PlanningSceneInterface; - auto planning_scene_interface = PlanningSceneInterface(); - - // Collision Objects define - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = move_group_interface.getPlanningFrame(); - collision_object.id = "box1"; - shape_msgs::msg::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[primitive.BOX_X] = 0.5; - primitive.dimensions[primitive.BOX_Y] = 0.5; - primitive.dimensions[primitive.BOX_Z] = 1.2; - - // Define a pose for the box (specified relative to frame_id). - geometry_msgs::msg::Pose stand_pose; - stand_pose.orientation.w = 1.0; - stand_pose.position.x = 0.0; - stand_pose.position.y = 0.0; - stand_pose.position.z = -0.6; - - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(stand_pose); - collision_object.operation = collision_object.ADD; - - std::vector collision_objects; - collision_objects.push_back(collision_object); - - planning_scene_interface.addCollisionObjects(collision_objects); - // End Collision Objects define - - - std::vector waypoints; - - moveit_msgs::msg::RobotTrajectory trajectory; - - // Motion start - - // First circle faceing down - // Move to origin point - 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.35; - msg.position.y = 0.0; - msg.position.z = 0.4; - waypoints.push_back(msg); - // Move circle - for (int i = 1; i < 63; i++) { - msg.position.y = 0.0 + sin(0.1 * i) * 0.18; - msg.position.x = 0.35 + cos(0.1 * i) * 0.18; - waypoints.push_back(msg); - } - - // Second circle faceing forward - // Move to origin point - msg.orientation.x = 0.0; - msg.orientation.y = -sqrt(2.0) / 2.0; - msg.orientation.z = 0.0; - msg.orientation.w = sqrt(2.0) / 2.0; - msg.position.x = 0.4; - msg.position.y = -0.2; - msg.position.z = 0.2; - waypoints.push_back(msg); - // Move circle - for (int i = 63; i > 0; i--) { - msg.position.y = -0.2 + sin(0.1 * i) * 0.15; - msg.position.z = 0.2 + cos(0.1 * i) * 0.15; - waypoints.push_back(msg); - } - - // Third cirlce faceing right - // Move to origin point - msg.orientation.x = sqrt(2.0) / 2.0; - msg.orientation.y = 0.0; - msg.orientation.z = 0.0; - msg.orientation.w = sqrt(2.0) / 2.0; - msg.position.x = 0.2; - msg.position.y = 0.2; - msg.position.z = 0.38; - waypoints.push_back(msg); - // Move circle - for (int i = 63; i > 0; i--) { - msg.position.x = 0.2 + sin(0.1 * i) * 0.18; - msg.position.z = 0.38 + cos(0.1 * i) * 0.25; - waypoints.push_back(msg); - } - - // Planing - // move_group_interface.setPlannerId(""); - double fraction = move_group_interface.computeCartesianPath(waypoints, 0.005, 0.0, trajectory); - - if (fraction < 0.1) {RCLCPP_ERROR(logger, "Planning failed!");} else { - move_group_interface.execute(trajectory); - } - - // Shutdown ROS - rclcpp::shutdown(); - return 0; -}