From 718ad320c27102c8da4c20fe0ea309e57d3d0f6d Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 4 Mar 2020 10:25:04 +0100 Subject: [PATCH] cartesian demo: illustrate merging of trajectories --- demo/src/cartesian.cpp | 26 ++++++++++++++++++-------- 1 file changed, 18 insertions(+), 8 deletions(-) diff --git a/demo/src/cartesian.cpp b/demo/src/cartesian.cpp index 728a08f9..5975e968 100644 --- a/demo/src/cartesian.cpp +++ b/demo/src/cartesian.cpp @@ -51,9 +51,12 @@ Task createTask() { t.stages()->setName("Cartesian Path"); const std::string group = "panda_arm"; + const std::string eef = "hand"; - // create Cartesian interpolation "planner" to be used in stages - auto cartesian = std::make_shared(); + // 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(); @@ -68,7 +71,7 @@ Task createTask() { } { - auto stage = std::make_unique("x +0.2", cartesian); + auto stage = std::make_unique("x +0.2", cartesian_interpolation); stage->setGroup(group); geometry_msgs::Vector3Stamped direction; direction.header.frame_id = "world"; @@ -78,7 +81,7 @@ Task createTask() { } { - auto stage = std::make_unique("y -0.3", cartesian); + auto stage = std::make_unique("y -0.3", cartesian_interpolation); stage->setGroup(group); geometry_msgs::Vector3Stamped direction; direction.header.frame_id = "world"; @@ -88,7 +91,7 @@ Task createTask() { } { // rotate about TCP - auto stage = std::make_unique("rz +45°", cartesian); + auto stage = std::make_unique("rz +45°", cartesian_interpolation); stage->setGroup(group); geometry_msgs::TwistStamped twist; twist.header.frame_id = "world"; @@ -98,16 +101,23 @@ Task createTask() { } { // perform a Cartesian motion, defined as a relative offset in joint space - auto stage = std::make_unique("joint offset", cartesian); + 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 - auto joint_interpolation = std::make_shared(); - stages::Connect::GroupPlannerVector planners = { { group, 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)); }