Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
def draw_plane_in_meshcat(vis, point, normal, size, name):
size = np.array(size)
vis["perception"]["tabletopsegmenter"][name].set_object(
meshcat_g.Box(size))
box_tf = get_tf_from_point_normal(point, normal)
vis["perception"]["tabletopsegmenter"][name].set_transform(box_tf)
from __future__ import absolute_import, division, print_function
import math
import time
import meshcat
vis = meshcat.Visualizer().open()
box = meshcat.geometry.Box([0.5, 0.5, 0.5])
vis.set_object(box)
draw_times = []
for i in range(200):
theta = (i + 1) / 100 * 2 * math.pi
now = time.time()
vis.set_transform(meshcat.transformations.rotation_matrix(theta, [0, 0, 1]))
draw_times.append(time.time() - now)
time.sleep(0.01)
print(sum(draw_times) / len(draw_times))
# that matches the LCM geometry.
# For MESH geometry, this function checks if a texture file exists next
# to the mesh file, and uses that to provide the material information if
# 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]))