nova_rerun_bridge.robot_visualizer API documentation

RobotVisualizer( robot: nova_rerun_bridge.dh_robot.DHRobot, robot_model_geometries: list[wandelbots_api_client.models.robot_link_geometry.RobotLinkGeometry], tcp_geometries: list[wandelbots_api_client.models.geometry.Geometry], static_transform: bool = True, base_entity_path: str = 'robot', albedo_factor: list = [255, 255, 255], collision_link_chain=None, collision_tcp=None, model_from_controller='', show_collision_link_chain: bool = False, show_safety_link_chain: bool = True) View Source

23    def __init__(
24        self,
25        robot: DHRobot,
26        robot_model_geometries: list[models.RobotLinkGeometry],
27        tcp_geometries: list[models.Geometry],
28        static_transform: bool = True,
29        base_entity_path: str = "robot",
30        albedo_factor: list = [255, 255, 255],
31        collision_link_chain=None,
32        collision_tcp=None,
33        model_from_controller="",
34        show_collision_link_chain: bool = False,
35        show_safety_link_chain: bool = True,
36    ):
37        """
38        :param robot: DHRobot instance
39        :param robot_model_geometries: List of geometries for each link
40        :param tcp_geometries: TCP geometries (similar structure to link geometries)
41        :param static_transform: If True, transforms are logged as static, else temporal.
42        :param base_entity_path: A base path prefix for logging the entities (e.g. motion group name)
43        :param albedo_factor: A list representing the RGB values [R, G, B] to apply as the albedo factor.
44        :param collision_link_chain: Collision link chain geometries for the robot
45        :param collision_tcp: Collision TCP geometries
46        :param model_from_controller: Model name from controller for loading robot mesh
47        :param show_collision_link_chain: Whether to render robot collision mesh geometry
48        :param show_safety_link_chain: Whether to render robot safety geometry (from controller)
49        """
50        self.robot = robot
51        self.link_geometries: dict[int, list[models.Geometry]] = {}
52        self.tcp_geometries: list[models.Geometry] = tcp_geometries
53        self.logged_meshes: set[str] = set()
54        self.static_transform = static_transform
55        self.base_entity_path = base_entity_path.rstrip("/")
56        self.albedo_factor = albedo_factor
57        self.mesh_loaded = False
58        self.collision_link_geometries = {}
59        self.collision_tcp_geometries = collision_tcp
60        self.show_collision_link_chain = show_collision_link_chain
61        self.show_safety_link_chain = show_safety_link_chain
62
63        # This will hold the names of discovered joints (e.g. ["robot_J00", "robot_J01", ...])
64        self.joint_names: list[str] = []
65        self.layer_nodes_dict: dict[str, list[str]] = {}
66        self.parent_nodes_dict: dict[str, str] = {}
67
68        # load mesh
69        try:
70            glb_path = get_model_path(model_from_controller)
71            self.scene = trimesh.load_scene(glb_path, file_type="glb")
72            self.mesh_loaded = True
73            self.edge_data = self.scene.graph.transforms.edge_data
74
75            # After loading, auto-discover any child nodes that match *_J0n
76            self.discover_joints()
77        except Exception as e:
78            print(f"Failed to load mesh: {e}")
79
80        # Group geometries by link
81        for gm in robot_model_geometries:
82            self.link_geometries.setdefault(gm.link_index, []).append(gm.geometry)
83
84        # Group geometries by link
85        self.collision_link_geometries = collision_link_chain

Parameters

robot: DHRobot instance
robot_model_geometries: List of geometries for each link
tcp_geometries: TCP geometries (similar structure to link geometries)
static_transform: If True, transforms are logged as static, else temporal.
base_entity_path: A base path prefix for logging the entities (e.g. motion group name)
albedo_factor: A list representing the RGB values [R, G, B] to apply as the albedo factor.
collision_link_chain: Collision link chain geometries for the robot
collision_tcp: Collision TCP geometries
model_from_controller: Model name from controller for loading robot mesh
show_collision_link_chain: Whether to render robot collision mesh geometry
show_safety_link_chain: Whether to render robot safety geometry (from controller)

robot

link_geometries: dict[int, list[wandelbots_api_client.models.geometry.Geometry]]

tcp_geometries: list[wandelbots_api_client.models.geometry.Geometry]

logged_meshes: set[str]

static_transform

base_entity_path

albedo_factor

