mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge 7f3bdaa607 into 1655762a63
This commit is contained in:
commit
745ff9d465
@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
|
|||||||
moveit_ros_move_group
|
moveit_ros_move_group
|
||||||
moveit_task_constructor_core
|
moveit_task_constructor_core
|
||||||
moveit_task_constructor_msgs
|
moveit_task_constructor_msgs
|
||||||
|
moveit_task_constructor_core
|
||||||
pluginlib
|
pluginlib
|
||||||
std_msgs
|
std_msgs
|
||||||
)
|
)
|
||||||
@ -19,6 +20,7 @@ catkin_package(
|
|||||||
CATKIN_DEPENDS
|
CATKIN_DEPENDS
|
||||||
actionlib
|
actionlib
|
||||||
moveit_task_constructor_msgs
|
moveit_task_constructor_msgs
|
||||||
|
moveit_task_constructor_core
|
||||||
std_msgs
|
std_msgs
|
||||||
)
|
)
|
||||||
|
|
||||||
@ -30,6 +32,7 @@ include(CheckCXXSymbolExists)
|
|||||||
|
|
||||||
add_library(${PROJECT_NAME}
|
add_library(${PROJECT_NAME}
|
||||||
src/execute_task_solution_capability.cpp
|
src/execute_task_solution_capability.cpp
|
||||||
|
src/plan_pick_place_capability.cpp
|
||||||
)
|
)
|
||||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||||
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
||||||
|
|||||||
@ -4,4 +4,10 @@
|
|||||||
Action server to execute solutions generated through the MoveIt Task Constructor.
|
Action server to execute solutions generated through the MoveIt Task Constructor.
|
||||||
</description>
|
</description>
|
||||||
</class>
|
</class>
|
||||||
|
|
||||||
|
<class name="move_group/PlanPickPlaceCapability" type="move_group::PlanPickPlaceCapability" base_class_type="move_group::MoveGroupCapability">
|
||||||
|
<description>
|
||||||
|
Action server to plan full pick and place motions using the MoveIt Task Constructor.
|
||||||
|
</description>
|
||||||
|
</class>
|
||||||
</library>
|
</library>
|
||||||
|
|||||||
@ -20,6 +20,7 @@
|
|||||||
<depend>std_msgs</depend>
|
<depend>std_msgs</depend>
|
||||||
<depend>moveit_task_constructor_core</depend>
|
<depend>moveit_task_constructor_core</depend>
|
||||||
<depend>moveit_task_constructor_msgs</depend>
|
<depend>moveit_task_constructor_msgs</depend>
|
||||||
|
<depend>moveit_task_constructor_core</depend>
|
||||||
|
|
||||||
<export>
|
<export>
|
||||||
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
|
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
|
||||||
|
|||||||
78
capabilities/src/plan_pick_place_capability.cpp
Normal file
78
capabilities/src/plan_pick_place_capability.cpp
Normal file
@ -0,0 +1,78 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2016, Kentaro Wada.
|
||||||
|
* 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 Willow Garage nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived
|
||||||
|
* from this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
/* Author: Henning Kayser */
|
||||||
|
|
||||||
|
#include "plan_pick_place_capability.h"
|
||||||
|
|
||||||
|
#include <moveit/move_group/capability_names.h>
|
||||||
|
#include <moveit/robot_state/conversions.h>
|
||||||
|
|
||||||
|
|
||||||
|
namespace move_group {
|
||||||
|
|
||||||
|
PlanPickPlaceCapability::PlanPickPlaceCapability() : MoveGroupCapability("PlanPickPlace") {}
|
||||||
|
|
||||||
|
void PlanPickPlaceCapability::initialize() {
|
||||||
|
// configure the action server
|
||||||
|
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>(
|
||||||
|
root_node_handle_, "plan_pick_place",
|
||||||
|
std::bind(&PlanPickPlaceCapability::goalCallback, this, std::placeholders::_1), false));
|
||||||
|
as_->registerPreemptCallback(std::bind(&PlanPickPlaceCapability::preemptCallback, this));
|
||||||
|
as_->start();
|
||||||
|
pick_place_task_ = std::make_unique<PickPlaceTask>("pick_place_task");
|
||||||
|
}
|
||||||
|
|
||||||
|
void PlanPickPlaceCapability::goalCallback(
|
||||||
|
const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal) {
|
||||||
|
moveit_task_constructor_msgs::PlanPickPlaceResult result;
|
||||||
|
|
||||||
|
// TODO: fill parameters
|
||||||
|
PickPlaceTask::Parameters parameters;
|
||||||
|
|
||||||
|
// initialize task
|
||||||
|
pick_place_task_->init(parameters);
|
||||||
|
// run plan
|
||||||
|
pick_place_task_->plan();
|
||||||
|
// retrieve and return result
|
||||||
|
}
|
||||||
|
|
||||||
|
void PlanPickPlaceCapability::preemptCallback() {
|
||||||
|
// TODO(henningkayser): abort planning
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace move_group
|
||||||
|
|
||||||
|
#include <class_loader/class_loader.hpp>
|
||||||
|
CLASS_LOADER_REGISTER_CLASS(move_group::PlanPickPlaceCapability, move_group::MoveGroupCapability)
|
||||||
71
capabilities/src/plan_pick_place_capability.h
Normal file
71
capabilities/src/plan_pick_place_capability.h
Normal file
@ -0,0 +1,71 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* Software License Agreement (BSD License)
|
||||||
|
*
|
||||||
|
* Copyright (c) 2018, Hamburg University.
|
||||||
|
* 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 Hamburg University nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived
|
||||||
|
* from this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||||
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||||
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||||
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||||
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||||
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||||
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||||
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||||
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
* POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Capability to plan pick and place motions using the MoveIt Task Constructor.
|
||||||
|
*
|
||||||
|
* Author: Henning Kayser
|
||||||
|
* */
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/move_group/move_group_capability.h>
|
||||||
|
#include <actionlib/server/simple_action_server.h>
|
||||||
|
|
||||||
|
#include <moveit_task_constructor_msgs/PlanPickPlaceAction.h>
|
||||||
|
#include <moveit/task_constructor/tasks/pick_place_task.h>
|
||||||
|
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
namespace move_group {
|
||||||
|
|
||||||
|
using moveit::task_constructor::tasks::PickPlaceTask;
|
||||||
|
|
||||||
|
class PlanPickPlaceCapability : public MoveGroupCapability
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
PlanPickPlaceCapability();
|
||||||
|
|
||||||
|
virtual void initialize();
|
||||||
|
|
||||||
|
private:
|
||||||
|
void goalCallback(const moveit_task_constructor_msgs::PlanPickPlaceGoalConstPtr& goal);
|
||||||
|
void preemptCallback();
|
||||||
|
|
||||||
|
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::PlanPickPlaceAction>> as_;
|
||||||
|
|
||||||
|
std::unique_ptr<PickPlaceTask> pick_place_task_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace move_group
|
||||||
128
core/include/moveit/task_constructor/tasks/pick_place_task.h
Normal file
128
core/include/moveit/task_constructor/tasks/pick_place_task.h
Normal file
@ -0,0 +1,128 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* BSD 3-Clause License
|
||||||
|
*
|
||||||
|
* Copyright (c) 2019 PickNik LLC.
|
||||||
|
* All rights reserved.
|
||||||
|
*
|
||||||
|
* Redistribution and use in source and binary forms, with or without
|
||||||
|
* modification, are permitted provided that the following conditions are met:
|
||||||
|
*
|
||||||
|
* * Redistributions of source code must retain the above copyright notice, this
|
||||||
|
* list of conditions and the following disclaimer.
|
||||||
|
*
|
||||||
|
* * Redistributions in binary form must reproduce the above copyright notice,
|
||||||
|
* this list of conditions and the following disclaimer in the documentation
|
||||||
|
* and/or other materials provided with the distribution.
|
||||||
|
*
|
||||||
|
* * Neither the name of the copyright holder nor the names of its
|
||||||
|
* contributors may be used to endorse or promote products derived from
|
||||||
|
* this software without specific prior written permission.
|
||||||
|
*
|
||||||
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
|
||||||
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||||
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||||||
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||||
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
||||||
|
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
|
||||||
|
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*********************************************************************/
|
||||||
|
|
||||||
|
/* Author: Henning Kayser, Simon Goldstein
|
||||||
|
Desc: A demo to show MoveIt Task Constructor in action
|
||||||
|
*/
|
||||||
|
|
||||||
|
// ROS
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
// MoveIt
|
||||||
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
#include <moveit/robot_model/robot_model.h>
|
||||||
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
|
||||||
|
// MTC
|
||||||
|
#include <moveit/task_constructor/task.h>
|
||||||
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
|
#include <moveit/task_constructor/stages/connect.h>
|
||||||
|
#include <moveit/task_constructor/stages/current_state.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_pose.h>
|
||||||
|
#include <moveit/task_constructor/stages/generate_place_pose.h>
|
||||||
|
#include <moveit/task_constructor/stages/modify_planning_scene.h>
|
||||||
|
#include <moveit/task_constructor/stages/move_relative.h>
|
||||||
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
|
#include <moveit/task_constructor/stages/predicate_filter.h>
|
||||||
|
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||||
|
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||||
|
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
|
||||||
|
|
||||||
|
#include <actionlib/client/simple_action_client.h>
|
||||||
|
#include <actionlib/server/simple_action_server.h>
|
||||||
|
|
||||||
|
#include <eigen_conversions/eigen_msg.h>
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
namespace moveit {
|
||||||
|
namespace task_constructor {
|
||||||
|
namespace tasks {
|
||||||
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
|
class PickPlaceTask
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
struct Parameters
|
||||||
|
{
|
||||||
|
// planning group properties
|
||||||
|
std::string arm_group_name_;
|
||||||
|
std::string eef_name_;
|
||||||
|
std::string hand_group_name_;
|
||||||
|
std::string hand_frame_;
|
||||||
|
|
||||||
|
// object + surface
|
||||||
|
std::vector<std::string> support_surfaces_;
|
||||||
|
std::string object_reference_frame_;
|
||||||
|
std::string surface_link_;
|
||||||
|
std::string object_name_;
|
||||||
|
std::string world_frame_;
|
||||||
|
std::string grasp_frame_;
|
||||||
|
std::vector<double> object_dimensions_;
|
||||||
|
|
||||||
|
// Predefined pose targets
|
||||||
|
std::string hand_open_pose_;
|
||||||
|
std::string hand_close_pose_;
|
||||||
|
std::string arm_home_pose_;
|
||||||
|
|
||||||
|
// Pick metrics
|
||||||
|
Eigen::Isometry3d grasp_frame_transform_;
|
||||||
|
double approach_object_min_dist_;
|
||||||
|
double approach_object_max_dist_;
|
||||||
|
double lift_object_min_dist_;
|
||||||
|
double lift_object_max_dist_;
|
||||||
|
double place_object_min_dist_;
|
||||||
|
double place_object_max_dist_;
|
||||||
|
double retreat_object_min_dist_;
|
||||||
|
double retreat_object_max_dist_;
|
||||||
|
|
||||||
|
// Place metrics
|
||||||
|
geometry_msgs::PoseStamped place_pose_;
|
||||||
|
double place_surface_offset_;
|
||||||
|
};
|
||||||
|
|
||||||
|
PickPlaceTask(const std::string& task_name);
|
||||||
|
~PickPlaceTask() = default;
|
||||||
|
|
||||||
|
void init(const Parameters& parameters);
|
||||||
|
|
||||||
|
bool plan();
|
||||||
|
|
||||||
|
private:
|
||||||
|
moveit::task_constructor::TaskPtr task_;
|
||||||
|
const std::string task_name_;
|
||||||
|
};
|
||||||
|
|
||||||
|
} // namespace tasks
|
||||||
|
} // namespace task_constructor
|
||||||
|
} // namespace moveit
|
||||||
@ -46,6 +46,7 @@ target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
|
|||||||
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
|
||||||
|
|
||||||
add_subdirectory(stages)
|
add_subdirectory(stages)
|
||||||
|
add_subdirectory(tasks)
|
||||||
|
|
||||||
install(TARGETS ${PROJECT_NAME}
|
install(TARGETS ${PROJECT_NAME}
|
||||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
|
|||||||
9
core/src/tasks/CMakeLists.txt
Normal file
9
core/src/tasks/CMakeLists.txt
Normal file
@ -0,0 +1,9 @@
|
|||||||
|
add_library(${PROJECT_NAME}_tasks
|
||||||
|
${PROJECT_INCLUDE}/tasks/pick_place_task.h
|
||||||
|
pick_place_task.cpp
|
||||||
|
)
|
||||||
|
target_link_libraries(${PROJECT_NAME}_tasks ${PROJECT_NAME} ${catkin_LIBRARIES})
|
||||||
|
|
||||||
|
install(TARGETS ${PROJECT_NAME}_tasks
|
||||||
|
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||||
|
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
|
||||||
401
core/src/tasks/pick_place_task.cpp
Normal file
401
core/src/tasks/pick_place_task.cpp
Normal file
@ -0,0 +1,401 @@
|
|||||||
|
/*********************************************************************
|
||||||
|
* 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 <moveit/task_constructor/tasks/pick_place_task.h>
|
||||||
|
#include <rosparam_shortcuts/rosparam_shortcuts.h>
|
||||||
|
|
||||||
|
namespace moveit {
|
||||||
|
namespace task_constructor {
|
||||||
|
namespace tasks {
|
||||||
|
|
||||||
|
constexpr char LOGNAME[] = "pick_place_task";
|
||||||
|
PickPlaceTask::PickPlaceTask(const std::string& task_name)
|
||||||
|
: task_name_(task_name) {}
|
||||||
|
|
||||||
|
void PickPlaceTask::init(const Parameters& parameters)
|
||||||
|
{
|
||||||
|
ROS_INFO_NAMED(LOGNAME, "Initializing task pipeline");
|
||||||
|
// Reset ROS introspection before constructing the new object
|
||||||
|
// TODO(henningkayser): verify this is a bug, fix if possible
|
||||||
|
task_.reset();
|
||||||
|
task_.reset(new moveit::task_constructor::Task(task_name_));
|
||||||
|
Task& t = *task_;
|
||||||
|
t.loadRobotModel();
|
||||||
|
|
||||||
|
// Sampling planner
|
||||||
|
// TODO(henningkayser): Setup and parameterize alternative planners
|
||||||
|
auto sampling_planner = std::make_shared<solvers::PipelinePlanner>();
|
||||||
|
sampling_planner->setProperty("goal_joint_tolerance", 1e-5);
|
||||||
|
|
||||||
|
// Cartesian planner
|
||||||
|
auto cartesian_planner = std::make_shared<solvers::CartesianPath>();
|
||||||
|
cartesian_planner->setMaxVelocityScaling(1.0);
|
||||||
|
cartesian_planner->setMaxAccelerationScaling(1.0);
|
||||||
|
cartesian_planner->setStepSize(.01);
|
||||||
|
|
||||||
|
// Set task properties
|
||||||
|
t.setProperty("group", parameters.arm_group_name_);
|
||||||
|
t.setProperty("eef", parameters.eef_name_);
|
||||||
|
t.setProperty("hand", parameters.hand_group_name_);
|
||||||
|
t.setProperty("ik_frame", parameters.hand_frame_);
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
* *
|
||||||
|
* Current State *
|
||||||
|
* *
|
||||||
|
***************************************************/
|
||||||
|
Stage* current_state = nullptr; // Forward current_state on to grasp pose generator
|
||||||
|
{
|
||||||
|
auto _current_state = std::make_unique<stages::CurrentState>("current state");
|
||||||
|
_current_state->setTimeout(10);
|
||||||
|
|
||||||
|
// Verify that object is not attachd
|
||||||
|
auto applicability_filter =
|
||||||
|
std::make_unique<stages::PredicateFilter>("applicability test", std::move(_current_state));
|
||||||
|
applicability_filter->setPredicate([&](const SolutionBase& s, std::string& comment) {
|
||||||
|
s.start()->scene()->printKnownObjects(std::cout);
|
||||||
|
if (s.start()->scene()->getCurrentState().hasAttachedBody(parameters.object_name_))
|
||||||
|
{
|
||||||
|
comment = "object with id '" + parameters.object_name_ + "' is already attached and cannot be picked";
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
});
|
||||||
|
|
||||||
|
current_state = applicability_filter.get();
|
||||||
|
t.add(std::move(applicability_filter));
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
* *
|
||||||
|
* Open Hand *
|
||||||
|
* *
|
||||||
|
***************************************************/
|
||||||
|
{ // Open Hand
|
||||||
|
auto stage = std::make_unique<stages::MoveTo>("open hand", sampling_planner);
|
||||||
|
stage->setGroup(parameters.hand_group_name_);
|
||||||
|
stage->setGoal(parameters.hand_open_pose_);
|
||||||
|
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{ { parameters.arm_group_name_, sampling_planner } });
|
||||||
|
stage->setTimeout(5.0);
|
||||||
|
stage->properties().configureInitFrom(Stage::PARENT);
|
||||||
|
t.add(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
* *
|
||||||
|
* Pick Object *
|
||||||
|
* *
|
||||||
|
***************************************************/
|
||||||
|
Stage* attach_object_stage = nullptr; // Forward attach_object_stage to place pose generator
|
||||||
|
{
|
||||||
|
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", parameters.hand_frame_);
|
||||||
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
|
stage->setMinMaxDistance(parameters.approach_object_min_dist_, parameters.approach_object_max_dist_);
|
||||||
|
|
||||||
|
// Set hand forward direction
|
||||||
|
geometry_msgs::Vector3Stamped vec;
|
||||||
|
vec.header.frame_id = parameters.object_name_;
|
||||||
|
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(parameters.hand_open_pose_);
|
||||||
|
stage->setObject(parameters.object_name_);
|
||||||
|
stage->setAngleDelta(M_PI / 12);
|
||||||
|
stage->setMonitoredStage(current_state); // Hook into current state
|
||||||
|
|
||||||
|
// Compute IK
|
||||||
|
auto wrapper = std::make_unique<stages::ComputeIK>("grasp pose IK", std::move(stage));
|
||||||
|
wrapper->setMaxIKSolutions(2);
|
||||||
|
wrapper->setMinSolutionDistance(1.0);
|
||||||
|
wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
|
||||||
|
//wrapper->setIgnoreCollisions(true);
|
||||||
|
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(
|
||||||
|
parameters.object_name_,
|
||||||
|
t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
|
||||||
|
true);
|
||||||
|
grasp->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
---- * Close Hand *
|
||||||
|
***************************************************/
|
||||||
|
{
|
||||||
|
auto stage = std::make_unique<stages::MoveTo>("close hand", sampling_planner);
|
||||||
|
stage->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
|
||||||
|
stage->setGoal(parameters.hand_close_pose_);
|
||||||
|
grasp->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
.... * Attach Object *
|
||||||
|
***************************************************/
|
||||||
|
{
|
||||||
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("attach object");
|
||||||
|
stage->attachObject(parameters.object_name_, parameters.hand_frame_);
|
||||||
|
attach_object_stage = stage.get();
|
||||||
|
grasp->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/****************************************************
|
||||||
|
.... * Allow collision (object support) *
|
||||||
|
***************************************************/
|
||||||
|
{
|
||||||
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("allow collision (object,support)");
|
||||||
|
stage->allowCollisions({ parameters.object_name_ }, "source_container", true); // TODO(henningkayser): parameterize
|
||||||
|
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(parameters.lift_object_min_dist_, parameters.lift_object_max_dist_);
|
||||||
|
stage->setIKFrame(parameters.hand_frame_);
|
||||||
|
stage->properties().set("marker_ns", "lift_object");
|
||||||
|
|
||||||
|
// Set upward direction
|
||||||
|
geometry_msgs::Vector3Stamped vec;
|
||||||
|
vec.header.frame_id = parameters.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({ parameters.object_name_ }, "source_container", false); // TODO(henningkayser): parameterize
|
||||||
|
grasp->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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{ { parameters.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", parameters.hand_frame_);
|
||||||
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
|
stage->setMinMaxDistance(parameters.place_object_min_dist_, parameters.place_object_max_dist_);
|
||||||
|
|
||||||
|
// Set downward direction
|
||||||
|
geometry_msgs::Vector3Stamped vec;
|
||||||
|
vec.header.frame_id = parameters.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(parameters.object_name_);
|
||||||
|
stage->setPose(parameters.place_pose_);
|
||||||
|
// Hook into attach_object_stage which allows us to use the attached object as IK frame
|
||||||
|
stage->setMonitoredStage(attach_object_stage);
|
||||||
|
|
||||||
|
// Compute IK
|
||||||
|
auto wrapper = std::make_unique<stages::ComputeIK>("place pose IK", std::move(stage));
|
||||||
|
wrapper->setMaxIKSolutions(2);
|
||||||
|
wrapper->setIKFrame(parameters.grasp_frame_transform_, parameters.hand_frame_);
|
||||||
|
// TODO(henningkayser): Enable collisions
|
||||||
|
wrapper->setIgnoreCollisions(true);
|
||||||
|
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->properties().property("group").configureInitFrom(Stage::PARENT, parameters.hand_group_name_);
|
||||||
|
stage->setGoal(parameters.hand_open_pose_);
|
||||||
|
place->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/******************************************************
|
||||||
|
---- * Forbid collision (hand, object) *
|
||||||
|
*****************************************************/
|
||||||
|
{
|
||||||
|
// TODO(henningkayser): Forbid collision after retreat?
|
||||||
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("forbid collision (hand,object)");
|
||||||
|
stage->allowCollisions(
|
||||||
|
parameters.object_name_,
|
||||||
|
t.getRobotModel()->getJointModelGroup(parameters.hand_group_name_)->getLinkModelNamesWithCollisionGeometry(),
|
||||||
|
false);
|
||||||
|
place->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/******************************************************
|
||||||
|
---- * Detach Object *
|
||||||
|
*****************************************************/
|
||||||
|
{
|
||||||
|
auto stage = std::make_unique<stages::ModifyPlanningScene>("detach object");
|
||||||
|
stage->detachObject(parameters.object_name_, parameters.hand_frame_);
|
||||||
|
place->insert(std::move(stage));
|
||||||
|
}
|
||||||
|
|
||||||
|
/******************************************************
|
||||||
|
---- * Retreat Motion *
|
||||||
|
*****************************************************/
|
||||||
|
{
|
||||||
|
// TODO(henningkayser): Do we need this if items are dropped?
|
||||||
|
auto stage = std::make_unique<stages::MoveRelative>("retreat after place", cartesian_planner);
|
||||||
|
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
|
||||||
|
stage->setMinMaxDistance(parameters.retreat_object_min_dist_, parameters.retreat_object_max_dist_);
|
||||||
|
stage->setIKFrame(parameters.hand_frame_);
|
||||||
|
stage->properties().set("marker_ns", "retreat");
|
||||||
|
geometry_msgs::Vector3Stamped vec;
|
||||||
|
vec.header.frame_id = parameters.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(parameters.arm_home_pose_);
|
||||||
|
stage->restrictDirection(stages::MoveTo::FORWARD);
|
||||||
|
t.add(std::move(stage));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool PickPlaceTask::plan() {
|
||||||
|
ROS_INFO_NAMED(LOGNAME, "Start searching for task solutions");
|
||||||
|
try {
|
||||||
|
task_->plan(10); // TODO: parameterize
|
||||||
|
} catch (InitStageException& e) {
|
||||||
|
ROS_ERROR_STREAM_NAMED(LOGNAME, "Initialization failed: " << e);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
if (task_->numSolutions() == 0) {
|
||||||
|
ROS_ERROR_NAMED(LOGNAME, "Planning failed");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace tasks
|
||||||
|
} // namespace task_constructor
|
||||||
|
} // namespace moveit
|
||||||
@ -28,6 +28,7 @@ add_service_files(DIRECTORY srv FILES
|
|||||||
|
|
||||||
add_action_files(DIRECTORY action FILES
|
add_action_files(DIRECTORY action FILES
|
||||||
ExecuteTaskSolution.action
|
ExecuteTaskSolution.action
|
||||||
|
PlanPickPlace.action
|
||||||
)
|
)
|
||||||
|
|
||||||
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
generate_messages(DEPENDENCIES ${MSG_DEPS})
|
||||||
|
|||||||
16
msgs/action/PlanPickPlace.action
Normal file
16
msgs/action/PlanPickPlace.action
Normal file
@ -0,0 +1,16 @@
|
|||||||
|
# goal
|
||||||
|
string object_id
|
||||||
|
string end_effector
|
||||||
|
|
||||||
|
moveit_msgs/PlaceLocation place_location
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
# result
|
||||||
|
moveit_msgs/MoveItErrorCodes error_code
|
||||||
|
Solution solution
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
# feedback
|
||||||
|
string feedback
|
||||||
Loading…
Reference in New Issue
Block a user