[Feature]: Add OcTree Visualization For Viser

[CihatAltiparmak]

🚀 Feature

The feature request here is to add OcTree visualization support for ViserVisualizer. If you wish, I can work on this.

Motivation

Hello, I am trying to add OcTree support into github.com/open-planning/roboplan . While adding this support, I've noticed that ViserVisualizer does not have OcTree visualization support. To support OcTree visualization, I have to implement my own visualizer to solve this, which is not sustainable.

Alternatives

For now, I can solve this problem in this code block https://github.com/CihatAltiparmak/roboplan/blob/feature/add_octree_support/bindings/src/roboplan/viser_visualizer.py#L227-L302 by inspiring

pinocchio/bindings/python/pinocchio/visualize/meshcat_visualizer.py

Lines 229 to 300 in 4845b01

	if (
	WITH_HPP_FCL_BINDINGS
	and tuple(map(int, hppfcl.__version__.split("."))) >= (3, 0, 0)
	and hppfcl.WITH_OCTOMAP
	):
	def loadOctree(octree: hppfcl.OcTree):
	boxes = octree.toBoxes()
	if len(boxes) == 0:
	return
	bs = boxes[0][3] / 2.0
	num_boxes = len(boxes)
	box_corners = np.array(
	[
	[bs, bs, bs],
	[bs, bs, -bs],
	[bs, -bs, bs],
	[bs, -bs, -bs],
	[-bs, bs, bs],
	[-bs, bs, -bs],
	[-bs, -bs, bs],
	[-bs, -bs, -bs],
	]
	)
	all_points = np.empty((8 * num_boxes, 3))
	all_faces = np.empty((12 * num_boxes, 3), dtype=int)
	face_id = 0
	for box_id, box_properties in enumerate(boxes):
	box_center = box_properties[:3]
	corners = box_corners + box_center
	point_range = range(box_id * 8, (box_id + 1) * 8)
	all_points[point_range, :] = corners
	A = box_id * 8
	B = A + 1
	C = B + 1
	D = C + 1
	E = D + 1
	F = E + 1
	G = F + 1
	H = G + 1
	all_faces[face_id] = np.array([C, D, B])
	all_faces[face_id + 1] = np.array([B, A, C])
	all_faces[face_id + 2] = np.array([A, B, F])
	all_faces[face_id + 3] = np.array([F, E, A])
	all_faces[face_id + 4] = np.array([E, F, H])
	all_faces[face_id + 5] = np.array([H, G, E])
	all_faces[face_id + 6] = np.array([G, H, D])
	all_faces[face_id + 7] = np.array([D, C, G])
	# # top
	all_faces[face_id + 8] = np.array([A, E, G])
	all_faces[face_id + 9] = np.array([G, C, A])
	# # bottom
	all_faces[face_id + 10] = np.array([B, H, F])
	all_faces[face_id + 11] = np.array([H, B, D])
	face_id += 12
	colors = np.empty((all_points.shape[0], 3))
	colors[:] = np.ones(3)
	mesh = mg.TriangularMeshGeometry(all_points, all_faces, colors)
	return mesh
	else:
	def loadOctree(octree):
	raise NotImplementedError("loadOctree need hppfcl with octomap support")

Additional context

Here is octree visualization on viser

Checklist

• I have checked that there is no similar issue/discussion in the repo.

[sea-bass]

Ir would be great for you to add this stuff here!

https://github.com/stack-of-tasks/pinocchio/blob/devel/bindings/python/pinocchio/visualize/viser_visualizer.py