Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Lianghongzhuo/main #15

Merged
merged 6 commits into from
Mar 2, 2024
Merged

Lianghongzhuo/main #15

Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
9cad313
mirror some mesh
lianghongzhuo Oct 16, 2023
dc2d7cb
add support for 0 radius sphere
lianghongzhuo Oct 16, 2023
ebe0705
set mesh color properly according to the urdf
lianghongzhuo Oct 16, 2023
780665b
bypass the missing trasmission joint error
lianghongzhuo Oct 16, 2023
2243adb
change the name
lianghongzhuo Oct 17, 2023
b8e7477
Abs to np.abs
fishbotics Mar 2, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
83 changes: 66 additions & 17 deletions urchin/urdf.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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,
)


Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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

Expand All @@ -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)
Expand Down Expand Up @@ -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]
)

Expand Down
Loading