def test_robot_append_made_link(self):
# Update scene
self.scene.scene.title = "Test Robot Append Made Link"
# Create 2 joints
joint1 = robot.RotationalJoint(self.se3, self.structure, self.scene)
joint2 = robot.RotationalJoint(self.se3, self.structure, self.scene)
# Create robot
robot1 = robot.GraphicalRobot(self.scene, "Robot 1")
# Add 1 joint
robot1.append_made_link(joint1)
# Print joint poses to prove its added
robot1.print_joint_poses()
# Create a new scene
scene2 = canvas.GraphicsCanvas3D(title="Test Robot Append Made Link 2")
# Create a new robot in new scene
robot2 = robot.GraphicalRobot(scene2, "Robot 2")
# Add other joint to new scene
# Expecting an error (can't add joint to robot in different scene
self.assertRaises(RuntimeError, robot2.append_made_link, joint2)
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 setUp(self):
self.scene = canvas.GraphicsCanvas3D()
# 0.707107 -0.707107 0 0
# 0.707107 0.707107 0 1
# 0 0 1 0.4
# 0 0 0 1
self.se3 = SE3().Ty(1) * SE3().Tz(0.4) * SE3().Rz(45, 'deg')
self.structure = 1.0
def test_grid_visibility(self):
# Create a scene, with grid=True (default)
scene = canvas.GraphicsCanvas3D(title="Test Grid Visibility", grid=True)
# Check all objects in scene are visible (default scene will just have grid, assuming init has grid=True)
self.assertGreater(len(scene.scene.objects), 0)
# Change visibility
scene.grid_visibility(False)
# Check all are invisible
# Invisible objects are not shown in the objects list
self.assertEqual(len(scene.scene.objects), 0)
def test_graphics_canvas_init(self):
# Create a canvas with all options being used (different to defaults)
scene = canvas.GraphicsCanvas3D(
height=360,
width=480,
title="Test Graphics Canvas Creation",
caption="Caption text here",
grid=False
)
try:
# Put a box in the created scene
box(canvas=scene.scene)
except:
# Something went wrong
self.assertEqual(False, True)
def test_import_object(self):
# Update Scene
scene = canvas.GraphicsCanvas3D(title="Test Import Object")
scene.grid_visibility(False)
# Check num objects
num_obj = len(scene.scene.objects)
# Import an object
graphic_obj = stl.import_object_from_numpy_stl(
'./roboticstoolbox/models/meshes/UNIMATE/puma560/link0.stl',
scene.scene
)
# Verify object was added
self.assertEqual(graphic_obj.pos, vector(0, 0, 0)) # Object is at origin
def test_add_robot(self):
# Create a scene (no grid visible)
scene = canvas.GraphicsCanvas3D(title="Test Add Robot", grid=False)
# Save number of objects
num_objs = len(scene.scene.objects)
# Create a 3-link robot
r = robot.GraphicalRobot(scene, 'robot 1')
r.append_link('r', SE3(), 1.0)
r.append_link('r', SE3().Tx(1), 1.0)
r.append_link('r', SE3().Tx(2), 1.0)
# Hide reference frames to only have robot joints in visible list
r.set_reference_visibility(False)
# Check number of new graphics
self.assertEqual(len(scene.scene.objects) - num_objs, 3)
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_draw_reference_axes(self):
# Create a scene, no grid
scene = canvas.GraphicsCanvas3D(title="Test Draw Reference Frame", grid=False)
# Check objects is empty
self.assertEqual(len(scene.scene.objects), 0)
# Add a reference frame
arr = array([
[-1, 0, 0, 3],
[0, 0, -1, 2],
[0, -1, 0, 3],
[0, 0, 0, 1]
])
expected = SE3(arr)
canvas.draw_reference_frame_axes(expected, scene.scene)
# Through objects, get position, and vectors
self.assertEqual(len(scene.scene.objects), 1)
obj = scene.scene.objects[0]
pos = obj.pos
x_vec = obj.axis
y_vec = obj.up
z_vec = x_vec.cross(y_vec)
# Recreate the SE3
arr = array([
[x_vec.x, y_vec.x, z_vec.x, pos.x],
[x_vec.y, y_vec.y, z_vec.y, pos.y],
[x_vec.z, y_vec.z, z_vec.z, pos.z],
[0, 0, 0, 1]
])
actual = SE3(arr)
# Check SE3 are equal
self.assertEqual(actual, expected)
def test_grid_init(self):
# Create a scene
scene = canvas.GraphicsCanvas3D(title="Test Grid Init", grid=False)
# Create a (technically second) graphics grid for the scene
grid = canvas.GraphicsGrid(scene.scene)