mesh_loaded

collision_link_geometries

collision_tcp_geometries

show_collision_link_chain

show_safety_link_chain

joint_names: list[str]

layer_nodes_dict: dict[str, list[str]]

parent_nodes_dict: dict[str, str]

def discover_joints(self): View Source

 87    def discover_joints(self):
 88        """
 89        Find all child node names that contain '_J0' followed by digits or '_FLG'.
 90        Store joints with their parent nodes and print layer information.
 91        """
 92        joint_pattern = re.compile(r"_J0(\d+)")
 93        flg_pattern = re.compile(r"_FLG")
 94        matches = []
 95        flg_nodes = []
 96        joint_parents = {}  # Store parent for each joint/FLG
 97
 98        for (parent, child), data in self.edge_data.items():
 99            # Check for joints
100            joint_match = joint_pattern.search(child)
101            if joint_match:
102                j_idx = int(joint_match.group(1))
103                matches.append((j_idx, child))
104                joint_parents[child] = parent
105
106            # Check for FLG
107            flg_match = flg_pattern.search(child)
108            if flg_match:
109                flg_nodes.append(child)
110                joint_parents[child] = parent
111
112        matches.sort(key=lambda x: x[0])
113        self.joint_names = [name for _, name in matches] + flg_nodes
114
115        # print("Discovered nodes:", self.joint_names)
116        # Print layer information for each joint
117        for joint in self.joint_names:
118            self.get_nodes_on_same_layer(joint_parents[joint], joint)
119            # print(f"\nNodes on same layer as {joint}:")
120            # print(f"Parent node: {joint_parents[joint]}")
121            # print(f"Layer nodes: {same_layer_nodes}")

Find all child node names that contain '_J0' followed by digits or '_FLG'. Store joints with their parent nodes and print layer information.

def get_nodes_on_same_layer(self, parent_node, joint): View Source

123    def get_nodes_on_same_layer(self, parent_node, joint):
124        """
125        Find nodes on same layer and only add descendants of link nodes.
126        """
127        same_layer = []
128        # First get immediate layer nodes
129        for (parent, child), data in self.edge_data.items():
130            if parent == parent_node:
131                if child == joint:
132                    continue
133                if "geometry" in data:
134                    same_layer.append(data["geometry"])
135                    self.parent_nodes_dict[data["geometry"]] = child
136
137                # Get all descendants for this link
138                parentChild = child
139                stack = [child]
140                while stack:
141                    current = stack.pop()
142                    for (p, c), data in self.edge_data.items():
143                        if p == current:
144                            if "geometry" in data:
145                                same_layer.append(data["geometry"])
146                                self.parent_nodes_dict[data["geometry"]] = parentChild
147                            stack.append(c)
148
149        self.layer_nodes_dict[joint] = same_layer
150        return same_layer

Find nodes on same layer and only add descendants of link nodes.

def geometry_pose_to_matrix( self, init_pose: wandelbots_api_client.models.planner_pose.PlannerPose): View Source

152    def geometry_pose_to_matrix(self, init_pose: models.PlannerPose):
153        return self.robot.pose_to_matrix(init_pose)

def compute_forward_kinematics(self, joint_values): View Source

155    def compute_forward_kinematics(self, joint_values):
156        """Compute link transforms using the robot's methods."""
157        accumulated = self.robot.pose_to_matrix(self.robot.mounting)
158        transforms = [accumulated.copy()]
159        for dh_param, joint_rot in zip(self.robot.dh_parameters, joint_values.joints, strict=False):
160            transform = self.robot.dh_transform(dh_param, joint_rot)
161            accumulated = accumulated @ transform
162            transforms.append(accumulated.copy())
163        return transforms

Compute link transforms using the robot's methods.

def rotation_matrix_to_axis_angle(self, Rm): View Source

165    def rotation_matrix_to_axis_angle(self, Rm):
166        """Use scipy for cleaner axis-angle extraction."""
167        rot = Rotation.from_matrix(Rm)
168        angle = rot.magnitude()
169        axis = rot.as_rotvec() / angle if angle > 1e-8 else np.array([1.0, 0.0, 0.0])
170        return axis, angle

Use scipy for cleaner axis-angle extraction.

def gamma_lift_single_color(self, color: numpy.ndarray, gamma: float = 0.8) -> numpy.ndarray: View Source

