def test(self):
task = core.Task()
self.assertEqual(task.id, "")
task = core.Task("foo", core.SerialContainer())
self.assertEqual(task.id, "foo")
task = core.Task("task")
self.assertEqual(task.id, "task")
current = stages.CurrentState("current")
self.assertEqual(current.name, "current")
current.timeout = 1.23
self.assertEqual(current.timeout, 1.23)
task.add(current)
# ownership of current was passed to task
with self.assertRaises(TypeError):
current.name
task.add(stages.Connect("connect", []))
task.add(stages.FixedState())
def test_MoveRelative(self):
task = core.Task()
task.add(stages.CurrentState("current"))
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.2, "joint_2": 0.4})
task.add(move)
task.enableIntrospection()
task.init()
task.plan()
self.assertEqual(len(task.solutions), 1)
for s in task.solutions:
print(s)
s = task.solutions[0]
task.execute(s)
def test_Merger(self):
cartesian = core.CartesianPath()
def createDisplacement(group, displacement):
move = stages.MoveRelative("displace", cartesian)
move.group = group
move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
dir = Vector3Stamped(header=Header(frame_id="base_link"),
vector=Vector3(*displacement))
move.setDirection(dir)
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
return move
task = core.Task()
task.add(stages.CurrentState("current"))
merger = core.Merger("merger")
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0]))
task.add(merger)
task.enableIntrospection()
task.init()
if task.plan():
task.publish(task.solutions[0])
def test_PyGenerator(self):
task = core.Task()
keep_alive = PyGenerator()
task.add(keep_alive)
task.plan()
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
from moveit.task_constructor import core, stages
from math import pi
import time
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
group = "panda_arm"
eef_frame = "panda_link8"
# Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath()
jointspace = core.JointInterpolationPlanner()
task = core.Task()
# start from current robot state
task.add(stages.CurrentState("current state"))
# move along x
move = stages.MoveRelative("x +0.2", cartesian)
move.group = group
header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
task.add(move)
# move along y
move = stages.MoveRelative("y -0.3", cartesian)
move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
def test_task(self):
self.check(core.Task())