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()