From 47a992960831d4324f45664140419195eb3e6063 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 26 Nov 2019 21:03:49 +0100 Subject: [PATCH] "cartesian" demo: add an example stage for relative joint-space offsets --- demo/src/cartesian.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/demo/src/cartesian.cpp b/demo/src/cartesian.cpp index edf8a76b..728a08f9 100644 --- a/demo/src/cartesian.cpp +++ b/demo/src/cartesian.cpp @@ -97,6 +97,14 @@ Task createTask() { 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); + 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 } };