/********************************************************************* * 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 namespace moveit_task_constructor_demo { constexpr char LOGNAME[] = "moveit_task_constructor_demo"; constexpr char PickPlaceTask::LOGNAME[]; void spawnObject(moveit::planning_interface::PlanningSceneInterface& psi, const moveit_msgs::CollisionObject& object) { if (!psi.applyCollisionObject(object)) throw std::runtime_error("Failed to spawn object: " + object.id); } moveit_msgs::CollisionObject createTable(const ros::NodeHandle& pnh) { std::string table_name, table_reference_frame; std::vector table_dimensions; geometry_msgs::Pose pose; std::size_t errors = 0; errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_name", table_name); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_reference_frame", table_reference_frame); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_dimensions", table_dimensions); errors += !rosparam_shortcuts::get(LOGNAME, pnh, "table_pose", pose); rosparam_shortcuts::shutdownIfError(LOGNAME, errors); moveit_msgs::CollisionObject object; object.id = table_name; object.header.frame_id = table_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::BOX; object.primitives[0].dimensions = table_dimensions; pose.position.z -= 0.5 * table_dimensions[2]; // align surface with world object.primitive_poses.push_back(pose); return object; } moveit_msgs::CollisionObject createObject(const ros::NodeHandle& pnh) { std::string object_name, object_reference_frame; std::vector object_dimensions; geometry_msgs::Pose pose; std::size_t error = 0; error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_name", object_name); error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_reference_frame", object_reference_frame); error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_dimensions", object_dimensions); error += !rosparam_shortcuts::get(LOGNAME, pnh, "object_pose", pose); rosparam_shortcuts::shutdownIfError(LOGNAME, error); moveit_msgs::CollisionObject object; object.id = object_name; object.header.frame_id = object_reference_frame; object.primitives.resize(1); object.primitives[0].type = shape_msgs::SolidPrimitive::CYLINDER; object.primitives[0].dimensions = object_dimensions; pose.position.z += 0.5 * object_dimensions[0]; object.primitive_poses.push_back(pose); return object; } void setupDemoScene(ros::NodeHandle& pnh) { // Add table and object to planning scene ros::Duration(1.0).sleep(); // Wait for ApplyPlanningScene service moveit::planning_interface::PlanningSceneInterface psi; if (pnh.param("spawn_table", true)) spawnObject(psi, createTable(pnh)); spawnObject(psi, createObject(pnh)); } PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh) : pnh_(pnh), task_name_(task_name) { loadParameters(); } void PickPlaceTask::loadParameters() { /**************************************************** * * * Load Parameters * * * ***************************************************/ ROS_INFO_NAMED(LOGNAME, "Loading task parameters"); // Planning group properties size_t errors = 0; errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_group_name", arm_group_name_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_group_name", hand_group_name_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "eef_name", eef_name_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_frame", hand_frame_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "world_frame", world_frame_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "grasp_frame_transform", grasp_frame_transform_); // Predefined pose targets errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_open_pose", hand_open_pose_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "hand_close_pose", hand_close_pose_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "arm_home_pose", arm_home_pose_); // Target object errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_name", object_name_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_dimensions", object_dimensions_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "object_reference_frame", object_reference_frame_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "surface_link", surface_link_); support_surfaces_ = { surface_link_ }; // Pick/Place metrics errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_min_dist", approach_object_min_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "approach_object_max_dist", approach_object_max_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_min_dist", lift_object_min_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "lift_object_max_dist", lift_object_max_dist_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_surface_offset", place_surface_offset_); errors += !rosparam_shortcuts::get(LOGNAME, pnh_, "place_pose", place_pose_); rosparam_shortcuts::shutdownIfError(LOGNAME, errors); } // Initialize the task pipeline, defining individual movement stages bool PickPlaceTask::init() { ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline"); const std::string object = object_name_; // 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(); /* 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(); 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", arm_group_name_); t.setProperty("eef", eef_name_); t.setProperty("hand", hand_group_name_); t.setProperty("hand_grasping_frame", hand_frame_); t.setProperty("ik_frame", 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](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(hand_group_name_); stage->setGoal(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{ { 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", hand_frame_); // link to perform IK for stage->properties().configureInitFrom(Stage::PARENT, { "group" }); // inherit group from parent stage stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); // Set hand forward direction geometry_msgs::Vector3Stamped vec; vec.header.frame_id = 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(hand_open_pose_); stage->setObject(object); // 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); wrapper->setIKFrame(grasp_frame_transform_, hand_frame_); // define virtual frame to reach the target_pose 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( object, t.getRobotModel()->getJointModelGroup(hand_group_name_)->getLinkModelNamesWithCollisionGeometry(), true); grasp->insert(std::move(stage)); } /**************************************************** ---- * Close Hand * ***************************************************/ { auto stage = std::make_unique("close hand", sampling_planner); stage->setGroup(hand_group_name_); stage->setGoal(hand_close_pose_); grasp->insert(std::move(stage)); } /**************************************************** .... * Attach Object * ***************************************************/ { auto stage = std::make_unique("attach object"); stage->attachObject(object, 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({ object }, support_surfaces_, 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(lift_object_min_dist_, lift_object_max_dist_); stage->setIKFrame(hand_frame_); stage->properties().set("marker_ns", "lift_object"); // Set upward direction geometry_msgs::Vector3Stamped vec; vec.header.frame_id = 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,support)"); stage->allowCollisions({ object }, support_surfaces_, 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{ { 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", hand_frame_); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(.03, .13); // Set downward direction geometry_msgs::Vector3Stamped vec; vec.header.frame_id = 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(object); // Set target pose geometry_msgs::PoseStamped p; p.header.frame_id = object_reference_frame_; p.pose = place_pose_; p.pose.position.z += 0.5 * object_dimensions_[0] + 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(grasp_frame_transform_, 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(hand_group_name_); stage->setGoal(hand_open_pose_); place->insert(std::move(stage)); } /****************************************************** ---- * Forbid collision (hand, object) * *****************************************************/ { auto stage = std::make_unique("forbid collision (hand,object)"); stage->allowCollisions(object_name_, *t.getRobotModel()->getJointModelGroup(hand_group_name_), false); place->insert(std::move(stage)); } /****************************************************** ---- * Detach Object * *****************************************************/ { auto stage = std::make_unique("detach object"); stage->detachObject(object_name_, 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(hand_frame_); stage->properties().set("marker_ns", "retreat"); geometry_msgs::Vector3Stamped vec; vec.header.frame_id = 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(arm_home_pose_); stage->restrictDirection(stages::MoveTo::FORWARD); t.add(std::move(stage)); } // prepare Task structure for planning try { t.init(); } catch (InitStageException& e) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e); return false; } return true; } bool PickPlaceTask::plan() { ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions"); int max_solutions = pnh_.param("max_solutions", 10); return static_cast(task_->plan(max_solutions)); } bool PickPlaceTask::execute() { ROS_INFO_NAMED(LOGNAME, "Executing solution trajectory"); moveit_msgs::MoveItErrorCodes execute_result; execute_result = task_->execute(*task_->solutions().front()); if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { ROS_ERROR_STREAM_NAMED(LOGNAME, "Task execution failed and returned: " << execute_result.val); return false; } return true; } } // namespace moveit_task_constructor_demo