mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
3887b32ba1
commit
a070524860
@ -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;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user