From 7606dedfcf38c1f62bc3f8cfee48f996c559b11f Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Wed, 14 Aug 2024 02:35:02 +0300 Subject: [PATCH] Implemented the scenario related to moveit_task_constructor_benchmark --- CMakeLists.txt | 40 +- config/pick_place_demo_configs.yaml | 49 ++ .../pick_place_task.hpp | 90 ++++ .../scenario_moveit_task_constructor.hpp | 94 ++++ ...oveit_task_constructor_benchmark.launch.py | 200 +++++++ ...moveit_task_constructor_benchmark_main.cpp | 50 ++ .../pick_place_demo_parameters.yaml | 98 ++++ .../pick_place_task.cpp | 502 ++++++++++++++++++ .../scenario_moveit_task_constructor.cpp | 72 +++ 9 files changed, 1192 insertions(+), 3 deletions(-) create mode 100644 config/pick_place_demo_configs.yaml create mode 100644 include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp create mode 100644 include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp create mode 100644 launch/scenario_moveit_task_constructor_benchmark.launch.py create mode 100644 src/scenario_moveit_task_constructor_benchmark_main.cpp create mode 100644 src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml create mode 100644 src/scenarios/moveit_task_constructor/pick_place_task.cpp create mode 100644 src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ac991b4..65f9ad4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,8 @@ find_package(ament_index_cpp REQUIRED) find_package(yaml-cpp REQUIRED) find_package(std_msgs REQUIRED) find_package(example_interfaces REQUIRED) +find_package(moveit_task_constructor_core REQUIRED) +find_package(generate_parameter_library REQUIRED) add_executable( scenario_perception_pipeline_benchmark_main @@ -63,9 +65,41 @@ target_include_directories( target_link_libraries(scenario_basic_service_client_benchmark_main PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES}) -install(TARGETS scenario_perception_pipeline_benchmark_main - scenario_basic_service_client_benchmark_main - DESTINATION lib/moveit_middleware_benchmark) +add_executable( + scenario_moveit_task_constructor_benchmark_main + src/scenario_moveit_task_constructor_benchmark_main.cpp + src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp + src/scenarios/moveit_task_constructor/pick_place_task.cpp) + +ament_target_dependencies( + scenario_moveit_task_constructor_benchmark_main + PUBLIC + "moveit_ros_planning_interface" + "rclcpp" + "benchmark" + "std_msgs" + "example_interfaces" + "moveit_task_constructor_core") + +target_include_directories( + scenario_moveit_task_constructor_benchmark_main + PUBLIC $ + $) + +generate_parameter_library( + pick_place_demo_parameters + src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml) + +target_link_libraries( + scenario_moveit_task_constructor_benchmark_main + PUBLIC "benchmark::benchmark" ${YAML_CPP_LIBRARIES} + pick_place_demo_parameters) + +install( + TARGETS scenario_perception_pipeline_benchmark_main + scenario_basic_service_client_benchmark_main + scenario_moveit_task_constructor_benchmark_main + DESTINATION lib/moveit_middleware_benchmark) install(DIRECTORY launch config DESTINATION share/moveit_middleware_benchmark) diff --git a/config/pick_place_demo_configs.yaml b/config/pick_place_demo_configs.yaml new file mode 100644 index 0000000..3cfd954 --- /dev/null +++ b/config/pick_place_demo_configs.yaml @@ -0,0 +1,49 @@ +/**: + ros__parameters: + # Total planning attempts + max_solutions: 10 + + # Planning group and link names + arm_group_name: "panda_arm" + eef_name: "hand" + hand_group_name: "hand" + hand_frame: "panda_link8" + + # Poses + hand_open_pose: "open" + hand_close_pose: "close" + arm_home_pose: "ready" + + # Scene frames + world_frame: "world" + table_reference_frame: "world" + object_reference_frame: "world" + surface_link: "table" + + # Collision object for picking + # CYLINDER object specifications + object_name: "object" + object_dimensions: [0.25, 0.02] # [height, radius] + object_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Table model + spawn_table: true + table_name: "table" + table_dimensions: [0.4, 0.5, 0.1] # [length, width, height] + table_pose: [0.5, -0.25, 0.0, 0.0, 0.0, 0.0] + + # Gripper grasp frame transform [x,y,z,r,p,y] + grasp_frame_transform: [0.0, 0.0, 0.1, 1.571, 0.785, 1.571] + + # Place pose [x,y,z,r,p,y] + place_pose: [0.6, -0.15, 0.0, 0.0, 0.0, 0.0] + place_surface_offset: 0.0001 # place offset from table + + # Valid distance range when approaching an object for picking + approach_object_min_dist: 0.1 + approach_object_max_dist: 0.15 + + # Valid height range when lifting an object after pick + lift_object_min_dist: 0.01 + lift_object_max_dist: 0.1 + diff --git a/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp new file mode 100644 index 0000000..68c985f --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp @@ -0,0 +1,90 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein + Desc: A demo to show MoveIt Task Constructor in action +*/ + +// ROS +#include + +// MoveIt +#include +#include +#include + +// MTC +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "pick_place_demo_parameters.hpp" + +#pragma once + +namespace moveit_task_constructor_demo { +using namespace moveit::task_constructor; + +// prepare a demo environment from ROS parameters under node +void setupDemoScene(const pick_place_task_demo::Params& params); + +// destroy the created demo environment from ROS parameters under node +void destroyDemoScene(const pick_place_task_demo::Params& params); + +class PickPlaceTask +{ +public: + PickPlaceTask(const std::string& task_name); + ~PickPlaceTask() = default; + + bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); + + bool plan(const std::size_t max_solutions); + + bool execute(); + +private: + std::string task_name_; + moveit::task_constructor::TaskPtr task_; +}; +} // namespace moveit_task_constructor_demo + diff --git a/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp new file mode 100644 index 0000000..a3018b5 --- /dev/null +++ b/include/moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp @@ -0,0 +1,94 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against moveit_task_constructor + */ + +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp" + +namespace +{ +} // namespace + +namespace moveit +{ +namespace middleware_benchmark +{ + +class ScenarioMoveItTaskConstructor +{ +public: + ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr & node); + ~ScenarioMoveItTaskConstructor(); + + void runTestCase(); + +private: + std::shared_ptr pick_place_task_param_listener_; + pick_place_task_demo::Params pick_place_task_demo_parameters_; + std::shared_ptr pick_place_task_; + rclcpp::Node::SharedPtr node_; +}; + +class ScenarioMoveItTaskConstructorFixture : public benchmark::Fixture +{ +public: + ScenarioMoveItTaskConstructorFixture(); + + void SetUp(::benchmark::State& /*state*/); + + void TearDown(::benchmark::State& /*state*/); + +protected: + rclcpp::Node::SharedPtr node_; + std::shared_ptr node_executor_; + std::unique_ptr spinning_thread_; +}; + +} // namespace middleware_benchmark +} // namespace moveit \ No newline at end of file diff --git a/launch/scenario_moveit_task_constructor_benchmark.launch.py b/launch/scenario_moveit_task_constructor_benchmark.launch.py new file mode 100644 index 0000000..aa7acec --- /dev/null +++ b/launch/scenario_moveit_task_constructor_benchmark.launch.py @@ -0,0 +1,200 @@ +import os +from launch import LaunchDescription +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch.conditions import IfCondition +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.actions import ( + DeclareLaunchArgument, + ExecuteProcess, + Shutdown, + LogInfo, + RegisterEventHandler, + OpaqueFunction, +) +from launch.event_handlers import OnProcessExit +from launch_ros.substitutions import FindPackageShare +from moveit_configs_utils import MoveItConfigsBuilder + + +def launch_setup(context, *args, **kwargs): + benchmark_command_args = context.perform_substitution( + LaunchConfiguration("benchmark_command_args") + ).split() + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description( + file_path="config/panda.urdf.xacro", + mappings={ + "ros2_control_hardware_type": LaunchConfiguration( + "ros2_control_hardware_type" + ) + }, + ) + .robot_description_semantic(file_path="config/panda.srdf") + .robot_description_kinematics(file_path="config/kinematics.yaml") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl"] + ) + .to_moveit_configs() + ) + + # RViz for DEBUGGING STUFFS + rviz_config_file = ( + get_package_share_directory("moveit_task_constructor_demo") + "/config/mtc.rviz" + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + ], + ) + + # Load ExecuteTaskSolutionCapability so we can execute found solutions in simulation + move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"} + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict(), + move_group_capabilities], + arguments=["--ros-args", "--log-level", "info"], + ) + + # Static TF + static_tf_node = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + panda_hand_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + benchmark_main_node = Node( + name="benchmark_main", + package="moveit_middleware_benchmark", + executable="scenario_moveit_task_constructor_benchmark_main", + output="both", + arguments=benchmark_command_args, + parameters=[ + os.path.join( + get_package_share_directory("moveit_middleware_benchmark"), + "config", + "pick_place_demo_configs.yaml", + ), + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + moveit_config.planning_pipelines, + moveit_config.joint_limits, + {"use_sim_time": True}, + ], + on_exit=Shutdown(), + ) + + return [ + move_group_node, + static_tf_node, + robot_state_publisher, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + rviz_node, + # for https://github.com/ros-controls/ros2_controllers/issues/981 + RegisterEventHandler( + OnProcessExit( + target_action=panda_arm_controller_spawner, + on_exit=[ + LogInfo( + msg="panda_arm_controller_spawner is finished. Now test_scenario_perception_pipeline will start" + ), + benchmark_main_node, + ], + ) + ), + ] + + +def generate_launch_description(): + declared_arguments = [] + + benchmark_command_args = DeclareLaunchArgument( + "benchmark_command_args", + default_value="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=6", + description="Google Benchmark Tool Arguments", + ) + declared_arguments.append(benchmark_command_args) + + ros2_control_hardware_type = DeclareLaunchArgument( + "ros2_control_hardware_type", + default_value="mock_components", + description="ROS 2 control hardware interface type to use for the launch file -- possible values: [mock_components, isaac]", + ) + declared_arguments.append(ros2_control_hardware_type) + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) + diff --git a/src/scenario_moveit_task_constructor_benchmark_main.cpp b/src/scenario_moveit_task_constructor_benchmark_main.cpp new file mode 100644 index 0000000..82bdecd --- /dev/null +++ b/src/scenario_moveit_task_constructor_benchmark_main.cpp @@ -0,0 +1,50 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, Cihat Kurtuluş Altıparmak + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Cihat Kurtuluş Altıparmak + Description: Benchmarking module to compare the effects of middlewares + against moveit task constructor scenarios + */ + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp" + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + benchmark::Initialize(&argc, argv); + benchmark::RunSpecifiedBenchmarks(); + benchmark::Shutdown(); + rclcpp::shutdown(); + return 0; +} diff --git a/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml new file mode 100644 index 0000000..7990e24 --- /dev/null +++ b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml @@ -0,0 +1,98 @@ +pick_place_task_demo: + execute: + type: bool + default_value: false + table_name: + type: string + validation: + not_empty<>: [] + table_reference_frame: + type: string + validation: + not_empty<>: [] + table_dimensions: + type: double_array + validation: + fixed_size<>: [3] + table_pose: + type: double_array + validation: + fixed_size<>: [6] + object_name: + type: string + validation: + not_empty<>: [] + object_reference_frame: + type: string + validation: + not_empty<>: [] + object_dimensions: + type: double_array + validation: + fixed_size<>: [2] + object_pose: + type: double_array + validation: + fixed_size<>: [6] + spawn_table: + type: bool + default_value: true + max_solutions: + type: int + default_value: 10 + arm_group_name: + type: string + validation: + not_empty<>: [] + eef_name: + type: string + validation: + not_empty<>: [] + hand_group_name: + type: string + validation: + not_empty<>: [] + hand_frame: + type: string + validation: + not_empty<>: [] + hand_open_pose: + type: string + validation: + not_empty<>: [] + hand_close_pose: + type: string + validation: + not_empty<>: [] + arm_home_pose: + type: string + validation: + not_empty<>: [] + # Scene frames + world_frame: + type: string + validation: + not_empty<>: [] + surface_link: + type: string + validation: + not_empty<>: [] + grasp_frame_transform: + type: double_array + validation: + fixed_size<>: [6] + place_pose: + type: double_array + validation: + fixed_size<>: [6] + place_surface_offset: + type: double + approach_object_min_dist: + type: double + approach_object_max_dist: + type: double + lift_object_min_dist: + type: double + lift_object_max_dist: + type: double + diff --git a/src/scenarios/moveit_task_constructor/pick_place_task.cpp b/src/scenarios/moveit_task_constructor/pick_place_task.cpp new file mode 100644 index 0000000..79bb5a8 --- /dev/null +++ b/src/scenarios/moveit_task_constructor/pick_place_task.cpp @@ -0,0 +1,502 @@ +/********************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019 PickNik LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Henning Kayser, Simon Goldstein, Cihat Kurtuluş Altıparmak + Desc: A demo to show MoveIt Task Constructor in action +*/ + +#include +#include +#include + +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp" + +static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_task_constructor_demo"); + +namespace { +Eigen::Isometry3d vectorToEigen(const std::vector& values) { + return Eigen::Translation3d(values[0], values[1], values[2]) * + Eigen::AngleAxisd(values[3], Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(values[4], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(values[5], Eigen::Vector3d::UnitZ()); +} +geometry_msgs::msg::Pose vectorToPose(const std::vector& values) { + return tf2::toMsg(vectorToEigen(values)); +}; +} // namespace + +namespace moveit_task_constructor_demo { + +void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, + const moveit_msgs::msg::CollisionObject& object) { + if (!psi.applyCollisionObject(object)) + throw std::runtime_error("Failed to spawn object: " + object.id); +} + +moveit_msgs::msg::CollisionObject createTable(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.table_pose); + moveit_msgs::msg::CollisionObject object; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX; + object.primitives[0].dimensions = { params.table_dimensions.at(0), params.table_dimensions.at(1), + params.table_dimensions.at(2) }; + pose.position.z -= 0.5 * params.table_dimensions[2]; // align surface with world + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::msg::CollisionObject destroyTable(const pick_place_task_demo::Params& params) { + moveit_msgs::msg::CollisionObject object; + object.id = params.table_name; + object.header.frame_id = params.table_reference_frame; + object.operation = object.REMOVE; + return object; +} + +moveit_msgs::msg::CollisionObject createObject(const pick_place_task_demo::Params& params) { + geometry_msgs::msg::Pose pose = vectorToPose(params.object_pose); + moveit_msgs::msg::CollisionObject object; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; + object.primitives.resize(1); + object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER; + object.primitives[0].dimensions = { params.object_dimensions.at(0), params.object_dimensions.at(1) }; + pose.position.z += 0.5 * params.object_dimensions[0]; + object.primitive_poses.push_back(pose); + return object; +} + +moveit_msgs::msg::CollisionObject destroyObject(const pick_place_task_demo::Params& params) { + moveit_msgs::msg::CollisionObject object; + object.id = params.object_name; + object.header.frame_id = params.object_reference_frame; + object.operation = object.REMOVE; + return object; +} + +void setupDemoScene(const pick_place_task_demo::Params& params) { + // Add table and object to planning scene + rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + if (params.spawn_table) + spawnObject(psi, createTable(params)); + spawnObject(psi, createObject(params)); +} + +void destroyDemoScene(const pick_place_task_demo::Params& params) { + // Add table and object to planning scene + rclcpp::sleep_for(std::chrono::microseconds(100)); // Wait for ApplyPlanningScene service + moveit::planning_interface::PlanningSceneInterface psi; + if (params.spawn_table) + spawnObject(psi, destroyTable(params)); + spawnObject(psi, destroyObject(params)); +} + +PickPlaceTask::PickPlaceTask(const std::string& task_name) : task_name_(task_name) {} + +bool PickPlaceTask::init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params) { + RCLCPP_INFO(LOGGER, "Initializing task pipeline"); + + // Reset ROS introspection before constructing the new object + // TODO(v4hn): global storage for Introspection services to enable one-liner + task_.reset(); + task_.reset(new moveit::task_constructor::Task()); + + // Individual movement stages are collected within the Task object + Task& t = *task_; + t.stages()->setName(task_name_); + t.loadRobotModel(node); + + /* Create planners used in various stages. Various options are available, + namely Cartesian, MoveIt pipeline, and joint interpolation. */ + // Sampling planner + auto sampling_planner = std::make_shared(node); + sampling_planner->setProperty("goal_joint_tolerance", 1e-5); + + // Cartesian planner + auto cartesian_planner = std::make_shared(); + cartesian_planner->setMaxVelocityScalingFactor(1.0); + cartesian_planner->setMaxAccelerationScalingFactor(1.0); + cartesian_planner->setStepSize(.01); + + // Set task properties + t.setProperty("group", params.arm_group_name); + t.setProperty("eef", params.eef_name); + t.setProperty("hand", params.hand_group_name); + t.setProperty("hand_grasping_frame", params.hand_frame); + t.setProperty("ik_frame", params.hand_frame); + + /**************************************************** + * * + * Current State * + * * + ***************************************************/ + { + auto current_state = std::make_unique("current state"); + + // Verify that object is not attached + auto applicability_filter = + std::make_unique("applicability test", std::move(current_state)); + applicability_filter->setPredicate([object = params.object_name](const SolutionBase& s, std::string& comment) { + if (s.start()->scene()->getCurrentState().hasAttachedBody(object)) { + comment = "object with id '" + object + "' is already attached and cannot be picked"; + return false; + } + return true; + }); + t.add(std::move(applicability_filter)); + } + + /**************************************************** + * * + * Open Hand * + * * + ***************************************************/ + Stage* initial_state_ptr = nullptr; + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); + initial_state_ptr = stage.get(); // remember start state for monitoring grasp pose generator + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Move to Pick * + * * + ***************************************************/ + // Connect initial open-hand state with pre-grasp pose defined in the following + { + auto stage = std::make_unique( + "move to pick", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /**************************************************** + * * + * Pick Object * + * * + ***************************************************/ + Stage* pick_stage_ptr = nullptr; + { + // A SerialContainer combines several sub-stages, here for picking the object + auto grasp = std::make_unique("pick object"); + t.properties().exposeTo(grasp->properties(), { "eef", "hand", "group", "ik_frame" }); + grasp->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group", "ik_frame" }); + + /**************************************************** + ---- * Approach Object * + ***************************************************/ + { + // Move the eef link forward along its z-axis by an amount within the given min-max range + auto stage = std::make_unique("approach object", cartesian_planner); + stage->properties().set("marker_ns", "approach_object"); + stage->properties().set("link", params.hand_frame); // link to perform IK for + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage + stage->setMinMaxDistance(params.approach_object_min_dist, params.approach_object_max_dist); + + // Set hand forward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.hand_frame; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Generate Grasp Pose * + ***************************************************/ + { + // Sample grasp pose candidates in angle increments around the z-axis of the object + auto stage = std::make_unique("generate grasp pose"); + stage->properties().configureInitFrom(Stage::PARENT); + stage->properties().set("marker_ns", "grasp_pose"); + stage->setPreGraspPose(params.hand_open_pose); + stage->setObject(params.object_name); // object to sample grasps for + stage->setAngleDelta(M_PI / 12); + stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions + + // Compute IK for sampled grasp poses + auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(8); // limit number of solutions + wrapper->setMinSolutionDistance(1.0); + // define virtual frame to reach the target_pose + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); // inherit properties from parent + wrapper->properties().configureInitFrom(Stage::INTERFACE, + { "target_pose" }); // inherit property from child solution + grasp->insert(std::move(wrapper)); + } + + /**************************************************** + ---- * Allow Collision (hand object) * + ***************************************************/ + { + // Modify planning scene (w/o altering the robot's pose) to allow touching the object for picking + auto stage = std::make_unique("allow collision (hand,object)"); + stage->allowCollisions( + params.object_name, + t.getRobotModel()->getJointModelGroup(params.hand_group_name)->getLinkModelNamesWithCollisionGeometry(), + true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + ---- * Close Hand * + ***************************************************/ + { + auto stage = std::make_unique("close hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_close_pose); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Attach Object * + ***************************************************/ + { + auto stage = std::make_unique("attach object"); + stage->attachObject(params.object_name, params.hand_frame); // attach object to hand_frame_ + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Allow collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("allow collision (object,support)"); + stage->allowCollisions({ params.object_name }, { params.surface_link }, true); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Lift object * + ***************************************************/ + { + auto stage = std::make_unique("lift object", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(params.lift_object_min_dist, params.lift_object_max_dist); + stage->setIKFrame(params.hand_frame); + stage->properties().set("marker_ns", "lift_object"); + + // Set upward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.world_frame; + vec.vector.z = 1.0; + stage->setDirection(vec); + grasp->insert(std::move(stage)); + } + + /**************************************************** + .... * Forbid collision (object support) * + ***************************************************/ + { + auto stage = std::make_unique("forbid collision (object,surface)"); + stage->allowCollisions({ params.object_name }, { params.surface_link }, false); + grasp->insert(std::move(stage)); + } + + pick_stage_ptr = grasp.get(); // remember for monitoring place pose generator + + // Add grasp container to task + t.add(std::move(grasp)); + } + + /****************************************************** + * * + * Move to Place * + * * + *****************************************************/ + { + // Connect the grasped state to the pre-place state, i.e. realize the object transport + auto stage = std::make_unique( + "move to place", stages::Connect::GroupPlannerVector{ { params.arm_group_name, sampling_planner } }); + stage->setTimeout(5.0); + stage->properties().configureInitFrom(Stage::PARENT); + t.add(std::move(stage)); + } + + /****************************************************** + * * + * Place Object * + * * + *****************************************************/ + // All placing sub-stages are collected within a serial container again + { + auto place = std::make_unique("place object"); + t.properties().exposeTo(place->properties(), { "eef", "hand", "group" }); + place->properties().configureInitFrom(Stage::PARENT, { "eef", "hand", "group" }); + + /****************************************************** + ---- * Lower Object * + *****************************************************/ + { + auto stage = std::make_unique("lower object", cartesian_planner); + stage->properties().set("marker_ns", "lower_object"); + stage->properties().set("link", params.hand_frame); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.03, .13); + + // Set downward direction + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.world_frame; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Generate Place Pose * + *****************************************************/ + { + // Generate Place Pose + auto stage = std::make_unique("generate place pose"); + stage->properties().configureInitFrom(Stage::PARENT, { "ik_frame" }); + stage->properties().set("marker_ns", "place_pose"); + stage->setObject(params.object_name); + + // Set target pose + geometry_msgs::msg::PoseStamped p; + p.header.frame_id = params.object_reference_frame; + p.pose = vectorToPose(params.place_pose); + p.pose.position.z += 0.5 * params.object_dimensions[0] + params.place_surface_offset; + stage->setPose(p); + stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions + + // Compute IK + auto wrapper = std::make_unique("place pose IK", std::move(stage)); + wrapper->setMaxIKSolutions(2); + wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); + wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); + wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); + place->insert(std::move(wrapper)); + } + + /****************************************************** + ---- * Open Hand * + *****************************************************/ + { + auto stage = std::make_unique("open hand", sampling_planner); + stage->setGroup(params.hand_group_name); + stage->setGoal(params.hand_open_pose); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Forbid collision (hand, object) * + *****************************************************/ + { + auto stage = std::make_unique("forbid collision (hand,object)"); + stage->allowCollisions(params.object_name, *t.getRobotModel()->getJointModelGroup(params.hand_group_name), + false); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Detach Object * + *****************************************************/ + { + auto stage = std::make_unique("detach object"); + stage->detachObject(params.object_name, params.hand_frame); + place->insert(std::move(stage)); + } + + /****************************************************** + ---- * Retreat Motion * + *****************************************************/ + { + auto stage = std::make_unique("retreat after place", cartesian_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setMinMaxDistance(.12, .25); + stage->setIKFrame(params.hand_frame); + stage->properties().set("marker_ns", "retreat"); + geometry_msgs::msg::Vector3Stamped vec; + vec.header.frame_id = params.hand_frame; + vec.vector.z = -1.0; + stage->setDirection(vec); + place->insert(std::move(stage)); + } + + // Add place container to task + t.add(std::move(place)); + } + + /****************************************************** + * * + * Move to Home * + * * + *****************************************************/ + { + auto stage = std::make_unique("move home", sampling_planner); + stage->properties().configureInitFrom(Stage::PARENT, { "group" }); + stage->setGoal(params.arm_home_pose); + stage->restrictDirection(stages::MoveTo::FORWARD); + t.add(std::move(stage)); + } + + // prepare Task structure for planning + try { + t.init(); + } catch (InitStageException& e) { + RCLCPP_ERROR_STREAM(LOGGER, "Initialization failed: " << e); + return false; + } + + return true; +} + +bool PickPlaceTask::plan(const std::size_t max_solutions) { + RCLCPP_INFO(LOGGER, "Start searching for task solutions"); + + return static_cast(task_->plan(max_solutions)); +} + +bool PickPlaceTask::execute() { + RCLCPP_INFO(LOGGER, "Executing solution trajectory"); + moveit_msgs::msg::MoveItErrorCodes execute_result; + + execute_result = task_->execute(*task_->solutions().front()); + + if (execute_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { + RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed and returned: " << execute_result.val); + return false; + } + + return true; +} +} // namespace moveit_task_constructor_demo + diff --git a/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp b/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp new file mode 100644 index 0000000..d6b1659 --- /dev/null +++ b/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp @@ -0,0 +1,72 @@ +#include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.hpp" + +namespace moveit { +namespace middleware_benchmark { + +ScenarioMoveItTaskConstructor::ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr & node) : node_(node) { + + pick_place_task_param_listener_ = std::make_shared(node_); + pick_place_task_demo_parameters_ = pick_place_task_param_listener_->get_params(); + + moveit_task_constructor_demo::setupDemoScene(pick_place_task_demo_parameters_); + pick_place_task_ = std::make_shared("pick_place_task"); + + if (!pick_place_task_->init(node_, pick_place_task_demo_parameters_)) { + RCLCPP_INFO(node->get_logger(), "Initialization failed"); + } +} + +void ScenarioMoveItTaskConstructor::runTestCase() { + if (pick_place_task_->plan(10)) { + pick_place_task_->execute(); + RCLCPP_INFO(node_->get_logger(), "Planning succeded"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Planning failed"); + } +} + +ScenarioMoveItTaskConstructor::~ScenarioMoveItTaskConstructor() { + moveit_task_constructor_demo::destroyDemoScene(pick_place_task_demo_parameters_); +} + +ScenarioMoveItTaskConstructorFixture::ScenarioMoveItTaskConstructorFixture() { + +} + +void ScenarioMoveItTaskConstructorFixture::SetUp(::benchmark::State& /*state*/) { + if (node_.use_count() == 0) + { + node_ = std::make_shared("test_scenario_moveit_task_constructor_node", + rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + + node_executor_ = std::make_shared(); + node_executor_->add_node(node_); + + spinning_thread_ = std::make_unique([this]() {node_executor_->spin();}); + } +} + +void ScenarioMoveItTaskConstructorFixture::TearDown(::benchmark::State& /*state*/) { + node_executor_->cancel(); + + if (spinning_thread_->joinable()) { + spinning_thread_->join(); + } + + node_executor_.reset(); + node_.reset(); +} + +BENCHMARK_DEFINE_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_task_constructor)(benchmark::State& st) +{ + for (auto _ : st) + { + auto sc = ScenarioMoveItTaskConstructor(node_); + sc.runTestCase(); + } +} + +BENCHMARK_REGISTER_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_task_constructor); + +} +} \ No newline at end of file