Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
# present. For BOX, SPHERE, CYLINDER geometry as well as MESH geometry
# not satisfying the above, this function uses the geom.color field to
# create a flat color for the object. In the case of other geometry types,
# 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")
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))
)
end = end_ray.position
if world_segment == "short":
if end_ray == history[-1][0]:
end = (
np.array(start_ray.position)
+ 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