def visualize(self, states, dt):
import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import time
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
vis["/Cameras/default"].set_transform(
tf.translation_matrix([0, 0, 0]).dot(
tf.euler_matrix(0, np.radians(-30), -np.pi / 2)))
vis["/Cameras/default/rotated/<object>"].set_transform(
tf.translation_matrix([1, 0, 0]))
vis["Quadrotor"].set_object(
g.StlMeshGeometry.from_file('systems/crazyflie2.stl'))
while True:
for state in states:
vis["Quadrotor"].set_transform(
tf.translation_matrix([state[0], state[1], state[2]]).dot(
tf.quaternion_matrix(state[6:10])))
time.sleep(dt)
def AlignSceneToModel(scene_points, model_points, max_distance=0.05,
num_iters=10, num_sample_points=250):
"""
Finds a good (x, y, sin(theta), cos(theta) transform between scene_points
and model_points.
Args:
@param scene_points An Nx3 numpy array of scene points.
@param model_points An Nx3 numpy array of model points.
@param max_distance float. The maximum distance in meters to consider of
nearest neighbors between scene_points and model_points.
@param num_iters int. The number of times to try the best fit optimization
from different initial guesses.
@param num_sample_points: int. The number of points to sample from the scene
to limit the time the algorithm takes to run.
Returns:
@return best_X_MS The best 4x4 homogenous transform between scene_points and
model_points.
@return best_cost float. The cost function evaluated at best_X_MS.
"""
scene_sample = scene_points[np.random.choice(scene_points.shape[0],
num_sample_points,
replace=False),
:]
homogenous_scene = np.ones((4, scene_sample.shape[0]))
homogenous_model = np.ones((4, model_points.shape[0]))
homogenous_scene[:3, :] = scene_sample.T
homogenous_model[:3, :] = model_points.T
centroid_scene = np.mean(homogenous_scene, axis=1)
centroid_model = np.mean(homogenous_model, axis=1)
best_X_MS = None
best_cost = float('inf')
for i in range(num_iters):
init_theta = np.random.uniform(0, 2*np.pi)
init_guess = reduce(np.dot,
[tf.translation_matrix((centroid_model[0],
centroid_model[1],
0.)),
tf.rotation_matrix(init_theta, (0, 0, 1)),
tf.translation_matrix((-centroid_scene[0],
-centroid_scene[1],
0.))])
X_MS, cost = FindBestFitTransform(
homogenous_scene,homogenous_model, init_guess, max_distance)
if cost < best_cost:
best_cost = cost
best_X_MS = X_MS
return best_X_MS, best_cost
def show(self, chain, showMeshes=False):
if 'google.colab' in sys.modules:
server_args = ['--ngrok_http_tunnel']
# Start a single meshcat server instance to use for the remainder of this notebook.
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
proc, zmq_url, web_url = start_zmq_server_as_subprocess(
server_args=server_args)
vis = meshcat.Visualizer(zmq_url=zmq_url)
else:
vis = meshcat.Visualizer().open()
if showMeshes:
for i, link in enumerate(chain.linkArray):
if link.meshObj == None:
print("No mesh: " + link.name)
continue
boxVis = vis["link:" + link.name]
boxVis.set_object(
link.meshObj,
g.MeshLambertMaterial(color=0xffffff, reflectivity=0.8))
rotationMatrix = np.pad(link.absoluteOrientation, [(0, 1),
(0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase) @ rotationMatrix)
else:
for i, link in enumerate(chain.linkArray):
boxVis = vis["link:" + link.name]
if link.primitiveObj != None:
if isinstance(link.primitiveObj, primitives.Box):
box = meshcat.geometry.Box(link.primitiveObj.size)
boxVis.set_object(box)
if isinstance(link.primitiveObj, primitives.Cylinder):
cylinder = meshcat.geometry.Cylinder(
link.primitiveObj.length, link.primitiveObj.radius)
boxVis.set_object(cylinder)
if isinstance(link.primitiveObj, primitives.Sphere):
sphere = meshcat.geometry.Sphere(
link.primitiveObj.radius)
boxVis.set_object(cylinder)
rotationMatrix = np.pad(link.absoluteOrientation, [(0, 1),
(0, 1)],
mode='constant')
rotationMatrix[-1][-1] = 1
boxVis.set_transform(
tf.translation_matrix(link.absoluteBase)
@ rotationMatrix)
boxVis = vis["skeleton"]
boxVis.set_object(
g.Line(g.PointsGeometry(chain.get_vertex_coords().T)))
def runTest(self):
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
animation = meshcat.animation.Animation()
with animation.at_frame(v, 0) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([0, 0, 0]))
with animation.at_frame(v, 30) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1])))
v.set_animation(animation)
def runTest(self):
""" Applications using meshcat may already have meshes loaded in memory. It is
more efficient to load these meshes with streams rather than going to and then
from a file on disk. To test this we are importing meshes from disk and
converting them into streams so it kind of defeats the intended purpose! But at
least it tests the functionality.
"""
self.vis.delete()
v = self.vis["meshes/convex"]
# Obj file
filename = os.path.join(meshcat.viewer_assets_path(),
"../tests/data/mesh_0_convex_piece_0.obj")
with open(filename, "r") as f:
fio = StringIO(f.read())
v["stream_obj"].set_object(
g.Mesh(g.ObjMeshGeometry.from_stream(fio)))
v["stream_stl_ascii"].set_transform(
tf.translation_matrix([0, 0.0, 0]))
# STL ASCII
filename = os.path.join(
meshcat.viewer_assets_path(),
"../tests/data/mesh_0_convex_piece_0.stl_ascii")
with open(filename, "r") as f:
fio = StringIO(f.read())
v["stream_stl_ascii"].set_object(
g.Mesh(g.StlMeshGeometry.from_stream(fio)))
v["stream_stl_ascii"].set_transform(
tf.translation_matrix([0, -0.5, 0]))
# STL Binary
filename = os.path.join(
meshcat.viewer_assets_path(),
"../tests/data/mesh_0_convex_piece_0.stl_binary")
with open(filename, "rb") as f:
fio = BytesIO(f.read())
v["stream_stl_binary"].set_object(
g.Mesh(g.StlMeshGeometry.from_stream(fio)))
v["stream_stl_binary"].set_transform(
tf.translation_matrix([0, -1.0, 0]))
# DAE
filename = os.path.join(meshcat.viewer_assets_path(),
"../tests/data/mesh_0_convex_piece_0.dae")
with open(filename, "r") as f:
fio = StringIO(f.read())
v["stream_dae"].set_object(
g.Mesh(g.DaeMeshGeometry.from_stream(fio)))
v["stream_dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
v["cube"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([-1, 0, 0]))
verts = np.random.rand(3, 100000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
def runTest(self):
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
animation = meshcat.animation.Animation()
with animation.at_frame(v, 0) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([0, 0, 0]))
with animation.at_frame(v, 30) as frame_vis:
frame_vis.set_transform(tf.translation_matrix([2, 0, 0]).dot(tf.rotation_matrix(np.pi/2, [0, 0, 1])))
with animation.at_frame(v, 0) as frame_vis:
frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 1)
with animation.at_frame(v, 30) as frame_vis:
frame_vis["/Cameras/default/rotated/<object>"].set_property("zoom", "number", 0.5)
v.set_animation(animation)
def draw_mblock(self):
self.mblock = self.vis["mblock"]
# create and draw the mblock
dim = [3, 1, 1]
self.mblock.set_object(g.Box(dim))
temp = tf.translation_matrix([0, 1, 0.5])
self.mblock.set_transform(temp)
def setCameraTransform(self,
translation=np.zeros(3),
rotation=np.zeros(3),
relative=False):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
H_abs = SE3(R_pnc, np.array(translation))
if relative:
H_orig = XYZQUATToSe3(
self._client.getCameraTransform(self._window_id))
H_abs = H_abs * H_orig
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(H_abs).tolist())
else:
if relative:
raise RuntimeError(
"'relative'=True not available with meshcat.")
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def visualize(self, states, dt):
import meshcat
import meshcat.geometry as g
import meshcat.transformations as tf
import time
# import meshcat.animation as anim
# Create a new visualizer
vis = meshcat.Visualizer()
vis.open()
vis["cart"].set_object(g.Box([0.2, 0.5, 0.2]))
vis["pole"].set_object(g.Cylinder(self.length_pole, 0.01))
# animation = anim.Animation()
# for i, state in enumerate(states):
# with animation.at_frame(vis, i*10) as frame:
# print(frame)
# frame["cart"].set_transform(tf.translation_matrix([0, state[0], 0]))
# frame["pole"].set_transform(
# tf.translation_matrix([0, state[0] + self.length_pole/2, 0]).dot(
# tf.rotation_matrix(np.pi/2 + state[1], [1,0,0], [0,-self.length_pole/2,0])))
# vis.set_animation(animation, play=True, repeat=10)
# time.sleep(10)
# # anim.convert_frame_to_video()
while True:
# vis["cart"].set_transform(tf.translation_matrix([0, 0, 0]))
# vis["pole"].set_transform(
# tf.translation_matrix([0, 0 + self.length_pole/2, 0]).dot(
# tf.rotation_matrix(np.pi/2 + 0, [1,0,0], [0,-self.length_pole/2,0])))
# time.sleep(dt)
for state in states:
vis["cart"].set_transform(
tf.translation_matrix([0, state[0], 0]))
vis["pole"].set_transform(
tf.translation_matrix(
[0, state[0] + self.length_pole / 2, 0]).dot(
tf.rotation_matrix(np.pi / 2 + state[1], [1, 0, 0],
[0, -self.length_pole / 2, 0])))
time.sleep(dt)
def PlotTrajectoryMeshcat(x, t, vis, wpts_list = None):
# initialize
vis.delete()
# plot waypoints
if not(wpts_list is None):
for i, wpts in enumerate(wpts_list):
vis["wpt_%d" % i].set_object(geometry.Sphere(0.03),
geometry.MeshLambertMaterial(color=0xffff00))
T_wp = tf.translation_matrix(wpts)
vis["wpt_%d" % i].set_transform(T_wp)
d_prop = 0.10 # propeller diameter
vis["quad"]["CG"].set_object(geometry.Sphere(0.03),
geometry.MeshLambertMaterial(color=0x00ffff))
vis["quad"]["body"].set_object(geometry.Box([0.2, 0.1, 0.1]),
geometry.MeshLambertMaterial(color=0x404040))
vis["quad"]["prop0"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0x00ff00))
vis["quad"]["prop1"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xff0000))
vis["quad"]["prop2"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xffffff))
vis["quad"]["prop3"].set_object(geometry.Cylinder(0.01, d_prop),
geometry.MeshLambertMaterial(color=0xffffff))
Rx_prop = CalcRx(np.pi/2)
TB = tf.translation_matrix([0,0,-0.05])
T0 = tf.translation_matrix([l, -l, 0])
T1 = tf.translation_matrix([l, l, 0])
T2 = tf.translation_matrix([-l, l, 0])
T3 = tf.translation_matrix([-l, -l, 0])
T0[0:3,0:3] = Rx_prop
T1[0:3,0:3] = Rx_prop
T2[0:3,0:3] = Rx_prop
T3[0:3,0:3] = Rx_prop
vis["quad"]["body"].set_transform(TB)
vis["quad"]["prop0"].set_transform(T0)
vis["quad"]["prop1"].set_transform(T1)
vis["quad"]["prop2"].set_transform(T2)
vis["quad"]["prop3"].set_transform(T3)
# visualize trajectory
time.sleep(1.0)
N = len(x)
if not (t is None):
assert N == len(t)
for i, xi in enumerate(x):
xyz = xi[0:3]
rpy = xi[3:6]
R_WB = CalcR_WB(rpy)
T = tf.translation_matrix(xyz)
T[0:3,0:3] = R_WB
vis["quad"].set_transform(T)
if i < N-1 and not(t is None):
dt = t[i+1] - t[i]
time.sleep(dt)
def draw_bond(v, label, p1, d, radius, color=0xffffff):
H = np.linalg.norm(d)
R = np.linalg.norm(d[:2])
e = d / H
x = np.array([0, 1, 0])
rot = -acos(e @ x)
if -1 < e @ x < 1:
axis = np.cross(e, x)
v[label].set_object(
g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color)))
v[label].set_transform(
tf.translation_matrix(p1).dot(
tf.rotation_matrix(rot, axis).dot(
tf.translation_matrix([0, H / 2, 0]))))
else:
v[label].set_object(
g.Mesh(g.Cylinder(H, radius), g.MeshLambertMaterial(color=color)))
v[label].set_transform(tf.translation_matrix(p1 + d / 2))
def viewer_draw_sphere(viewer, sphere, color=None, id=None):
import meshcat.transformations as tf
if color == None:
color = 0x777777
if id == None:
id = str(uuid.uuid1())
s = mcg.Sphere(sphere.radius)
viewer[id].set_object(s), mcg.MeshLambertMaterial(color=color)
viewer[id].set_transform(tf.translation_matrix(list(sphere.point)))
return id
def runTest(self):
"""
Test that we can set_object with a PerspectiveCamera.
"""
self.vis.set_object(g.Box([0.5, 0.5, 0.5]))
camera = g.PerspectiveCamera(fov=90)
self.vis['/Cameras/default/rotated'].set_object(camera)
self.vis['/Cameras/default'].set_transform(
tf.translation_matrix([1, -1, 0.5]))
self.vis['/Cameras/default/rotated/<object>'].set_property(
"position", [0, 0, 0])
def meshcat_draw_lights(vis, light_locations, light_attenuations):
N = light_locations.shape[1]
colors = np.zeros((3, N))
colors[2, :] = light_attenuations
for k in range(N):
vis["lights"]["%d" % k].set_object(
g.Sphere(radius=0.05),
g.MeshLambertMaterial(
color=0xffffff,
reflectivity=0.8))
vis["lights"]["%d" % k].set_transform(
tf.translation_matrix(light_locations[:, k]))
def runTest(self):
"""
Test that we can set_object with an OrthographicCamera.
"""
self.vis.set_object(g.Box([0.5, 0.5, 0.5]))
camera = g.OrthographicCamera(
left=-1, right=1, bottom=-1, top=1, near=-1000, far=1000)
self.vis['/Cameras/default/rotated'].set_object(camera)
self.vis['/Cameras/default'].set_transform(
tf.translation_matrix([0, -1, 0]))
self.vis['/Cameras/default/rotated/<object>'].set_property(
"position", [0, 0, 0])
self.vis['/Grid'].set_property("visible", False)
def draw_transformation(self, state):
state = list(state)
origin = state[0:3]
origin[0] = state[1]
origin[1] = state[0]
origin[2] = state[2] + self.cube_dim[2] / 2.0
theta = state[4]
wheel_angle = state[6]
temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y
temp[0:3,
-1] = tf.translation_from_matrix(tf.translation_matrix(origin))
self.cube.set_transform(temp)
self.wheel.set_transform(tf.rotation_matrix(
-wheel_angle, [1, 0, 0])) # rotate the pole
def draw_transformation(self, state, dim=2.0):
nd = len(state)
state = list(state)
origin = state[0:3] # so you can edit
origin[0] = 0.0
origin[1] = state[0]
origin[2] = state[1] + self.cube_dim[2] / 2.0
theta = state[dim]
wheel_angle = state[len(state) / 2 - 1]
temp = tf.rotation_matrix(theta, [1, 0, 0]) # assume rotate about y
temp[0:3,
-1] = tf.translation_from_matrix(tf.translation_matrix(origin))
self.cube.set_transform(temp)
self.wheel.set_transform(tf.rotation_matrix(
wheel_angle, [1, 0, 0])) # rotate the pole
def getCylinderTransform(p1, p2):
cylinder_up_vector = [0, 1, 0]
cylinder_direction = p2 - p1
if np.linalg.norm(cylinder_direction) == 0:
return np.zeros((4, 4))
rotation_matrix = rotationTransform(
cylinder_up_vector,
cylinder_direction / np.linalg.norm(cylinder_direction))
scale_matrix = np.eye(4)
scale_matrix[1, 1] = np.linalg.norm(cylinder_direction)
full_transform = tf.translation_matrix(p1 + 0.5 * cylinder_direction).dot(
rotation_matrix.dot(scale_matrix))
return np.array(full_transform)
def __init__(self):
self.vis = meshcat.Visualizer()
self.cube = self.vis["cube"]
self.pivot = self.cube["pivot"]
self.wheel = self.pivot["wheel"]
# create and draw the cube
self.cube_dim = [1.0, 1.0, 1.0] # x,y,z
self.cube.set_object(g.Box(self.cube_dim))
# pivot and wheel
self.pivot.set_transform(tf.translation_matrix(
[0, 0, 0])) # set location of pole
wheel_dim = [1.5, .5, .5] # x,y,z
self.wheel.set_object(g.Box(wheel_dim))
self.initialize()
def runTest(self):
"""
Test that we can render meshes from raw vertices and faces as
numpy arrays
"""
v = self.vis["triangular_mesh"]
v.set_transform(tf.rotation_matrix(np.pi / 2, [0., 0, 1]))
vertices = np.array([[0, 0, 0], [1, 0, 0], [1, 0, 1], [0, 0, 1]])
faces = np.array([[0, 1, 2], [3, 0, 2]])
v.set_object(g.TriangularMeshGeometry(vertices, faces),
g.MeshLambertMaterial(color=0xeedd22, wireframe=True))
v = self.vis["triangular_mesh_w_vertex_coloring"]
v.set_transform(
tf.translation_matrix([1, 0, 0]).dot(
tf.rotation_matrix(np.pi / 2, [0, 0, 1])))
colors = vertices
v.set_object(g.TriangularMeshGeometry(vertices, faces, colors),
g.MeshLambertMaterial(vertexColors=True, wireframe=True))
def visualize(self,states,dt): import meshcat import meshcat.geometry as g import meshcat.transformations as tf import time # Create a new visualizer vis = meshcat.Visualizer() vis.open() for i in range(self.n_agents): vis["agent"+str(i)].set_object(g.Sphere(self.r_agent)) while True: for state in states: for i in range(self.n_agents): idx = self.agent_idx_to_state_idx(i) + np.arange(0,2) pos = state[idx] vis["agent" + str(i)].set_transform(tf.translation_matrix([pos[0], pos[1], 0])) time.sleep(dt)
def setCameraTransform(self, translation, rotation):
# translation : [Px, Py, Pz],
# rotation : [Roll, Pitch, Yaw]
R_pnc = rpyToMatrix(np.array(rotation))
if Viewer.backend == 'gepetto-gui':
T_pnc = np.array(translation)
T_R = SE3(R_pnc, T_pnc)
self._client.setCameraTransform(self._window_id,
se3ToXYZQUAT(T_R).tolist())
else:
import meshcat.transformations as tf
# Transformation of the camera object
T_meshcat = tf.translation_matrix(translation)
self._client.viewer[
"/Cameras/default/rotated/<object>"].set_transform(T_meshcat)
# Orientation of the camera object
Q_pnc = Quaternion(R_pnc).coeffs()
Q_meshcat = np.roll(Q_pnc, shift=1)
R_meshcat = tf.quaternion_matrix(Q_meshcat)
self._client.viewer["/Cameras/default"].set_transform(R_meshcat)
def _draw_contact_forces(self, context, X_WB_map):
contact_results = self.EvalAbstractInput(context, 1).get_value()
t = context.get_time()
# First, set all existing contacts to be pruned.
for contact in self._contacts:
contact.needs_pruning = True
# Check if every element in contact_results is already in
# self._contacts.
# If True, update the magnitude and location of
# the _ContactState in self._contacts.
# If False, add the new contact to self._contacts
vis = self._meshcat_viz.vis
prefix = self._meshcat_viz.prefix
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info_i = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info_i.contact_force())
if force_norm < self._force_threshold:
continue
# contact point in frame B
bodyB = self._plant.get_body(contact_info_i.bodyB_index())
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`.
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
def plot_mathematical_program(meshcat, prog, X, Y, result=None):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = meshcat["constraint"]
for binding in prog.GetAllConstraints():
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
# Special case linear constraints
if False: # isinstance(c, LinearConstraint):
# TODO: take these as (optional) arguments to avoid computing them
# inefficiently.
xmin = np.min(X.reshape(-1))
xmax = np.max(X.reshape(-1))
ymin = np.min(Y.reshape(-1))
ymax = np.max(Y.reshape(-1))
A = c.A()
lower = c.lower_bound()
upper = c.upper_bound()
# find line / box intersections
# https://gist.github.com/ChickenProp/3194723
else:
v = cv[str(binding)]
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(v,
X,
Y,
Zc.reshape((X.shape[1], X.shape[0])),
color=0x9999dd)
if costs:
plot_surface(meshcat["objective"],
X,
Y,
Z.reshape(X.shape[1], X.shape[0]),
wireframe=True)
if result:
v = meshcat["solution"]
v.set_object(g.Sphere(0.1), g.MeshLambertMaterial(color=0x99ff99))
x_solution = result.get_x_val()
v.set_transform(
tf.translation_matrix(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
def plot_mathematical_program(meshcat,
prog,
X,
Y,
result=None,
point_size=0.05):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = meshcat["constraints"]
for binding in prog.GetAllConstraints():
if isinstance(
binding.evaluator(),
pydrake.solvers.mathematicalprogram.BoundingBoxConstraint):
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
v = cv[type(c).__name__]
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(v,
X,
Y,
Zc.reshape((X.shape[1], X.shape[0])),
color=0xff3333,
wireframe=True)
else:
Zc = prog.EvalBindingVectorized(binding, values)
evaluator = binding.evaluator()
low = evaluator.lower_bound()
up = evaluator.upper_bound()
cvb = cv[type(evaluator).__name__]
for index in range(Zc.shape[0]):
color = np.repeat([[0.3], [0.3], [1.0]], N, axis=1)
infeasible = np.logical_or(Zc[index, :] < low[index],
Zc[index, :] > up[index])
color[0, infeasible] = 1.0
color[2, infeasible] = 0.3
plot_surface(cvb[str(index)],
X,
Y,
Zc[index, :].reshape(X.shape[1], X.shape[0]),
color=color,
wireframe=True)
if costs:
plot_surface(meshcat["objective"],
X,
Y,
Z.reshape(X.shape[1], X.shape[0]),
color=0x77cc77,
wireframe=True)
if result:
v = meshcat["solution"]
v.set_object(g.Sphere(point_size),
g.MeshLambertMaterial(color=0x55ff55))
x_solution = result.get_x_val()
v.set_transform(
tf.translation_matrix(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
def runTest(self):
self.vis.delete()
v = self.vis["shapes"]
v.set_transform(tf.translation_matrix([1., 0, 0]))
v["box"].set_object(g.Box([1.0, 0.2, 0.3]))
v["box"].delete()
v["box"].set_object(g.Box([0.1, 0.2, 0.3]))
v["box"].set_transform(tf.translation_matrix([0.05, 0.1, 0.15]))
v["cylinder"].set_object(g.Cylinder(0.2, 0.1), g.MeshLambertMaterial(color=0x22dd22))
v["cylinder"].set_transform(tf.translation_matrix([0, 0.5, 0.1]).dot(tf.rotation_matrix(-np.pi / 2, [1, 0, 0])))
v["sphere"].set_object(g.Mesh(g.Sphere(0.15), g.MeshLambertMaterial(color=0xff11dd)))
v["sphere"].set_transform(tf.translation_matrix([0, 1, 0.15]))
v["ellipsoid"].set_object(g.Ellipsoid([0.3, 0.1, 0.1]))
v["ellipsoid"].set_transform(tf.translation_matrix([0, 1.5, 0.1]))
v["transparent_ellipsoid"].set_object(g.Mesh(
g.Ellipsoid([0.3, 0.1, 0.1]),
g.MeshLambertMaterial(color=0xffffff,
opacity=0.5)))
v["transparent_ellipsoid"].set_transform(tf.translation_matrix([0, 2.0, 0.1]))
v = self.vis["meshes/valkyrie/head"]
v.set_object(g.Mesh(
g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "data/head_multisense.obj")),
g.MeshLambertMaterial(
map=g.ImageTexture(
image=g.PngImage.from_file(os.path.join(meshcat.viewer_assets_path(), "data/HeadTextureMultisense.png"))
)
)
))
v.set_transform(tf.translation_matrix([0, 0.5, 0.5]))
v = self.vis["meshes/convex"]
v["obj"].set_object(g.Mesh(g.ObjMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.obj"))))
v["stl_ascii"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_ascii"))))
v["stl_ascii"].set_transform(tf.translation_matrix([0, -0.5, 0]))
v["stl_binary"].set_object(g.Mesh(g.StlMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.stl_binary"))))
v["stl_binary"].set_transform(tf.translation_matrix([0, -1, 0]))
v["dae"].set_object(g.Mesh(g.DaeMeshGeometry.from_file(os.path.join(meshcat.viewer_assets_path(), "../tests/data/mesh_0_convex_piece_0.dae"))))
v["dae"].set_transform(tf.translation_matrix([0, -1.5, 0]))
v = self.vis["points"]
v.set_transform(tf.translation_matrix([0, 2, 0]))
verts = np.random.rand(3, 1000000)
colors = verts
v["random"].set_object(g.PointCloud(verts, colors))
v["random"].set_transform(tf.translation_matrix([-0.5, -0.5, 0]))
v = self.vis["lines"]
v.set_transform(tf.translation_matrix(([-2, -3, 0])))
vertices = np.random.random((3, 10)).astype(np.float32)
v["line_segments"].set_object(g.LineSegments(g.PointsGeometry(vertices)))
v["line"].set_object(g.Line(g.PointsGeometry(vertices)))
v["line"].set_transform(tf.translation_matrix([0, 1, 0]))
v["line_loop"].set_object(g.LineLoop(g.PointsGeometry(vertices)))
v["line_loop"].set_transform(tf.translation_matrix([0, 2, 0]))
v["line_loop_with_material"].set_object(g.LineLoop(g.PointsGeometry(vertices), g.LineBasicMaterial(color=0xff0000)))
v["line_loop_with_material"].set_transform(tf.translation_matrix([0, 3, 0]))
colors = vertices # Color each line by treating its xyz coordinates as RGB colors
v["line_with_vertex_colors"].set_object(g.Line(g.PointsGeometry(vertices, colors), g.LineBasicMaterial(vertexColors=True)))
v["line_with_vertex_colors"].set_transform(tf.translation_matrix([0, 4, 0]))
v["triad"].set_object(g.LineSegments(
g.PointsGeometry(position=np.array([
[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T,
color=np.array([
[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
),
g.LineBasicMaterial(vertexColors=True)))
v["triad"].set_transform(tf.translation_matrix(([0, 5, 0])))
v["triad_function"].set_object(g.triad(0.5))
v["triad_function"].set_transform(tf.translation_matrix([0, 6, 0]))
def _draw_contact_forces(self, context, X_WB_map):
contact_results = self.EvalAbstractInput(context, 1).get_value()
t = context.get_time()
# First, set all existing contacts to be pruned.
for contact in self._contacts:
contact.needs_pruning = True
# Check if every element in contact_results is already in
# self._contacts.
# If True, update the magnitude and location of
# the _ContactState in self._contacts.
# If False, add the new contact to self._contacts
vis = self._meshcat_viz.vis
prefix = self._meshcat_viz.prefix
for i_contact in range(contact_results.num_contacts()):
contact_info_i = contact_results.contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info_i.contact_force())
if force_norm < self._force_threshold:
continue
# contact point in frame B
bodyB = self._plant.get_body(contact_info_i.bodyB_index())
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`.
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
def runTest(self):
v = self.vis["shapes"]
v["cube"].set_object(g.Box([0.1, 0.2, 0.3]))
v.set_transform(tf.translation_matrix([1., 0, 0]))
v.set_transform(tf.translation_matrix([1., 1., 0]))
def add_history(
self,
history: Tuple,
baubles: bool = True,
world_segment: str = "short",
short_length: float = 1.0,
bauble_radius: float = 0.01,
):
""" Similar to `add_ray_path` but with improved visualisation options.
Parameters
----------
history: tuple
Tuple of rays and events as returned from `photon_tracer.follow`
baubles: bool (optional)
Default is True. Draws baubles at exit location.
world_segment: str (optional)
Opt-out (`'exclude'`) or draw short (`'short`) path segments to the
world node.
short_length: float
The length of the final path segment when `world_segment='short'`.
bauble_radius: float
The bauble radius when `baubles=True`.
"""
vis = self.vis
if not world_segment in {"exclude", "short"}:
raise ValueError(
"`world_segment` should be either `'exclude'` or `'short'`.")
if world_segment == "exclude":
rays, events = zip(*history)
try:
idx = events.index(Event.EXIT)
history = history[0:idx]
if len(history) < 2:
# nothing left to render
return
except ValueError:
pass
if len(history) < 2:
raise AppError("Need at least two points to render a line.")
ids = []
rays, events = zip(*history)
for (start_part, end_part) in zip(history[:-1], history[1:]):
start_ray, end_ray = start_part[0], end_part[0]
nanometers = start_ray.wavelength
start = start_ray.position
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
def DoPublish(self, context, event):
# TODO(SeanCurtis-TRI) We want to be able to use this visualizer to
# draw without having it part of a Simulator. That means we'd like
# vis.Publish(context) to work. Currently, pydrake offers no mechanism
# to declare a forced event. However, by overriding DoPublish and
# putting the forced event callback code in the override, we can
# simulate it.
# We need to bind a mechanism for declaring forced events so we don't
# have to resort to overriding the dispatcher.
LeafSystem.DoPublish(self, context, event)
contact_results = self.EvalAbstractInput(context, 0).get_value()
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
contacts = []
for i_contact in range(contact_results.num_point_pair_contacts()):
contact_info = contact_results.point_pair_contact_info(i_contact)
# Do not display small forces.
force_norm = np.linalg.norm(contact_info.contact_force())
if force_norm < self._force_threshold:
continue
point_pair = contact_info.point_pair()
key = (point_pair.id_A.get_value(), point_pair.id_B.get_value())
cvis = vis[str(key)]
contacts.append(key)
arrow_height = self._radius * 2.0
if key not in self._published_contacts:
# New key, so create the geometry. Note: the height of the
# cylinder is 2 and gets scaled to twice the contact force
# length, because I am drawing both (equal and opposite)
# forces. Note also that meshcat (following three.js) puts
# the height of the cylinder along the y axis.
cvis["cylinder"].set_object(
meshcat.geometry.Cylinder(height=2.0, radius=self._radius),
meshcat.geometry.MeshLambertMaterial(color=0x33cc33))
cvis["head"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=0,
radiusBottom=self._radius * 2.0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
cvis["tail"].set_object(
meshcat.geometry.Cylinder(height=arrow_height,
radiusTop=self._radius * 2.0,
radiusBottom=0),
meshcat.geometry.MeshLambertMaterial(color=0x00dd00))
height = force_norm / self._contact_force_scale
cvis["cylinder"].set_transform(
tf.scale_matrix(height, direction=[0, 1, 0]))
cvis["head"].set_transform(
tf.translation_matrix([0, height + arrow_height / 2.0, 0.0]))
cvis["tail"].set_transform(
tf.translation_matrix([0, -height - arrow_height / 2.0, 0.0]))
# The contact frame's origin is located at contact_point and is
# oriented such that Cy is aligned with the contact force.
p_WC = contact_info.contact_point() # documented as in world.
if force_norm < 1e-6:
# We cannot rely on self._force_threshold to determine if the
# force can be normalized; that threshold can be zero.
R_WC = RotationMatrix()
else:
fhat_C_W = contact_info.contact_force() / force_norm
R_WC = RotationMatrix.MakeFromOneVector(b_A=fhat_C_W,
axis_index=1)
X_WC = RigidTransform(R=R_WC, p=p_WC)
cvis.set_transform(X_WC.GetAsMatrix4())
# We only delete any contact vectors that did not persist into this
# publish. It is tempting to just delete() the root branch at the
# beginning of this publish, but this leads to visual artifacts
# (flickering) in the browser.
for key in set(self._published_contacts) - set(contacts):
vis[str(key)].delete()
self._published_contacts = contacts
