moveit_task_constructor/demo/src/pick_place_task.cpp
2023-05-26 16:38:15 +02:00

473 lines
20 KiB
C++

/*********************************************************************
* 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 <Eigen/Geometry>
#include <moveit_task_constructor_demo/pick_place_task.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#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<double>& 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<double>& 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<solvers::PipelinePlanner>(node);
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
// Cartesian planner
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
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<stages::CurrentState>("current state");
// Verify that object is not attached
auto applicability_filter =
std::make_unique<stages::PredicateFilter>("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<stages::MoveTo>("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<stages::Connect>(
"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<SerialContainer>("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<stages::MoveRelative>("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<stages::GenerateGraspPose>("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<stages::ComputeIK>("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<stages::ModifyPlanningScene>("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<stages::MoveTo>("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<stages::ModifyPlanningScene>("attach object");
stage->attachObject(params.object_name, params.hand_frame);
grasp->insert(std::move(stage));
}
/****************************************************
.... * Allow collision (object support) *
***************************************************/
{
auto stage = std::make_unique<stages::ModifyPlanningScene>("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<stages::MoveRelative>("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<stages::ModifyPlanningScene>("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<stages::Connect>(
"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<SerialContainer>("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<stages::MoveRelative>("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<stages::GeneratePlacePose>("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<stages::ComputeIK>("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<stages::MoveTo>("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<stages::ModifyPlanningScene>("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<stages::ModifyPlanningScene>("detach object");
stage->detachObject(params.object_name, params.hand_frame);
place->insert(std::move(stage));
}
/******************************************************
---- * Retreat Motion *
*****************************************************/
{
auto stage = std::make_unique<stages::MoveRelative>("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<stages::MoveTo>("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<bool>(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<moveit_task_constructor_msgs::msg::ExecuteTaskSolutionAction>
// 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