def check(self, container):
container = core.SerialContainer()
container.add(stages.CurrentState("1"))
container.add(stages.CurrentState("2"))
container.add(stages.CurrentState("3"))
with self.assertRaises(IndexError):
container["unknown"]
child = container["2"]
self.assertEqual(child.name, "2")
self.assertEqual([child.name for child in container], ["1", "2", "3"])
def test(self):
task = core.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_CurrentState(self):
stage = stages.CurrentState("current")
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)))
task.add(move)
# rotate about z
def test_move(self):
container = core.SerialContainer()
stage = stages.CurrentState()
container.add(stage)
with self.assertRaises(TypeError):
stage.name