/********************************************************************* * 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 using namespace moveit::task_constructor; Task createTask() { Task t; t.stages()->setName("Cartesian Path"); const std::string group = "panda_arm"; const std::string eef = "hand"; // create Cartesian interpolation "planner" to be used in various stages auto cartesian_interpolation = std::make_shared(); // create a joint-space interpolation "planner" to be used in various stages auto joint_interpolation = 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_interpolation); stage->setGroup(group); geometry_msgs::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.x = 0.2; stage->setDirection(direction); t.add(std::move(stage)); } { auto stage = std::make_unique("y -0.3", cartesian_interpolation); stage->setGroup(group); geometry_msgs::Vector3Stamped direction; direction.header.frame_id = "world"; direction.vector.y = -0.3; stage->setDirection(direction); t.add(std::move(stage)); } { // rotate about TCP auto stage = std::make_unique("rz +45°", cartesian_interpolation); stage->setGroup(group); geometry_msgs::TwistStamped twist; twist.header.frame_id = "world"; twist.twist.angular.z = M_PI / 4.; stage->setDirection(twist); t.add(std::move(stage)); } { // perform a Cartesian motion, defined as a relative offset in joint space auto stage = std::make_unique("joint offset", cartesian_interpolation); 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 gripper into predefined open state auto stage = std::make_unique("open gripper", joint_interpolation); stage->setGroup(eef); stage->setGoal("open"); t.add(std::move(stage)); } { // move from reached state back to the original state, using joint interpolation // specifying two groups (arm and hand) will try to merge both trajectories stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation }, { eef, 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; } 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(1); spinner.start(); auto task = createTask(); try { if (task.plan()) task.introspection().publishSolution(*task.solutions().front()); } catch (const InitStageException& ex) { std::cerr << "planning failed with exception" << std::endl << ex << task; } ros::waitForShutdown(); // keep alive for interactive inspection in rviz return 0; }