172    def gamma_lift_single_color(self, color: np.ndarray, gamma: float = 0.8) -> np.ndarray:
173        """
174        Apply gamma correction to a single RGBA color in-place.
175        color: shape (4,) with [R, G, B, A] in 0..255, dtype=uint8
176        gamma: < 1.0 brightens midtones, > 1.0 darkens them.
177        """
178        rgb_float = color[:3].astype(np.float32) / 255.0
179        rgb_float = np.power(rgb_float, gamma)
180        color[:3] = (rgb_float * 255.0).astype(np.uint8)
181
182        return color

Apply gamma correction to a single RGBA color in-place. color: shape (4,) with [R, G, B, A] in 0..255, dtype=uint8 gamma: < 1.0 brightens midtones, > 1.0 darkens them.

def get_transform_matrix(self): View Source

184    def get_transform_matrix(self):
185        """
186        Creates a transformation matrix that converts from glTF's right-handed Y-up
187        coordinate system to Rerun's right-handed Z-up coordinate system.
188
189        Returns:
190            np.ndarray: A 4x4 transformation matrix
191        """
192        # Convert from glTF's Y-up to Rerun's Z-up coordinate system
193        return np.array(
194            [
195                [1.0, 0.0, 0.0, 0.0],  # X stays the same
196                [0.0, 0.0, -1.0, 0.0],  # Y becomes -Z
197                [0.0, 1.0, 0.0, 0.0],  # Z becomes Y
198                [0.0, 0.0, 0.0, 1.0],  # Homogeneous coordinate
199            ]
200        )

Creates a transformation matrix that converts from glTF's right-handed Y-up coordinate system to Rerun's right-handed Z-up coordinate system.

Returns:

np.ndarray: A 4x4 transformation matrix

def init_mesh(self, entity_path: str, geom, joint_name): View Source

202    def init_mesh(self, entity_path: str, geom, joint_name):
203        """Generic method to log a single geometry, either capsule or box."""
204
205        if entity_path not in self.logged_meshes:
206            if geom.metadata.get("node") not in self.parent_nodes_dict:
207                return
208
209            base_transform = np.eye(4)
210            # if the dh parameters are not at 0,0,0 from the mesh we have to move the first mesh joint
211            if "J00" in joint_name:
212                base_transform_, _ = self.scene.graph.get(frame_to=joint_name)
213                base_transform = base_transform_.copy()
214            base_transform[:3, 3] *= 1000
215
216            # if the mesh has the pivot not in the center, we need to adjust the transform
217            cumulative_transform, _ = self.scene.graph.get(
218                frame_to=self.parent_nodes_dict[geom.metadata.get("node")]
219            )
220            ctransform = cumulative_transform.copy()
221
222            # scale positions to mm
223            ctransform[:3, 3] *= 1000
224
225            # scale mesh to mm
226            transform = base_transform @ ctransform
227            mesh_scale_matrix = np.eye(4)
228            mesh_scale_matrix[:3, :3] *= 1000
229            transform = transform @ mesh_scale_matrix
230            transformed_mesh = geom.copy()
231
232            transformed_mesh.apply_transform(transform)
233
234            if transformed_mesh.visual is not None:
235                transformed_mesh.visual = transformed_mesh.visual.to_color()
236
237            vertex_colors = None
238            if transformed_mesh.visual and hasattr(transformed_mesh.visual, "vertex_colors"):
239                vertex_colors = transformed_mesh.visual.vertex_colors
240
241            rr.log(
242                entity_path,
243                rr.Mesh3D(
244                    vertex_positions=transformed_mesh.vertices,
245                    triangle_indices=transformed_mesh.faces,
246                    vertex_normals=getattr(transformed_mesh, "vertex_normals", None),
247                    albedo_factor=self.gamma_lift_single_color(vertex_colors, gamma=0.5)
248                    if vertex_colors is not None
249                    else None,
250                ),
251            )
252
253            self.logged_meshes.add(entity_path)

Generic method to log a single geometry, either capsule or box.

def init_collision_geometry( self, entity_path: str, collider: wandelbots_api_client.models.collider.Collider, pose: wandelbots_api_client.models.planner_pose.PlannerPose): View Source

