diff --git a/config/pick_place_demo_configs.yaml b/config/pick_place_demo_configs.yaml index 3cfd954..1c2b69b 100644 --- a/config/pick_place_demo_configs.yaml +++ b/config/pick_place_demo_configs.yaml @@ -46,4 +46,3 @@ # 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 index 68c985f..8897c3d 100644 --- 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 @@ -61,7 +61,8 @@ #pragma once -namespace moveit_task_constructor_demo { +namespace moveit_task_constructor_demo +{ using namespace moveit::task_constructor; // prepare a demo environment from ROS parameters under node @@ -73,18 +74,17 @@ void destroyDemoScene(const pick_place_task_demo::Params& params); class PickPlaceTask { public: - PickPlaceTask(const std::string& task_name); - ~PickPlaceTask() = default; + PickPlaceTask(const std::string& task_name); + ~PickPlaceTask() = default; - bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); + bool init(const rclcpp::Node::SharedPtr& node, const pick_place_task_demo::Params& params); - bool plan(const std::size_t max_solutions); + bool plan(const std::size_t max_solutions); - bool execute(); + bool execute(); private: - std::string task_name_; - moveit::task_constructor::TaskPtr task_; + 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 index a3018b5..238c0f7 100644 --- 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 @@ -43,12 +43,6 @@ #include #include -#include - -#include -#include -#include - #include "moveit_middleware_benchmark/scenarios/moveit_task_constructor/pick_place_task.hpp" namespace @@ -63,7 +57,7 @@ namespace middleware_benchmark class ScenarioMoveItTaskConstructor { public: - ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr & node); + ScenarioMoveItTaskConstructor(const rclcpp::Node::SharedPtr& node); ~ScenarioMoveItTaskConstructor(); void runTestCase(); @@ -91,4 +85,4 @@ class ScenarioMoveItTaskConstructorFixture : public benchmark::Fixture }; } // namespace middleware_benchmark -} // namespace moveit \ No newline at end of file +} // namespace moveit diff --git a/launch/scenario_moveit_task_constructor_benchmark.launch.py b/launch/scenario_moveit_task_constructor_benchmark.launch.py index aa7acec..48c4b87 100644 --- a/launch/scenario_moveit_task_constructor_benchmark.launch.py +++ b/launch/scenario_moveit_task_constructor_benchmark.launch.py @@ -38,40 +38,21 @@ def launch_setup(context, *args, **kwargs): publish_robot_description=True, publish_robot_description_semantic=True ) .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") - .planning_pipelines( - pipelines=["ompl"] - ) + .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"} + 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], + parameters=[moveit_config.to_dict(), move_group_capabilities], arguments=["--ros-args", "--log-level", "info"], ) @@ -161,7 +142,6 @@ def launch_setup(context, *args, **kwargs): 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( @@ -182,7 +162,7 @@ def generate_launch_description(): benchmark_command_args = DeclareLaunchArgument( "benchmark_command_args", - default_value="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=6", + default_value="--benchmark_out=middleware_benchmark_results.json --benchmark_out_format=json --benchmark_repetitions=20", description="Google Benchmark Tool Arguments", ) declared_arguments.append(benchmark_command_args) @@ -197,4 +177,3 @@ def generate_launch_description(): return LaunchDescription( declared_arguments + [OpaqueFunction(function=launch_setup)] ) - diff --git a/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml index 7990e24..a05c527 100644 --- a/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml +++ b/src/scenarios/moveit_task_constructor/pick_place_demo_parameters.yaml @@ -95,4 +95,3 @@ pick_place_task_demo: 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 index 79bb5a8..cfc8fec 100644 --- a/src/scenarios/moveit_task_constructor/pick_place_task.cpp +++ b/src/scenarios/moveit_task_constructor/pick_place_task.cpp @@ -42,461 +42,480 @@ 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()); +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)); +geometry_msgs::msg::Pose vectorToPose(const std::vector& values) +{ + return tf2::toMsg(vectorToEigen(values)); }; } // namespace -namespace moveit_task_constructor_demo { +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); + 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 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 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 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; +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 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)); +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" }); - - /**************************************************** +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)); - } - - /**************************************************** + ***************************************************/ + { + // 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)); - } - - /**************************************************** + ***************************************************/ + { + // 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)); - } - - /**************************************************** + ***************************************************/ + { + // 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)); - } - - /**************************************************** + ***************************************************/ + { + 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)); - } - - /**************************************************** + ***************************************************/ + { + 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)); - } - - /**************************************************** + ***************************************************/ + { + 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)); - } - - /**************************************************** + ***************************************************/ + { + 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" }); - - /****************************************************** + ***************************************************/ + { + 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)); - } - - /****************************************************** + *****************************************************/ + { + 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)); - } - - /****************************************************** + *****************************************************/ + { + // 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)); - } - - /****************************************************** + *****************************************************/ + { + 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)); - } - - /****************************************************** + *****************************************************/ + { + 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)); - } - - /****************************************************** + *****************************************************/ + { + 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; + *****************************************************/ + { + 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"); +bool PickPlaceTask::plan(const std::size_t max_solutions) +{ + RCLCPP_INFO(LOGGER, "Start searching for task solutions"); - return static_cast(task_->plan(max_solutions)); + return static_cast(task_->plan(max_solutions)); } -bool PickPlaceTask::execute() { - RCLCPP_INFO(LOGGER, "Executing solution trajectory"); - moveit_msgs::msg::MoveItErrorCodes execute_result; +bool PickPlaceTask::execute() +{ + RCLCPP_INFO(LOGGER, "Executing solution trajectory"); + moveit_msgs::msg::MoveItErrorCodes execute_result; - execute_result = task_->execute(*task_->solutions().front()); + 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; - } + 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; + 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 index d6b1659..4c86c33 100644 --- a/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp +++ b/src/scenarios/moveit_task_constructor/scenario_moveit_task_constructor.cpp @@ -1,39 +1,48 @@ #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) { +namespace moveit +{ +namespace middleware_benchmark +{ - pick_place_task_param_listener_ = std::make_shared(node_); - pick_place_task_demo_parameters_ = pick_place_task_param_listener_->get_params(); +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"); + 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"); - } + 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"); - } +void ScenarioMoveItTaskConstructor::runTestCase() +{ + if (pick_place_task_->plan(pick_place_task_demo_parameters_.max_solutions)) + { + pick_place_task_->execute(); + RCLCPP_INFO(node_->get_logger(), "Planning succeeded"); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "Planning failed"); + } } -ScenarioMoveItTaskConstructor::~ScenarioMoveItTaskConstructor() { - moveit_task_constructor_demo::destroyDemoScene(pick_place_task_demo_parameters_); +ScenarioMoveItTaskConstructor::~ScenarioMoveItTaskConstructor() +{ + moveit_task_constructor_demo::destroyDemoScene(pick_place_task_demo_parameters_); } -ScenarioMoveItTaskConstructorFixture::ScenarioMoveItTaskConstructorFixture() { - +ScenarioMoveItTaskConstructorFixture::ScenarioMoveItTaskConstructorFixture() +{ } -void ScenarioMoveItTaskConstructorFixture::SetUp(::benchmark::State& /*state*/) { +void ScenarioMoveItTaskConstructorFixture::SetUp(::benchmark::State& /*state*/) +{ if (node_.use_count() == 0) { node_ = std::make_shared("test_scenario_moveit_task_constructor_node", @@ -42,14 +51,16 @@ void ScenarioMoveItTaskConstructorFixture::SetUp(::benchmark::State& /*state*/) node_executor_ = std::make_shared(); node_executor_->add_node(node_); - spinning_thread_ = std::make_unique([this]() {node_executor_->spin();}); + spinning_thread_ = std::make_unique([this]() { node_executor_->spin(); }); } } -void ScenarioMoveItTaskConstructorFixture::TearDown(::benchmark::State& /*state*/) { +void ScenarioMoveItTaskConstructorFixture::TearDown(::benchmark::State& /*state*/) +{ node_executor_->cancel(); - if (spinning_thread_->joinable()) { + if (spinning_thread_->joinable()) + { spinning_thread_->join(); } @@ -68,5 +79,5 @@ BENCHMARK_DEFINE_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_ta BENCHMARK_REGISTER_F(ScenarioMoveItTaskConstructorFixture, test_scenario_moveit_task_constructor); -} -} \ No newline at end of file +} // namespace middleware_benchmark +} // namespace moveit