def AddMeshcatTriad(meshcat,
path,
length=.25,
radius=0.01,
opacity=1.,
X_PT=RigidTransform()):
meshcat.SetTransform(path, X_PT)
# x-axis
X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2., 0, 0])
meshcat.SetTransform(path + "/x-axis", X_TG)
meshcat.SetObject(path + "/x-axis", Cylinder(radius, length),
Rgba(1, 0, 0, opacity))
# y-axis
X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2., 0])
meshcat.SetTransform(path + "/y-axis", X_TG)
meshcat.SetObject(path + "/y-axis", Cylinder(radius, length),
Rgba(0, 1, 0, opacity))
# z-axis
X_TG = RigidTransform([0, 0, length / 2.])
meshcat.SetTransform(path + "/z-axis", X_TG)
meshcat.SetObject(path + "/z-axis", Cylinder(radius, length),
Rgba(0, 0, 1, opacity))
def test_contact_visualizer(self, T):
meshcat = Meshcat()
params = ContactVisualizerParams()
params.publish_period = 0.123
params.default_color = Rgba(0.5, 0.5, 0.5)
params.prefix = "py_visualizer"
params.delete_on_initialization_event = False
params.force_threshold = 0.2
params.newtons_per_meter = 5
params.radius = 0.1
self.assertNotIn("object at 0x", repr(params))
params2 = ContactVisualizerParams(publish_period=0.4)
self.assertEqual(params2.publish_period, 0.4)
vis = ContactVisualizer_[T](meshcat=meshcat, params=params)
vis.Delete()
vis.contact_results_input_port()
builder = DiagramBuilder_[T]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.01)
plant.Finalize()
ContactVisualizer_[T].AddToBuilder(builder=builder,
plant=plant,
meshcat=meshcat,
params=params)
ContactVisualizer_[T].AddToBuilder(
builder=builder,
contact_results_port=plant.get_contact_results_output_port(),
meshcat=meshcat,
params=params)
def plot_surface(meshcat,
path,
X,
Y,
Z,
rgba=Rgba(.87, .6, .6, 1.0),
wireframe=False,
wireframe_line_width=1.0):
(rows, cols) = Z.shape
assert (np.array_equal(X.shape, Y.shape))
assert (np.array_equal(X.shape, Z.shape))
vertices = np.empty((rows * cols, 3), dtype=np.float32)
vertices[:, 0] = X.reshape((-1))
vertices[:, 1] = Y.reshape((-1))
vertices[:, 2] = Z.reshape((-1))
# Vectorized faces code from https://stackoverflow.com/questions/44934631/making-grid-triangular-mesh-quickly-with-numpy # noqa
faces = np.empty((rows - 1, cols - 1, 2, 3), dtype=np.uint32)
r = np.arange(rows * cols).reshape(rows, cols)
faces[:, :, 0, 0] = r[:-1, :-1]
faces[:, :, 1, 0] = r[:-1, 1:]
faces[:, :, 0, 1] = r[:-1, 1:]
faces[:, :, 1, 1] = r[1:, 1:]
faces[:, :, :, 2] = r[1:, :-1, None]
faces.shape = (-1, 3)
# TODO(Russ): support per vertex / Colormap colors.
meshcat.SetTriangleMesh(path, vertices.T, faces.T, rgba, wireframe,
wireframe_line_width)
def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH:
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)
def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH and geom.string_data:
# A mesh to be loaded from a file.
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.MESH:
assert not geom.string_data
# A mesh with the data inline, i.e.,
# V | T | v0 | v1 | ... vN | t0 | t1 | ... | tM
# where
# V: The number of vertices.
# T: The number of triangles.
# N: 3V, the number of floating point values for the V vertices.
# M: 3T, the number of vertex indices for the T triangles.
_logger.warning("Meldis cannot yet display hydroelastic collision "
"meshes; that geometry will be ignored.")
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)
# q - quaternion message
return RigidTransform(Quaternion(wxyz=[q.w, q.x, q.y, q.z]),
[p.x, p.y, p.z])
def from_ros_pose(pose):
"""Converts ROS pose to Drake transform."""
return _read_pose_msg(p=pose.position, q=pose.orientation)
def from_ros_transform(tr):
"""Converts ROS transform to Drake transform."""
return _read_pose_msg(p=tr.translation, q=tr.rotation)
DEFAULT_RGBA = Rgba(0.9, 0.9, 0.9, 1.0)
def to_ros_markers(shape, stamp, frame_name, X_FG, color_rgba):
assert isinstance(shape, Shape), shape
assert isinstance(frame_name, str), frame_name
assert isinstance(X_FG, RigidTransform), X_FG
marker = Marker()
marker.header.stamp = stamp
marker.header.frame_id = frame_name
marker.pose = to_ros_pose(X_FG)
marker.action = Marker.ADD
marker.lifetime = Duration(0.)
marker.frame_locked = True
def use_color():
def plot_mathematical_program(meshcat,
path,
prog,
X,
Y,
result=None,
point_size=0.05):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = f"{path}/constraints"
for binding in prog.GetAllConstraints():
if isinstance(binding.evaluator(), BoundingBoxConstraint):
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
v = f"{cv}/{type(c).__name__}"
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(meshcat,
v,
X,
Y,
Zc.reshape(X.shape),
rgba=Rgba(1.0, .2, .2, 1.0),
wireframe=True)
else:
Zc = prog.EvalBindingVectorized(binding, values)
evaluator = binding.evaluator()
low = evaluator.lower_bound()
up = evaluator.upper_bound()
cvb = f"{cv}/{type(evaluator).__name__}"
for index in range(Zc.shape[0]):
# TODO(russt): Plot infeasible points in a different color.
infeasible = np.logical_or(Zc[index, :] < low[index],
Zc[index, :] > up[index])
plot_surface(meshcat,
f"{cvb}/{index}",
X,
Y,
Zc[index, :].reshape(X.shape),
rgba=Rgba(1.0, .3, 1.0, 1.0),
wireframe=True)
if costs:
plot_surface(meshcat,
f"{path}/objective",
X,
Y,
Z.reshape(X.shape),
rgba=Rgba(.3, 1.0, .3, 1.0),
wireframe=True)
if result:
v = f"{path}/solution"
meshcat.SetObject(v, Sphere(point_size), Rgba(.3, 1.0, .3, 1.0))
x_solution = result.get_x_val()
meshcat.SetTransform(
v,
RigidTransform(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
def _build_body_patches(self, use_random_colors,
substitute_collocated_mesh_files, inspector):
"""
Generates body patches. self._patch_Blist stores a list of patches for
each body (starting at body id 1). A body patch is a list of all 3D
vertices of a piece of visual geometry.
"""
self._patch_Blist = {}
self._patch_Blist_colors = {}
for frame_id in inspector.GetAllFrameIds():
count = inspector.NumGeometriesForFrameWithRole(
frame_id, Role.kIllustration)
if count == 0:
continue
frame_name = self.frame_name(frame_id, inspector)
this_body_patches = []
this_body_colors = []
for g_id in inspector.GetGeometries(frame_id, Role.kIllustration):
X_BG = inspector.GetPoseInFrame(g_id)
shape = inspector.GetShape(g_id)
if isinstance(shape, Box):
# Draw a bounding box.
patch_G = np.vstack(
(shape.width() /
2. * np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
shape.depth() /
2. * np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
shape.height() / 2. *
np.array([-1, -1, -1, -1, 1, 1, 1, 1])))
elif isinstance(shape, Sphere):
# Sphere is the only shape that allows a zero-measure. A
# zero-radius sphere is a point, and we skip it.
if shape.radius() == 0:
continue
lati, longi = np.meshgrid(np.arange(0., 2. * math.pi, 0.5),
np.arange(0., 2. * math.pi, 0.5))
lati = lati.ravel()
longi = longi.ravel()
patch_G = np.vstack([
np.sin(lati) * np.cos(longi),
np.sin(lati) * np.sin(longi),
np.cos(lati)
])
patch_G *= shape.radius()
elif isinstance(shape, Cylinder):
radius = shape.radius()
length = shape.length()
# In the lcm geometry, cylinders are along +z
# https://github.com/RobotLocomotion/drake/blob/last_sha_with_original_matlab/drake/matlab/systems/plants/RigidBodyCylinder.m
# Two circles: one at bottom, one at top.
sample_pts = np.arange(0., 2. * math.pi, 0.25)
patch_G = np.hstack([
np.array([[
radius * math.cos(pt), radius * math.sin(pt),
-length / 2.
],
[
radius * math.cos(pt),
radius * math.sin(pt), length / 2.
]]).T for pt in sample_pts
])
elif isinstance(shape, (Mesh, Convex)):
filename = shape.filename()
base, ext = os.path.splitext(filename)
if (ext.lower() != ".obj"
and substitute_collocated_mesh_files):
# Check for a co-located .obj file (case insensitive).
for f in glob.glob(base + '.*'):
if f[-4:].lower() == '.obj':
filename = f
break
if filename[-4:].lower() != '.obj':
raise RuntimeError(
f"The given file {filename} is not "
f"supported and no alternate {base}"
".obj could be found.")
if not os.path.exists(filename):
raise FileNotFoundError(errno.ENOENT,
os.strerror(errno.ENOENT),
filename)
# Get mesh scaling.
scale = shape.scale()
mesh = ReadObjToTriangleSurfaceMesh(filename, scale)
patch_G = np.vstack(mesh.vertices())
# Only store the vertices of the (3D) convex hull of the
# mesh, as any interior vertices will still be interior
# vertices after projection, and will therefore be removed
# in _update_body_fill_verts().
vpoly = optimization.VPolytope(patch_G.T)
patch_G = vpoly.GetMinimalRepresentation().vertices()
elif isinstance(shape, HalfSpace):
# For a half space, we'll simply create a large box with
# the top face at z = 0, the bottom face at z = -1 and the
# far corners at +/- 50 in the x- and y- directions.
x = 50
y = 50
z = -1
patch_G = np.vstack(
(x * np.array([-1, -1, 1, 1, -1, -1, 1, 1]),
y * np.array([-1, 1, -1, 1, -1, 1, -1, 1]),
z * np.array([-1, -1, -1, -1, 0, 0, 0, 0])))
# TODO(SeanCurtis-TRI): Provide support for capsule and
# ellipsoid.
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
type(shape)))
continue
# Compute pose in body.
patch_B = X_BG @ patch_G
# Close path if not closed.
if (patch_B[:, -1] != patch_B[:, 0]).any():
patch_B = np.hstack((patch_B, patch_B[:, 0][np.newaxis].T))
this_body_patches.append(patch_B)
if not use_random_colors:
# If we need to use random colors, we apply them after the
# fact. See below.
props = inspector.GetIllustrationProperties(g_id)
assert props is not None
rgba = props.GetPropertyOrDefault("phong", "diffuse",
Rgba(0.9, 0.9, 0.9, 1.0))
color = np.array((rgba.r(), rgba.g(), rgba.b(), rgba.a()))
this_body_colors.append(color)
self._patch_Blist[frame_name] = this_body_patches
self._patch_Blist_colors[frame_name] = this_body_colors
# Spawn a random color generator. Each body will be given a unique
# color when using this random generator, with each visual element of
# the body colored the same.
if use_random_colors:
color = iter(
plt_cm.rainbow(np.linspace(0, 1,
len(self._patch_Blist_colors))))
for name in self._patch_Blist_colors.keys():
this_color = next(color)
patch_count = len(self._patch_Blist[name])
self._patch_Blist_colors[name] = [this_color] * patch_count