255    def init_collision_geometry(
256        self, entity_path: str, collider: models.Collider, pose: models.PlannerPose
257    ):
258        if entity_path in self.logged_meshes:
259            return
260
261        if isinstance(collider.shape.actual_instance, models.Sphere2):
262            rr.log(
263                f"{entity_path}",
264                rr.Ellipsoids3D(
265                    radii=[
266                        collider.shape.actual_instance.radius,
267                        collider.shape.actual_instance.radius,
268                        collider.shape.actual_instance.radius,
269                    ],
270                    centers=[[pose.position.x, pose.position.y, pose.position.z]]
271                    if pose.position
272                    else [0, 0, 0],
273                    colors=[(221, 193, 193, 255)],
274                ),
275            )
276
277        elif isinstance(collider.shape.actual_instance, models.Box2):
278            rr.log(
279                f"{entity_path}",
280                rr.Boxes3D(
281                    centers=[[pose.position.x, pose.position.y, pose.position.z]]
282                    if pose.position
283                    else [0, 0, 0],
284                    sizes=[
285                        collider.shape.actual_instance.size_x,
286                        collider.shape.actual_instance.size_y,
287                        collider.shape.actual_instance.size_z,
288                    ],
289                    colors=[(221, 193, 193, 255)],
290                ),
291            )
292
293        elif isinstance(collider.shape.actual_instance, models.Capsule2):
294            height = collider.shape.actual_instance.cylinder_height
295            radius = collider.shape.actual_instance.radius
296
297            # Generate trimesh capsule
298            capsule = trimesh.creation.capsule(height=height, radius=radius, count=[6, 8])
299
300            # Extract vertices and faces for solid visualization
301            vertices = np.array(capsule.vertices)
302
303            # Transform vertices to world position
304            transform = np.eye(4)
305            if pose.position:
306                transform[:3, 3] = [pose.position.x, pose.position.y, pose.position.z - height / 2]
307            else:
308                transform[:3, 3] = [0, 0, -height / 2]
309
310            if collider.pose and collider.pose.orientation:
311                rot_mat = Rotation.from_quat(
312                    [
313                        collider.pose.orientation[0],
314                        collider.pose.orientation[1],
315                        collider.pose.orientation[2],
316                        collider.pose.orientation[3],
317                    ]
318                )
319                transform[:3, :3] = rot_mat.as_matrix()
320
321            vertices = np.array([transform @ np.append(v, 1) for v in vertices])[:, :3]
322
323            polygons = HullVisualizer.compute_hull_outlines_from_points(vertices)
324
325            if polygons:
326                line_segments = [p.tolist() for p in polygons]
327                rr.log(
328                    f"{entity_path}",
329                    rr.LineStrips3D(
330                        line_segments,
331                        radii=rr.Radius.ui_points(0.75),
332                        colors=[[221, 193, 193, 255]],
333                    ),
334                    static=True,
335                )
336
337        elif isinstance(collider.shape.actual_instance, models.ConvexHull2):
338            polygons = HullVisualizer.compute_hull_outlines_from_points(
339                collider.shape.actual_instance.vertices
340            )
341
342            if polygons:
343                line_segments = [p.tolist() for p in polygons]
344                rr.log(
345                    f"{entity_path}",
346                    rr.LineStrips3D(
347                        line_segments, radii=rr.Radius.ui_points(1.5), colors=[colors.colors[2]]
348                    ),
349                    static=True,
350                )
351
352                vertices, triangles, normals = HullVisualizer.compute_hull_mesh(polygons)
353
354                rr.log(
355                    f"{entity_path}",
356                    rr.Mesh3D(
357                        vertex_positions=vertices,
358                        triangle_indices=triangles,
359                        vertex_normals=normals,
360                        albedo_factor=colors.colors[0],
361                    ),
362                    static=True,
363                )
364
365        self.logged_meshes.add(entity_path)

def init_geometry( self, entity_path: str, geometry: wandelbots_api_client.models.geometry.Geometry): View Source

