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