Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def add_geometry(self, geometry, pathname, transform):
vis = self.vis
material = g.MeshBasicMaterial(
reflectivity=self.reflectivity, sides=0, wireframe=self.wireframe
)
material.transparency = self.transparency
material.opacity = self.opacity
if isinstance(geometry, Sphere):
sphere = geometry
vis[pathname].set_object(g.Sphere(sphere.radius), material)
vis[pathname].set_transform(transform)
elif isinstance(geometry, Cylinder):
cyl = geometry
vis[pathname].set_object(g.Cylinder(cyl.length, cyl.radius), material)
# meshcat cylinder is aligned along y-axis. Align along z then apply the
# node's transform as normal.
transform = np.copy(transform)
# Change basic XYZ -> XZY
transform[:, [1, 2]] = transform[:, [2, 1]]
vis[pathname].set_transform(transform)
elif isinstance(geometry, Mesh):
obj = meshcat.geometry.StlMeshGeometry.from_stream(
io.BytesIO(trimesh.exchange.stl.export_stl(geometry.trimesh))
)
vis[pathname].set_object(obj, material)
vis[pathname].set_transform(transform)
else:
raise NotImplementedError(
"Cannot yet add {} to visualiser".format(type(geometry))
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
"""
delta_xyz = np.array([[length / 2, 0, 0],
[0, length / 2, 0],
[0, 0, length / 2]])
axes_name = ['x', 'y', 'z']
colors = [0xff0000, 0x00ff00, 0x0000ff]
rotation_axes = [[0, 0, 1], [0, 1, 0], [1, 0, 0]]
for i in range(3):
material = meshcat.geometry.MeshLambertMaterial(
color=colors[i], opacity=opacity)
vis[prefix][name][axes_name[i]].set_object(
meshcat.geometry.Cylinder(length, radius), material)
X = meshcat.transformations.rotation_matrix(
np.pi/2, rotation_axes[i])
X[0:3, 3] = delta_xyz[i]
vis[prefix][name][axes_name[i]].set_transform(X)
# both fields are returned as None.
meshcat_geom = None
material = None
element_local_tf = RigidTransform(
RotationMatrix(Quaternion(geom.quaternion)),
geom.position).GetAsMatrix4()
if geom.type == geom.BOX:
assert geom.num_float_data == 3
meshcat_geom = meshcat.geometry.Box(geom.float_data)
elif geom.type == geom.SPHERE:
assert geom.num_float_data == 1
meshcat_geom = meshcat.geometry.Sphere(geom.float_data[0])
elif geom.type == geom.CYLINDER:
assert geom.num_float_data == 2
meshcat_geom = meshcat.geometry.Cylinder(
geom.float_data[1],
geom.float_data[0])
# 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 == geom.MESH:
meshcat_geom = meshcat.geometry.ObjMeshGeometry.from_file(
geom.string_data[0:-3] + "obj")
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
X_WB_key = (int(bodyB.model_instance()), bodyB.name())
X_WB = X_WB_map[X_WB_key]
p_BC = X_WB.inverse().multiply(contact_info_i.contact_point())
new_contact = self._ContactState(
key=str(self._contact_key_counter), needs_pruning=False,
info=contact_info_i, p_BC=p_BC)
contact = self._find_duplicate_contact(
new_contact, dt=t - self._t_previous)
if contact is None:
# contact is new
self._contacts.append(new_contact)
self._contact_key_counter += 1
# create cylinders with small radius.
vis[prefix]["contact_forces"][new_contact.key].set_object(
meshcat.geometry.Cylinder(
height=1. / self._force_cylinder_longitudinal_scale,
radius=0.01 / self._force_cylinder_radial_scale),
meshcat.geometry.MeshLambertMaterial(color=0xff0000))
else:
# contact is not new, but it's valid.
contact.needs_pruning = False
# Prune old contact forces
for contact in list(self._contacts):
if contact.needs_pruning:
self._contacts.remove(contact)
vis[prefix]["contact_forces"][contact.key].delete()
# visualize all valid contact forces
for contact in self._contacts:
# Compute pose of contact cylinder `C` in world frame `W`.