367    def init_geometry(self, entity_path: str, geometry: models.Geometry):
368        """Generic method to log a single geometry, either capsule or box."""
369
370        if entity_path in self.logged_meshes:
371            return
372
373        # Sphere geometry
374        if geometry.sphere:
375            radius = geometry.sphere.radius
376            rr.log(
377                entity_path,
378                rr.Ellipsoids3D(
379                    radii=[radius, radius, radius],
380                    centers=[0, 0, 0],
381                    fill_mode=rr.components.FillMode.Solid,
382                    colors=[(221, 193, 193, 255) if self.static_transform else self.albedo_factor],
383                ),
384            )
385
386        # Box geometry
387        elif geometry.box:
388            rr.log(
389                entity_path,
390                rr.Boxes3D(
391                    centers=[0, 0, 0],
392                    fill_mode=rr.components.FillMode.Solid,
393                    sizes=[geometry.box.size_x, geometry.box.size_y, geometry.box.size_z],
394                    colors=[(221, 193, 193, 255) if self.static_transform else self.albedo_factor],
395                ),
396            )
397
398        # Rectangle geometry
399        elif geometry.rectangle:
400            # Create a flat box with minimal height
401            rr.log(
402                entity_path,
403                rr.Boxes3D(
404                    fill_mode=rr.components.FillMode.Solid,
405                    centers=[0, 0, 0],
406                    sizes=[
407                        geometry.rectangle.size_x,
408                        geometry.rectangle.size_y,
409                        1.0,  # Minimal height for visibility
410                    ],
411                    colors=[(221, 193, 193, 255) if self.static_transform else self.albedo_factor],
412                ),
413            )
414
415        # Cylinder geometry
416        elif geometry.cylinder:
417            radius = geometry.cylinder.radius
418            height = geometry.cylinder.height
419
420            # Create cylinder mesh
421            cylinder = trimesh.creation.cylinder(radius=radius, height=height, sections=16)
422            vertex_normals = cylinder.vertex_normals.tolist()
423
424            rr.log(
425                entity_path,
426                rr.Mesh3D(
427                    vertex_positions=cylinder.vertices.tolist(),
428                    triangle_indices=cylinder.faces.tolist(),
429                    vertex_normals=vertex_normals,
430                    albedo_factor=self.albedo_factor,
431                ),
432            )
433
434        # Convex hull geometry
435        elif geometry.convex_hull:
436            polygons = HullVisualizer.compute_hull_outlines_from_points(
437                [[v.x, v.y, v.z] for v in geometry.convex_hull.vertices]
438            )
439
440            if polygons:
441                # First log wireframe outline
442                line_segments = [p.tolist() for p in polygons]
443                rr.log(
444                    f"{entity_path}_wireframe",
445                    rr.LineStrips3D(
446                        line_segments,
447                        radii=rr.Radius.ui_points(1.0),
448                        colors=[colors.colors[2] if self.static_transform else self.albedo_factor],
449                    ),
450                    static=self.static_transform,
451                )
452
453                # Then log solid mesh
454                vertices, triangles, normals = HullVisualizer.compute_hull_mesh(polygons)
455
456                rr.log(
457                    entity_path,
458                    rr.Mesh3D(
459                        vertex_positions=vertices,
460                        triangle_indices=triangles,
461                        vertex_normals=normals,
462                        albedo_factor=self.albedo_factor,
463                    ),
464                    static=self.static_transform,
465                )
466
467        # Capsule geometry
468        elif geometry.capsule:
469            radius = geometry.capsule.radius
470            height = geometry.capsule.cylinder_height
471
472            # Slightly shrink the capsule if static to reduce z-fighting
473            if self.static_transform:
474                radius *= 0.99
475                height *= 0.99
476
477            # Create capsule and retrieve normals
478            cap_mesh = trimesh.creation.capsule(radius=radius, height=height)
479            vertex_normals = cap_mesh.vertex_normals.tolist()
480
481            rr.log(
482                entity_path,
483                rr.Mesh3D(
484                    vertex_positions=cap_mesh.vertices.tolist(),
485                    triangle_indices=cap_mesh.faces.tolist(),
486                    vertex_normals=vertex_normals,
487                    albedo_factor=self.albedo_factor,
488                ),
489            )
490
491        # Rectangular capsule geometry
492        elif geometry.rectangular_capsule:
493            radius = geometry.rectangular_capsule.radius
494            distance_x = geometry.rectangular_capsule.sphere_center_distance_x
495            distance_y = geometry.rectangular_capsule.sphere_center_distance_y
496
497            # Create a rectangular capsule from its definition - a hull around 4 spheres
498            # First, create the four spheres at the corners
499            sphere_centers = [
500                [distance_x, distance_y, 0],
501                [distance_x, -distance_y, 0],
502                [-distance_x, distance_y, 0],
503                [-distance_x, -distance_y, 0],
504            ]
505
506            # Generate points to create a convex hull
507            all_points = []
508            for center in sphere_centers:
509                # Generate points for each sphere (simplified with key points on the sphere)
510                for dx, dy, dz in [
511                    (1, 0, 0),
512                    (-1, 0, 0),
513                    (0, 1, 0),
514                    (0, -1, 0),
515                    (0, 0, 1),
516                    (0, 0, -1),
517                ]:
518                    all_points.append(
519                        [center[0] + radius * dx, center[1] + radius * dy, center[2] + radius * dz]
520                    )
521
522            # Use our hull visualizer to create outlines
523            polygons = HullVisualizer.compute_hull_outlines_from_points(all_points)
524
525            if polygons:
526                # Log wireframe outline
527                line_segments = [p.tolist() for p in polygons]
528                rr.log(
529                    f"{entity_path}_wireframe",
530                    rr.LineStrips3D(
531                        line_segments,
532                        radii=rr.Radius.ui_points(1.0),
533                        colors=[
534                            (221, 193, 193, 255) if self.static_transform else self.albedo_factor
535                        ],
536                    ),
537                    static=self.static_transform,
538                )
539
540                # Log solid mesh
541                vertices, triangles, normals = HullVisualizer.compute_hull_mesh(polygons)
542
543                rr.log(
544                    entity_path,
545                    rr.Mesh3D(
546                        vertex_positions=vertices,
547                        triangle_indices=triangles,
548                        vertex_normals=normals,
549                        albedo_factor=self.albedo_factor,
550                    ),
551                    static=self.static_transform,
552                )
553
554        # Plane geometry (simplified as a large, thin rectangle)
555        elif geometry.plane:
556            # Create a large, thin rectangle to represent an infinite plane
557            size = 5000  # Large enough to seem infinite in the visualization
558            rr.log(
559                entity_path,
560                rr.Boxes3D(
561                    centers=[0, 0, 0],
562                    sizes=[size, size, 1.0],  # Very thin in z direction
563                    colors=[(200, 200, 220, 100) if self.static_transform else self.albedo_factor],
564                ),
565            )
566
567        # Compound geometry - recursively process child geometries
568        elif geometry.compound and geometry.compound.child_geometries:
569            for i, child_geom in enumerate(geometry.compound.child_geometries):
570                child_path = f"{entity_path}/child_{i}"
571                self.init_geometry(child_path, child_geom)
572
573        # Default fallback for unsupported geometry types
574        else:
575            # Fallback to a box
576            rr.log(
577                entity_path,
578                rr.Boxes3D(
579                    half_sizes=[[50, 50, 50]],
580                    colors=[(255, 0, 0, 128)],  # Red, semi-transparent to indicate unknown type
581                ),
582            )
583
584        self.logged_meshes.add(entity_path)

