mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
mwe examples for core classes
This commit is contained in:
parent
1b1dadb94a
commit
0fbdfbc818
64
demo/scripts/alternatives.py
Executable file
64
demo/scripts/alternatives.py
Executable 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
42
demo/scripts/fallbacks.py
Executable 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
40
demo/scripts/merger.py
Executable 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)
|
||||
Loading…
Reference in New Issue
Block a user