def test_reflection():
point = [1, 1, 1]
normal = [0, 0, 1]
R1 = Reflection(point, normal)
R2 = Transformation.from_matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 2],
[0, 0, 0, 1]])
assert R1 == R2
def rcf_xform():
# def rcf_xform(rcf_matrix):
M = [
[1.0, 0.0, 0.0, -100],
[0.0, 0.0, 1.0, -100],
[0.0, -1.0, 0.0, 100],
[0.0, 0.0, 0.0, 1.0],
]
return Transformation.from_matrix(M)
def test_reflection_from_frame():
point = [1, 1, 1]
x = [1, 0, 0]
y = [0, 1, 0]
f = Frame(point, x, y)
R1 = Reflection.from_frame(f)
R2 = Transformation.from_matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, -1, 2], [0, 0, 0, 1]])
assert R1 == R2
def visualize_urscript(script):
M = [
[-1000, 0, 0, 0],
[0, 1000, 0, 0],
[0, 0, 1000, 0],
[0, 0, 0, 1],
]
rgT = matrix_to_rgtransform(M)
cgT = Transformation.from_matrix(M)
robot = Robot()
viz_planes = []
movel_matcher = re.compile(r"^\s*move([lj]).+((-?\d+\.\d+,?\s?){6}).*$")
for line in script.splitlines():
mo = re.search(movel_matcher, line)
if mo:
if mo.group(1) == "l": # movel
ptX, ptY, ptZ, rX, rY, rZ = mo.group(2).split(",")
pt = Point(float(ptX), float(ptY), float(ptZ))
pt.transform(cgT)
frame = Frame(pt, [1, 0, 0], [0, 1, 0])
R = Rotation.from_axis_angle_vector(
[float(rX), float(rY), float(rZ)], pt)
T = matrix_to_rgtransform(R)
plane = cgframe_to_rgplane(frame)
plane.Transform(T)
viz_planes.append(plane)
else: # movej
joint_values = mo.group(2).split(",")
configuration = Configuration.from_revolute_values(
[float(d) for d in joint_values])
frame = robot.forward_kinematics(configuration)
plane = cgframe_to_rgplane(frame)
plane.Transform(rgT)
viz_planes.append(plane)
return viz_planes
def data(self, data):
p_name = data.get('name') or None
p_id = data.get('id') or None
p_geo = data.get('geo') or {}
p_connections = data.get('connections') or []
p_active_connections = data.get('active_connections') or []
p_transformation = data.get('transformation') or []
p_parent = data.get('parent') or None
p_children = data.get('children') or []
self.name = p_name
self.id = p_id
self.geo = Mesh.from_data(p_geo)
self.connections = [Connection.from_data(c) for c in p_connections]
self.active_connections = p_active_connections
self.transformation = Transformation.from_matrix(p_transformation)
center_coords = self.geo.centroid()
self.center = Point(center_coords[0], center_coords[1], center_coords[2])
self.parent = p_parent
self.children = p_children
def test_from_matrix():
t = Transformation().matrix
assert Transformation.from_matrix(t).matrix == t
def test_from_matrix():
T = Transformation()
matrix = T.matrix
assert Transformation.from_matrix(matrix).matrix == matrix
def _dae_mesh_importer(filename, precision):
"""This is a very simple implementation of a DAE/Collada parser.
Collada specification: https://www.khronos.org/files/collada_spec_1_5.pdf
"""
dae = XML.from_file(filename)
meshes = []
visual_scenes = dae.root.find('library_visual_scenes')
materials = dae.root.find('library_materials')
effects = dae.root.find('library_effects')
for geometry in dae.root.findall('library_geometries/geometry'):
mesh_xml = geometry.find('mesh')
mesh_id = geometry.attrib['id']
matrix_node = visual_scenes.find('visual_scene/node/instance_geometry[@url="#{}"]/../matrix'.format(mesh_id))
transform = None
if matrix_node is not None:
M = [float(i) for i in matrix_node.text.split()]
# If it's the identity matrix, then ignore, we don't need to transform it
if M != [1., 0., 0., 0.,
0., 1., 0., 0.,
0., 0., 1., 0.,
0., 0., 0., 1.]:
M = M[0:4], M[4:8], M[8:12], M[12:16]
transform = Transformation.from_matrix(M)
# primitive elements can be any combination of:
# lines, linestrips, polygons, polylist, triangles, trifans, tristrips
# The current implementation only supports triangles and polylist of triangular meshes
primitive_element_sets = []
primitive_element_sets.extend(mesh_xml.findall('triangles'))
primitive_element_sets.extend(mesh_xml.findall('polylist'))
if len(primitive_element_sets) == 0:
raise Exception('No primitive elements found (currently only triangles and polylist are supported)')
for primitive_element_set in primitive_element_sets:
primitive_tag = primitive_element_set.tag
primitive_set_data = primitive_element_set.find('p').text.split()
# Try to retrieve mesh colors
mesh_colors = {}
if materials is not None and effects is not None:
try:
instance_effect = None
material_id = primitive_element_set.attrib.get('material')
primitive_count = int(primitive_element_set.attrib['count'])
if material_id is not None:
instance_effect = materials.find('material[@id="{}"]/instance_effect'.format(material_id))
if instance_effect is not None:
instance_effect_id = instance_effect.attrib['url'][1:]
colors = effects.findall('effect[@id="{}"]/profile_COMMON/technique/phong/*/color'.format(instance_effect_id))
for color_node in colors:
rgba = [float(i) for i in color_node.text.split()]
mesh_colors['mesh_color.{}'.format(color_node.attrib['sid'])] = rgba
except Exception:
LOGGER.exception('Exception while loading materials, all materials of mesh file %s will be ignored ', filename)
# Parse vertices
all_offsets = sorted([int(i.attrib['offset']) for i in primitive_element_set.findall('input[@offset]')])
if not all_offsets:
raise Exception('Primitive element node does not contain offset information! Primitive tag={}'.format(primitive_tag))
vertices_input = primitive_element_set.find('input[@semantic="VERTEX"]')
vertices_id = vertices_input.attrib['source'][1:]
vertices_link = mesh_xml.find('vertices[@id="{}"]/input'.format(vertices_id))
positions = mesh_xml.find('source[@id="{}"]/float_array'.format(vertices_link.attrib['source'][1:]))
positions = positions.text.split()
vertices = [[float(p) for p in positions[i:i + 3]] for i in range(0, len(positions), 3)]
# Parse faces
# Every nth element is a vertex key, we ignore the rest based on the offsets defined
# Usually, every second item is the normal, but there can be other items offset in there (vertex tangents, etc)
skip_step = 1 + all_offsets[-1]
if primitive_tag == 'triangles':
vcount = [3] * primitive_count
elif primitive_tag == 'polylist':
vcount = [int(v) for v in primitive_element_set.find('vcount').text.split()]
if len(vcount) != primitive_count:
raise Exception('Primitive count does not match vertex per face count, vertex input id={}'.format(vertices_id))
fkeys = [int(f) for f in primitive_set_data[::skip_step]]
faces = []
for i in range(primitive_count):
a = i * vcount[i]
b = a + vcount[i]
faces.append(fkeys[a:b])
# Rebuild vertices and faces using the same logic that other importers
# use remapping everything based on a selected precision
index_key = OrderedDict()
vertex = OrderedDict()
for i, xyz in enumerate(vertices):
key = geometric_key(xyz, precision)
index_key[i] = key
vertex[key] = xyz
key_index = {key: index for index, key in enumerate(vertex)}
index_index = {index: key_index[key] for index, key in iter(index_key.items())}
vertices = [xyz for xyz in iter(vertex.values())]
faces = [[index_index[index] for index in face] for face in faces]
mesh = Mesh.from_vertices_and_faces(vertices, faces)
if mesh_colors:
mesh.attributes.update(mesh_colors)
if transform:
mesh_transform(mesh, transform)
meshes.append(mesh)
return meshes