diff --git a/urchin/urdf.py b/urchin/urdf.py index f509549..582a183 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -1,21 +1,21 @@ -from collections import OrderedDict import copy import os import time +from collections import OrderedDict -from lxml import etree as ET import networkx as nx import numpy as np import PIL -import trimesh import six +import trimesh +from lxml import etree as ET from .utils import ( - parse_origin, - unparse_origin, + configure_origin, get_filename, load_meshes, - configure_origin, + parse_origin, + unparse_origin, ) @@ -137,9 +137,9 @@ def _parse_simple_elements(cls, node, path): else: vs = node.findall(t._TAG) if len(vs) == 0 and r: - raise ValueError( - "Missing required subelement(s) of type {} when " - "parsing an object of type {}".format(t.__name__, cls.__name__) + print( + f"Missing required subelement(s) of type {t.__name__} when " + f"parsing an object of type {cls.__name__}." ) v = [t._from_xml(n, path) for n in vs] kwargs[a] = v @@ -563,6 +563,9 @@ def meshes(self): that represent this object. """ if len(self._meshes) == 0: + if self.radius == 0: + print("[urchin]: radius equal to 0 is not supported, using 1e-5.") + self.radius = 1e-5 self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes @@ -2076,7 +2079,10 @@ def actuators(self, value): @classmethod def _from_xml(cls, node, path): kwargs = cls._parse(node, path) - kwargs["trans_type"] = node.find("type").text + trans_type = node.attrib.get("type") + if trans_type is None: + trans_type = node.find("type").text + kwargs["trans_type"] = trans_type return Transmission(**kwargs) def _to_xml(self, parent, path): @@ -2776,6 +2782,7 @@ def __init__( self.name = name self.other_xml = other_xml + self.mesh_need_to_mirror = [] # No setters for these self._links = list(links) @@ -3302,17 +3309,58 @@ def visual_trimesh_fk(self, cfg=None, links=None): to the base link's frame. """ lfk = self.link_fk(cfg=cfg, links=links) - + self.mesh_name_list = [] fk = OrderedDict() for link in lfk: for visual in link.visuals: - for mesh in visual.geometry.meshes: + for i, mesh in enumerate(visual.geometry.meshes): pose = lfk[link].dot(visual.origin) if visual.geometry.mesh is not None: + self.mesh_name_list.append(visual.geometry.mesh.filename) if visual.geometry.mesh.scale is not None: + if ( + np.sum( + visual.geometry.mesh.scale + != abs(visual.geometry.mesh.scale) + ) + > 0 + ): + if ( + visual.geometry.mesh.filename + not in self.mesh_need_to_mirror + ): + print( + f"[urchin]: {visual.geometry.mesh.filename} needs to mirror" + ) + self.mesh_need_to_mirror.append( + visual.geometry.mesh.filename + ) + mesh_vertices = np.copy(mesh.vertices) + mesh_faces = np.copy(mesh.faces) + mesh_faces_new = np.hstack( + [ + mesh_faces[:, 2].reshape(-1, 1), + mesh_faces[:, 1].reshape(-1, 1), + mesh_faces[:, 0].reshape(-1, 1), + ] + ) + mesh = trimesh.Trimesh() + mirror_axis = np.where( + visual.geometry.mesh.scale < 0 + )[0][0] + mesh_vertices[:, mirror_axis] = -mesh_vertices[ + :, mirror_axis + ] + mesh.vertices = mesh_vertices + mesh.faces = mesh_faces_new + visual.geometry.meshes[i] = mesh S = np.eye(4, dtype=np.float64) - S[:3, :3] = np.diag(visual.geometry.mesh.scale) + S[:3, :3] = np.abs(np.diag(visual.geometry.mesh.scale)) pose = pose.dot(S) + else: + self.mesh_name_list.append("") + if visual.material is not None: + mesh.visual.face_colors = visual.material.color fk[mesh] = pose return fk @@ -3527,7 +3575,7 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): .. image:: /_static/ur5_three_joints.gif """ - import pyrender # Save pyrender import for here for CI + import pyribbit # Save pyribbit import for here for CI ct = cfg_trajectory @@ -3552,6 +3600,7 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): raise ValueError("Trajectories must be same length") ct_np[k] = val elif isinstance(ct, (list, tuple, np.ndarray)): + traj_len = len(ct) ct = np.asanyarray(ct).astype(np.float64) if ct.ndim == 1: ct = ct.reshape(-1, 1) @@ -3597,10 +3646,10 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): fk = self.visual_trimesh_fk() node_map = {} - scene = pyrender.Scene() + scene = pyribbit.Scene() for tm in fk: pose = fk[tm] - mesh = pyrender.Mesh.from_trimesh(tm, smooth=False) + mesh = pyribbit.Mesh.from_trimesh(tm, smooth=False) node = scene.add(mesh, pose=pose) node_map[tm] = node @@ -3608,7 +3657,7 @@ def animate(self, cfg_trajectory=None, loop_time=3.0, use_collision=False): blp = self.link_fk(links=[self.base_link])[self.base_link] # Pop the visualizer asynchronously - v = pyrender.Viewer( + v = pyribbit.Viewer( scene, run_in_thread=True, use_raymond_lighting=True, view_center=blp[:3, 3] )