Merge pull request #15 from fishbotics/lianghongzhuo/main

Lianghongzhuo/main

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,18 +3646,18 @@ 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
 
         # Get base pose to focus on
         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]
         )