diff --git a/capabilities/src/execute_task_solution_capability.cpp b/capabilities/src/execute_task_solution_capability.cpp index bf8dd8ca2..0a8803407 100644 --- a/capabilities/src/execute_task_solution_capability.cpp +++ b/capabilities/src/execute_task_solution_capability.cpp @@ -168,6 +168,12 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr exec_traj.trajectory_ = std::make_shared(model, group); exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory); + if (!sub_traj.controller_names.empty()) { + for (auto cn : sub_traj.controller_names) { + exec_traj.controller_names_.push_back(cn.data.data()); + } + } + /* TODO add action feedback and markers */ exec_traj.effect_on_success_ = [this, sub_traj, description](const plan_execution::ExecutableMotionPlan*) { #if MOVEIT_MASTER diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 5bd813a5b..dbd9f7676 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -179,6 +179,9 @@ class Stage /// marker namespace of solution markers const std::string& markerNS() { return properties().get("marker_ns"); } + void setControllers(std::vector controllers) { setProperty("controllers", controllers); } + std::vector controllers() { return properties().get>("controllers"); } + /// forwarding of properties between interface states void forwardProperties(const InterfaceState& source, InterfaceState& dest); std::set forwardedProperties() const { diff --git a/core/src/container.cpp b/core/src/container.cpp index ba8562369..899b8b832 100644 --- a/core/src/container.cpp +++ b/core/src/container.cpp @@ -201,6 +201,9 @@ bool ContainerBase::insert(Stage::pointer&& stage, int before) { return false; } + if (!this->controllers().empty()) + stage->setControllers(this->controllers()); + ContainerBasePrivate::const_iterator where = pimpl()->childByIndex(before, true); ContainerBasePrivate::iterator it = pimpl()->children_.insert(where, std::move(stage)); impl->setHierarchy(this, it); diff --git a/core/src/stage.cpp b/core/src/stage.cpp index cad416bba..a1f41b677 100644 --- a/core/src/stage.cpp +++ b/core/src/stage.cpp @@ -185,6 +185,7 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) { auto& p = properties(); p.declare("timeout", "timeout per run (s)"); p.declare("marker_ns", name(), "marker namespace"); + p.declare>("controllers", {}, "list of controllers to use for execution"); p.declare>("forwarded_properties", std::set(), "set of interface properties to forward"); diff --git a/core/src/storage.cpp b/core/src/storage.cpp index be480e1d9..8fde2fb72 100644 --- a/core/src/storage.cpp +++ b/core/src/storage.cpp @@ -157,6 +157,15 @@ void SubTrajectory::fillMessage(moveit_task_constructor_msgs::Solution& msg, Int moveit_task_constructor_msgs::SubTrajectory& t = msg.sub_trajectory.back(); SolutionBase::fillInfo(t.info, introspection); + std::vector controllers = creator()->me()->properties().get>("controllers"); + if (!controllers.empty()) { + for (auto cn : controllers) { + std_msgs::String s; + s.data = cn; + t.controller_names.push_back(s); + } + } + if (trajectory()) trajectory()->getRobotTrajectoryMsg(t.trajectory); diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt index 0960aec84..7b0c137ab 100644 --- a/demo/CMakeLists.txt +++ b/demo/CMakeLists.txt @@ -25,6 +25,9 @@ target_link_libraries(cartesian ${catkin_LIBRARIES}) add_executable(modular src/modular.cpp) target_link_libraries(modular ${catkin_LIBRARIES}) +add_executable(ctr_change src/ctrl_change.cpp) +target_link_libraries(ctr_change ${catkin_LIBRARIES}) + add_library(${PROJECT_NAME}_lib src/pick_place_task.cpp) set_target_properties(${PROJECT_NAME}_lib PROPERTIES OUTPUT_NAME moveit_task_constructor_demo_pick_place) add_dependencies(${PROJECT_NAME}_lib ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/demo/src/ctrl_change.cpp b/demo/src/ctrl_change.cpp new file mode 100644 index 000000000..7dd201d9b --- /dev/null +++ b/demo/src/ctrl_change.cpp @@ -0,0 +1,192 @@ +/********************************************************************* + * Copyright (c) 2019 Bielefeld 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 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: Robert Haschke + Desc: Planning a simple sequence of Cartesian motions +*/ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +using namespace moveit::task_constructor; + +Task createTask() { + Task t; + t.stages()->setName("Cartesian Path"); + + const std::string group = "panda_arm"; + + // create Cartesian interpolation "planner" to be used in stages + auto cartesian = std::make_shared(); + + // start from a fixed robot state + t.loadRobotModel(); + auto scene = std::make_shared(t.getRobotModel()); + { + auto& state = scene->getCurrentStateNonConst(); + state.setToDefaultValues(state.getJointModelGroup(group), "ready"); + + auto fixed = std::make_unique("initial state"); + fixed->setState(scene); + t.add(std::move(fixed)); + } + + { + auto stage = std::make_unique("x +0.2", cartesian); + stage->setGroup(group); + stage->setControllers({ "fake_via_panda_arm_controller" }); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.x = 0.2; + stage->setDirection(direction); + t.add(std::move(stage)); + } + + { + auto serial = std::make_unique(); + serial->setControllers({ "fake_last_panda_arm_controller" }); + // TODO smth like serial->setForwardedProperties({"controllers"}); but for children instead InterfaceStates + + { + auto stage = std::make_unique("y -0.3", cartesian); + stage->setGroup(group); + // stage->properties().configureInitFrom(Stage::PARENT, { "controllers" }); + geometry_msgs::Vector3Stamped direction; + direction.header.frame_id = "world"; + direction.vector.y = -0.3; + stage->setDirection(direction); + serial->insert(std::move(stage)); + } + + { // rotate about TCP + auto stage = std::make_unique("rz +45°", cartesian); + stage->setGroup(group); + // stage->properties().configureInitFrom(Stage::PARENT, { "controllers" }); + geometry_msgs::TwistStamped twist; + twist.header.frame_id = "world"; + twist.twist.angular.z = M_PI / 4.; + stage->setDirection(twist); + serial->insert(std::move(stage)); + } + + t.add(std::move(serial)); + } + + { // perform a Cartesian motion, defined as a relative offset in joint space + auto stage = std::make_unique("joint offset", cartesian); + stage->setGroup(group); + std::map offsets = { { "panda_joint1", M_PI / 6. }, { "panda_joint3", -M_PI / 6 } }; + stage->setDirection(offsets); + t.add(std::move(stage)); + } + + { // move from reached state back to the original state, using joint interpolation + auto joint_interpolation = std::make_shared(); + stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } }; + auto connect = std::make_unique("connect", planners); + t.add(std::move(connect)); + } + + { // final state is original state again + auto fixed = std::make_unique("final state"); + fixed->setState(scene); + t.add(std::move(fixed)); + } + + return t; +} + +std::string buildControllerString(moveit_task_constructor_msgs::SubTrajectory st) { + if (st.controller_names.empty()) + return ""; + + std::stringstream ss; + for (auto cn : st.controller_names) { + ss << cn.data << ";"; + } + return ss.str(); +} + +int main(int argc, char** argv) { + ros::init(argc, argv, "mtc_tutorial"); + // run an asynchronous spinner to communicate with the move_group node and rviz + ros::AsyncSpinner spinner(4); + spinner.start(); + + auto task = createTask(); + try { + if (task.plan()) { + task.introspection().publishSolution(*task.solutions().front()); + } else { + std::cerr << "planning failed" << std::endl; + return 0; + } + } catch (const InitStageException& ex) { + std::cerr << "planning failed with exception" << std::endl << ex << task; + } + + moveit_task_constructor_msgs::ExecuteTaskSolutionGoal execute_goal; + task.solutions().front()->fillMessage(execute_goal.solution); + + printf("SubTrajectories: %zu\n", execute_goal.solution.sub_trajectory.size()); + + for (int i = 0; i < execute_goal.solution.sub_trajectory.size(); i++) { + auto st = execute_goal.solution.sub_trajectory[i]; + printf("\tST%d with %zu points and controllers \"%s\"\n", i, st.trajectory.joint_trajectory.points.size(), + buildControllerString(st).c_str()); + } + + actionlib::SimpleActionClient ac("execute_task_solution", + false); + ac.waitForServer(); + + ac.sendGoal(execute_goal); + ac.waitForResult(); + + moveit_msgs::MoveItErrorCodes execute_result = ac.getResult()->error_code; + + if (execute_result.val != moveit_msgs::MoveItErrorCodes::SUCCESS) { + ROS_ERROR_STREAM_NAMED("move_cart", "Task execution failed and returned: " << ac.getState().toString()); + return false; + } + + ros::waitForShutdown(); // keep alive for interactive inspection in rviz + return 0; +} diff --git a/msgs/CMakeLists.txt b/msgs/CMakeLists.txt index 07ed05951..7c5e5145d 100644 --- a/msgs/CMakeLists.txt +++ b/msgs/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 2.8.12) project(moveit_task_constructor_msgs) -set(MSG_DEPS moveit_msgs visualization_msgs) +set(MSG_DEPS moveit_msgs visualization_msgs std_msgs) find_package(catkin REQUIRED COMPONENTS message_generation diff --git a/msgs/msg/SubTrajectory.msg b/msgs/msg/SubTrajectory.msg index 10e3fde8a..4810c32a1 100644 --- a/msgs/msg/SubTrajectory.msg +++ b/msgs/msg/SubTrajectory.msg @@ -6,3 +6,6 @@ moveit_msgs/RobotTrajectory trajectory # planning scene of end state as diff w.r.t. start state moveit_msgs/PlanningScene scene_diff + +# list of controllers to use for execution +std_msgs/String[] controller_names diff --git a/msgs/package.xml b/msgs/package.xml index f0bf4ec57..4eafbaa80 100644 --- a/msgs/package.xml +++ b/msgs/package.xml @@ -10,6 +10,7 @@ catkin message_generation + std_msgs moveit_msgs visualization_msgs