Generic method to log a single geometry, either capsule or box.

def log_robot_geometry(self, joint_position): View Source

586    def log_robot_geometry(self, joint_position):
587        transforms = self.compute_forward_kinematics(joint_position)
588
589        def log_geometry(entity_path, transform):
590            translation = transform[:3, 3]
591            Rm = transform[:3, :3]
592            axis, angle = self.rotation_matrix_to_axis_angle(Rm)
593            rr.log(
594                entity_path,
595                rr.InstancePoses3D(
596                    translations=[translation.tolist()],
597                    rotation_axis_angles=[
598                        rr.RotationAxisAngle(axis=axis.tolist(), angle=float(angle))
599                    ],
600                ),
601                static=self.static_transform,
602            )
603
604        # Log robot joint geometries
605        if self.mesh_loaded:
606            for link_index, joint_name in enumerate(self.joint_names):
607                link_transform = transforms[link_index]
608
609                # Get nodes on same layer using dictionary
610                same_layer_nodes = self.layer_nodes_dict.get(joint_name)
611                if not same_layer_nodes:
612                    continue
613
614                filtered_geoms = []
615                for node_name in same_layer_nodes:
616                    if node_name in self.scene.geometry:
617                        geom = self.scene.geometry[node_name]
618                        # Add metadata that would normally come from dump
619                        geom.metadata = {"node": node_name}
620                        filtered_geoms.append(geom)
621
622                for geom in filtered_geoms:
623                    entity_path = f"{self.base_entity_path}/visual/links/link_{link_index}/mesh/{geom.metadata.get('node')}"
624
625                    # calculate the inverse transform to get the mesh in the correct position
626                    cumulative_transform, _ = self.scene.graph.get(frame_to=joint_name)
627                    ctransform = cumulative_transform.copy()
628                    inverse_transform = np.linalg.inv(ctransform)
629
630                    # DH theta is rotated, rotate mesh around z in direction of theta
631                    rotation_matrix_z_4x4 = np.eye(4)
632                    if len(self.robot.dh_parameters) > link_index:
633                        rotation_z_minus_90 = Rotation.from_euler(
634                            "z", self.robot.dh_parameters[link_index].theta, degrees=False
635                        ).as_matrix()
636                        rotation_matrix_z_4x4[:3, :3] = rotation_z_minus_90
637
638                    # scale positions to mm
639                    inverse_transform[:3, 3] *= 1000
640
641                    root_transform = self.get_transform_matrix()
642
643                    transform = root_transform @ inverse_transform
644
645                    final_transform = link_transform @ rotation_matrix_z_4x4 @ transform
646
647                    self.init_mesh(entity_path, geom, joint_name)
648                    log_geometry(entity_path, final_transform)
649
650        # Log link geometries (safety from controller)
651        if self.show_safety_link_chain:
652            for link_index, geometries in self.link_geometries.items():
653                link_transform = transforms[link_index]
654                for i, geom in enumerate(geometries):
655                    entity_path = f"{self.base_entity_path}/safety_from_controller/links/link_{link_index}/geometry_{i}"
656                    final_transform = link_transform @ self.geometry_pose_to_matrix(geom.init_pose)
657
658                    self.init_geometry(entity_path, geom)
659                    log_geometry(entity_path, final_transform)
660
661        # Log TCP geometries (safety from controller)
662        if self.show_safety_link_chain and self.tcp_geometries:
663            tcp_transform = transforms[-1]  # the final frame transform
664            for i, geom in enumerate(self.tcp_geometries):
665                entity_path = f"{self.base_entity_path}/safety_from_controller/tcp/geometry_{i}"
666                final_transform = tcp_transform @ self.geometry_pose_to_matrix(geom.init_pose)
667
668                self.init_geometry(entity_path, geom)
669                log_geometry(entity_path, final_transform)

