|
3 | 3 | # Licensed under the MIT License [see LICENSE for details]. |
4 | 4 |
|
5 | 5 | # Utils for load URDF for meshcat server |
6 | | -from typing import List |
7 | 6 |
|
8 | 7 | import meshcat.geometry as g |
9 | 8 | import numpy as np |
10 | 9 | import transforms3d |
11 | 10 | import trimesh |
12 | | -from meshcat.visualizer import Visualizer |
13 | 11 |
|
14 | 12 | import sim_web_visualizer.parser.yourdfpy as urdf |
15 | 13 | from sim_web_visualizer.parser.mesh_parser import load_mesh, get_trimesh_geometry_material, rgb_to_hex, AssetResource |
@@ -163,91 +161,3 @@ def load_urdf_with_yourdfpy(urdf_path: str, collapse_fixed_joints: bool, |
163 | 161 | pose_data=offline_pose_dict, |
164 | 162 | ) |
165 | 163 | return resource |
166 | | - |
167 | | - |
168 | | -def load_urdf_into_viewer_pin(filename: str, viewer: Visualizer, collapsed_link_names: List[str]): |
169 | | - import collada |
170 | | - import pinocchio as pin |
171 | | - # First parse the urdf with yourdfpy since it can better handle ROS pacakge format |
172 | | - my_urdf = urdf.URDF.load(filename) |
173 | | - urdf_string = my_urdf.write_xml_string() |
174 | | - |
175 | | - # Then store the internal data structure inside pinocchio |
176 | | - robot: pin.pinocchio_pywrap.Model = pin.buildModelFromXML(urdf_string) |
177 | | - frame_id_map = {i: frame.name for i, frame in enumerate(robot.frames)} |
178 | | - |
179 | | - # TODO: use collision model |
180 | | - visual_model = pin.buildGeomFromUrdfString(robot, my_urdf.write_xml_string(), pin.VISUAL) |
181 | | - # collision_model = pin.buildGeomFromUrdfString(robot, my_urdf.write_xml_string(), pin.COLLISION) |
182 | | - # data, collision_data, visual_data = pin.createDatas(robot, collision_model, visual_model) |
183 | | - # joint_pos = pin.neutral(robot) |
184 | | - # pin.forwardKinematics(robot, data, joint_pos) |
185 | | - # pin.updateGeometryPlacements(robot, data, visual_model, visual_data) |
186 | | - |
187 | | - collapse_fixed_joints = len(collapsed_link_names) > 0 |
188 | | - if collapse_fixed_joints: |
189 | | - parent_frame_map = {} |
190 | | - # The first two frames in pinocchio are 'universe' and 'root_joint', which are not presented in URDF |
191 | | - for frame_id, link_info in enumerate(robot.frames): # type: int, pin.pinocchio_pywrap.Frame |
192 | | - if link_info.type != pin.pinocchio_pywrap.FrameType.BODY: |
193 | | - continue |
194 | | - name = link_info.name |
195 | | - if name in collapsed_link_names: |
196 | | - parent_frame_map[link_info.parent] = frame_id |
197 | | - |
198 | | - # Load mesh based on geometry data |
199 | | - link_mesh_count = {} |
200 | | - for geom in visual_model.geometryObjects: # type: pin.GeometryObject |
201 | | - color = geom.meshColor[:3] |
202 | | - opacity = geom.meshColor[3] |
203 | | - mat = None |
204 | | - if geom.meshPath == 'BOX': |
205 | | - geometry = g.Box(2 * geom.geometry.halfSide) |
206 | | - elif geom.meshPath == 'SPHERE': |
207 | | - geometry = g.Sphere(geom.geometry.radius) |
208 | | - elif geom.meshPath == 'CYLINDER': |
209 | | - radius, length = geom.geometry.radius, 2 * geom.geometry.halfLength |
210 | | - geometry = g.Cylinder(radius=radius, height=length) |
211 | | - else: |
212 | | - path = geom.meshPath |
213 | | - if path.endswith("dae"): |
214 | | - geometry = g.DaeMeshGeometry.from_file(path) |
215 | | - temp_mesh = collada.Collada(geom.meshPath) |
216 | | - img_info = [(im.path, im.data) for im in temp_mesh.images if im.path is not None] |
217 | | - if len(img_info) > 0: |
218 | | - texture = g.ImageTexture(g.PngImage(data=img_info[0][1])) |
219 | | - mat = g.MeshLambertMaterial(map=texture, opacity=opacity) |
220 | | - |
221 | | - else: |
222 | | - exp_mesh = load_mesh(geom.meshPath, geom.meshScale) |
223 | | - geometry = g.ObjMeshGeometry.from_stream(trimesh.util.wrap_as_stream(exp_mesh)) |
224 | | - texture_file = geom.meshTexturePath if geom.meshTexturePath else None |
225 | | - texture = g.ImageTexture(g.PngImage.from_file(texture_file)) if isinstance(texture_file, |
226 | | - str) else None # Create material |
227 | | - if texture is not None: |
228 | | - mat = g.MeshLambertMaterial(map=texture, opacity=opacity) |
229 | | - |
230 | | - # Only create color texture if the color is not the default value for better performance |
231 | | - if not np.allclose(geom.meshColor, np.array([0.9, 0.9, 0.9, 1])): |
232 | | - if not (np.any(color > 1) and color.dtype == np.int): |
233 | | - color *= 255 |
234 | | - color = np.clip(color, 0, 255).astype(np.uint8) |
235 | | - mat = g.MeshLambertMaterial(color=rgb_to_hex(color), opacity=opacity) |
236 | | - |
237 | | - # Compute relative pose |
238 | | - geom_frame_id = geom.parentFrame |
239 | | - frame_id = frame_id_map[geom_frame_id] if not collapse_fixed_joints else parent_frame_map[geom.parentJoint] |
240 | | - link_name = frame_id_map[frame_id] |
241 | | - link_pose = robot.frames[frame_id].placement |
242 | | - |
243 | | - # Count geom name for each link |
244 | | - if link_name not in link_mesh_count: |
245 | | - link_mesh_count[link_name] = 0 |
246 | | - else: |
247 | | - link_mesh_count[link_name] += 1 |
248 | | - geom_id = link_mesh_count[link_name] |
249 | | - |
250 | | - # Set mesh and pose |
251 | | - relative_pose = (link_pose.inverse() * geom.placement).homogeneous |
252 | | - viewer[f"{link_name}/{geom_id}"].set_object(geometry, material=mat) |
253 | | - viewer[f"{link_name}/{geom_id}"].set_transform(relative_pose) |
0 commit comments