def test_vpython_to_se3(self):
# Create a scene
scene = canvas.GraphicsCanvas3D(title="TEST VPYTHON TO SE3")
# Create a basic entity
# pos = 1, 2, 3
# X = 0, 0, -1
# Y = -1, 0, 0
# Z = 0, 1, 0
entity = box(
pos=vector(1, 2, 3),
axis=vector(0, 0, -1),
up=vector(-1, 0, 0)
)
scene.scene.waitfor("draw_complete")
# Check resulting SE3
arr = array([
[0, -1, 0, 1],
[0, 0, 1, 2],
[-1, 0, 0, 3],
[0, 0, 0, 1]
])
expected = SE3(arr)
self.assertEqual(common.vpython_to_se3(entity), expected)
def test_import_puma560(self):
# Create scene
scene = canvas.GraphicsCanvas3D(title="Test Import Puma560")
# Import puma560
robot1 = import_puma_560(scene)
# Check all joints are added
self.assertEqual(len(robot1.joints), 7)
# For each joint, check it's in the right pose
puma = Puma560()
correct_poses = puma.fkine(puma.qz, alltout=True)
# Initial doesn't have an SE3 in correct_poses
self.assertEqual(common.vpython_to_se3(robot1.joints[0].get_graphic_object()), SE3())
# As the base doesn't have a correct pose, need to offset the check indices
for idx in range(len(correct_poses)):
self.assertEqual(common.vpython_to_se3(robot1.joints[idx].get_graphic_object()), correct_poses[idx])
def test_robot_animate(self):
# Update scene
self.scene.scene.title = "Test Robot Animate"
self.scene.grid_visibility(False)
# Create a two link robot
robot1 = robot.GraphicalRobot(self.scene, "Robot 1")
robot1.append_link("r", self.se3, self.structure)
robot1.append_link("r", self.se3 * SE3().Tx(1), self.structure)
s1 = SE3().Tx(2) * SE3().Tz(0.3) * SE3().Ry(23, 'deg')
s2 = SE3().Ty(0.5) * SE3().Tx(1.2) * SE3().Rz(-34, 'deg')
# Set each joint to a known location
robot1.animate([[s2, s1], [s1, s2]], 1)
# As it can't test positions mid frame, just check final positions are correct
# For each obj in scene, make sure it is one of the two locations
# Ensure objects visible are just reference frames (they have same pose as the graphic itself)
robot1.set_robot_visibility(False)
self.assertEqual(len(self.scene.scene.objects), 2) # Should only have 2 reference frames in the scene
# Both objects must be in either of the poses (but not the same one)
self.assertTrue(
# 0 in s1, 1 in s2
(common.vpython_to_se3(self.scene.scene.objects[0]) == s1 and
common.vpython_to_se3(self.scene.scene.objects[1]) == s2)
or
# 1 in s1, 0 in s2
(common.vpython_to_se3(self.scene.scene.objects[1]) == s1 and
common.vpython_to_se3(self.scene.scene.objects[0]) == s2)
)
# Try giving no frames
self.assertRaises(ValueError, robot1.animate, [], 1)
# Try giving bad frame count
self.assertRaises(ValueError, robot1.animate, [[s1, s2]], -1)
# Try giving wrong number SE3s
self.assertRaises(UserWarning, robot1.animate, [[]], 1)
def test_robot_set_poses(self):
# Update scene
self.scene.scene.title = "Test Robot Set Poses"
self.scene.grid_visibility(False)
# Create a two link robot
robot1 = robot.GraphicalRobot(self.scene, "Robot 1")
robot1.append_link("r", self.se3, self.structure)
robot1.append_link("r", self.se3 * SE3().Tx(1), self.structure)
s1 = SE3().Tx(2) * SE3().Tz(0.3) * SE3().Ry(23, 'deg')
s2 = SE3().Ty(0.5) * SE3().Tx(1.2) * SE3().Rz(-34, 'deg')
# Set each joint to a known location
robot1.set_joint_poses([s1, s2])
# For each obj in scene, make sure it is one of the two locations
# Ensure objects visible are just reference frames (they have same pose as the graphic itself)
robot1.set_robot_visibility(False)
self.assertEqual(len(self.scene.scene.objects), 2) # Should only have 2 reference frames in the scene
# Both objects must be in either of the poses (but not the same one)
self.assertTrue(
# 0 in s1, 1 in s2
(common.vpython_to_se3(self.scene.scene.objects[0]) == s1 and
common.vpython_to_se3(self.scene.scene.objects[1]) == s2)
or
# 1 in s1, 0 in s2
(common.vpython_to_se3(self.scene.scene.objects[1]) == s1 and
common.vpython_to_se3(self.scene.scene.objects[0]) == s2)
)
# Try giving not enough poses
self.assertRaises(UserWarning, robot1.set_joint_poses, [])
# Create new robot
robot2 = robot.GraphicalRobot(self.scene, "Robot 2")
# Try setting poses on empty robot
self.assertRaises(UserWarning, robot2.set_joint_poses, [s1, s2])
def check_obj_pose(self, obj, pose):
self.assertEqual(common.vpython_to_se3(obj.get_graphic_object()), pose)