def log_robot_geometries( self, trajectory: list[wandelbots_api_client.models.trajectory_sample.TrajectorySample], times_column): View Source

671    def log_robot_geometries(self, trajectory: list[models.TrajectorySample], times_column):
672        """
673        Log the robot geometries for each link and TCP as separate entities.
674
675        Args:
676            trajectory (List[wb.models.TrajectorySample]): The list of trajectory sample points.
677            times_column (rr.TimeColumn): The time column associated with the trajectory points.
678        """
679        link_positions = {}
680        link_rotations = {}
681
682        def collect_geometry_data(entity_path, transform):
683            """Helper to collect geometry data for a given entity."""
684            translation = transform[:3, 3].tolist()
685            Rm = transform[:3, :3]
686            axis, angle = self.rotation_matrix_to_axis_angle(Rm)
687            if entity_path not in link_positions:
688                link_positions[entity_path] = []
689                link_rotations[entity_path] = []
690            link_positions[entity_path].append(translation)
691            link_rotations[entity_path].append(rr.RotationAxisAngle(axis=axis, angle=angle))
692
693        for point in trajectory:
694            transforms = self.compute_forward_kinematics(point.joint_position)
695
696            # Log robot joint geometries
697            if self.mesh_loaded:
698                for link_index, joint_name in enumerate(self.joint_names):
699                    if link_index >= len(transforms):
700                        break
701                    link_transform = transforms[link_index]
702
703                    # Get nodes on same layer using dictionary
704                    same_layer_nodes = self.layer_nodes_dict.get(joint_name)
705                    if not same_layer_nodes:
706                        continue
707
708                    filtered_geoms = []
709                    for node_name in same_layer_nodes:
710                        if node_name in self.scene.geometry:
711                            geom = self.scene.geometry[node_name]
712                            # Add metadata that would normally come from dump
713                            geom.metadata = {"node": node_name}
714                            filtered_geoms.append(geom)
715
716                    for geom in filtered_geoms:
717                        entity_path = f"{self.base_entity_path}/visual/links/link_{link_index}/mesh/{geom.metadata.get('node')}"
718
719                        # calculate the inverse transform to get the mesh in the correct position
720                        cumulative_transform, _ = self.scene.graph.get(frame_to=joint_name)
721                        ctransform = cumulative_transform.copy()
722                        inverse_transform = np.linalg.inv(ctransform)
723
724                        # DH theta is rotated, rotate mesh around z in direction of theta
725                        rotation_matrix_z_4x4 = np.eye(4)
726                        if len(self.robot.dh_parameters) > link_index:
727                            rotation_z_minus_90 = Rotation.from_euler(
728                                "z", self.robot.dh_parameters[link_index].theta, degrees=False
729                            ).as_matrix()
730                            rotation_matrix_z_4x4[:3, :3] = rotation_z_minus_90
731
732                        # scale positions to mm
733                        inverse_transform[:3, 3] *= 1000
734
735                        root_transform = self.get_transform_matrix()
736
737                        transform = root_transform @ inverse_transform
738
739                        final_transform = link_transform @ rotation_matrix_z_4x4 @ transform
740
741                        self.init_mesh(entity_path, geom, joint_name)
742                        collect_geometry_data(entity_path, final_transform)
743
744            # Collect data for link geometries (safety from controller)
745            if self.show_safety_link_chain:
746                for link_index, geometries in self.link_geometries.items():
747                    link_transform = transforms[link_index]
748                    for i, geom in enumerate(geometries):
749                        entity_path = f"{self.base_entity_path}/safety_from_controller/links/link_{link_index}/geometry_{i}"
750                        final_transform = link_transform @ self.geometry_pose_to_matrix(
751                            geom.init_pose
752                        )
753                        self.init_geometry(entity_path, geom)
754                        collect_geometry_data(entity_path, final_transform)
755
756            # Collect data for TCP geometries (safety from controller)
757            if self.show_safety_link_chain and self.tcp_geometries:
758                tcp_transform = transforms[-1]  # End-effector transform
759                for i, geom in enumerate(self.tcp_geometries):
760                    entity_path = f"{self.base_entity_path}/safety_from_controller/tcp/geometry_{i}"
761                    final_transform = tcp_transform @ self.geometry_pose_to_matrix(geom.init_pose)
762                    self.init_geometry(entity_path, geom)
763                    collect_geometry_data(entity_path, final_transform)
764
765            # Collect data for collision link geometries (only if enabled)
766            if self.show_collision_link_chain and self.collision_link_geometries:
767                for link_index, geometries in enumerate(self.collision_link_geometries):
768                    link_transform = transforms[link_index]
769                    for i, geom_id in enumerate(geometries):
770                        entity_path = f"{self.base_entity_path}/collision/links/link_{link_index}/geometry_{geom_id}"
771
772                        pose = normalize_pose(geometries[geom_id].pose)
773
774                        final_transform = link_transform @ self.geometry_pose_to_matrix(pose)
775                        self.init_collision_geometry(entity_path, geometries[geom_id], pose)
776                        collect_geometry_data(entity_path, final_transform)
777
778            # Collect data for collision TCP geometries (only if enabled)
779            if self.show_collision_link_chain and self.collision_tcp_geometries:
780                tcp_transform = transforms[-1]  # End-effector transform
781                for i, geom_id in enumerate(self.collision_tcp_geometries):
782                    entity_path = f"{self.base_entity_path}/collision/tcp/geometry_{geom_id}"
783
784                    pose = normalize_pose(self.collision_tcp_geometries[geom_id].pose)
785                    final_transform = tcp_transform @ self.geometry_pose_to_matrix(pose)
786
787                    # tcp collision geometries are defined in flange frame
788                    identity_pose = models.PlannerPose(
789                        position=models.Vector3d(x=0, y=0, z=0),
790                        orientation=models.Quaternion(x=0, y=0, z=0, w=1),
791                    )
792                    self.init_collision_geometry(
793                        entity_path, self.collision_tcp_geometries[geom_id], identity_pose
794                    )
795                    collect_geometry_data(entity_path, final_transform)
796
797        # Send collected columns for all geometries
798        for entity_path, positions in link_positions.items():
799            rr.send_columns(
800                entity_path,
801                indexes=[times_column],
802                columns=[
803                    *rr.Transform3D.columns(
804                        translation=positions, rotation_axis_angle=link_rotations[entity_path]
805                    )
806                ],
807            )

Log the robot geometries for each link and TCP as separate entities.

Arguments:

trajectory (List[wb.models.TrajectorySample]): The list of trajectory sample points.
times_column (rr.TimeColumn): The time column associated with the trajectory points.