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