mwe examples for core classes

This commit is contained in:
cpetersmeier 2021-12-28 15:33:18 +01:00 committed by Robert Haschke
parent 1b1dadb94a
commit 0fbdfbc818
3 changed files with 146 additions and 0 deletions

64
demo/scripts/alternatives.py Executable file
View File

@ -0,0 +1,64 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_alternatives")
# Use the joint interpolation planner
jointPlanner = core.JointInterpolationPlanner()
# Create a task
task = core.Task()
# Start from current robot state
currentState = stages.CurrentState("current state")
# Add the current state to the task hierarchy
task.add(currentState)
# The alternatives stage supports multiple execution paths
alternatives = core.Alternatives("Alternatives")
# goal 1
goalConfig1 = {
"panda_joint1": 1.0,
"panda_joint2": -1.0,
"panda_joint3": 0.0,
"panda_joint4": -2.5,
"panda_joint5": 1.0,
"panda_joint6": 1.0,
"panda_joint7": 1.0,
}
# goal 2
goalConfig2 = {
"panda_joint1": -3.0,
"panda_joint2": -1.0,
"panda_joint3": 0.0,
"panda_joint4": -2.0,
"panda_joint5": 1.0,
"panda_joint6": 2.0,
"panda_joint7": 0.5,
}
# First motion plan to compare
moveTo1 = stages.MoveTo("Move To Goal Configuration 1", jointPlanner)
moveTo1.group = "panda_arm"
moveTo1.setGoal(goalConfig1)
alternatives.insert(moveTo1)
# Second motion plan to compare
moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner)
moveTo2.group = "panda_arm"
moveTo2.setGoal(goalConfig2)
alternatives.insert(moveTo2)
# Add the alternatives stage to the task hierarchy
task.add(alternatives)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)

42
demo/scripts/fallbacks.py Executable file
View File

@ -0,0 +1,42 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_fallbacks")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()
jointPlanner = core.JointInterpolationPlanner()
# initialize the mtc task
task = core.Task()
# add the current planning scene state to the task hierarchy
currentState = stages.CurrentState("Current State")
task.add(currentState)
# create a fallback container to fall back to a different planner
# if motion generation fails with the primary one
fallbacks = core.Fallbacks("Fallbacks")
# primary motion plan
moveTo1 = stages.MoveTo("Move To Goal Configuration 1", cartesianPlanner)
moveTo1.group = "panda_arm"
moveTo1.setGoal("extended")
fallbacks.insert(moveTo1)
# fallback motion plan
moveTo2 = stages.MoveTo("Move To Goal Configuration 2", jointPlanner)
moveTo2.group = "panda_arm"
moveTo2.setGoal("extended")
fallbacks.insert(moveTo2)
# add the fallback container to the task hierarchy
task.add(fallbacks)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)

40
demo/scripts/merger.py Executable file
View File

@ -0,0 +1,40 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
import time
roscpp_init("mtc_tutorial_merger")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()
# the task will contain our stages
task = core.Task()
# start from current robot state
currentState = stages.CurrentState("current state")
task.add(currentState)
# the merger plans for two parallel execution paths
merger = core.Merger("Merger")
# first simultaneous execution
moveTo1 = stages.MoveTo("Move To Home", planner)
moveTo1.group = "hand"
moveTo1.setGoal("close")
merger.insert(moveTo1)
# second simultaneous execution
moveTo2 = stages.MoveTo("Move To Ready", planner)
moveTo2.group = "panda_arm"
moveTo2.setGoal("extended")
merger.insert(moveTo2)
# add the merger stage to the task hierarchy
task.add(merger)
if task.plan():
task.publish(task.solutions[0])
time.sleep(1)