def test_idm_controller(self):
rg = make_two_lane_road()
idm = IdmController(
road=rg,
path_or_branches=ScanStrategy.kPath,
road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
period_sec=0.)
context = idm.CreateDefaultContext()
output = idm.AllocateOutput()
# Fix the inputs.
pose_vector1 = PoseVector()
pose_vector1.set_translation([1., 2., 3.])
ego_pose_index = idm.ego_pose_input().get_index()
context.FixInputPort(ego_pose_index, pose_vector1)
w = [0., 0., 0.]
v = [1., 0., 0.]
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
ego_velocity_index = idm.ego_velocity_input().get_index()
context.FixInputPort(ego_velocity_index, frame_velocity1)
pose_vector2 = PoseVector()
pose_vector2.set_translation([6., 0., 0.])
bundle = PoseBundle(num_poses=1)
bundle.set_pose(
0, Isometry3(Quaternion(), pose_vector2.get_translation()))
traffic_index = idm.traffic_input().get_index()
context.FixInputPort(traffic_index,
framework.AbstractValue.Make(bundle))
# Verify the inputs.
pose_vector_eval = idm.EvalVectorInput(context, 0)
self.assertTrue(
np.allclose(pose_vector1.get_translation(),
pose_vector_eval.get_translation()))
frame_velocity_eval = idm.EvalVectorInput(context, 1)
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().translational(),
frame_velocity_eval.get_velocity().translational()))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().rotational(),
frame_velocity_eval.get_velocity().rotational()))
# Verify the outputs.
idm.CalcOutput(context, output)
accel_command_index = idm.acceleration_output().get_index()
accel = output.get_vector_data(accel_command_index)
self.assertEqual(len(accel.get_value()), 1)
self.assertTrue(accel.get_value() < 0.)
def test_pose_selector(self):
kScanDistance = 4.
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pose0 = PoseVector()
pose0.set_translation([1., 0., 0.])
pose1 = PoseVector()
# N.B. Set pose1 3 meters ahead of pose0.
pose1.set_translation([4., 0., 0.])
bundle = PoseBundle(num_poses=2)
bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation()))
bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation()))
closest_pose = PoseSelector.FindSingleClosestPose(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
side=AheadOrBehind.kAhead,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane.id().string())
self.assertTrue(closest_pose.distance == 3.)
closest_pair = PoseSelector.FindClosestPair(
lane=lane,
ego_pose=pose0,
traffic_poses=bundle,
scan_distance=kScanDistance,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(
closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.)
self.assertEqual(
closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance,
float('inf'))
lane_pos = LanePosition(s=1., r=0., h=0.)
road_pos = RoadPosition(lane=lane, pos=lane_pos)
w = [1., 2., 3.]
v = [-4., -5., -6.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(road_position=road_pos,
frame_velocity=frame_velocity)
sigma_v = PoseSelector.GetSigmaVelocity(road_odom)
self.assertEqual(sigma_v, v[0])
def test_idm_controller(self):
rg = make_two_lane_road()
idm = IdmController(
road=rg, path_or_branches=ScanStrategy.kPath,
road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
period_sec=0.)
context = idm.CreateDefaultContext()
output = idm.AllocateOutput()
# Fix the inputs.
pose_vector1 = PoseVector()
pose_vector1.set_translation([1., 2., 3.])
ego_pose_index = idm.ego_pose_input().get_index()
context.FixInputPort(ego_pose_index, pose_vector1)
w = [0., 0., 0.]
v = [1., 0., 0.]
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
ego_velocity_index = idm.ego_velocity_input().get_index()
context.FixInputPort(ego_velocity_index, frame_velocity1)
pose_vector2 = PoseVector()
pose_vector2.set_translation([6., 0., 0.])
bundle = PoseBundle(num_poses=1)
bundle.set_pose(
0, Isometry3(Quaternion(), pose_vector2.get_translation()))
traffic_index = idm.traffic_input().get_index()
context.FixInputPort(
traffic_index, framework.AbstractValue.Make(bundle))
# Verify the inputs.
pose_vector_eval = idm.EvalVectorInput(context, 0)
self.assertTrue(np.allclose(pose_vector1.get_translation(),
pose_vector_eval.get_translation()))
frame_velocity_eval = idm.EvalVectorInput(context, 1)
self.assertTrue(np.allclose(
frame_velocity1.get_velocity().translational(),
frame_velocity_eval.get_velocity().translational()))
self.assertTrue(np.allclose(
frame_velocity1.get_velocity().rotational(),
frame_velocity_eval.get_velocity().rotational()))
# Verify the outputs.
idm.CalcOutput(context, output)
accel_command_index = idm.acceleration_output().get_index()
accel = output.get_vector_data(accel_command_index)
self.assertEqual(len(accel.get_value()), 1)
self.assertTrue(accel.get_value() < 0.)
def __init__(self,
scene_graph,
draw_period=0.033333,
Tview=np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]),
xlim=[-1., 1],
ylim=[-1, 1],
facecolor=[1, 1, 1],
use_random_colors=False,
ax=None):
default_size = matplotlib.rcParams['figure.figsize']
scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0])
figsize = (default_size[0], default_size[0]*scalefactor)
PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize,
ax=ax, draw_timestep=draw_period)
self.set_name('planar_multibody_visualizer')
self._scene_graph = scene_graph
self.Tview = Tview
self.Tview_pinv = np.linalg.pinv(self.Tview)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
self.ax.axis('equal')
self.ax.axis('off')
# Achieve the desired view limits
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
default_size = self.fig.get_size_inches()
scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0])
self.fig.set_size_inches(default_size[0],
default_size[0]*scalefactor)
# Populate body patches
self.buildViewPatches(use_random_colors)
# Populate the body fill list -- which requires doing most of
# a draw pass, but with an ax.fill() command rather than
# an in-place replacement of vertex positions to initialize
# the draw patches.
# The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by
# order of viewPatches). Drawing the tree should update them
# by iterating over bodies and patches in the same order.
self.body_fill_dict = {}
n_bodies = len(self.viewPatches.keys())
tf = np.eye(4)
for full_name in self.viewPatches.keys():
viewPatches, viewColors = self.getViewPatches(full_name, tf)
self.body_fill_dict[full_name] = []
for patch, color in zip(viewPatches, viewColors):
self.body_fill_dict[full_name] += self.ax.fill(
patch[0, :], patch[1, :], zorder=0, edgecolor='k',
facecolor=color, closed=True)
def test_pose_selector(self):
kScanDistance = 4.
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pose0 = PoseVector()
pose0.set_translation([1., 0., 0.])
pose1 = PoseVector()
# N.B. Set pose1 3 meters ahead of pose0.
pose1.set_translation([4., 0., 0.])
bundle = PoseBundle(num_poses=2)
bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation()))
bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation()))
closest_pose = PoseSelector.FindSingleClosestPose(
lane=lane, ego_pose=pose0, traffic_poses=bundle,
scan_distance=kScanDistance, side=AheadOrBehind.kAhead,
path_or_branches=ScanStrategy.kPath)
self.assertEqual(closest_pose.odometry.lane.id().string(),
lane.id().string())
self.assertTrue(closest_pose.distance == 3.)
closest_pair = PoseSelector.FindClosestPair(
lane=lane, ego_pose=pose0, traffic_poses=bundle,
scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath)
self.assertEqual(
closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.)
self.assertEqual(
closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(),
lane.id().string())
self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance,
float('inf'))
lane_pos = LanePosition(s=1., r=0., h=0.)
road_pos = RoadPosition(lane=lane, pos=lane_pos)
w = [1., 2., 3.]
v = [-4., -5., -6.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
road_odom = RoadOdometry(
road_position=road_pos, frame_velocity=frame_velocity)
sigma_v = PoseSelector.GetSigmaVelocity(road_odom)
self.assertEqual(sigma_v, v[0])
def ReadPosesFromFile(filename):
"""
Reads in 4x4 poses from a text file and wraps them into a PoseBundle.
@param filename str. The path of the .txt file to read.
@return a PoseBundle with all of the poses.
"""
pose_dict = {}
row_num = 0
object_name = ""
cur_matrix = np.eye(4)
with open(filename, "r") as f:
for line in f:
line = line.rstrip()
if not line.lstrip(" ").startswith("["):
object_name = line
else:
row = np.matrix(line)
cur_matrix[row_num, :] = row
row_num += 1
if row_num == 4:
pose_dict[object_name] = RigidTransform(cur_matrix)
cur_matrix = np.eye(4)
row_num %= 4
pose_bundle = PoseBundle(num_poses=len(pose_dict))
i = 0
for object_name in pose_dict:
pose_bundle.set_name(i, object_name)
pose_bundle.set_pose(i, pose_dict[object_name])
i += 1
return pose_bundle
def __init__(self,
meshcat_viz,
force_threshold=1e-2,
contact_force_scale=10,
plant=None,
contact_force_radius=0.01):
"""
Args:
meshcat_viz: A pydrake MeshcatVisualizer instance.
force_threshold: contact forces whose norms are smaller than
force_threshold are not displayed.
contact_force_scale: a contact force with norm F (in Newtons) is
displayed as a cylinder with length F/contact_force_scale
(in meters).
plant: the MultibodyPlant associated with meshcat_viz.scene_graph.
contact_force_radius: sets the constant radius of the cylinder used
to visualize the forces.
"""
LeafSystem.__init__(self)
assert plant is not None
self._meshcat_viz = meshcat_viz
self._force_threshold = force_threshold
self._contact_force_scale = contact_force_scale
self._plant = plant
self._radius = contact_force_radius
self.set_name('meshcat_contact_visualizer')
self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("pose_bundle",
AbstractValue.Make(PoseBundle(0)))
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(ContactResults()))
# Make force cylinders smaller at initialization.
self._force_cylinder_radial_scale = 1.
self._force_cylinder_longitudinal_scale = 100.
# This system has undeclared states, see #4330.
# - All contacts (previous and current), of type `_ContactState`.
self._contacts = []
# - Unique key for contacts in meshcat.
self._contact_key_counter = 0
# - Previous time at which contact was published.
self._t_previous = 0.
def __init__(self,
meshcat_viz,
force_threshold=1e-2,
contact_force_scale=10,
plant=None,
contact_force_radius=0.01):
"""
Args:
meshcat_viz: A pydrake MeshcatVisualizer instance.
force_threshold: contact forces whose norms are smaller than
force_threshold are not displayed.
contact_force_scale: a contact force with norm F (in Newtons) is
displayed as a cylinder with length F/contact_force_scale
(in meters).
plant: the MultibodyPlant associated with meshcat_viz.scene_graph.
contact_force_radius: sets the constant radius of the cylinder used
to visualize the forces.
"""
LeafSystem.__init__(self)
assert plant is not None
self._meshcat_viz = meshcat_viz
self._force_threshold = force_threshold
self._contact_force_scale = contact_force_scale
self._plant = plant
self._radius = contact_force_radius
self.set_name('meshcat_contact_visualizer')
self.DeclarePeriodicPublish(self._meshcat_viz.draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("pose_bundle",
AbstractValue.Make(PoseBundle(0)))
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(ContactResults()))
# This system has undeclared states, see #4330.
self._warned_pose_bundle_input_port_connected = False
self._published_contacts = []
# Zap any previous contact forces on this prefix
vis = self._meshcat_viz.vis[self._meshcat_viz.prefix]["contact_forces"]
vis.delete()
def test_pose_aggregator(self):
aggregator = PoseAggregator()
# - Set-up.
instance_id1 = 5 # Supply a random instance id.
port1 = aggregator.AddSingleInput("pose_only", instance_id1)
self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued)
self.assertEqual(port1.size(), PoseVector.kSize)
instance_id2 = 42 # Supply another random, but unique, id.
ports2 = aggregator.AddSinglePoseAndVelocityInput(
"pose_and_velocity", instance_id2)
self.assertEqual(ports2.pose_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize)
self.assertEqual(ports2.velocity_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.velocity_input_port.size(),
FrameVelocity.kSize)
num_poses = 1
port3 = aggregator.AddBundleInput("pose_bundle", num_poses)
self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued)
# - CalcOutput.
self.assertEqual(aggregator.get_output_port(0).get_data_type(),
PortDataType.kAbstractValued)
context = aggregator.CreateDefaultContext()
output = aggregator.AllocateOutput()
p1 = [0, 1, 2]
pose1 = PoseVector()
pose1.set_translation(p1)
p2 = [5, 7, 9]
pose2 = PoseVector()
pose2.set_translation(p2)
w = [0.3, 0.4, 0.5]
v = [0.5, 0.6, 0.7]
velocity = FrameVelocity()
velocity.set_velocity(SpatialVelocity(w=w, v=v))
p3 = [50, 70, 90]
q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
bundle = PoseBundle(num_poses)
bundle.set_pose(0, Isometry3(q3, p3))
bundle_value = AbstractValue.Make(bundle)
context.FixInputPort(0, pose1)
context.FixInputPort(1, pose2)
context.FixInputPort(2, velocity)
context.FixInputPort(3, bundle_value)
aggregator.CalcOutput(context, output)
value = output.get_data(0).get_value()
self.assertEqual(value.get_num_poses(), 3)
isom1_actual = Isometry3()
isom1_actual.set_translation(p1)
self.assertTrue(
(value.get_pose(0).matrix() == isom1_actual.matrix()).all())
isom2_actual = Isometry3()
isom2_actual.set_translation(p2)
self.assertTrue(
(value.get_pose(1).matrix() == isom2_actual.matrix()).all())
vel_actual = value.get_velocity(1).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
self.assertTrue(
(value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def test_pose_bundle(self):
num_poses = 7
bundle = PoseBundle(num_poses)
# - Accessors.
self.assertEqual(bundle.get_num_poses(), num_poses)
self.assertTrue(isinstance(bundle.get_pose(0), Isometry3))
self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity))
# - Mutators.
kIndex = 5
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
bundle.set_pose(kIndex, Isometry3(q, p))
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
bundle.set_velocity(kIndex, frame_velocity)
self.assertTrue((bundle.get_pose(kIndex).matrix() ==
Isometry3(q, p).matrix()).all())
vel_actual = bundle.get_velocity(kIndex).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
name = "Alice"
bundle.set_name(kIndex, name)
self.assertEqual(bundle.get_name(kIndex), name)
instance_id = 42 # Supply a random instance id.
bundle.set_model_instance_id(kIndex, instance_id)
self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def __init__(self,
scene_graph,
draw_period=0.033333,
prefix="drake",
zmq_url="default",
open_browser=None):
"""
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 url to connect to the visualizer.
Use zmp_url="default" to the value obtained by running a
single `meshcat-server` in another terminal.
Use zmp_url=None or zmq_url="new" to start a new server (as a
child of this process); a new web browser will be opened (the
url will also be printed to the console).
Use e.g. zmq_url="tcp://127.0.0.1:6000" to specify a
specific address.
open_browser: Set to True to open the meshcat browser url in your
default web browser. The default value of None will open the
browser iff a new meshcat server is created as a subprocess.
Set to False to disable this.
Note: This call will not return until it connects to the
meshcat-server.
"""
LeafSystem.__init__(self)
self.set_name('meshcat_visualizer')
self._DeclarePeriodicPublish(draw_period, 0.0)
# Pose bundle (from SceneGraph) input port.
self._DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
elif zmq_url == "new":
zmq_url = None
if zmq_url is None and open_browser is None:
open_browser = True
# Set up meshcat.
self.prefix = prefix
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
print("Connected to meshcat-server.")
self._scene_graph = scene_graph
if open_browser:
webbrowser.open(self.vis.url())
def on_initialize(context, event):
self.load()
self._DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
def test_pose_aggregator(self):
aggregator = PoseAggregator()
# - Set-up.
instance_id1 = 5 # Supply a random instance id.
port1 = aggregator.AddSingleInput("pose_only", instance_id1)
self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued)
self.assertEqual(port1.size(), PoseVector.kSize)
instance_id2 = 42 # Supply another random, but unique, id.
ports2 = aggregator.AddSinglePoseAndVelocityInput(
"pose_and_velocity", instance_id2)
self.assertEqual(ports2.pose_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize)
self.assertEqual(ports2.velocity_input_port.get_data_type(),
PortDataType.kVectorValued)
self.assertEqual(ports2.velocity_input_port.size(),
FrameVelocity.kSize)
num_poses = 1
port3 = aggregator.AddBundleInput("pose_bundle", num_poses)
self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued)
# - CalcOutput.
self.assertEqual(
aggregator.get_output_port(0).get_data_type(),
PortDataType.kAbstractValued)
context = aggregator.CreateDefaultContext()
output = aggregator.AllocateOutput()
p1 = [0, 1, 2]
pose1 = PoseVector()
pose1.set_translation(p1)
p2 = [5, 7, 9]
pose2 = PoseVector()
pose2.set_translation(p2)
w = [0.3, 0.4, 0.5]
v = [0.5, 0.6, 0.7]
velocity = FrameVelocity()
velocity.set_velocity(SpatialVelocity(w=w, v=v))
p3 = [50, 70, 90]
q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
bundle = PoseBundle(num_poses)
bundle.set_transform(0, RigidTransform(quaternion=q3, p=p3))
bundle_value = AbstractValue.Make(bundle)
aggregator.get_input_port(0).FixValue(context, pose1)
aggregator.get_input_port(1).FixValue(context, pose2)
aggregator.get_input_port(2).FixValue(context, velocity)
aggregator.get_input_port(3).FixValue(context, bundle_value)
aggregator.CalcOutput(context, output)
value = output.get_data(0).get_value()
self.assertEqual(value.get_num_poses(), 3)
pose1_actual = RigidTransform(p=p1)
self.assertTrue((value.get_transform(0).GetAsMatrix34() ==
pose1_actual.GetAsMatrix34()).all())
pose2_actual = RigidTransform(p=p2)
self.assertTrue((value.get_transform(1).GetAsMatrix34() ==
pose2_actual.GetAsMatrix34()).all())
vel_actual = value.get_velocity(1).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
pose3_actual = RigidTransform(quaternion=q3, p=p3)
self.assertTrue((value.get_transform(2).GetAsMatrix34() ==
pose3_actual.GetAsMatrix34()).all())
def __init__(self,
scene_graph,
draw_period=_DEFAULT_PUBLISH_PERIOD,
prefix="drake",
zmq_url="default",
open_browser=None,
frames_to_draw={},
frames_opacity=1.,
axis_length=0.15,
axis_radius=0.006,
delete_prefix_on_load=True,
**kwargs):
"""
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 url to connect to the visualizer.
Use ``zmp_url="default"`` to the value obtained by running a
single ``meshcat-server`` in another terminal.
Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server
(as a child of this process); a new web browser will be opened
(the url will also be printed to the console).
Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a
specific address.
open_browser: Set to True to open the meshcat browser url in your
default web browser. The default value of None will open the
browser iff a new meshcat server is created as a subprocess.
Set to False to disable this.
frames_to_draw: a dictionary describing which body frames to draw.
It is keyed on the names of model instances, and the values are
sets of the names of the bodies whose body frames are shown.
For example, if we want to draw the frames of body "A" and
"B" in model instance "1", then frames_to_draw is
{"1": {"A", "B"}}.
frames_opacity, axis_length and axis_radius are the opacity, length
and radius of the coordinate axes to be drawn.
delete_prefix_on_load: Specifies whether we should delete the
visualizer path associated with prefix on our load
initialization event. True ensures a clean slate for every
simulation. False allows for the possibility of caching object
meshes on the zmqserver/clients, to avoid repeatedly
downloading meshes over the websockets link, but will cause
geometry from previous simulations to remain in the scene. You
may call ``delete_prefix()`` manually to clear the scene.
Additional kwargs will be passed to the MeshcatVisualizer constructor.
Note:
This call will not return until it connects to the
``meshcat-server``.
"""
LeafSystem.__init__(self)
self.set_name('meshcat_visualizer')
self.DeclarePeriodicPublish(draw_period, 0.0)
self.draw_period = draw_period
self._delete_prefix_on_load = delete_prefix_on_load
# Recording.
self._is_recording = False
self.reset_recording()
# Pose bundle (from SceneGraph) input port.
# TODO(tehbelinda): Rename the `lcm_visualization` port to match
# SceneGraph once its output port has been updated. See #12214.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
elif zmq_url == "new":
zmq_url = None
if zmq_url is None and open_browser is None:
open_browser = True
# Set up meshcat.
self.prefix = prefix
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url, **kwargs)
print("Connected to meshcat-server.")
self._scene_graph = scene_graph
# Set background color (to match drake-visualizer).
self.vis['/Background'].set_property("top_color", [242, 242, 255])
self.vis['/Background'].set_property("bottom_color", [77, 77, 89])
if open_browser:
webbrowser.open(self.vis.url())
def on_initialize(context, event):
self.load()
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
# drawing coordinate frames
self.frames_to_draw = frames_to_draw
self.frames_opacity = frames_opacity
self.axis_length = axis_length
self.axis_radius = axis_radius
def __init__(self,
scene_graph,
draw_period=1. / 30,
T_VW=np.array([[1., 0., 0., 0.], [0., 0., 1., 0.],
[0., 0., 0., 1.]]),
xlim=[-1., 1],
ylim=[-1, 1],
facecolor=[1, 1, 1],
use_random_colors=False,
substitute_collocated_mesh_files=True,
ax=None,
show=None):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes to the
visualizer.
T_VW: The view projection matrix from world to view coordinates.
xlim: View limit into the scene.
ylim: View limit into the scene.
facecolor: Passed through to figure() and sets background color.
Both color name strings and RGB triplets are allowed. Defaults
to white.
use_random_colors: If set to True, will render each body with a
different color. (Multiple visual elements on the same body
will be the same color.)
substitute_collocated_mesh_files: If True, then a mesh file
specified with an unsupported filename extension may be
replaced by a file of the same base name in the same directory,
but with a supported filename extension. Currently only .obj
files are supported.
ax: If supplied, the visualizer will draw onto those axes instead
of creating a new set of axes. The visualizer will still change
the view range and figure size of those axes.
show: Opens a window during initialization / publish iff True.
Default is None, which implies show=True unless
matplotlib.get_backend() is 'template'.
"""
default_size = matplotlib.rcParams['figure.figsize']
scalefactor = (ylim[1] - ylim[0]) / (xlim[1] - xlim[0])
figsize = (default_size[0], default_size[0] * scalefactor)
PyPlotVisualizer.__init__(self,
facecolor=facecolor,
figsize=figsize,
ax=ax,
draw_period=draw_period,
show=show)
self.set_name('planar_multibody_visualizer')
self._scene_graph = scene_graph
self._T_VW = T_VW
# Pose bundle (from SceneGraph) input port.
# TODO(tehbelinda): Rename the `lcm_visualization` port to match
# SceneGraph once its output port has been updated. See #12214.
self._pose_bundle_port = self.DeclareAbstractInputPort(
"lcm_visualization", AbstractValue.Make(PoseBundle(0)))
self.ax.axis('equal')
self.ax.axis('off')
# Achieve the desired view limits.
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
default_size = self.fig.get_size_inches()
self.fig.set_size_inches(figsize[0], figsize[1])
# Populate body patches.
self._build_body_patches(use_random_colors,
substitute_collocated_mesh_files)
# Populate the body fill list -- which requires doing most of a draw
# pass, but with an ax.fill() command to initialize the draw patches.
# After initialization, we can then use in-place replacement of vertex
# positions. The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by order of view_
# patches). Drawing the tree should update them by iterating over
# bodies and patches in the same order.
self._body_fill_dict = {}
X_WB_initial = RigidTransform.Identity()
for full_name in self._patch_Blist.keys():
patch_Wlist, view_colors = self._get_view_patches(
full_name, X_WB_initial)
self._body_fill_dict[full_name] = []
for patch_W, color in zip(patch_Wlist, view_colors):
# Project the full patch the first time, to initialize a vertex
# list with enough space for any possible convex hull of this
# vertex set.
patch_V = self._project_patch(patch_W)
body_fill = self.ax.fill(patch_V[0, :],
patch_V[1, :],
zorder=0,
edgecolor='k',
facecolor=color,
closed=True)[0]
self._body_fill_dict[full_name].append(body_fill)
# Then update the vertices for a more accurate initial draw.
self._update_body_fill_verts(body_fill, patch_V)
def __init__(self,
scene_graph,
zmq_url="default",
draw_period=0.033333,
camera_tfs=[RigidTransform()],
global_transform=RigidTransform(),
out_prefix=None,
show_figure=False,
save_scene=False):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes rendered
images.
zmq_url: Optionally set a url to connect to the blender server.
Use zmp_url="default" to the value obtained by running a
single blender server in another terminal.
TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a
new server (as a child of this process); a new web browser
will be opened (the url will also be printed to the console).
Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a
specific address.
camera tfs: List of RigidTransform camera tfs.
global transform: RigidTransform that gets premultiplied to every object.
Note: This call will not return until it connects to the
Blender server.
TODO(gizatt) Consolidate functionality with BlenderColorCamera
into a single BlenderCamera. Needs careful architecting to still support
rendering color + label images with different Blender servers in parallel.
"""
LeafSystem.__init__(self)
if out_prefix is not None:
self.out_prefix = out_prefix
else:
self.out_prefix = "/tmp/drake_blender_vis_labels_"
self.current_publish_num = 0
self.set_name('blender_label_camera')
self.DeclarePeriodicPublish(draw_period, 0.)
self.draw_period = draw_period
self.save_scene = save_scene
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:5556"
elif zmq_url == "new":
raise NotImplementedError("TODO")
if zmq_url is not None:
print("Connecting to blender server at zmq_url=" + zmq_url + "...")
self.bsi = BlenderServerInterface(zmq_url=zmq_url)
print("Connected to Blender server.")
self._scene_graph = scene_graph
self.global_transform = global_transform
self.camera_tfs = camera_tfs
def on_initialize(context, event):
self.load()
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
self.show_figure = show_figure
if self.show_figure:
plt.figure()
def test_pose_bundle(self):
num_poses = 7
bundle = PoseBundle(num_poses)
# - Accessors.
self.assertEqual(bundle.get_num_poses(), num_poses)
self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform))
self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity))
# - Mutators.
kIndex = 5
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
pose = RigidTransform(quaternion=q, p=p)
bundle.set_transform(kIndex, pose)
self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() ==
pose.GetAsMatrix34()).all())
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v))
bundle.set_velocity(kIndex, frame_velocity)
vel_actual = bundle.get_velocity(kIndex).get_velocity()
self.assertTrue(np.allclose(vel_actual.rotational(), w))
self.assertTrue(np.allclose(vel_actual.translational(), v))
name = "Alice"
bundle.set_name(kIndex, name)
self.assertEqual(bundle.get_name(kIndex), name)
instance_id = 42 # Supply a random instance id.
bundle.set_model_instance_id(kIndex, instance_id)
self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def __init__(self,
scene_graph,
zmq_url="default",
draw_period=0.033333,
camera_tfs=[RigidTransform()],
material_overrides=[],
global_transform=RigidTransform(),
env_map_path=None,
out_prefix=None,
show_figure=False,
role=Role.kIllustration,
save_scene=False):
"""
Args:
scene_graph: A SceneGraph object.
draw_period: The rate at which this class publishes rendered
images.
zmq_url: Optionally set a url to connect to the blender server.
Use zmp_url="default" to the value obtained by running a
single blender server in another terminal.
TODO(gizatt): Use zmp_url=None or zmq_url="new" to start a
new server (as a child of this process); a new web browser
will be opened (the url will also be printed to the console).
Use e.g. zmq_url="tcp://127.0.0.1:5556" to specify a
specific address.
camera tfs: List of RigidTransform camera tfs.
material_overrides: A list of tuples of regex rules and
material override arguments to be passed into a
a register material call, not including names. (Those are
assigned automatically by this class.)
global transform: RigidTransform that gets premultiplied to every object.
Note: This call will not return until it connects to the
Blender server.
"""
LeafSystem.__init__(self)
if out_prefix is not None:
self.out_prefix = out_prefix
else:
self.out_prefix = "/tmp/drake_blender_vis_"
self.current_publish_num = 0
self.set_name('blender_color_camera')
self.DeclarePeriodicPublish(draw_period, 0.)
self.draw_period = draw_period
self.save_scene = save_scene
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
# Optional pose bundle of bounding boxes.
self.DeclareAbstractInputPort("bounding_box_bundle",
AbstractValue.Make(BoundingBoxBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:5556"
elif zmq_url == "new":
raise NotImplementedError("TODO")
if zmq_url is not None:
print("Connecting to blender server at zmq_url=" + zmq_url + "...")
self.bsi = BlenderServerInterface(zmq_url=zmq_url)
print("Connected to Blender server.")
self._scene_graph = scene_graph
self._role = role
self.env_map_path = env_map_path
# Compile regex for the material overrides
self.material_overrides = [(re.compile(x[0]), x[1])
for x in material_overrides]
self.global_transform = global_transform
self.camera_tfs = camera_tfs
def on_initialize(context, event):
self.load()
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
self.show_figure = show_figure
if self.show_figure:
plt.figure()
def __init__(self,
scene_graph,
draw_period=_DEFAULT_PUBLISH_PERIOD,
prefix="drake",
zmq_url="default",
open_browser=None,
frames_to_draw={},
frames_opacity=1.,
axis_length=0.15,
axis_radius=0.006):
"""
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 url to connect to the visualizer.
Use ``zmp_url="default"`` to the value obtained by running a
single ``meshcat-server`` in another terminal.
Use ``zmp_url=None`` or ``zmq_url="new"`` to start a new server
(as a child of this process); a new web browser will be opened
(the url will also be printed to the console).
Use e.g. ``zmq_url="tcp://127.0.0.1:6000"`` to specify a
specific address.
open_browser: Set to True to open the meshcat browser url in your
default web browser. The default value of None will open the
browser iff a new meshcat server is created as a subprocess.
Set to False to disable this.
frames_to_draw: a dictionary describing which body frames to draw.
It is keyed on the names of model instances, and the values are
sets of the names of the bodies whose body frames are shown.
For example, if we want to draw the frames of body "A" and
"B" in model instance "1", then frames_to_draw is
{"1": {"A", "B"}}.
frames_opacity, axis_length and axis_radius are the opacity, length
and radius of the coordinate axes to be drawn.
Note:
This call will not return until it connects to the
``meshcat-server``.
"""
LeafSystem.__init__(self)
self.set_name('meshcat_visualizer')
self.DeclarePeriodicPublish(draw_period, 0.0)
self.draw_period = draw_period
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
elif zmq_url == "new":
zmq_url = None
if zmq_url is None and open_browser is None:
open_browser = True
# Set up meshcat.
self.prefix = prefix
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
print("Connected to meshcat-server.")
self._scene_graph = scene_graph
if open_browser:
webbrowser.open(self.vis.url())
def on_initialize(context, event):
self.load()
self.DeclareInitializationEvent(
event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=on_initialize))
# drawing coordinate frames
self.frames_to_draw = frames_to_draw
self.frames_opacity = frames_opacity
self.axis_length = axis_length
self.axis_radius = axis_radius
def __init__(self,
mbp,
sg,
all_manipulable_body_ids=[],
zmq_url="default"):
LeafSystem.__init__(self)
self.all_manipulable_body_ids = all_manipulable_body_ids
self.set_name('HydraInteractionLeafSystem')
# Pose bundle (from SceneGraph) input port.
#default_sg_context = sg.CreateDefaultContext()
#print("Default sg context: ", default_sg_context)
#query_object = sg.get_query_output_port().Eval(default_sg_context)
#print("Query object: ", query_object)
#self.DeclareAbstractInputPort("query_object",
# AbstractValue.Make(query_object))
self.pose_bundle_input_port = self.DeclareAbstractInputPort(
"pose_bundle", AbstractValue.Make(PoseBundle(0)))
self.robot_state_input_port = self.DeclareVectorInputPort(
"robot_state",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
self.spatial_forces_output_port = self.DeclareAbstractOutputPort(
"spatial_forces_vector",
lambda: AbstractValue.Make(VectorExternallyAppliedSpatialForced()),
self.DoCalcAbstractOutput)
self.DeclarePeriodicPublish(0.01, 0.0)
if zmq_url == "default":
zmq_url = "tcp://127.0.0.1:6000"
if zmq_url is not None:
print("Connecting to meshcat-server at zmq_url=" + zmq_url + "...")
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
fwd_pt_in_hydra_frame = RigidTransform(p=[0.0, 0.0, 0.0])
self.vis["hydra_origin"]["hand"].set_object(
meshcat_geom.ObjMeshGeometry.from_file(
os.path.join(os.getcwd(), "hand-regularfinal-scaled-1.obj")))
self.vis["hydra_origin"]["hand"].set_transform(
meshcat_tf.compose_matrix(scale=[0.001, 0.001, 0.001],
angles=[np.pi / 2, 0., np.pi / 2],
translate=[-0.25, 0., 0.]))
#self.vis["hydra_origin"]["center"].set_object(meshcat_geom.Sphere(0.02))
#self.vis["hydra_origin"]["center"].set_transform(meshcat_tf.translation_matrix([-0.025, 0., 0.]))
#self.vis["hydra_origin"]["mid"].set_object(meshcat_geom.Sphere(0.015))
#self.vis["hydra_origin"]["mid"].set_transform(meshcat_tf.translation_matrix([0.0, 0., 0.]))
#self.vis["hydra_origin"]["fwd"].set_object(meshcat_geom.Sphere(0.01))
#self.vis["hydra_origin"]["fwd"].set_transform(fwd_pt_in_hydra_frame.matrix())
#self.vis["hydra_grab"].set_object(meshcat_geom.Sphere(0.01),
# meshcat_geom.MeshLambertMaterial(
# color=0xff22dd,
# alphaMap=0.1))
self.vis["hydra_grab"]["grab_point"].set_object(
meshcat_geom.Sphere(0.01),
meshcat_geom.MeshLambertMaterial(color=0xff22dd, alphaMap=0.1))
# Hide it sketchily
self.vis["hydra_grab"].set_transform(
meshcat_tf.translation_matrix([0., 0., -1000.]))
# State for selecting objects
self.grab_needs_update = False
self.grab_in_progress = False
self.grab_update_hydra_pose = None
self.selected_body = None
self.selected_pose_in_body_frame = None
self.desired_pose_in_world_frame = None
self.stop = False
self.freeze_rotation = False
self.previously_freezing_rotation = False
# Set up subscription to Razer Hydra
self.mbp = mbp
self.mbp_context = mbp.CreateDefaultContext()
self.sg = sg
self.hasNewMessage = False
self.lastMsg = None
self.hydra_origin = RigidTransform(p=[1.0, 0., -0.1],
rpy=RollPitchYaw([0., 0., 0.]))
self.hydra_prescale = 3.0
self.callback_lock = Lock()
self.hydraSubscriber = rospy.Subscriber("/hydra_calib",
razer_hydra.msg.Hydra,
self.callback,
queue_size=1)
print("Waiting for hydra startup...")
while not self.hasNewMessage and not rospy.is_shutdown():
rospy.sleep(0.01)
print("Got hydra.")
