/********************************************************************* * 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 */ #include #include #include #include #include "pick_place_demo_parameters.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 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; } 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)); } 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()); Task& t = *task_; t.stages()->setName(task_name_); t.loadRobotModel(node); // 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; { // Open Hand 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 * * * ***************************************************/ { // Move-to pre-grasp 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; { 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 * ***************************************************/ { auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); stage->properties().set("link", params.hand_frame); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); 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 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); stage->setAngleDelta(M_PI / 12); stage->setMonitoredStage(initial_state_ptr); // hook into successful initial-phase solutions // Compute IK auto wrapper = std::make_unique("grasp pose IK", std::move(stage)); wrapper->setMaxIKSolutions(8); wrapper->setMinSolutionDistance(1.0); wrapper->setIKFrame(vectorToEigen(params.grasp_frame_transform), params.hand_frame); wrapper->properties().configureInitFrom(Stage::PARENT, { "eef", "group" }); wrapper->properties().configureInitFrom(Stage::INTERFACE, { "target_pose" }); grasp->insert(std::move(wrapper)); } /**************************************************** ---- * Allow Collision (hand object) * ***************************************************/ { 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); 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 * * * *****************************************************/ { 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 * * * *****************************************************/ { 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 you want to inspect the goal message, use this instead: // actionlib::SimpleActionClient // execute("execute_task_solution", true); execute.waitForServer(); // moveit_task_constructor_msgs::msg::ExecuteTaskSolution::Goal execute_goal; // task_->solutions().front()->toMsg(execute_goal.solution); // execute.sendGoalAndWait(execute_goal); // execute_result = execute.getResult()->error_code; 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