From 9cad313ac065fe8ef8742cf1ba1ead366837a600 Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Mon, 16 Oct 2023 19:08:08 +0200 Subject: [PATCH 1/6] mirror some mesh --- urchin/urdf.py | 34 +++++++++++++++++++++++++++------- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/urchin/urdf.py b/urchin/urdf.py index f509549..9bfbfb9 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -2776,6 +2776,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 +3303,35 @@ 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"[urdfpy]: {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] = abs(np.diag(visual.geometry.mesh.scale)) pose = pose.dot(S) + else: + self.mesh_name_list.append("") fk[mesh] = pose return fk @@ -3527,7 +3546,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 +3571,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 +3617,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 +3628,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] ) From dc2d7cbbecd8c097b8a37606229a2421e9190bdb Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Mon, 16 Oct 2023 22:37:12 +0200 Subject: [PATCH 2/6] add support for 0 radius sphere --- urchin/urdf.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/urchin/urdf.py b/urchin/urdf.py index 9bfbfb9..02be5ba 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -563,6 +563,9 @@ def meshes(self): that represent this object. """ if len(self._meshes) == 0: + if self.radius == 0: + print("[urdfpy]: radius equal to 0 is not supported, use a small number.") + self.radius = 0.001 self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes From ebe07052807b7829368fbbf25f72ea0003dd024d Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Mon, 16 Oct 2023 22:45:26 +0200 Subject: [PATCH 3/6] set mesh color properly according to the urdf --- urchin/urdf.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/urchin/urdf.py b/urchin/urdf.py index 02be5ba..c98ffd5 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -3335,6 +3335,8 @@ def visual_trimesh_fk(self, cfg=None, links=None): 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 From 780665b2a1cd891e3a0382ab38885c00a1a2498f Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Mon, 16 Oct 2023 22:58:04 +0200 Subject: [PATCH 4/6] bypass the missing trasmission joint error --- urchin/urdf.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/urchin/urdf.py b/urchin/urdf.py index c98ffd5..208036f 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -137,7 +137,7 @@ def _parse_simple_elements(cls, node, path): else: vs = node.findall(t._TAG) if len(vs) == 0 and r: - raise ValueError( + print( "Missing required subelement(s) of type {} when " "parsing an object of type {}".format(t.__name__, cls.__name__) ) @@ -2079,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): From 2243adb2f822d66436797bce4a0c50ef9be200fe Mon Sep 17 00:00:00 2001 From: Hongzhuo Liang Date: Tue, 17 Oct 2023 14:49:14 +0200 Subject: [PATCH 5/6] change the name --- urchin/urdf.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/urchin/urdf.py b/urchin/urdf.py index 208036f..46be472 100644 --- a/urchin/urdf.py +++ b/urchin/urdf.py @@ -564,7 +564,7 @@ def meshes(self): """ if len(self._meshes) == 0: if self.radius == 0: - print("[urdfpy]: radius equal to 0 is not supported, use a small number.") + print("[urchin]: radius equal to 0 is not supported, use a small number.") self.radius = 0.001 self._meshes = [trimesh.creation.icosphere(radius=self.radius)] return self._meshes @@ -3320,7 +3320,7 @@ def visual_trimesh_fk(self, cfg=None, links=None): 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"[urdfpy]: {visual.geometry.mesh.filename} needs 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) From b8e7477ef698b7d9b4fe388fba966f468d62900c Mon Sep 17 00:00:00 2001 From: Adam Fishman Date: Fri, 1 Mar 2024 16:19:49 -0800 Subject: [PATCH 6/6] Abs to np.abs --- urchin/urdf.py | 61 +++++++++++++++++++++++++++++++++----------------- 1 file changed, 41 insertions(+), 20 deletions(-) diff --git a/urchin/urdf.py b/urchin/urdf.py index 46be472..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, ) @@ -138,8 +138,8 @@ def _parse_simple_elements(cls, node, path): vs = node.findall(t._TAG) if len(vs) == 0 and r: print( - "Missing required subelement(s) of type {} when " - "parsing an object of type {}".format(t.__name__, cls.__name__) + 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 @@ -564,8 +564,8 @@ def meshes(self): """ if len(self._meshes) == 0: if self.radius == 0: - print("[urchin]: radius equal to 0 is not supported, use a small number.") - self.radius = 0.001 + 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 @@ -3318,23 +3318,44 @@ def visual_trimesh_fk(self, cfg=None, links=None): 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) + 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_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] + 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] = abs(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("")