def test_meshcat_point_cloud_visualizer(self, T):
meshcat = mut.Meshcat()
visualizer = mut.MeshcatPointCloudVisualizer_[T](
meshcat=meshcat, path="cloud", publish_period=1/12.0)
visualizer.set_point_size(0.1)
visualizer.set_default_rgba(mut.Rgba(0, 0, 1, 1))
context = visualizer.CreateDefaultContext()
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
visualizer.cloud_input_port().FixValue(
context, AbstractValue.Make(cloud))
self.assertIsInstance(visualizer.pose_input_port(), InputPort_[T])
visualizer.Publish(context)
visualizer.Delete()
if T == float:
ad_visualizer = visualizer.ToAutoDiffXd()
self.assertIsInstance(
ad_visualizer, mut.MeshcatPointCloudVisualizer_[AutoDiffXd])
def draw_open3d_point_cloud(meshcat,
path,
pcd,
normals_scale=0.0,
point_size=0.001):
pts = np.asarray(pcd.points)
assert (pcd.has_colors()) # TODO(russt): handle this case better
cloud = PointCloud(pts.shape[0], Fields(BaseField.kXYZs | BaseField.kRGBs))
cloud.mutable_xyzs()[:] = pts.T
cloud.mutable_rgbs()[:] = 255 * np.asarray(pcd.colors).T
meshcat.SetObject(path, cloud, point_size=point_size)
if pcd.has_normals() and normals_scale > 0.0:
assert ('need to implement LineSegments in meshcat c++')
normals = np.asarray(pcd.normals)
vertices = np.hstack(
(pts, pts + normals_scale * normals)).reshape(-1, 3).T
meshcat["normals"].set_object(
g.LineSegments(g.PointsGeometry(vertices),
g.MeshBasicMaterial(color=0x000000)))
class TestPointCloudConcatenation(unittest.TestCase):
def setUp(self):
builder = DiagramBuilder()
X_WP_0 = RigidTransform.Identity()
X_WP_1 = RigidTransform.Identity()
X_WP_1.set_translation([1.0, 0, 0])
id_list = ["0", "1"]
self.pc_concat = builder.AddSystem(PointCloudConcatenation(id_list))
self.num_points = 10000
xyzs = np.random.uniform(-0.1, 0.1, (3, self.num_points))
# Only go to 254 to distinguish between point clouds with and without
# color.
rgbs = np.random.uniform(0., 254.0, (3, self.num_points))
self.pc = PointCloud(self.num_points,
Fields(BaseField.kXYZs | BaseField.kRGBs))
self.pc.mutable_xyzs()[:] = xyzs
self.pc.mutable_rgbs()[:] = rgbs
self.pc_no_rgbs = PointCloud(self.num_points, Fields(BaseField.kXYZs))
self.pc_no_rgbs.mutable_xyzs()[:] = xyzs
diagram = builder.Build()
simulator = Simulator(diagram)
self.context = diagram.GetMutableSubsystemContext(
self.pc_concat, simulator.get_mutable_context())
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_0").get_index(),
AbstractValue.Make(X_WP_0))
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_1").get_index(),
AbstractValue.Make(X_WP_1))
def test_no_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# Tthe second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
# Even if both input point clouds don't have rgbs, the fused point
# cloud should contain rgbs of the default color.
self.assertTrue(fused_pc.has_rgbs())
self.assertTrue(
np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255])))
self.assertTrue(
np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])))
def test_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# The second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
self.assertTrue(fused_pc.has_rgbs())
self.assertTrue(
np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255])))
self.assertTrue(
np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255])))
def test_mix_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc_no_rgbs))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# The second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
self.assertTrue(fused_pc.has_rgbs())
# We don't know in what order the two point clouds will be combined.
rgb_first = np.all(fused_pc.rgbs()[:, 0] != np.array([255, 255, 255]))
rgb_last = np.all(fused_pc.rgbs()[:, -1] != np.array([255, 255, 255]))
no_rgb_first = np.all(fused_pc.rgbs()[:,
0] == np.array([255, 255, 255]))
no_rgb_last = np.all(fused_pc.rgbs()[:,
-1] == np.array([255, 255, 255]))
self.assertTrue((rgb_first and no_rgb_last)
or (no_rgb_first and rgb_last))
def test_meshcat(self):
port = 7051
params = mut.MeshcatParams(host="*",
port=port,
web_url_pattern="http://host:{port}")
meshcat = mut.Meshcat(params=params)
self.assertEqual(meshcat.port(), port)
with self.assertRaises(RuntimeError):
meshcat2 = mut.Meshcat(port=port)
self.assertIn("http", meshcat.web_url())
self.assertIn("ws", meshcat.ws_url())
meshcat.SetObject(path="/test/box",
shape=mut.Box(1, 1, 1),
rgba=mut.Rgba(.5, .5, .5))
meshcat.SetTransform(path="/test/box", X_ParentPath=RigidTransform())
meshcat.SetTransform(path="/test/box", matrix=np.eye(4))
self.assertTrue(meshcat.HasPath("/test/box"))
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
meshcat.SetObject(path="/test/cloud",
cloud=cloud,
point_size=0.01,
rgba=mut.Rgba(.5, .5, .5))
mesh = mut.TriangleSurfaceMesh(triangles=[
mut.SurfaceTriangle(0, 1, 2),
mut.SurfaceTriangle(3, 0, 2)
],
vertices=[[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]])
meshcat.SetObject(path="/test/triangle_surface_mesh",
mesh=mesh,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetLine(path="/test/line",
vertices=np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetLineSegments(path="/test/line_segments",
start=np.eye(3),
end=2 * np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetTriangleMesh(path="/test/triangle_mesh",
vertices=np.array([[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]]).T,
faces=np.array([[0, 1, 2], [3, 0, 2]]).T,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetProperty(path="/Background", property="visible", value=True)
meshcat.SetProperty(path="/Lights/DirectionalLight/<object>",
property="intensity",
value=1.0)
meshcat.SetProperty(path="/Background",
property="top_color",
value=[0, 0, 0])
meshcat.Set2dRenderMode(X_WC=RigidTransform(),
xmin=-1,
xmax=1,
ymin=-1,
ymax=1)
meshcat.ResetRenderMode()
meshcat.AddButton(name="button")
self.assertEqual(meshcat.GetButtonClicks(name="button"), 0)
meshcat.DeleteButton(name="button")
meshcat.AddSlider(name="slider", min=0, max=1, step=0.01, value=0.5)
meshcat.SetSliderValue(name="slider", value=0.7)
self.assertAlmostEqual(meshcat.GetSliderValue(name="slider"),
0.7,
delta=1e-14)
meshcat.DeleteSlider(name="slider")
meshcat.DeleteAddedControls()
self.assertIn("data:application/octet-binary;base64",
meshcat.StaticHtml())
meshcat.Flush()
# PerspectiveCamera
camera = mut.Meshcat.PerspectiveCamera(fov=80,
aspect=1.2,
near=0.2,
far=200,
zoom=1.3)
self.assertEqual(camera.fov, 80)
self.assertEqual(camera.aspect, 1.2)
self.assertEqual(camera.near, 0.2)
self.assertEqual(camera.far, 200)
self.assertEqual(camera.zoom, 1.3)
self.assertEqual(
repr(camera), "".join([
r"PerspectiveCamera(", r"fov=80.0, "
r"aspect=1.2, ", r"near=0.2, ", r"far=200.0, ", r"zoom=1.3)"
]))
meshcat.SetCamera(camera=camera, path="mypath")
# OrthographicCamera
camera = mut.Meshcat.OrthographicCamera(left=0.1,
right=1.3,
top=0.3,
bottom=1.4,
near=0.2,
far=200,
zoom=1.3)
self.assertEqual(camera.left, 0.1)
self.assertEqual(camera.right, 1.3)
self.assertEqual(camera.top, 0.3)
self.assertEqual(camera.bottom, 1.4)
self.assertEqual(camera.near, 0.2)
self.assertEqual(camera.far, 200)
self.assertEqual(camera.zoom, 1.3)
self.assertEqual(
repr(camera), "".join([
r"OrthographicCamera(", r"left=0.1, "
r"right=1.3, ", r"top=0.3, "
r"bottom=1.4, ", r"near=0.2, ", r"far=200.0, ", r"zoom=1.3)"
]))
meshcat.SetCamera(camera=camera, path="mypath")
def test_meshcat(self):
meshcat = mut.Meshcat(port=7051)
self.assertEqual(meshcat.port(), 7051)
with self.assertRaises(RuntimeError):
meshcat2 = mut.Meshcat(port=7051)
self.assertIn("http", meshcat.web_url())
self.assertIn("ws", meshcat.ws_url())
meshcat.SetObject(path="/test/box",
shape=mut.Box(1, 1, 1),
rgba=mut.Rgba(.5, .5, .5))
meshcat.SetTransform(path="/test/box", X_ParentPath=RigidTransform())
meshcat.SetTransform(path="/test/box", matrix=np.eye(4))
self.assertTrue(meshcat.HasPath("/test/box"))
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
meshcat.SetObject(path="/test/cloud",
cloud=cloud,
point_size=0.01,
rgba=mut.Rgba(.5, .5, .5))
mesh = mut.TriangleSurfaceMesh(triangles=[
mut.SurfaceTriangle(0, 1, 2),
mut.SurfaceTriangle(3, 0, 2)
],
vertices=[[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]])
meshcat.SetObject(path="/test/triangle_surface_mesh",
mesh=mesh,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetLine(path="/test/line",
vertices=np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetLineSegments(path="/test/line_segments",
start=np.eye(3),
end=2 * np.eye(3),
line_width=2.0,
rgba=mut.Rgba(.3, .3, .3))
meshcat.SetTriangleMesh(path="/test/triangle_mesh",
vertices=np.array([[0, 0, 0], [1, 0, 0],
[1, 0, 1], [0, 0, 1]]).T,
faces=np.array([[0, 1, 2], [3, 0, 2]]).T,
rgba=mut.Rgba(0.3, 0.3, 0.3),
wireframe=True,
wireframe_line_width=2.0)
meshcat.SetProperty(path="/Background", property="visible", value=True)
meshcat.SetProperty(path="/Lights/DirectionalLight/<object>",
property="intensity",
value=1.0)
meshcat.SetProperty(path="/Background",
property="top_color",
value=[0, 0, 0])
meshcat.Set2dRenderMode(X_WC=RigidTransform(),
xmin=-1,
xmax=1,
ymin=-1,
ymax=1)
meshcat.ResetRenderMode()
meshcat.AddButton(name="button")
self.assertEqual(meshcat.GetButtonClicks(name="button"), 0)
meshcat.DeleteButton(name="button")
meshcat.AddSlider(name="slider", min=0, max=1, step=0.01, value=0.5)
meshcat.SetSliderValue(name="slider", value=0.7)
self.assertAlmostEqual(meshcat.GetSliderValue(name="slider"),
0.7,
delta=1e-14)
meshcat.DeleteSlider(name="slider")
meshcat.DeleteAddedControls()
self.assertIn("data:application/octet-binary;base64",
meshcat.StaticHtml())
meshcat.Flush()