mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
45 lines
1.2 KiB
Python
Executable File
45 lines
1.2 KiB
Python
Executable File
#! /usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
|
|
from moveit.task_constructor import core, stages
|
|
from moveit_commander.roscpp_initializer import roscpp_initialize
|
|
import time
|
|
|
|
roscpp_initialize("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)
|
|
|
|
# [initAndConfigFallbacks]
|
|
# 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)
|
|
# [initAndConfigFallbacks]
|
|
|
|
if task.plan():
|
|
task.publish(task.solutions[0])
|
|
time.sleep(1)
|