diff --git a/kuka_driver_examples/eci_demo/CMakeLists.txt b/kuka_driver_examples/eci_demo/CMakeLists.txt old mode 100644 new mode 100755 index 80286dd1..d8e63c0d --- a/kuka_driver_examples/eci_demo/CMakeLists.txt +++ b/kuka_driver_examples/eci_demo/CMakeLists.txt @@ -18,16 +18,51 @@ find_package(rclcpp REQUIRED) find_package(moveit_visual_tools REQUIRED) find_package(rviz_visual_tools REQUIRED) find_package(moveit_msgs REQUIRED) +find_package(kuka_driver_interfaces REQUIRED) -add_executable(moveit_basic_plan src/MoveitBasicPlan.cpp) -ament_target_dependencies(moveit_basic_plan +include_directories(include) + +add_executable(moveit_basic_planners_example src/MoveitBasicPlannersExample.cpp) +ament_target_dependencies(moveit_basic_planners_example + moveit_ros_planning_interface + rclcpp + rviz_visual_tools + moveit_visual_tools + 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 rclcpp rviz_visual_tools moveit_visual_tools + kuka_driver_interfaces ) -install(TARGETS moveit_basic_plan +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/config/dummy_publisher.yaml b/kuka_driver_examples/eci_demo/config/dummy_publisher.yaml index dedf10ba..8c2bce81 100644 --- a/kuka_driver_examples/eci_demo/config/dummy_publisher.yaml +++ b/kuka_driver_examples/eci_demo/config/dummy_publisher.yaml @@ -5,10 +5,14 @@ wait_sec_between_publish: 6 goal_names: ["pos1", "pos2", "pos3", "pos4"] - pos1: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0] - pos2: [0.0, -1.37, 0.0, 0.0, 0.0, 0.0] - pos3: [0.2, -1.57, 0.2, 0.2, 0.2, 0.2] - pos4: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2] + pos1: + positions: [0.0, -1.57, 0.0, 0.0, 0.0, 0.0] + pos2: + positions: [0.0, -1.37, 0.0, 0.0, 0.0, 0.0] + pos3: + positions: [0.2, -1.57, 0.2, 0.2, 0.2, 0.2] + pos4: + positions: [-0.2, -1.57, -0.2, -0.2, -0.2, -0.2] joints: - joint_a1 @@ -18,4 +22,4 @@ - joint_a5 - joint_a6 - check_starting_point: false \ No newline at end of file + check_starting_point: false diff --git a/kuka_driver_examples/eci_demo/include/eci_demo/moveit_example.hpp b/kuka_driver_examples/eci_demo/include/eci_demo/moveit_example.hpp new file mode 100755 index 00000000..4a6a3b02 --- /dev/null +++ b/kuka_driver_examples/eci_demo/include/eci_demo/moveit_example.hpp @@ -0,0 +1,347 @@ +// 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. + +#ifndef ECI_DEMO__MOVEIT_EXAMPLE_HPP_ +#define ECI_DEMO__MOVEIT_EXAMPLE_HPP_ + +#include + +#include +#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" +#include "geometry_msgs/msg/vector3.hpp" + +class MoveitExample : public rclcpp::Node +{ +public: + MoveitExample() + : rclcpp::Node("moveit_example") + { + } + + void initialize() + { + move_group_interface_ = std::make_shared( + shared_from_this(), + PLANNING_GROUP); + + moveit_visual_tools_ = std::make_shared( + shared_from_this(), "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, + move_group_interface_->getRobotModel()); + + moveit_visual_tools_->deleteAllMarkers(); + moveit_visual_tools_->loadRemoteControl(); + moveit_visual_tools_->trigger(); + + planning_scene_diff_publisher_ = this->create_publisher( + "planning_scene", 10); + + move_group_interface_->setMaxVelocityScalingFactor(0.1); + move_group_interface_->setMaxAccelerationScalingFactor(0.1); + } + + moveit_msgs::msg::RobotTrajectory::SharedPtr drawCircle() + { + std::vector waypoints; + moveit_msgs::msg::RobotTrajectory trajectory; + geometry_msgs::msg::Pose msg; + + // circle facing forward + msg.orientation.x = 0.0; + msg.orientation.y = sqrt(2) / 2; + msg.orientation.z = 0.0; + msg.orientation.w = sqrt(2) / 2; + msg.position.x = 0.4; + // Define waypoints in a circle + for (int i = 0; i < 63; i++) { + msg.position.y = -0.2 + sin(0.1 * i) * 0.15; + msg.position.z = 0.4 + cos(0.1 * i) * 0.15; + waypoints.push_back(msg); + } + + RCLCPP_INFO(LOGGER, "Start planning"); + double fraction = + move_group_interface_->computeCartesianPath(waypoints, 0.005, 0.0, trajectory); + RCLCPP_INFO(LOGGER, "Planning done!"); + + if (fraction < 1) { + RCLCPP_ERROR(LOGGER, "Could not compute trajectory through all waypoints!"); + return nullptr; + } else { + return std::make_shared(trajectory); + } + } + + moveit_msgs::msg::RobotTrajectory::SharedPtr planToPoint( + const Eigen::Isometry3d & pose, + const std::string & planning_pipeline = "pilz_industrial_motion_planner", + const std::string & planner_id = "PTP") + { + // Create planning request using pilz industrial motion planner + move_group_interface_->setPlanningPipelineId(planning_pipeline); + move_group_interface_->setPlannerId(planner_id); + move_group_interface_->setPoseTarget(pose); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + RCLCPP_INFO(LOGGER, "Sending planning request"); + if (!move_group_interface_->plan(plan)) { + RCLCPP_INFO(LOGGER, "Planning failed"); + return nullptr; + } else { + RCLCPP_INFO(LOGGER, "Planning successful"); + return std::make_shared(plan.trajectory_); + } + } + + moveit_msgs::msg::RobotTrajectory::SharedPtr planToPosition( + const std::vector & joint_pos) + { + move_group_interface_->setJointValueTarget(joint_pos); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + RCLCPP_INFO(LOGGER, "Sending planning request"); + if (!move_group_interface_->plan(plan)) { + RCLCPP_INFO(LOGGER, "Planning failed"); + return nullptr; + } else { + RCLCPP_INFO(LOGGER, "Planning successful"); + return std::make_shared(plan.trajectory_); + } + } + + moveit_msgs::msg::RobotTrajectory::SharedPtr planToPointUntilSuccess( + const Eigen::Isometry3d & pose, + const std::string & planning_pipeline = "pilz_industrial_motion_planner", + const std::string & planner_id = "PTP") + { + // Create planning request using given motion planner + move_group_interface_->setPlanningPipelineId(planning_pipeline); + move_group_interface_->setPlannerId(planner_id); + move_group_interface_->setPoseTarget(pose); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + RCLCPP_INFO(LOGGER, "Sending planning request"); + moveit::core::MoveItErrorCode err_code; + auto start = std::chrono::high_resolution_clock::now(); + do{ + RCLCPP_INFO(LOGGER, "Planning ..."); + err_code = move_group_interface_->plan(plan); + } while (err_code != moveit::core::MoveItErrorCode::SUCCESS); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(stop - start); + RCLCPP_INFO(LOGGER, "Planning successful after %li ms", duration.count()); + return std::make_shared(plan.trajectory_); + } + + void AddObject(const moveit_msgs::msg::CollisionObject & object) + { + moveit_msgs::msg::PlanningScene planning_scene; + planning_scene.name = "scene"; + planning_scene.world.collision_objects.push_back(object); + planning_scene.is_diff = true; + planning_scene_diff_publisher_->publish(planning_scene); + } + + void addRobotPlatform() + { + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = move_group_interface_->getPlanningFrame(); + collision_object.id = "robot_stand"; + 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_pose1; + stand_pose1.orientation.w = 1.0; + stand_pose1.position.x = 0.0; + stand_pose1.position.y = 0.0; + stand_pose1.position.z = -0.6; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(stand_pose1); + collision_object.operation = collision_object.ADD; + + AddObject(collision_object); + } + + void addCollisionBox( + const geometry_msgs::msg::Vector3 & position, + const geometry_msgs::msg::Vector3 & size) + { + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = move_group_interface_->getPlanningFrame(); + collision_object.id = "collision_box"; + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[primitive.BOX_X] = size.x; + primitive.dimensions[primitive.BOX_Y] = size.y; + primitive.dimensions[primitive.BOX_Z] = size.z; + + // 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 = position.x; + stand_pose.position.y = position.y; + stand_pose.position.z = position.z; + + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(stand_pose); + collision_object.operation = collision_object.ADD; + + AddObject(collision_object); + } + + void addPalletObjects() + { + for (int k = 0; k < 3; k++) { + for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) { + moveit_msgs::msg::CollisionObject pallet_object; + pallet_object.header.frame_id = move_group_interface_->getPlanningFrame(); + + pallet_object.id = "pallet_" + std::to_string(9 * k + 3 * j + i); + shape_msgs::msg::SolidPrimitive primitive; + primitive.type = primitive.BOX; + primitive.dimensions.resize(3); + primitive.dimensions[primitive.BOX_X] = 0.097; + primitive.dimensions[primitive.BOX_Y] = 0.097; + primitive.dimensions[primitive.BOX_Z] = 0.097; + + // 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.3 + i * 0.1; + stand_pose.position.y = -0.1 + j * 0.1; + stand_pose.position.z = 0.3 - 0.1 * k; + + pallet_object.primitives.push_back(primitive); + pallet_object.primitive_poses.push_back(stand_pose); + pallet_object.operation = pallet_object.ADD; + + AddObject(pallet_object); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + } + } + + void AttachObject(const std::string & object_id) + { + moveit_msgs::msg::PlanningScene planning_scene; + planning_scene.name = "scene"; + moveit_msgs::msg::AttachedCollisionObject attached_object; + + attached_object.link_name = "flange"; + attached_object.object.id = object_id; + + // Carry out the REMOVE + ATTACH operation + RCLCPP_INFO(LOGGER, "Attaching the object to the hand and removing it from the world."); + planning_scene.robot_state.attached_collision_objects.push_back(attached_object); + planning_scene.robot_state.is_diff = true; + planning_scene.is_diff = true; + planning_scene_diff_publisher_->publish(planning_scene); + } + + void DetachAndRemoveObject(const std::string & object_id) + { + moveit_msgs::msg::PlanningScene planning_scene; + planning_scene.name = "scene"; + moveit_msgs::msg::AttachedCollisionObject attached_object; + + attached_object.link_name = "flange"; + attached_object.object.id = object_id; + attached_object.object.operation = attached_object.object.REMOVE; + + // Carry out the DETACH operation + RCLCPP_INFO(LOGGER, "Detaching the object from the hand"); + planning_scene.robot_state.attached_collision_objects.push_back(attached_object); + planning_scene.robot_state.is_diff = true; + planning_scene.world.collision_objects.push_back(attached_object.object); + planning_scene.is_diff = true; + planning_scene_diff_publisher_->publish(planning_scene); + } + + void setOrientationConstraint(const geometry_msgs::msg::Quaternion & orientation) + { + 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.weight = 1.0; + + constraints.orientation_constraints.emplace_back(orientation_constraint); + move_group_interface_->setPathConstraints(constraints); + } + + void clearConstraints() + { + move_group_interface_->clearPathConstraints(); + } + + void drawTrajectory(const moveit_msgs::msg::RobotTrajectory & trajectory) + { + moveit_visual_tools_->deleteAllMarkers(); + moveit_visual_tools_->publishTrajectoryLine( + trajectory, + moveit_visual_tools_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)); + } + + void drawTitle(const std::string & text) + { + auto const text_pose = [] + { + auto msg = Eigen::Isometry3d::Identity(); + msg.translation().z() = 1.0; + return msg; + } (); + moveit_visual_tools_->publishText( + text_pose, text, rviz_visual_tools::RED, + rviz_visual_tools::XXLARGE); + } + + void addBreakPoint() + { + moveit_visual_tools_->trigger(); + moveit_visual_tools_->prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); + } + + std::shared_ptr moveGroupInterface() + { + return move_group_interface_; + } + +protected: + std::shared_ptr move_group_interface_; + rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; + std::shared_ptr moveit_visual_tools_; + const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan"); + const std::string PLANNING_GROUP = "lbr_iisy_arm"; +}; + +#endif // ECI_DEMO__MOVEIT_EXAMPLE_HPP_ diff --git a/kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.py b/kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.launch.py similarity index 96% rename from kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.py rename to kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.launch.py index 5eab7a22..dc5b298c 100644 --- a/kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.py +++ b/kuka_driver_examples/eci_demo/launch/launch_impedance_publisher.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_joint_impedance_controller", name="publisher_joint_impedance_controller", parameters=[impedance_config], diff --git a/kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.py b/kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.launch.py similarity index 95% rename from kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.py rename to kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.launch.py index 408b2b77..60d47c11 100644 --- a/kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.py +++ b/kuka_driver_examples/eci_demo/launch/launch_trajectory_publisher.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): return LaunchDescription( [ Node( - package="ros2_control_test_nodes", + package="ros2_controllers_test_nodes", executable="publisher_joint_trajectory_controller", name="publisher_joint_trajectory_controller", parameters=[position_goals], diff --git a/kuka_driver_examples/eci_demo/launch/moveit_planning_example.launch.py b/kuka_driver_examples/eci_demo/launch/moveit_planning_example.launch.py index 3a2c4d9d..5bcabacd 100644 --- a/kuka_driver_examples/eci_demo/launch/moveit_planning_example.launch.py +++ b/kuka_driver_examples/eci_demo/launch/moveit_planning_example.launch.py @@ -51,7 +51,7 @@ def launch_setup(context, *args, **kwargs): package="moveit_ros_move_group", executable="move_group", output="screen", - parameters=[moveit_config.to_dict()], + parameters=[moveit_config.to_dict(), {'publish_planning_scene_hz': 30.0}], ) rviz = Node( diff --git a/kuka_driver_examples/eci_demo/package.xml b/kuka_driver_examples/eci_demo/package.xml index cca5e6a7..327a0f99 100644 --- a/kuka_driver_examples/eci_demo/package.xml +++ b/kuka_driver_examples/eci_demo/package.xml @@ -15,6 +15,7 @@ rclcpp moveit_visual_tools moveit_configs_utils + kuka_driver_interfaces ros2_controllers moveit diff --git a/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp b/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp deleted file mode 100644 index 3f4f36d2..00000000 --- a/kuka_driver_examples/eci_demo/src/MoveitBasicPlan.cpp +++ /dev/null @@ -1,178 +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" - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_basic_plan"); - -moveit_msgs::msg::RobotTrajectory::SharedPtr planThroughwaypoints( - moveit::planning_interface::MoveGroupInterface & move_group_interface) -{ - std::vector waypoints; - moveit_msgs::msg::RobotTrajectory trajectory; - geometry_msgs::msg::Pose msg; - - // circle facing forward - msg.orientation.x = 0.0; - msg.orientation.y = sqrt(2) / 2; - msg.orientation.z = 0.0; - msg.orientation.w = sqrt(2) / 2; - msg.position.x = 0.4; - // Define waypoints in a circle - for (int i = 0; i < 63; i++) { - msg.position.y = -0.2 + sin(0.1 * i) * 0.15; - msg.position.z = 0.4 + cos(0.1 * i) * 0.15; - waypoints.push_back(msg); - } - - RCLCPP_INFO(LOGGER, "Start planning"); - double fraction = move_group_interface.computeCartesianPath(waypoints, 0.005, 0.0, trajectory); - RCLCPP_INFO(LOGGER, "Planning done!"); - - if (fraction < 1) { - RCLCPP_ERROR(LOGGER, "Could not compute trajectory through all waypoints!"); - return nullptr; - } else { - return std::make_shared(trajectory); - } -} - -moveit_msgs::msg::RobotTrajectory::SharedPtr planToPoint( - moveit::planning_interface::MoveGroupInterface & move_group_interface) -{ - // Create planning request using pilz industrial motion planner - Eigen::Isometry3d pose = Eigen::Isometry3d( - Eigen::Translation3d(0.1, 0.0, 0.8) * Eigen::Quaterniond::Identity()); - move_group_interface.setPlanningPipelineId("pilz_industrial_motion_planner"); - move_group_interface.setPlannerId("PTP"); - move_group_interface.setPoseTarget(pose); - - - moveit::planning_interface::MoveGroupInterface::Plan plan; - RCLCPP_INFO(LOGGER, "Sending planning request"); - if (!move_group_interface.plan(plan)) { - RCLCPP_INFO(LOGGER, "Planning failed"); - return nullptr; - } else { - RCLCPP_INFO(LOGGER, "Planning successful"); - return std::make_shared(plan.trajectory_); - } -} - -std::vector createCollisionObjects( - moveit::planning_interface::MoveGroupInterface & move_group_interface) -{ - std::vector collision_objects; - - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = move_group_interface.getPlanningFrame(); - collision_object.id = "robot_stand"; - 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; - - collision_objects.push_back(collision_object); - return collision_objects; -} - - -int main(int argc, char * argv[]) -{ - // Setup - // Initialize ROS and create the Node - rclcpp::init(argc, argv); - auto const node = std::make_shared( - "moveit_basic_plan", - rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true) - ); - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - std::thread([&executor]() {executor.spin();}).detach(); - - // Define Planning group - static const std::string PLANNING_GROUP = "lbr_iisy_arm"; - - // Create the MoveIt MoveGroup Interface - using moveit::planning_interface::MoveGroupInterface; - auto move_group_interface = MoveGroupInterface(node, PLANNING_GROUP); - - // Construct and initialize MoveItVisualTools - auto moveit_visual_tools = moveit_visual_tools::MoveItVisualTools{ - node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC, - move_group_interface.getRobotModel()}; - moveit_visual_tools.deleteAllMarkers(); - moveit_visual_tools.loadRemoteControl(); - moveit_visual_tools.trigger(); - - // Define lambda for visualization - auto const draw_trajectory_tool_path = - [&moveit_visual_tools, &move_group_interface](auto const trajectory) { - moveit_visual_tools.deleteAllMarkers(); - moveit_visual_tools.publishTrajectoryLine( - trajectory, - moveit_visual_tools.getRobotModel()->getJointModelGroup(PLANNING_GROUP)); - }; - - // Create Planning Scene Interface, witch is for adding collision boxes - auto planning_scene_interface = moveit::planning_interface::PlanningSceneInterface(); - - planning_scene_interface.addCollisionObjects(createCollisionObjects(move_group_interface)); - // End Collision Objects define - - auto planned_trajectory = planToPoint(move_group_interface); - if (planned_trajectory != nullptr) { - draw_trajectory_tool_path(*planned_trajectory); - moveit_visual_tools.trigger(); - moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); - move_group_interface.execute(*planned_trajectory); - } - - planned_trajectory = planThroughwaypoints(move_group_interface); - if (planned_trajectory != nullptr) { - draw_trajectory_tool_path(*planned_trajectory); - moveit_visual_tools.trigger(); - moveit_visual_tools.prompt("Press 'Next' in the RvizVisualToolsGui window to execute"); - move_group_interface.execute(*planned_trajectory); - } - - // Get the current joint values - auto jv = move_group_interface.getCurrentJointValues(); - - // Shutdown ROS - rclcpp::shutdown(); - return 0; -} diff --git a/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp new file mode 100755 index 00000000..63e204c1 --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitBasicPlannersExample.cpp @@ -0,0 +1,103 @@ +// 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 "eci_demo/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.1, 0, + 0.8) * + 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.4, -0.15, + 0.55) * + 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); + } + + // Add collision object + example_node->addCollisionBox( + geometry_msgs::build().x(0.25).y(-0.075).z(0.675), + 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; +} 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..1dfafa57 --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitCollisionAvoidanceExample.cpp @@ -0,0 +1,70 @@ +// 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 "eci_demo/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(); + + // 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}); + 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->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..2aaaf019 --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitConstrainedPlanningExample.cpp @@ -0,0 +1,78 @@ +// 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 "eci_demo/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(); + + // 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}); + 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->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->moveGroupInterface()->setPlanningTime(30.0); + + example_node->setOrientationConstraint(q); + // Plan with collision avoidance + auto planned_trajectory = + example_node->planToPointUntilSuccess(cart_goal, "ompl", "RRTkConfigDefault"); + 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/MoveitDepalletizingExample.cpp b/kuka_driver_examples/eci_demo/src/MoveitDepalletizingExample.cpp new file mode 100755 index 00000000..0ad1e5db --- /dev/null +++ b/kuka_driver_examples/eci_demo/src/MoveitDepalletizingExample.cpp @@ -0,0 +1,99 @@ +// 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 "eci_demo/moveit_example.hpp" + +class Depalletizer : public MoveitExample +{ +public: + void Depalletize() + { + for (int k = 0; k < 3; k++) { + for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) { + std::string object_name = "pallet_" + std::to_string(9 * k + 3 * j + i); + RCLCPP_INFO(LOGGER, "Going for object %s", object_name.c_str()); + + // Go to pickup + Eigen::Isometry3d pose = Eigen::Isometry3d( + Eigen::Translation3d( + 0.3 + i * 0.1, j * 0.1 - 0.1, + 0.35 - 0.1 * k) * + Eigen::Quaterniond(0, 1, 0, 0)); + auto planned_trajectory = + planToPointUntilSuccess(pose, "ompl", "RRTConnectkConfigDefault"); + if (planned_trajectory != nullptr) { + move_group_interface_->execute(*planned_trajectory); + } else { + RCLCPP_ERROR(LOGGER, "Planning failed"); + } + + // Attach object + AttachObject(object_name); + + // Drop off to -0.3, 0.0, 0.35 pointing down + Eigen::Isometry3d dropoff_pose = Eigen::Isometry3d( + Eigen::Translation3d(-0.3, 0.0, 0.35) * Eigen::Quaterniond(0, 1, 0, 0)); + auto drop_trajectory = planToPointUntilSuccess( + dropoff_pose, "ompl", + "RRTConnectkConfigDefault"); + if (drop_trajectory != nullptr) { + move_group_interface_->execute(*drop_trajectory); + } else { + RCLCPP_ERROR(LOGGER, "Planning failed"); + } + + // Detach + DetachAndRemoveObject(object_name); + } + } + } + } +}; + +int main(int argc, char * argv[]) +{ + // Setup + // Initialize ROS and create the Node + rclcpp::init(argc, argv); + auto const node = std::make_shared(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread( + [&executor]() + {executor.spin();}) + .detach(); + + node->initialize(); + + node->moveGroupInterface()->setMaxVelocityScalingFactor(1.0); + node->moveGroupInterface()->setMaxAccelerationScalingFactor(1.0); + // Add robot platform + node->addRobotPlatform(); + node->addBreakPoint(); + + // Add pallets + node->addPalletObjects(); + node->addBreakPoint(); + + node->Depalletize(); + + // Shutdown ROS + rclcpp::shutdown(); + return 0; +} diff --git a/kuka_driver_interfaces/CMakeLists.txt b/kuka_driver_interfaces/CMakeLists.txt index 7884231c..1eec5ef0 100644 --- a/kuka_driver_interfaces/CMakeLists.txt +++ b/kuka_driver_interfaces/CMakeLists.txt @@ -18,6 +18,7 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} @@ -25,6 +26,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/GetInt.srv" "srv/SetDouble.srv" "msg/RobotState.msg" + DEPENDENCIES geometry_msgs ) diff --git a/kuka_driver_interfaces/package.xml b/kuka_driver_interfaces/package.xml index 922f71e5..42903baf 100644 --- a/kuka_driver_interfaces/package.xml +++ b/kuka_driver_interfaces/package.xml @@ -13,6 +13,8 @@ ament_lint_auto ament_lint_common + geometry_msgs + rosidl_default_runtime rosidl_interface_packages diff --git a/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp b/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp index e1aee34c..f51cb6f6 100644 --- a/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp +++ b/kuka_rox_hardware_interface/src/rox_hardware_interface.cpp @@ -69,6 +69,11 @@ CallbackReturn KukaRoXHardwareInterface::on_init(const hardware_interface::Hardw RCLCPP_ERROR(rclcpp::get_logger("KukaRoXHardwareInterface"), "Could not setup udp replier"); return CallbackReturn::FAILURE; } +#else +// Start from home position in mock mode + hw_position_commands_[1] = -90 * (M_PI / 180); + hw_position_commands_[2] = 90 * (M_PI / 180); + hw_position_commands_[4] = 90 * (M_PI / 180); #endif RCLCPP_INFO(rclcpp::get_logger("KukaRoXHardwareInterface"), "Init successful");