a move_group capability to execute Task Solutions

This does not work perfectly yet in practice,
but this is (probably) due to problems in upstream MoveIt.
This commit is contained in:
v4hn 2018-06-05 18:36:20 +02:00 committed by Robert Haschke
parent 7bd9bcf972
commit 73b7475cdb
7 changed files with 345 additions and 0 deletions

View File

@ -0,0 +1,37 @@
cmake_minimum_required(VERSION 2.8.3)
project(moveit_task_constructor_capabilities)
add_compile_options(-std=c++14)
find_package(catkin REQUIRED COMPONENTS
actionlib
moveit_core
moveit_ros_move_group
moveit_task_constructor_msgs
pluginlib
std_msgs
)
catkin_package(
LIBRARIES $PROJECT_NAME}
CATKIN_DEPENDS
std_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
)
install(FILES capabilities_plugin_description.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

View File

@ -0,0 +1,7 @@
<library path="libmoveit_task_constructor_capabilities">
<class name="move_group/ExecuteTaskSolutionCapability" type="move_group::ExecuteTaskSolutionCapability" base_class_type="move_group::MoveGroupCapability">
<description>
Action server to execute solutions generated through the MoveIt Task Constructor.
</description>
</class>
</library>

29
capabilities/package.xml Normal file
View File

@ -0,0 +1,29 @@
<?xml version="1.0"?>
<package>
<name>moveit_task_constructor_capabilities</name>
<version>0.0.0</version>
<description>
MoveGroupCapabilites to interact with MoveIt
</description>
<maintainer email="me@v4hn.de">Michael v4hn Goerner</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>moveit_core</build_depend>
<build_depend>moveit_ros_move_group</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>moveit_core</run_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>std_msgs</run_depend>
<export>
<moveit_ros_move_group plugin="${prefix}/capabilities_plugin_description.xml"/>
</export>
</package>

View File

@ -0,0 +1,186 @@
/*********************************************************************
* 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: Michael 'v4hn' Goerner */
#include "execute_task_solution_capability.h"
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h>
namespace {
// TODO: move to moveit::core::RobotModel
const moveit::core::JointModelGroup* findJointModelGroup(const moveit::core::RobotModel& model, const std::vector<std::string>& joints) {
std::set<std::string> joint_set(joints.begin(), joints.end());
const std::vector<const moveit::core::JointModelGroup*>& jmgs = model.getJointModelGroups();
for(const moveit::core::JointModelGroup* jmg : jmgs){
const std::vector<std::string>& jmg_joints = jmg->getJointModelNames();
std::set<std::string> jmg_joint_set(jmg_joints.begin(), jmg_joints.end());
// return group if sets agree on all active joints
if(std::includes(jmg_joint_set.begin(), jmg_joint_set.end(), joint_set.begin(), joint_set.end())) {
std::set<std::string> difference;
std::set_difference(jmg_joint_set.begin(), jmg_joint_set.end(),
joint_set.begin(), joint_set.end(),
std::inserter(difference, difference.begin()));
for(const std::string& diff_joint : difference){
const moveit::core::JointModel* diff_jm = model.getJointModel(diff_joint);
if(!diff_jm->isPassive() && !diff_jm->getMimic())
continue;
}
return jmg;
}
}
return nullptr;
}
}
namespace move_group
{
ExecuteTaskSolutionCapability::ExecuteTaskSolutionCapability()
: MoveGroupCapability("ExecuteTaskSolution")
{}
void ExecuteTaskSolutionCapability::initialize() {
// configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
root_node_handle_, "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(
std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
as_->start();
}
void ExecuteTaskSolutionCapability::goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
if (!context_->plan_execution_) {
const std::string response = "Cannot execute solution. ~allow_trajectory_execution was set to false";
result.error_code.val = moveit_msgs::MoveItErrorCodes::CONTROL_FAILED;
as_->setAborted(result, response);
return;
}
plan_execution::ExecutableMotionPlan plan;
if(!constructMotionPlan(goal->task_solution, plan))
result.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
else {
ROS_INFO_NAMED("ExecuteTaskSolution", "Executing TaskSolution");
result.error_code = context_->plan_execution_->executeAndMonitor(plan);
}
const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code);
if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
as_->setSucceeded(result, response);
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
as_->setPreempted(result, response);
else
as_->setAborted(result, response);
}
void ExecuteTaskSolutionCapability::preemptCallback() {
if(context_->plan_execution_)
context_->plan_execution_->stop();
}
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan) {
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
robot_state::RobotState state(model);
{
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
state= scene->getCurrentState();
}
plan.plan_components_.reserve(solution.sub_trajectory.size());
for(size_t i= 0; i < solution.sub_trajectory.size(); ++i) {
const moveit_task_constructor_msgs::SubTrajectory& sub_traj = solution.sub_trajectory[i];
plan.plan_components_.emplace_back();
plan_execution::ExecutableTrajectory& exec_traj = plan.plan_components_.back();
exec_traj.description_ = std::to_string(i+1) + "/"
+ std::to_string(solution.sub_trajectory.size())
+ " - subsolution " + std::to_string(sub_traj.id)
+ " of stage " + std::to_string(sub_traj.stage_id);
const moveit::core::JointModelGroup* group = nullptr;
{
std::vector<std::string> joint_names(sub_traj.trajectory.joint_trajectory.joint_names);
joint_names.insert(joint_names.end(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.begin(),
sub_traj.trajectory.multi_dof_joint_trajectory.joint_names.end());
group = findJointModelGroup(*model, joint_names);
if(!group){
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {" << boost::algorithm::join(joint_names, ", ") << "}" );
return false;
}
}
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this,sub_traj](const plan_execution::ExecutableMotionPlan*){
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)){
planning_scene_monitor::LockedPlanningSceneRW scene(context_->planning_scene_monitor_);
return scene->usePlanningSceneMsg(sub_traj.scene_diff);
}
return true;
};
if(!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)){
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "invalid intermediate robot state in scene diff of SubTrajectory " << exec_traj.description_);
return false;
}
}
return true;
}
} // namespace move_group
#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)

View File

@ -0,0 +1,70 @@
/*********************************************************************
* 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 execute trajectories through a ROS action
* that were generated by the MoveIt Task Constructor.
*
* Author: Michael 'v4hn' Goerner
* */
#pragma once
#include <moveit/move_group/move_group_capability.h>
#include <actionlib/server/simple_action_server.h>
#include <moveit_task_constructor_msgs/ExecuteTaskSolutionAction.h>
#include <memory>
namespace move_group
{
class ExecuteTaskSolutionCapability : public MoveGroupCapability
{
public:
ExecuteTaskSolutionCapability();
virtual void initialize();
private:
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution, plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void preemptCallback();
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;
};
} // namespace move_group

View File

@ -24,6 +24,10 @@ add_service_files(DIRECTORY srv FILES
GetSolution.srv
)
add_action_files(DIRECTORY action FILES
ExecuteTaskSolution.action
)
generate_messages(DEPENDENCIES ${MSG_DEPS})
catkin_package(

View File

@ -0,0 +1,12 @@
# Task solution to execute
Solution task_solution
---
# result of execution
moveit_msgs/MoveItErrorCodes error_code
---
# current stage/subtrajectory during execution
string state