Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
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`.
R = np.zeros((3, 3))
magnitude = np.linalg.norm(contact.info.contact_force())
y = contact.info.contact_force() / magnitude
# Handle scaling.
# TODO(gizatt): See meshcat-python#40 for incorporating scale as a
# field rather than a matrix multiplication.
scale = geom.float_data[:3]
element_local_tf[:3, :3] = element_local_tf[:3, :3].dot(np.diag(scale))
# Attempt to find a texture for the object by looking for an
# identically-named *.png next to the model.
# TODO(gizatt): Support .MTLs and prefer them over png, since they're
# both more expressive and more standard.
# TODO(gizatt): In the long term, this kind of material information
# should be gleaned from the SceneGraph constituents themselves, so
# that we visualize what the simulation is *actually* reasoning about
# rather than what files happen to be present.
candidate_texture_path_png = geom.string_data[0:-3] + "png"
if os.path.exists(candidate_texture_path_png):
material = meshcat.geometry.MeshLambertMaterial(
map=meshcat.geometry.ImageTexture(
image=meshcat.geometry.PngImage.from_file(
candidate_texture_path_png)))
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
else:
print("UNSUPPORTED GEOMETRY TYPE {} IGNORED".format(
geom.type))
return meshcat_geom, material
if material is None:
def rgb_2_hex(rgb):
# Turn a list of R,G,B elements (any indexable list
# of >= 3 elements will work), where each element is
# specified on range [0., 1.], into the equivalent
# 24-bit value 0xRRGGBB.
val = 0
for i in range(3):
val += (256**(2 - i)) * int(255 * rgb[i])
return val
material = meshcat.geometry.MeshLambertMaterial(
color=rgb_2_hex(geom.color[:3]),
transparent=geom.color[3] != 1.,
opacity=geom.color[3])
return meshcat_geom, material, element_local_tf
prefix: (string) name of the node in the meshcat tree to which this
triad is added.
length: the length of each axis in meters.
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)