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 __init__(self,
camera,
rbt,
draw_timestep=0.033333,
prefix="RBCameraViz",
zmq_url="tcp://127.0.0.1:6000"):
LeafSystem.__init__(self)
self.set_name('camera meshcat visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
self.camera = camera
self.rbt = rbt
self.prefix = prefix
self.camera_input_port = \
self._DeclareAbstractInputPort(
"depth_im",
AbstractValue.Make(Image[PixelType.kDepth32F](
self.camera.depth_camera_info().width(),
self.camera.depth_camera_info().height())))
self.state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
# Set up meshcat
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[prefix].delete()
def _create_meshcat_backend():
import meshcat
from contextlib import redirect_stdout
with redirect_stdout(None):
Viewer._backend_obj = meshcat.Visualizer()
Viewer._backend_proc = Viewer._backend_obj.window.server_proc
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 __init__(self,
open_meshcat=False,
print_url=False,
wait_for_open=False,
zmq_url=None,
show_frames=False,
frame_scale=1.,
object_prefix="objects",
render_to_animation=False,
animation_fps=30,
**kwargs) -> None:
super().__init__()
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
if open_meshcat:
self.vis.open()
if print_url:
self.vis.url()
if wait_for_open:
self.vis.wait()
self.vis["/Background"].set_property("top_color", [1] * 3)
self.vis["/Background"].set_property("bottom_color", [1] * 3)
self.actors_and_offsets = []
self.show_frames = show_frames
self.frame_scale = frame_scale
self.object_prefix = object_prefix
self.animation = meshcat.animation.Animation(
default_framerate=animation_fps) if render_to_animation else None
self.itr = 0
self._vis_group = None
def __init__(self,
scene_graph,
draw_period=0.033333,
prefix="drake",
zmq_url="tcp://127.0.0.1:6000"):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes to the
visualizer.
prefix: Appears as the root of the tree structure in the meshcat
data structure
zmq_url: Optionally set a non-default url to connect to the
visualizer. The default value is the url obtained by
running `meshcat-server` in another terminal. If
zmp_url=None, then then a new server will be automatically
started (as a child of this process).
"""
LeafSystem.__init__(self)
self.set_name('meshcat_visualizer')
self._DeclarePeriodicPublish(draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self._DeclareInputPort("lcm_visualization",
PortDataType.kAbstractValued, 0)
# Set up meshcat.
self.prefix = prefix
print("Connecting to meshcat-server...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
print("Connected.")
self.vis[self.prefix].delete()
self._scene_graph = scene_graph
def __init__(self, scene_graph, prefix="SceneGraph", zmq_url="tcp://127.0.0.1:6000"):
self._zmq_url = zmq_url
self._scene_graph = scene_graph
self._scene_graph_output = scene_graph.AllocateOutput()
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[self.prefix].delete()
def show_cloud(pc, use_native=False, **kwargs):
# kwargs go to ctor.
builder = DiagramBuilder()
# Add point cloud visualization.
if use_native:
viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
else:
plant, scene_graph = AddMultibodyPlantSceneGraph(builder)
plant.Finalize()
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
pc_viz = builder.AddSystem(
MeshcatPointCloudVisualizer(viz, **kwargs))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
context.FixInputPort(
pc_viz.GetInputPort("point_cloud_P").get_index(),
AbstractValue.Make(pc))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.StepTo(sim_time)
def __init__(self,
rbtree,
old_mrbv=None,
draw_timestep=0.033333,
prefix="RBViz",
zmq_url="tcp://127.0.0.1:6000",
draw_collision=False,
clear_vis=False):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
self.rbtree = rbtree
self.draw_collision = draw_collision
self._DeclareInputPort(
PortDataType.kVectorValued,
self.rbtree.get_num_positions() + self.rbtree.get_num_velocities())
# Set up meshcat
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.old_mrbv = old_mrbv
# Don't both with caching logic if prefixes are different
if self.old_mrbv is not None and self.old_mrbv.prefix != self.prefix:
raise ValueError("Can't do any caching since old_mrbv has ",
"different prefix than current mrbv.")
self.body_names = []
if clear_vis:
self.vis.delete()
# Publish the tree geometry to get the visualizer started
self.PublishAllGeometry()
def __init__(self, zmq_url=None, max_histories=10000, open_browser=False):
super(MeshcatRenderer, self).__init__()
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
if open_browser:
self.vis.open()
self.ray_histories = deque(maxlen=max_histories)
self.max_histories = max_histories
self.added_index = 0
def draw_scene_tree_meshcat(scene_tree,
zmq_url=None,
alpha=1.0,
node_sphere_size=0.1):
pruned_tree = scene_tree.get_tree_without_production_rules()
# Do actual drawing in meshcat, starting from root of tree
# So first find the root...
root_node = get_tree_root(pruned_tree)
vis = meshcat.Visualizer(zmq_url=zmq_url or "tcp://127.0.0.1:6000")
vis["scene_tree"].delete()
node_queue = [root_node]
# Assign functionally random colors to each new node
# type we discover.
node_class_to_color_dict = {}
cmap = plt.cm.get_cmap('jet')
cmap_counter = 0.
k = 0
while len(node_queue) > 0:
node = node_queue.pop(0)
children = list(pruned_tree.successors(node))
node_queue += children
# Draw this node
node_type_string = node.__class__.__name__
if node_type_string in node_class_to_color_dict.keys():
color = node_class_to_color_dict[node_type_string]
else:
color = rgb_2_hex(cmap(cmap_counter))
node_class_to_color_dict[node_type_string] = color
cmap_counter = np.fmod(cmap_counter + np.pi * 2., 1.)
vis["scene_tree"][node.name + "%d" % k].set_object(
meshcat_geom.Sphere(node_sphere_size),
meshcat_geom.MeshToonMaterial(color=color,
opacity=alpha,
transparent=(alpha != 1.)))
tf = node.tf.cpu().detach().numpy()
vis["scene_tree"][node.name + "%d" % k].set_transform(tf)
# Draw connections to children
verts = []
for child in children:
verts.append(node.tf[:3, 3].cpu().detach().numpy())
verts.append(child.tf[:3, 3].cpu().detach().numpy())
if len(verts) > 0:
verts = np.vstack(verts).T
vis["scene_tree"][node.name + "%d" % k +
"_child_connections"].set_object(
meshcat_geom.Line(
meshcat_geom.PointsGeometry(verts),
meshcat_geom.LineBasicMaterial(
linewidth=10, color=color)))
k += 1
def setUp(self):
# the blocking_vis will take up the default fileserver and ZMQ ports
self.blocking_vis = meshcat.Visualizer()
# this should still work, by chosing a new port
self.vis = meshcat.Visualizer()
if "CI" in os.environ:
port = self.vis.url().split(":")[-1].split("/")[0]
self.dummy_proc = subprocess.Popen([
sys.executable, "-m", "meshcat.tests.dummy_websocket_client",
str(port)
])
else:
self.vis.open()
self.dummy_proc = None
self.vis.wait()
def setUp(self):
self.vis = meshcat.Visualizer()
if "CI" in os.environ:
port = self.vis.url().split(":")[-1].split("/")[0]
self.dummy_proc = subprocess.Popen([sys.executable, "-m", "meshcat.tests.dummy_websocket_client", str(port)])
else:
self.vis.open()
self.dummy_proc = None
self.vis.wait()
def main():
parser = argparse.ArgumentParser()
# four-frame | simple_frame | djmm_test_block | mars_bubble | sig_artopt-bunny | topopt-100 | topopt-205 | topopt-310 | voronoi
parser.add_argument('-p', '--problem', default='simple_frame', help='The name of the problem to solve')
parser.add_argument('-cl', '--check_log', default='none',
help='e.g. backward_sp, forward_random. It will search for <problem>_backward_sp_csp_log.json')
parser.add_argument('-vd', '--viz_deform', action='store_true', help='visualize deformation, default to false')
parser.add_argument('-vp', '--viz_pose', action='store_true', help='visualize feasible ee poses, default to false')
parser.add_argument('-s', '--scale', type=float, default=5.0, help='The vis scale')
parser.add_argument('-dt', '--delta_t', type=float, default=0.5, help='Vis time step')
parser.add_argument('-dl', '--dir_len', type=float, default=0.05, help='EE direction len')
args = parser.parse_args()
print('Arguments:', args)
# Create a new visualizer
vis = meshcat.Visualizer()
try:
vis.open()
except:
vis.url()
try:
elements, node_points, ground_nodes, shape_file_path = load_extrusion(args.problem, parse_layers=True)
node_order = list(range(len(node_points)))
print('file path: {0}'.format(shape_file_path))
# sc = None
# if args.viz_deform:
sc = stiffness_checker(json_file_path=shape_file_path, verbose=False)
sc.set_self_weight_load(True)
# vert indices sanity check
ground_nodes = [n for n in ground_nodes if n in node_order]
elements = [element for element in elements if all(n in node_order for n in element.node_ids)]
assembly_network = AssemblyNetwork(node_points, elements, ground_nodes)
# TODO: safeguarding
if args.check_log != 'none':
assign_history = read_csp_log_json(args.problem, specs=args.check_log)
meshcat_visualize_csp_log(vis, assembly_network, assign_history,
scale=args.scale, time_step=args.delta_t,
stiffness_checker=sc)
else:
element_seq, seq_poses = read_seq_json(args.problem)
meshcat_visualize_assembly_sequence(vis, assembly_network, element_seq, seq_poses,
stiffness_checker=sc, viz_pose=args.viz_pose, viz_deform=args.viz_deform,
scale=args.scale, direction_len=args.dir_len, time_step=args.delta_t)
except:
vis.delete()
# TODO: better exit to release meshcat port
user_input('ctrl-c to exit...')
def visualizer(self):
'''
Visualizer property. Initializes the visualizer if it hasn't been
initialized yet.
'''
if self._visualizer is None:
self._visualizer = meshcat.Visualizer(zmq_url=self.zmq_url)
self._visualizer.open()
self._visualizer["pendulum"].set_object(
meshcat.geometry.Box([0.1, 0.1, 0.1]))
return self._visualizer
def test_sampling_baseline(set_seed):
vis = meshcat.Visualizer()
# Draw a random sample from the grammar and visualize it.
grammar = SpatialSceneGrammar(
root_node_type = DishBinBaseline,
root_node_tf = torch.eye(4)
)
torch.random.manual_seed(42)
tree = grammar.sample_tree()
assert torch.isfinite(tree.score(verbose=True)), "Sampled tree was infeasible."
def main():
#time_step = 0.0002 # TODO: context.get_continuous_state_vector() fails
time_step = 2e-3
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='Disables collisions when planning')
parser.add_argument('-d', '--deterministic', action='store_true',
help='Manually sets the random seeds used by the stream generators')
parser.add_argument('-s', '--simulate', action='store_true',
help='Simulates the system')
args = parser.parse_args()
if args.deterministic:
# TODO: still not fully deterministic
random.seed(0)
np.random.seed(0)
import meshcat
meshcat_vis = meshcat.Visualizer()
task, diagram, state_machine = load_station(time_step=time_step)
print(task)
plant = task.mbp
#dump_plant(plant)
#dump_models(plant)
RenderSystemWithGraphviz(diagram) # Useful for getting port names
context = diagram.GetMutableSubsystemContext(plant, task.diagram_context)
task.publish()
initial_state = get_state(plant, context)
trajectories = plan_trajectories(task, context, collisions=not args.cfree)
if trajectories is None:
return
##################################################
set_state(plant, context, initial_state)
if args.simulate:
from manipulation_station.robot_plans import JointSpacePlan
splines, gripper_setpoints = convert_splines(plant, task.robot, task.gripper, context, trajectories)
sim_duration = compute_duration(splines)
plan_list = [JointSpacePlan(spline) for spline in splines]
print('Splines: {}\nDuration: {:.3f} seconds'.format(len(splines), sim_duration))
task, diagram, state_machine = load_station(time_step=time_step, plan=(plan_list, gripper_setpoints))
task.set_initial()
#set_state(plant, context, initial_state)
#state_machine.Load(plan_list, gripper_setpoints)
simulate_splines(task.diagram, task.diagram_context, sim_duration)
else:
step_trajectories(diagram, task.diagram_context, context, trajectories, time_step=0.001)
def draw_clearance_geometry_meshcat(scene_tree, zmq_url=None, alpha=0.25):
vis = meshcat.Visualizer(zmq_url=zmq_url or "tcp://127.0.0.1:6000")
vis["clearance_geom"].delete()
builder, mbp, scene_graph = compile_scene_tree_clearance_geometry_to_mbp_and_sg(scene_tree)
mbp.Finalize()
vis = ConnectMeshcatVisualizer(builder, scene_graph,
zmq_url="default", prefix="clearance")
diagram = builder.Build()
context = diagram.CreateDefaultContext()
vis.load(vis.GetMyContextFromRoot(context))
diagram.Publish(context)
def test_sampling():
vis = meshcat.Visualizer()
# Draw a random sample from the grammar and visualize it.
grammar = SpatialSceneGrammar(root_node_type=Desk,
root_node_tf=torch.eye(4))
torch.random.manual_seed(42)
tree = grammar.sample_tree()
assert torch.isfinite(
tree.score(verbose=True)), "Sampled tree was infeasible."
draw_scene_tree_contents_meshcat(tree, zmq_url=vis.window.zmq_url)
draw_scene_tree_structure_meshcat(tree, zmq_url=vis.window.zmq_url)
def do_generation_and_simulation(sim_time=10):
vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
scene_tree, satisfied_clearance = rejection_sample_feasible_tree(num_attempts=1000)
scene_tree, satisfied_feasibility = project_tree_to_feasibility(scene_tree, num_attempts=3)
serialize_scene_tree_to_yamls_and_sdfs(scene_tree, package_name='save', package_dir=".", remove_directory=True)
# Draw generated tree in meshcat.
#draw_scene_tree_meshcat(scene_tree, alpha=1.0, node_sphere_size=0.1)
# Draw its clearance geometry for debugging.
#draw_clearance_geometry_meshcat(scene_tree, alpha=0.3)
# Simulate the resulting scene, with a PR2 for scale.
builder, mbp, scene_graph = compile_scene_tree_to_mbp_and_sg(
scene_tree, timestep=0.001)
# Add PR2 and shift it back in front of where I know the table will be.
parser = Parser(mbp)
pr2_model_path = "/home/gizatt/drake/build/install/share/drake/examples/pr2/models/pr2_description/urdf/pr2_simplified.urdf"
parser.package_map().PopulateUpstreamToDrake(pr2_model_path);
pr2_model_id = parser.AddModelFromFile(
file_name=pr2_model_path, model_name="PR2_for_scale")
# Get the tf of the robot spawn node, and put the PR2 at that xy location.
robot_spawn_tf = scene_tree.find_nodes_by_type(RobotSpawnLocation)[0].tf.numpy()
mbp.GetJointByName("x", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[0, 3])
mbp.GetJointByName("y", model_instance=pr2_model_id).set_default_translation(robot_spawn_tf[1, 3])
mbp.Finalize()
visualizer = ConnectMeshcatVisualizer(builder, scene_graph,
zmq_url="default")
diagram = builder.Build()
diag_context = diagram.CreateDefaultContext()
mbp_context = diagram.GetMutableSubsystemContext(mbp, diag_context)
# Fix input port for PR2 to zero.
actuation_port = mbp.get_actuation_input_port(model_instance=pr2_model_id)
nu = mbp.num_actuated_dofs(model_instance=pr2_model_id)
actuation_port.FixValue(mbp_context, np.zeros(nu))
sim = Simulator(diagram, diag_context)
sim.set_target_realtime_rate(1.0)
if not satisfied_clearance:
print("WARNING: SCENE TREE NOT SATISFYING CLEARANCE")
if not satisfied_feasibility:
print("WARNING: SCENE TREE NOT SATISFYING FEASIBILITY, SIM MAY FAIL")
try:
sim.AdvanceTo(sim_time)
except RuntimeError as e:
print("Encountered error in sim: ", e)
def initViewer(self, viewer=None, open=False, loadModel=False):
"""Start a new MeshCat server and client.
Note: the server can also be started separately using the "meshcat-server" command in a terminal:
this enables the server to remain active after the current script ends.
"""
import meshcat
self.viewer = meshcat.Visualizer() if viewer is None else viewer
if open:
self.viewer.open()
if loadModel:
self.loadViewerModel()
def __init__(self, num_landmarks: int, seed: int,
bbox_length=1., landmarks_offset=(0, 0)):
random.seed(seed)
self.nl = num_landmarks
# edge length of the square in which the robot runs.
bbox_length = bbox_length
self.box_length = bbox_length
self.r_range_max = 1
self.r_range_min = 0.2
# coordinates of landmarks.
self.l_xy = (random.rand(self.nl, 2) * bbox_length - bbox_length / 2 +
np.array(landmarks_offset))
# self.l_xy = np.array([[0, 0.5], [-0.6, 0.1], [-0.7, -0.2]])
# visualizer
self.vis = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
self.vis.delete()
x_min = -min(self.l_xy[:, 0]) - 1
x_max = max(self.l_xy[:, 0]) + 1
y_min = -min(self.l_xy[:, 1]) - 1
y_max = max(self.l_xy[:, 1]) + 1
set_orthographic_camera_xy(self.vis)
self.draw_landmarks()
# initialize robot
self.X_WB = meshcat.transformations.rotation_matrix(
np.pi/2, np.array([1., 0, 0]), np.zeros(3))
material0 = meshcat.geometry.MeshLambertMaterial(
color=0xff0000, opacity=1.0)
material1 = meshcat.geometry.MeshLambertMaterial(
color=0xff0000, opacity=0.2)
self.vis["robot"]["sensing"].set_object(
meshcat.geometry.Cylinder(height=0.005, radius=self.r_range_max),
material1)
self.vis["robot"]["body"].set_object(
meshcat.geometry.Cylinder(height=0.2, radius=0.04),
material0)
self.vis["robot"].set_transform(self.X_WB)
# noise
self.sigma_odometry = 0.05
self.sigma_range = 0.05
self.sigma_bearing = 0.05
# von-mises distribution.
self.kappa_bearing = 1 / self.sigma_bearing ** 2
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 __init__(self,
rbtree,
draw_timestep=0.033333,
prefix="RBViz",
zmq_url="tcp://127.0.0.1:6000",
draw_collision=False):
self.timestep = draw_timestep
self.rbtree = rbtree
self.draw_collision = draw_collision
# Set up meshcat
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[self.prefix].delete()
# Publish the tree geometry to get the visualizer started
self.PublishAllGeometry()
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 __init__(
self,
prefix='RBViz', # type: str
zmq_url='tcp://127.0.0.1:6000' # type: str
):
# The help info
print('Please make sure meshcat-server is running in another terminal')
# Setup the visualizer
self._prefix = prefix
self._meshcat_vis = meshcat.Visualizer(zmq_url=zmq_url)
self._meshcat_vis[self._prefix].delete()
# The map from key to rigid body tree
# Note that rbt would be copied
self._rbt_map = dict() # type: Dict[str, RigidBodyTree]
self._rbt_geometry_element_set = set()
# The set of point cloud keys
self._pointcloud_set = set()
def __init__(self,
scene_graph,
draw_timestep=0.033333,
prefix="SceneGraph",
zmq_url="tcp://127.0.0.1:6000"):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
# Pose bundle (from SceneGraph) input port.
self._DeclareInputPort(PortDataType.kAbstractValued, 0)
# Set up meshcat.
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[self.prefix].delete()
self._scene_graph = scene_graph
def __init__(self,
zmq_url=None,
max_histories=10000,
open_browser=False,
wireframe=False,
transparency=True,
opacity=0.5,
reflectivity=1.0):
super(MeshcatRenderer, self).__init__()
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
if open_browser:
self.vis.open()
self.ray_histories = deque(maxlen=max_histories)
self.max_histories = max_histories
self.added_index = 0
self.wireframe = wireframe
self.transparency = transparency
self.opacity = opacity
self.reflectivity = reflectivity
def setUp(self):
self.zmq_url = "tcp://127.0.0.1:5560"
args = ["meshcat-server", "--zmq-url", self.zmq_url]
if "CI" not in os.environ:
args.append("--open")
self.server_proc = subprocess.Popen(args)
self.vis = meshcat.Visualizer(self.zmq_url)
# self.vis = meshcat.Visualizer()
# self.vis.open()
if "CI" in os.environ:
port = self.vis.url().split(":")[-1].split("/")[0]
self.dummy_proc = subprocess.Popen([sys.executable, "-m", "meshcat.tests.dummy_websocket_client", str(port)])
else:
# self.vis.open()
self.dummy_proc = None
self.vis.wait()
def show_cloud(pc, pc2=None, use_native=False, **kwargs):
# kwargs go to ctor.
builder = DiagramBuilder()
# Add point cloud visualization.
if use_native:
viz = meshcat.Visualizer(zmq_url=ZMQ_URL)
else:
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
plant.Finalize()
viz = builder.AddSystem(
MeshcatVisualizer(scene_graph,
zmq_url=ZMQ_URL,
open_browser=False))
builder.Connect(scene_graph.get_pose_bundle_output_port(),
viz.get_input_port(0))
pc_viz = builder.AddSystem(
MeshcatPointCloudVisualizer(viz, **kwargs))
if pc2:
pc_viz2 = builder.AddSystem(
MeshcatPointCloudVisualizer(viz,
name='second_point_cloud',
X_WP=se3_from_xyz([0, 0.3, 0]),
default_rgb=[0., 255., 0.]))
# Make sure the system runs.
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
context = diagram.GetMutableSubsystemContext(
pc_viz, diagram_context)
# TODO(eric.cousineau): Replace `AbstractValue.Make(pc)` with just
# `pc` (#12086).
pc_viz.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc))
if pc2:
context = diagram.GetMutableSubsystemContext(
pc_viz2, diagram_context)
pc_viz2.GetInputPort("point_cloud_P").FixValue(
context, AbstractValue.Make(pc2))
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.AdvanceTo(sim_time)
