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.pc_concat.GetInputPort("X_FCi_0").FixValue(self.context, X_WP_0)
self.pc_concat.GetInputPort("X_FCi_1").FixValue(self.context, X_WP_1)
def __init__(self, id_list, default_rgb=[255., 255., 255.]):
"""
A system that takes in N point clouds of points Si in frame Ci, and N
RigidTransforms from frame Ci to F, to put each point cloud in a common
frame F. The system returns one point cloud combining all of the
transformed point clouds. Each point cloud must have XYZs. RGBs are
optional. If absent, those points will be the provided default color.
@param id_list A list containing the string IDs of all of the point
clouds. This is often the serial number of the camera they came
from, such as "1" for a simulated camera or "805212060373" for a
real camera.
@param default_rgb A list of length 3 containing the RGB values to use
in the absence of PointCloud.rgbs. Values should be between 0 and
255. The default is white.
@system{
@input_port{point_cloud_CiSi_id0}
@input_port{X_FCi_id0}
.
.
.
@input_port{point_cloud_CiSi_idN}
@input_port{X_FCi_idN}
@output_port{point_cloud_FS}
}
"""
LeafSystem.__init__(self)
self._point_cloud_ports = {}
self._transform_ports = {}
self._id_list = id_list
self._default_rgb = np.array(default_rgb)
output_fields = Fields(BaseField.kXYZs | BaseField.kRGBs)
for id in self._id_list:
self._point_cloud_ports[id] = self.DeclareAbstractInputPort(
"point_cloud_CiSi_{}".format(id),
AbstractValue.Make(PointCloud(fields=output_fields)))
self._transform_ports[id] = self.DeclareAbstractInputPort(
"X_FCi_{}".format(id),
AbstractValue.Make(RigidTransform.Identity()))
self.DeclareAbstractOutputPort(
"point_cloud_FS",
lambda: AbstractValue.Make(PointCloud(fields=output_fields)),
self.DoCalcOutput)
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()
