def test_abstract_pass_through(self):
model_value = AbstractValue.Make("Hello world")
system = PassThrough(model_value)
context = system.CreateDefaultContext()
system.get_input_port(0).FixValue(context, model_value)
output = system.AllocateOutput()
input_eval = system.EvalAbstractInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
output_value = output.get_data(0)
compare_value(self, output_value, model_value)
def test_publisher(self):
lcm = DrakeLcm()
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=lcmt_quaternion,
lcm=lcm,
publish_period=0.1)
subscriber = Subscriber(lcm, "TEST_CHANNEL", lcmt_quaternion)
model_message = self._model_message()
self._fix_and_publish(dut, AbstractValue.Make(model_message))
lcm.HandleSubscriptions(0)
self.assert_lcm_equal(subscriber.message, model_message)
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)
def __init__(self, id_list, default_rgb=[255., 255., 255.]):
"""
A system that takes in N point clouds of points Si in frame Ci, and N
RigidTransforms from frame Ci to F, to put each point cloud in a common
frame F. The system returns one point cloud combining all of the
transformed point clouds. Each point cloud must have XYZs. RGBs are
optional. If absent, those points will be the provided default color.
@param id_list A list containing the string IDs of all of the point
clouds. This is often the serial number of the camera they came
from, such as "1" for a simulated camera or "805212060373" for a
real camera.
@param default_rgb A list of length 3 containing the RGB values to use
in the absence of PointCloud.rgbs. Values should be between 0 and
255. The default is white.
"""
LeafSystem.__init__(self)
self._point_cloud_ports = {}
self._transform_ports = {}
self._id_list = id_list
self._default_rgb = np.array(default_rgb)
output_fields = Fields(BaseField.kXYZs | BaseField.kRGBs)
for id in self._id_list:
self._point_cloud_ports[id] = self.DeclareAbstractInputPort(
"point_cloud_CiSi_{}".format(id),
AbstractValue.Make(PointCloud(fields=output_fields)))
self._transform_ports[id] = self.DeclareAbstractInputPort(
"X_FCi_{}".format(id),
AbstractValue.Make(RigidTransform.Identity()))
self.DeclareAbstractOutputPort("point_cloud_FS",
lambda: AbstractValue.Make(
PointCloud(fields=output_fields)),
self.DoCalcOutput)
def test_abstract_output_port_eval(self):
model_value = AbstractValue.Make("Hello World")
source = ConstantValueSource(copy.copy(model_value))
context = source.CreateDefaultContext()
output_port = source.get_output_port(0)
value = output_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, model_value.get_value())
value_abs = output_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_no_rgb(self):
self.pc_concat.GetInputPort("point_cloud_CiSi_0").FixValue(
self.context, AbstractValue.Make(self.pc_no_rgbs))
self.pc_concat.GetInputPort("point_cloud_CiSi_1").FixValue(
self.context, AbstractValue.Make(self.pc_no_rgbs))
fused_pc = self.pc_concat.GetOutputPort("point_cloud_FS").Eval(
self.context)
self.assertEqual(fused_pc.size(), 2 * self.num_points)
# The first point cloud should be from [-0.1 to 0.1].
# The second point cloud should be from [0.9 to 1.1].
self.assertTrue(np.max(fused_pc.xyzs()[0, :]) >= 1.0)
self.assertTrue(np.min(fused_pc.xyzs()[0, :]) <= 0.0)
# Even if both input point clouds don't have rgbs, the fused point
# cloud should contain rgbs of the default color.
self.assertTrue(fused_pc.has_rgbs())
self.assertTrue(
np.all(fused_pc.rgbs()[:, 0] == np.array([255, 255, 255])))
self.assertTrue(
np.all(fused_pc.rgbs()[:, -1] == np.array([255, 255, 255])))
def test_state_output_port_declarations(self):
"""Checks that DeclareStateOutputPort is bound."""
dut = LeafSystem()
xc_index = dut.DeclareContinuousState(2)
xc_port = dut.DeclareStateOutputPort(name="xc", state_index=xc_index)
self.assertEqual(xc_port.size(), 2)
xd_index = dut.DeclareDiscreteState(3)
xd_port = dut.DeclareStateOutputPort(name="xd", state_index=xd_index)
self.assertEqual(xd_port.size(), 3)
xa_index = dut.DeclareAbstractState(AbstractValue.Make(1))
xa_port = dut.DeclareStateOutputPort(name="xa", state_index=xa_index)
self.assertEqual(xa_port.get_name(), "xa")
def test_context_fix_input_port(self):
# WARNING: This is not the recommend workflow; instead, use
# `InputPort.FixValue` instead. This is here just for testing /
# coverage purposes.
dt = 0.1 # Arbitrary.
system_vec = ZeroOrderHold(period_sec=dt, vector_size=1)
context_vec = system_vec.CreateDefaultContext()
context_vec.FixInputPort(index=0, data=[0.])
context_vec.FixInputPort(index=0, vec=BasicVector([0.]))
# Test abstract.
model_value = AbstractValue.Make("Hello")
system_abstract = ZeroOrderHold(
period_sec=dt, abstract_model_value=model_value.Clone())
context_abstract = system_abstract.CreateDefaultContext()
context_abstract.FixInputPort(index=0, value=model_value.Clone())
def test_abstract_input_port_fix_object(self):
# The port type is py::object, not any specific C++ type.
model_value = AbstractValue.Make(object())
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
input_port = system.get_input_port(0)
# Fix to a type-erased py::object.
input_port.FixValue(context, AbstractValue.Make(object()))
# Fix to an int.
input_port.FixValue(context, 1)
value = input_port.Eval(context)
self.assertEqual(type(value), int)
self.assertEqual(value, 1)
# Fixing to an explicitly-typed Value instantiation is an error ...
with self.assertRaises(RuntimeError):
input_port.FixValue(context, AbstractValue.Make("string"))
# ... but implicit typing works just fine.
input_port.FixValue(context, "string")
value = input_port.Eval(context)
self.assertEqual(type(value), str)
self.assertEqual(value, "string")
def test_subscriber_wait_for_message(self):
"""Checks how `WaitForMessage` works without Python threads."""
lcm = DrakeLcm()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for old_message_count in range(3):
message = header_t()
message.utime = old_message_count + 1
lcm.Publish("TEST_LOOP", message.encode())
for attempt in range(10):
new_count = sub.WaitForMessage(
old_message_count, value, timeout=0.02)
if new_count > old_message_count:
break
lcm.HandleSubscriptions(0)
self.assertEqual(value.get_value().utime, old_message_count + 1)
def test_abstract_input_port_eval(self):
model_value = AbstractValue.Make("Hello World")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
fixed = system.get_input_port(0).FixValue(context,
copy.copy(model_value))
self.assertIsInstance(fixed.GetMutableData(), AbstractValue)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), type(model_value.get_value()))
self.assertEqual(value, model_value.get_value())
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(value_abs.get_value(), model_value.get_value())
def test_point_cloud_api(self):
self.assertEqual(mut.PointCloud.T, np.float32)
self.assertEqual(mut.PointCloud.C, np.uint8)
self.assertEqual(mut.PointCloud.D, np.float32)
self.assertTrue(
mut.PointCloud.IsDefaultValue(value=mut.PointCloud.kDefaultValue))
self.assertTrue(
mut.PointCloud.IsInvalidValue(value=mut.PointCloud.kDefaultValue))
fields = mut.Fields(mut.BaseField.kXYZs)
pc = mut.PointCloud(new_size=0, fields=fields)
self.assertEqual(pc.fields(), fields)
self.assertEqual(pc.size(), 0)
pc.resize(new_size=2)
self.assertEqual(pc.size(), 2)
self.assertTrue(pc.has_xyzs())
self.assertEqual(pc.xyzs().shape, (3, 2))
# Test reference semantics with NumPy + Pybind11.
test_xyzs = [[1., 2., 3.], [4., 5., 6.]]
pc.mutable_xyzs().T[:] = test_xyzs
np.testing.assert_equal(pc.xyzs().T, test_xyzs)
test_xyz = [10., 20., 30.]
pc.mutable_xyz(i=0)[:] = test_xyz
np.testing.assert_equal(pc.xyz(i=0), test_xyz)
np.testing.assert_equal(pc.xyzs().T[0], test_xyz)
pc_new = mut.PointCloud()
pc_new.SetFrom(other=pc)
np.testing.assert_equal(pc.xyzs(), pc_new.xyzs())
# - Additional types are tested via simple existence checks.
all_fields = mut.Fields(mut.BaseField.kXYZs | mut.BaseField.kNormals
| mut.BaseField.kRGBs)
count = 1
pc = mut.PointCloud(new_size=count, fields=all_fields)
self.check_array(pc.mutable_xyzs(), np.float32, (3, count))
self.check_array(pc.mutable_normals(), np.float32, (3, count))
pc.normals()
pc.mutable_normal(i=0)
pc.normal(i=0)
self.check_array(pc.mutable_rgbs(), np.uint8, (3, count))
pc.rgbs()
pc.mutable_rgb(i=0)
pc.rgb(i=0)
# - Check for none.
with self.assertRaises(RuntimeError) as ex:
mut.PointCloud(new_size=0, fields=mut.Fields(mut.BaseField.kNone))
# Test Systems' value registration.
self.assertIsInstance(AbstractValue.Make(pc), Value[mut.PointCloud])
def _get_abstract_model_and_example(cls):
# Permit for annotations, which should generally be types. Returns
# (model, example), where `model` should be of type `AbstractValue`.
assert isinstance(cls, type), f"Must supply a type: {cls}"
if issubclass(cls, AbstractValue):
# User wants to explicitly deal with abstract values.
inner_cls = _get_value_inner_cls(cls)
assert not _is_basic_vector(inner_cls), (
f"Cannot specify Value[BasicVector_[]]; specify "
"BasicVector_[](size) instead: {cls}")
model = cls()
example = model
return model, example
else:
example = cls()
model = AbstractValue.Make(example)
return model, example
def __init__(self, plant):
super().__init__()
# A multibody plant with a robot added to it
self._plant = plant
# Drake needs context to go along with a class instance
self._plant_context = self._plant.CreateDefaultContext()
num_positions = self._plant.num_positions()
# Input: List of joint positions matching order known by plant
self._joint_positions_port = self.DeclareVectorInputPort(
'joint_positions', BasicVector_[float](num_positions))
# Output: Rigid transforms of each body in base frame
self.DeclareAbstractOutputPort(
'transforms', lambda: AbstractValue.Make([RigidTransform()]),
self._do_forward_kinematics)
def test_meshcat_point_cloud_visualizer(self, T):
meshcat = mut.Meshcat()
visualizer = mut.MeshcatPointCloudVisualizer_[T](
meshcat=meshcat, path="cloud", publish_period=1/12.0)
visualizer.set_point_size(0.1)
visualizer.set_default_rgba(mut.Rgba(0, 0, 1, 1))
context = visualizer.CreateDefaultContext()
cloud = PointCloud(4)
cloud.mutable_xyzs()[:] = np.zeros((3, 4))
visualizer.cloud_input_port().FixValue(
context, AbstractValue.Make(cloud))
self.assertIsInstance(visualizer.pose_input_port(), InputPort_[T])
visualizer.Publish(context)
visualizer.Delete()
if T == float:
ad_visualizer = visualizer.ToAutoDiffXd()
self.assertIsInstance(
ad_visualizer, mut.MeshcatPointCloudVisualizer_[AutoDiffXd])
def __init__(self, scene_graph):
LeafSystem.__init__(self)
assert scene_graph
mbp = MultibodyPlant(0.0)
parser = Parser(mbp, scene_graph)
model_id = parser.AddModelFromFile(
FindResource("models/glider/glider.urdf"))
mbp.Finalize()
self.source_id = mbp.get_source_id()
self.body_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[0])
self.elevator_frame_id = mbp.GetBodyFrameIdOrThrow(
mbp.GetBodyIndices(model_id)[1])
self.DeclareVectorInputPort("state", BasicVector(7))
self.DeclareAbstractOutputPort(
"geometry_pose", lambda: AbstractValue.Make(FramePoseVector()),
self.OutputGeometryPose)
def test_parameters_api(self):
def compare(actual, expected):
self.assertEqual(type(actual), type(expected))
if isinstance(actual, VectorBase):
self.assertTrue(
np.allclose(actual.get_value(), expected.get_value()))
else:
self.assertEqual(actual.get_value(), expected.get_value())
model_numeric = BasicVector([0.])
model_abstract = AbstractValue.Make("Hello")
params = Parameters(
numeric=[model_numeric.Clone()], abstract=[model_abstract.Clone()])
self.assertEqual(params.num_numeric_parameter_groups(), 1)
self.assertEqual(params.num_abstract_parameters(), 1)
# Numeric.
compare(params.get_numeric_parameter(index=0), model_numeric)
compare(params.get_mutable_numeric_parameter(index=0), model_numeric)
# WARNING: This will invalidate old references!
params.set_numeric_parameters(params.get_numeric_parameters().Clone())
# Abstract.
compare(params.get_abstract_parameter(index=0), model_abstract)
compare(params.get_mutable_abstract_parameter(index=0), model_abstract)
# WARNING: This will invalidate old references!
params.set_abstract_parameters(
params.get_abstract_parameters().Clone())
# WARNING: This may invalidate old references!
params.SetFrom(copy.deepcopy(params))
# Test alternative constructors.
ctor_test = [
Parameters(),
Parameters(numeric=[model_numeric.Clone()]),
Parameters(abstract=[model_abstract.Clone()]),
Parameters(
numeric=[model_numeric.Clone()],
abstract=[model_abstract.Clone()]),
Parameters(vec=model_numeric.Clone()),
Parameters(value=model_abstract.Clone()),
]
def test_vector_output_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
source = ConstantVectorSource(np_value)
context = source.CreateDefaultContext()
output_port = source.get_output_port(0)
value = output_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = output_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = output_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_vector_input_port_eval(self):
np_value = np.array([1., 2., 3.])
model_value = AbstractValue.Make(BasicVector(np_value))
system = PassThrough(len(np_value))
context = system.CreateDefaultContext()
system.get_input_port(0).FixValue(context, np_value)
input_port = system.get_input_port(0)
value = input_port.Eval(context)
self.assertEqual(type(value), np.ndarray)
np.testing.assert_equal(value, np_value)
value_abs = input_port.EvalAbstract(context)
self.assertEqual(type(value_abs), type(model_value))
self.assertEqual(type(value_abs.get_value().get_value()), np.ndarray)
np.testing.assert_equal(value_abs.get_value().get_value(), np_value)
basic = input_port.EvalBasicVector(context)
self.assertEqual(type(basic), BasicVector)
self.assertEqual(type(basic.get_value()), np.ndarray)
np.testing.assert_equal(basic.get_value(), np_value)
def test_abstract_value_make(self):
value = AbstractValue.Make("Hello world")
self.assertIsInstance(value, Value[str])
value = AbstractValue.Make(1.0)
self.assertIsInstance(value, Value[float])
value = AbstractValue.Make(MoveOnlyType(10))
self.assertIsInstance(value, Value[MoveOnlyType])
value = AbstractValue.Make({"x": 10})
self.assertIsInstance(value, Value[object])
# N.B. Empty lists cannot have their type inferred, so the type will
# default to `Value[object]`.
value = AbstractValue.Make([])
self.assertIsInstance(value, Value[object])
# N.B. Non-empty lists can have their type inferred.
value = AbstractValue.Make([CustomType()])
self.assertIsInstance(value, Value[List[CustomType]])
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)
_warn_deprecated(_DEPRECATION, date="2022-09-01")
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)
# Contact results input port from MultibodyPlant
self.DeclareAbstractInputPort(
"contact_results", AbstractValue.Make(ContactResults()))
# This system has undeclared states, see #4330.
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 __init__(self,
meshcat,
visible=Visible(),
min_range=MinRange(),
max_range=MaxRange(),
value=Value()):
"""
Args:
meshcat: A Meshcat instance.
visible: An object with boolean elements for 'roll', 'pitch',
'yaw', 'x', 'y', 'z'; the intention is for this to be the
PoseSliders.Visible() namedtuple. Defaults to all true.
min_range, max_range, value: Objects with float values for 'roll',
'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the
caller to use the PoseSliders.MinRange, MaxRange, and
Value namedtuples. See those tuples for default values.
"""
LeafSystem.__init__(self)
port = self.DeclareAbstractOutputPort(
"pose", lambda: AbstractValue.Make(RigidTransform()),
self.DoCalcOutput)
# The widgets themselves have undeclared state. For now, we accept it,
# and simply disable caching on the output port.
# TODO(russt): consider implementing the more elaborate methods seen
# in, e.g., LcmMessageSubscriber.
port.disable_caching_by_default()
self._meshcat = meshcat
self._visible = visible
self._value = list(value)
for i in range(6):
if visible[i]:
meshcat.AddSlider(min=min_range[i],
max=max_range[i],
value=value[i],
step=0.01,
name=value._fields[i])
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 AllocateOutput(self):
return AbstractValue.Make(lcmt_header())
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,
visible=Visible(),
min_range=MinRange(),
max_range=MaxRange(),
value=Value()):
"""
Args:
visible: An object with boolean elements for 'roll', 'pitch',
'yaw', 'x', 'y', 'z'; the intention is for this to be the
PoseSliders.Visible() namedtuple. Defaults to all true.
min_range, max_range, value: Objects with float values for 'roll',
'pitch', 'yaw', 'x', 'y', 'z'; the intention is for the
caller to use the PoseSliders.MinRange, MaxRange, and
Value namedtuples. See those tuples for default values.
"""
LeafSystem.__init__(self)
port = self.DeclareAbstractOutputPort(
"pose", lambda: AbstractValue.Make(RigidTransform()),
self.DoCalcOutput)
# The widgets themselves have undeclared state. For now, we accept it,
# and simply disable caching on the output port.
# TODO(russt): consider implementing the more elaborate methods seen
# in, e.g., LcmMessageSubscriber.
port.disable_caching_by_default()
# Note: This timing affects the keyboard teleop performance. A larger
# time step causes more lag in the response.
self.DeclarePeriodicPublish(0.01, 0.0)
self._roll = FloatSlider(min=min_range.roll,
max=max_range.roll,
value=value.roll,
step=0.01,
continuous_update=True,
description="roll",
layout=Layout(width='90%'))
self._pitch = FloatSlider(min=min_range.pitch,
max=max_range.pitch,
value=value.pitch,
step=0.01,
continuous_update=True,
description="pitch",
layout=Layout(width='90%'))
self._yaw = FloatSlider(min=min_range.yaw,
max=max_range.yaw,
value=value.yaw,
step=0.01,
continuous_update=True,
description="yaw",
layout=Layout(width='90%'))
self._x = FloatSlider(min=min_range.x,
max=max_range.x,
value=value.x,
step=0.01,
continuous_update=True,
description="x",
orient='horizontal',
layout=Layout(width='90%'))
self._y = FloatSlider(min=min_range.y,
max=max_range.y,
value=value.y,
step=0.01,
continuous_update=True,
description="y",
layout=Layout(width='90%'))
self._z = FloatSlider(min=min_range.z,
max=max_range.z,
value=value.z,
step=0.01,
continuous_update=True,
description="z",
layout=Layout(width='90%'))
if visible.roll:
display(self._roll)
if visible.pitch:
display(self._pitch)
if visible.yaw:
display(self._yaw)
if visible.x:
display(self._x)
if visible.y:
display(self._y)
if visible.z:
display(self._z)
def CreateDefaultValue(self):
return AbstractValue.Make(self._lcm_type())
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_scenegraph_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 test_context_api(self):
# Capture miscellaneous functions not yet tested.
model_value = AbstractValue.Make("Hello")
model_vector = BasicVector([1., 2.])
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.DeclareContinuousState(1)
self.DeclareDiscreteState(2)
self.DeclareAbstractState(model_value.Clone())
self.DeclareAbstractParameter(model_value.Clone())
self.DeclareNumericParameter(model_vector.Clone())
system = TrivialSystem()
context = system.CreateDefaultContext()
self.assertTrue(context.get_state() is context.get_mutable_state())
self.assertEqual(context.num_continuous_states(), 1)
self.assertTrue(context.get_continuous_state_vector() is
context.get_mutable_continuous_state_vector())
self.assertEqual(context.num_discrete_state_groups(), 1)
self.assertTrue(context.get_discrete_state_vector() is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state_vector())
self.assertTrue(
context.get_discrete_state(0) is
context.get_discrete_state().get_vector(0))
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state_vector())
self.assertTrue(
context.get_mutable_discrete_state(0) is
context.get_mutable_discrete_state().get_vector(0))
self.assertEqual(context.num_abstract_states(), 1)
self.assertTrue(context.get_abstract_state() is
context.get_mutable_abstract_state())
self.assertTrue(
context.get_abstract_state(0) is
context.get_mutable_abstract_state(0))
self.assertEqual(
context.get_abstract_state(0).get_value(), model_value.get_value())
# Check abstract state API (also test AbstractValues).
values = context.get_abstract_state()
self.assertEqual(values.size(), 1)
self.assertEqual(
values.get_value(0).get_value(), model_value.get_value())
self.assertEqual(
values.get_mutable_value(0).get_value(), model_value.get_value())
values.SetFrom(values.Clone())
# Check parameter accessors.
self.assertEqual(system.num_abstract_parameters(), 1)
self.assertEqual(
context.get_abstract_parameter(index=0).get_value(),
model_value.get_value())
self.assertEqual(system.num_numeric_parameter_groups(), 1)
np.testing.assert_equal(
context.get_numeric_parameter(index=0).get_value(),
model_vector.get_value())
# Check diagram context accessors.
builder = DiagramBuilder()
builder.AddSystem(system)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Existence check.
self.assertIsNot(diagram.GetMutableSubsystemState(system, context),
None)
subcontext = diagram.GetMutableSubsystemContext(system, context)
self.assertIsNot(subcontext, None)
self.assertIs(diagram.GetSubsystemContext(system, context), subcontext)
subcontext2 = system.GetMyMutableContextFromRoot(context)
self.assertIsNot(subcontext2, None)
self.assertIs(subcontext2, subcontext)
self.assertIs(system.GetMyContextFromRoot(context), subcontext2)
def __init__(self,
scene_graph=None,
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,
role=Role.kIllustration,
prefer_hydro=False,
**kwargs):
"""
Args:
scene_graph: A SceneGraph object. This argument is optional, and
is only needed to enable calls to ``load()`` without providing
a Context.
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 list or array containing pydrake.geometry.FrameId
for which body frames to draw. Note that these are frames
registered within the scene_graph. For multibody frames, users
may obtain the ids from plant using GetBodyFrameIdIfExists.
Currently, frames that are not body frames are not supported
as they do not exist in the scene_graph yet.
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.
role: Renders geometry of the specified pydrake.geometry.Role
type -- defaults to Role.kIllustration to draw visual geometry,
and also supports Role.kProximity to draw collision geometry.
prefer_hydro: If True (and role == Role.kProximity) any geometry
that has a mesh hydroelastic representation will be visualized
by that discrete mesh and not the declared primitive. In the
case of *compliant* hydroelastic geometries, only the surface
of the volume mesh will be drawn. This is *not* the *contact*
surface used to characterize contact between two geometries
with hydroelastic representations -- it is the mesh
representations of those geometries.
Additional kwargs will be passed to the meshcat.Visualizer 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
if role not in [Role.kIllustration, Role.kProximity]:
raise ValueError("Unsupported role type specified: ", role)
self._role = role
self._prefer_hydro = prefer_hydro
# Recording.
self._is_recording = False
self.reset_recording()
self._scene_graph = scene_graph
self._geometry_query_input_port = self.DeclareAbstractInputPort(
"geometry_query", AbstractValue.Make(QueryObject()))
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.")
# Set background color (to match drake-visualizer).
self.vis['/Background'].set_property("top_color", [0.95, 0.95, 1.0])
self.vis['/Background'].set_property("bottom_color", [.32, .32, .35])
if open_browser:
webbrowser.open(self.vis.url())
def on_initialize(context, event):
self.load(context)
# TODO(russt): #13776 recommends we stop using initialization events
# for this.
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
# TODO(russt): Move this to a cache entry as in DrakeVisualizer.
# Requires #14287.
self._dynamic_frames = []
# drawing coordinate frames
self.frames_to_draw = set(frames_to_draw)
self.frames_opacity = frames_opacity
self.axis_length = axis_length
self.axis_radius = axis_radius
