def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
v["cube"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([-1, 0, 0]))
verts = np.random.rand(3, 100000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
point_cloud_P = self.EvalAbstractInput(context, 0).get_value()
# `Q` is a point in the point cloud.
p_PQs = point_cloud_P.xyzs()
# Use only valid points.
valid = np.logical_not(np.isnan(p_PQs))
valid = np.all(valid, axis=0) # Reduce along XYZ axis.
p_PQs = p_PQs[:, valid]
if point_cloud_P.has_rgbs():
rgbs = point_cloud_P.rgbs()[:, valid]
else:
# Need manual broadcasting.
count = p_PQs.shape[1]
rgbs = np.tile(np.array([self._default_rgb]).T, (1, count))
# pydrake `PointCloud.rgbs()` are on [0..255], while meshcat
# `PointCloud` colors are on [0..1].
rgbs = rgbs / 255. # Do not use in-place so we can promote types.
# Send to meshcat.
self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs))
self._meshcat_viz[self._name].set_transform(self._X_WP.GetAsMatrix4())
def visualize_point_cloud_meshcat(meshcat_vis, clouds: np.ndarray):
'''
Visualize the ground truth (red), observation (blue), and transformed
(yellow) point clouds in meshcat.
Args:
meschat_vis: an instance of a meshcat visualizer
scene: an Nx3 numpy array representing scene point cloud
model: an Mx3 numpy array representing model point cloud
X_GO: 4x4 numpy array of the homogeneous transformation from the
scene point cloud to the model point cloud.
'''
meshcat_vis['model'].delete()
meshcat_vis['observations'].delete()
meshcat_vis['transformed_observations'].delete()
for i, cloud in enumerate(clouds):
# Make meshcat color arrays.
N = cloud.shape[0]
if i < 3:
color_arr = make_meshcat_color_array(N, *rgb_sequences[i])
else:
raise NotImplementedError
# Create red and blue meshcat point clouds for visualization.
cloud_meshcat = g.PointCloud(cloud[:, :3].T, color_arr, size=0.01)
meshcat_vis[str(i)].set_object(cloud_meshcat)
def add_pointcloud(self, cloud, color=None, cloud_key='cloud_0'):
# type: (np.ndarray, np.ndarray, str) -> None
"""
Add a point cloud to the visualizer
:param cloud: (N, 3) np.ndarray
:param color: (N, 3) np.ndarray of (r, g, b) color or None
:param cloud_key: The key used to index the point cloud
:return:
"""
if cloud_key in self._pointcloud_set:
print('Duplicate pointcloud key detected')
print('You should use update_pointcloud instread')
return
# The color
if color is None:
color = MeshCatVisualizer._random_color_meshcat(cloud.shape[0])
# Check shape
assert cloud.shape[0] == color.shape[0]
assert cloud.shape[1] == 3
assert color.shape[1] == 3
# Add to meshcat
self._meshcat_vis[self._prefix][cloud_key].set_object(meshcat_geometry.PointCloud(cloud.T, color.T))
def VisualizeTransform(meshcat_vis, points, transform):
"""Visualizes the points transformed by transform in yellow.
Args:
@param meschat_vis An instance of a meshcat visualizer.
@param points An Nx3 numpy array representing a point cloud.
@param transform a 4x4 numpy array representing a homogeneous
transformation.
"""
# Make meshcat color arrays.
N = points.shape[0]
yellow = MakeMeshcatColorArray(N, 1, 1, 0)
homogenous_points = np.ones((N, 4))
homogenous_points[:, :3] = np.copy(points)
transformed_points = transform.dot(homogenous_points.T)
transformed_points_meshcat = \
g.PointCloud(transformed_points[:3, :], yellow.T)
meshcat_vis['transformed_observations'].set_object(
transformed_points_meshcat)
def draw_points(meshcat, points, color, **kwargs):
"""Helper for sending a 3xN points of a single color to MeshCat"""
points = np.asarray(points)
assert points.shape[0] == 3
if points.size == 3:
points.shape = (3, 1)
colors = np.tile(np.asarray(color).reshape(3, 1), (1, points.shape[1]))
meshcat.set_object(g.PointCloud(points, colors, **kwargs))
def VisualizeICP(meshcat_vis, scene, model, X_MS):
"""
Visualizes the ground truth (red), observation (blue), and transformed
(yellow) point clouds in meshcat.
@param meschat_vis An instance of a meshcat visualizer.
@param scene An Nx3 numpy array representing the scene point cloud.
@param model An Mx3 numpy array representing the model point cloud.
@param X_GO A 4x4 numpy array of the homogeneous transformation from the
scene point cloud to the model point cloud.
"""
meshcat_vis['model'].delete()
meshcat_vis['observations'].delete()
meshcat_vis['transformed_observations'].delete()
# Make meshcat color arrays.
N = scene.shape[0]
M = model.shape[0]
red = MakeMeshcatColorArray(M, 0.5, 0, 0)
blue = MakeMeshcatColorArray(N, 0, 0, 0.5)
yellow = MakeMeshcatColorArray(N, 1, 1, 0)
# Create red and blue meshcat point clouds for visualization.
model_meshcat = g.PointCloud(model.T, red, size=0.01)
observations_meshcat = g.PointCloud(scene.T, blue, size=0.01)
meshcat_vis['model'].set_object(model_meshcat)
meshcat_vis['observations'].set_object(observations_meshcat)
# Create a copy of the scene point cloud that is homogenous
# so we can apply a 4x4 homogenous transform to it.
homogenous_scene = np.ones((N, 4))
homogenous_scene[:, :3] = np.copy(scene)
# Apply the returned transformation to the scene samples to align the
# scene point cloud with the ground truth point cloud.
transformed_scene = X_MS.dot(homogenous_scene.T)
# Create a yellow meshcat point cloud for visualization.
transformed_scene_meshcat = \
g.PointCloud(transformed_scene[:3, :], yellow, size=0.01)
meshcat_vis['transformed_observations'].set_object(
transformed_scene_meshcat)
def draw_open3d_point_cloud(meshcat, pcd, normals_scale=0.0, size=0.001):
pts = np.asarray(pcd.points)
meshcat.set_object(g.PointCloud(pts.T, np.asarray(pcd.colors).T, size=size))
if pcd.has_normals() and normals_scale > 0.0:
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)))
def visualize_train_input(self, in_img, p, q, theta, z, roll, pitch):
"""Visualize the training input."""
points = []
colors = []
height = in_img[:, :, 3]
for i in range(in_img.shape[0]):
for j in range(in_img.shape[1]):
pixel = (i, j)
position = utils.pixel_to_position(pixel, height, self.bounds,
self.pixel_size)
points.append(position)
colors.append(in_img[i, j, :3])
points = np.array(points).T # shape (3, N)
colors = np.array(colors).T / 255.0 # shape (3, N)
self.vis["pointclouds/scene"].set_object(
g.PointCloud(position=points, color=colors))
pick_position = utils.pixel_to_position(p, height, self.bounds,
self.pixel_size)
label = "pick"
utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1)
pick_transform = np.eye(4)
pick_transform[0:3, 3] = pick_position
self.vis[label].set_transform(pick_transform)
place_position = utils.pixel_to_position(q, height, self.bounds,
self.pixel_size)
label = "place"
utils.make_frame(self.vis, label, h=0.05, radius=0.0012, o=0.1)
place_transform = np.eye(4)
place_transform[0:3, 3] = place_position
place_transform[2, 3] = z
rotation = utils.get_pybullet_quaternion_from_rot(
(roll, pitch, -theta))
quaternion_wxyz = np.asarray(
[rotation[3], rotation[0], rotation[1], rotation[2]])
place_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3,
0:3]
self.vis[label].set_transform(place_transform)
_, ax = plt.subplots(2, 1)
ax[0].imshow(in_img.transpose(1, 0, 2)[:, :, :3] / 255.0)
ax[0].scatter(p[0], p[1])
ax[0].scatter(q[0], q[1])
ax[1].imshow(in_img.transpose(1, 0, 2)[:, :, 3])
ax[1].scatter(p[0], p[1])
ax[1].scatter(q[0], q[1])
plt.show()
def PlotMeshcatPointCloud(meshcat_vis, point_cloud_name, points, colors):
"""A wrapper function to plot meshcat point clouds.
Args:
@param meshcat_vis An instance of a meshcat visualizer.
@param point_cloud_name string. The name of the meshcat point clouds.
@param points An Nx3 numpy array of (x, y, z) points.
@param colors An Nx3 numpy array of (r, g, b) colors corresponding to
points.
"""
meshcat_vis[point_cloud_name].set_object(g.PointCloud(points.T, colors.T))
def _DoPublish(self, context, event):
u_data = self.EvalAbstractInput(context, 0).get_value()
x = self.EvalVectorInput(context, 1).get_value()
w, h, _ = u_data.data.shape
depth_image = u_data.data[:, :, 0]
# Convert depth image to point cloud, with +z being
# camera "forward"
Kinv = np.linalg.inv(
self.camera.depth_camera_info().intrinsic_matrix())
U, V = np.meshgrid(np.arange(h), np.arange(w))
points_in_camera_frame = np.vstack([
U.flatten(),
V.flatten(),
np.ones(w*h)])
points_in_camera_frame = Kinv.dot(points_in_camera_frame) * \
depth_image.flatten()
# The depth camera has some offset from the camera's root frame,
# so take than into account.
pose_mat = self.camera.depth_camera_optical_pose().matrix()
points_in_camera_frame = pose_mat[0:3, 0:3].dot(points_in_camera_frame)
points_in_camera_frame += np.tile(pose_mat[0:3, 3], [w*h, 1]).T
kinsol = self.rbt.doKinematics(x[:self.rbt.get_num_positions()])
points_in_world_frame = self.rbt.transformPoints(
kinsol,
points_in_camera_frame,
self.camera.frame().get_frame_index(),
0)
# Color points according to their normalized height
min_height = 0.6
max_height = 0.9
colors = cm.jet(
(points_in_world_frame[2, :]-min_height)/(max_height-min_height)
).T[0:3, :]
self.vis[self.prefix]["points"].set_object(
g.PointCloud(position=points_in_world_frame,
color=colors,
size=0.005))
def draw_points(vis,
vis_prefix,
name,
points,
normals=None,
colors=None,
size=0.001,
normals_length=0.01):
vis[vis_prefix][name].set_object(
g.PointCloud(position=points, color=colors, size=size))
n_pts = points.shape[1]
if normals is not None:
# Drawing normals for debug
lines = np.zeros([3, n_pts * 2])
inds = np.array(range(0, n_pts * 2, 2))
lines[:, inds] = points[0:3, :]
lines[:, inds+1] = points[0:3, :] + \
normals * normals_length
vis[vis_prefix]["%s_normals" % name].set_object(
meshcat.geometry.LineSegmentsGeometry(lines, None))
def meshcat_visualize(vis, obs, act, info):
"""Visualize data using meshcat."""
for key in sorted(info.keys()):
pose = info[key]
pick_transform = np.eye(4)
pick_transform[0:3, 3] = pose[0]
quaternion_wxyz = np.asarray(
[pose[1][3], pose[1][0], pose[1][1], pose[1][2]])
pick_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3,
0:3]
label = 'obj_' + str(key)
make_frame(vis, label, h=0.05, radius=0.0012, o=1.0)
vis[label].set_transform(pick_transform)
for cam_index in range(len(act['camera_config'])):
verts = unproject_depth_vectorized(
obs['depth'][cam_index], [0, 1],
np.array(act['camera_config'][cam_index]['intrinsics']).reshape(
3, 3), np.zeros(5))
# switch from [N,3] to [3,N]
verts = verts.T
cam_transform = np.eye(4)
cam_transform[0:3, 3] = act['camera_config'][cam_index]['position']
quaternion_xyzw = act['camera_config'][cam_index]['rotation']
quaternion_wxyz = np.asarray([
quaternion_xyzw[3], quaternion_xyzw[0], quaternion_xyzw[1],
quaternion_xyzw[2]
])
cam_transform[0:3, 0:3] = mtf.quaternion_matrix(quaternion_wxyz)[0:3,
0:3]
verts = apply_transform(cam_transform, verts)
colors = obs['color'][cam_index].reshape(-1, 3).T / 255.0
vis['pointclouds/' + str(cam_index)].set_object(
g.PointCloud(position=verts, color=colors))
def DoPublish(self, context, event):
LeafSystem.DoPublish(self, context, event)
point_cloud_P = self.EvalAbstractInput(context, 0).get_value()
# `Q` is a point in the point cloud.
p_PQs = point_cloud_P.xyzs()
# Use only valid points.
valid = np.logical_not(np.isnan(p_PQs))
valid = np.all(valid, axis=0) # Reduce along XYZ axis.
p_PQs = p_PQs[:, valid]
if point_cloud_P.has_rgbs():
rgbs = point_cloud_P.rgbs()[:, valid]
else:
# Need manual broadcasting.
count = p_PQs.shape[1]
rgbs = np.tile(np.array([self._default_rgb]).T, (1, count))
# pydrake `PointCloud.rgbs()` are on [0..255], while meshcat
# `PointCloud` colors are on [0..1].
rgbs = rgbs / 255. # Do not use in-place so we can promote types.
# Send to meshcat.
self._meshcat_viz[self._name].set_object(g.PointCloud(p_PQs, rgbs))
self._meshcat_viz[self._name].set_transform(self._X_WP.matrix())
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["box"].set_object(g.Box([1.0, 0.2, 0.3]))
v["box"].delete()
v["box"].set_object(g.Box([0.1, 0.2, 0.3]))
v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v["transparent_ellipsoid"].set_object(g.Mesh(
g.Ellipsoid([0.3, 0.1, 0.1]),
g.MeshLambertMaterial(color=0xffffff,
opacity=0.5)))
v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["meshes/convex"]
v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj"))))
v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii"))))
v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0]))
v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary"))))
v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0]))
v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae"))))
v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([0, 2, 0]))
verts = np.random.rand(3, 1000000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
v = self.vis["lines"]
v.set_transform(tf.translation_matrix(([-2, -3, 0])))
vertices = np.random.random((3, 10)).astype(np.float32)
v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices)))
v["line"].set_object(g.Line(g.PointsGeometry(vertices)))
v["line"].set_transform(tf.translation_matrix([0, 1, 0]))
v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices)))
v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0]))
v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000)))
v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0]))
colors = vertices # Color each line by treating its xyz coordinates as RGB colors
v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True)))
v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0]))
v["triad"].set_object(g.LineSegments(
g.PointsGeometry(position=np.array([
[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T,
color=np.array([
[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
),
g.LineBasicMaterial(vertexColors=True)))
v["triad"].set_transform(tf.translation_matrix(([0, 5, 0])))
v["triad_function"].set_object(g.triad(0.5))
v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))
def _add_rbt_geometry(self, rbt, rbt_key, draw_collision=False):
# type: (RigidBodyTree, str, bool) -> None
"""
Add the geometry of a RigidBodyTree to the meshcat visualizer.
Note that later update to rbt WOULD NOT be reflected
:param rbt:
:param rbt_key: The key to index the rigid body tree
:param draw_collision:
:return:
"""
n_bodies = rbt.get_num_bodies() - 1
# all_meshcat_geometry = {}
for body_i in range(n_bodies):
# Get the body index
body = rbt.get_body(body_i + 1)
body_name = rbt_key + body.get_name() + ("(%d)" % body_i)
if draw_collision:
draw_elements = [rbt.FindCollisionElement(k) for k in body.get_collision_element_ids()]
else:
draw_elements = body.get_visual_elements()
for element_i, element in enumerate(draw_elements):
element_local_tf = element.getLocalTransform()
if element.hasGeometry():
geom = element.getGeometry()
geom_type = geom.getShape()
if geom_type == Shape.SPHERE:
meshcat_geom = meshcat.geometry.Sphere(geom.radius)
elif geom_type == Shape.BOX:
meshcat_geom = meshcat.geometry.Box(geom.size)
elif geom_type == Shape.CYLINDER:
meshcat_geom = meshcat.geometry.Cylinder(
geom.length, geom.radius)
# In Drake, cylinders are along +z
# In meshcat, cylinders are along +y
# Rotate to fix this misalignment
extra_rotation = tf.rotation_matrix(
math.pi / 2., [1, 0, 0])
element_local_tf[0:3, 0:3] = \
element_local_tf[0:3, 0:3].dot(
extra_rotation[0:3, 0:3])
elif geom_type == Shape.MESH:
meshcat_geom = \
meshcat.geometry.ObjMeshGeometry.from_file(
geom.resolved_filename[0:-3] + "obj")
# respect mesh scale
element_local_tf[0:3, 0:3] *= geom.scale
elif geom_type == Shape.MESH_POINTS:
# The shape of points is (3, N)
points = geom.getPoints() # type: np.ndarray
n_points = points.shape[1]
color = MeshCatVisualizer._make_meshcat_color(n_points, r=255, g=0, b=0)
meshcat_geom = meshcat_geometry.PointCloud(points, color.T, size=0.01)
self._meshcat_vis[self._prefix][body_name][str(element_i)] \
.set_object(meshcat_geom)
self._meshcat_vis[self._prefix][body_name][str(element_i)]. \
set_transform(element_local_tf)
self._rbt_geometry_element_set.add(body_name)
continue
else:
print "UNSUPPORTED GEOMETRY TYPE ", \
geom.getShape(), " IGNORED"
continue
# The color of the body
rgba = [1., 0.7, 0., 1.]
if not draw_collision:
rgba = element.getMaterial()
self._meshcat_vis[self._prefix][body_name][str(element_i)] \
.set_object(meshcat_geom,
meshcat.geometry.MeshLambertMaterial(
color=MeshCatVisualizer.rgba2hex(rgba)))
self._meshcat_vis[self._prefix][body_name][str(element_i)]. \
set_transform(element_local_tf)
self._rbt_geometry_element_set.add(body_name)
