Enable infinite number of repetitions for "modular" demo

Using a joint-space interpolator to move back to start pose,
we avoid pose drift in the Cartesian loop, which pushes the robot
into its joint limits over time. Fixes #113.
This commit is contained in:
Robert Haschke 2019-11-26 21:02:00 +01:00
parent 3887b32ba1
commit a070524860

View File

@ -53,6 +53,8 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
// create Cartesian interpolation "planner" to be used in stages
auto cartesian = std::make_shared<solvers::CartesianPath>();
// create joint interpolation "planner"
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
{
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian);
@ -85,7 +87,7 @@ std::unique_ptr<SerialContainer> createModule(const std::string& group) {
}
{ // move back to ready pose
auto stage = std::make_unique<stages::MoveTo>("moveTo ready", cartesian);
auto stage = std::make_unique<stages::MoveTo>("moveTo ready", joint_interpolation);
stage->properties().configureInitFrom(Stage::PARENT);
stage->setGoal("ready");
c->insert(std::move(stage));
@ -101,6 +103,9 @@ Task createTask() {
const std::string group = "panda_arm";
t.add(createModule(group));
t.add(createModule(group));
t.add(createModule(group));
t.add(createModule(group));
t.add(createModule(group));
return t;
}