Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
# 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
R[:, 1] = y
R[:, 0] = [0, -y[2], y[1]]
R[:, 2] = np.cross(R[:, 0], y)
X_WC = np.eye(4)
X_WC[0:3, 0:3] = R
X_WC[0:3, 3] = contact.info.contact_point()
# Scale cylinder
visual_magnitude = self._get_visual_magnitude(magnitude)
T_scale = tf.translation_matrix(
[0, visual_magnitude / 2, 0])
T_scale[1, 1] = \
visual_magnitude * self._force_cylinder_longitudinal_scale
# - "expand" cylinders to a visible size.
T_scale[0, 0] *= self._force_cylinder_radial_scale
T_scale[2, 2] *= self._force_cylinder_radial_scale
# Publish.
vis[prefix]["contact_forces"][contact.key].set_transform(
X_WC.dot(T_scale))
# update time
self._t_previous = t
+ np.array(start_ray.direction) * short_length
)
colour = wavelength_to_hex_int(nanometers)
ids.append(self.add_line_segment(start, end, colour=colour))
if baubles:
event = start_part[1]
if event in {Event.TRANSMIT}:
baubid = self.get_next_identifer()
vis[f"exit/{baubid}"].set_object(
g.Sphere(bauble_radius),
g.MeshBasicMaterial(
color=colour, transparency=False, opacity=1
),
)
vis[f"exit/{baubid}"].set_transform(tf.translation_matrix(start))
ids.append(baubid)
return ids