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_publisher(self):
lcm = DrakeMockLcm()
dut = mut.LcmPublisherSystem.Make(channel="TEST_CHANNEL",
lcm_type=quaternion_t,
lcm=lcm)
model = self._model_message()
dut.set_publish_period(period=0.1)
context = dut.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(model))
dut.Publish(context)
raw = lcm.get_last_published_message("TEST_CHANNEL")
value = quaternion_t.decode(raw)
self.assert_lcm_equal(value, model)
def setUp(self):
builder = DiagramBuilder()
X_WP_0 = RigidTransform.Identity()
X_WP_1 = RigidTransform.Identity()
X_WP_1.set_translation([1.0, 0, 0])
id_list = ["0", "1"]
self.pc_concat = builder.AddSystem(PointCloudConcatenation(id_list))
self.num_points = 10000
xyzs = np.random.uniform(-0.1, 0.1, (3, self.num_points))
# Only go to 254 to distinguish between point clouds with and without
# color.
rgbs = np.random.uniform(0., 254.0, (3, self.num_points))
self.pc = PointCloud(self.num_points,
Fields(BaseField.kXYZs | BaseField.kRGBs))
self.pc.mutable_xyzs()[:] = xyzs
self.pc.mutable_rgbs()[:] = rgbs
self.pc_no_rgbs = PointCloud(self.num_points, Fields(BaseField.kXYZs))
self.pc_no_rgbs.mutable_xyzs()[:] = xyzs
diagram = builder.Build()
simulator = Simulator(diagram)
self.context = diagram.GetMutableSubsystemContext(
self.pc_concat, simulator.get_mutable_context())
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_0").get_index(),
AbstractValue.Make(X_WP_0))
self.context.FixInputPort(
self.pc_concat.GetInputPort("X_FCi_1").get_index(),
AbstractValue.Make(X_WP_1))
def __init__(self, config_file, segment_scene_function=None,
get_pose_function=None):
"""
A system that takes in 3 Drake PointClouds and ImageRgba8U from
RGBDCameras and determines the pose of an object in them. The user must
supply a segmentation function and pose alignment to determine the pose.
If these functions aren't supplied, the returned pose will always be the
4x4 identity matrix.
@param config_file str. A path to a .yml configuration file for the
cameras.
@param segment_scene_function A Python function that returns a subset of
the scene point cloud. See self.SegmentScene for more details.
@param get_pose_function A Python function that calculates a pose from a
segmented point cloud. See self.GetPose for more details.
@system{
@input_port{camera_left_rgb},
@input_port{camera_middle_rgb},
@input_port{camera_right_rgb},
@input_port{left_point_cloud},
@input_port{middle_point_cloud},
@input_port{right_point_cloud},
@output_port{X_WObject}
}
"""
LeafSystem.__init__(self)
# TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB
# fields.
self.left_rgb = self._DeclareAbstractInputPort(
"camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.middle_rgb = self._DeclareAbstractInputPort(
"camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.right_rgb = self._DeclareAbstractInputPort(
"camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480)))
self.left_depth = self._DeclareAbstractInputPort(
"left_point_cloud", AbstractValue.Make(mut.PointCloud()))
self.middle_depth = self._DeclareAbstractInputPort(
"middle_point_cloud", AbstractValue.Make(mut.PointCloud()))
self.right_depth = self._DeclareAbstractInputPort(
"right_point_cloud", AbstractValue.Make(mut.PointCloud()))
self._DeclareAbstractOutputPort("X_WObject",
lambda: AbstractValue.Make(
Isometry3.Identity()),
self._DoCalcOutput)
self.segment_scene_function = segment_scene_function
self.get_pose_function = get_pose_function
self._LoadConfigFile(config_file)
def _construct(self, nv, target_body_index, converter=None):
LeafSystem_[T].__init__(self, converter=converter)
self.set_name("applied_force_test_system")
self.nv = nv
self.target_body_index = target_body_index
self.DeclareAbstractOutputPort(
"spatial_forces_vector",
lambda: AbstractValue.Make(
VectorExternallyAppliedSpatialForced_[T]()),
self.DoCalcAbstractOutput)
self.DeclareVectorOutputPort(
"generalized_forces",
BasicVector_[T](self.nv),
self.DoCalcVectorOutput)
def test_contact_results_to_lcm(self):
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant_[float]()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(ContactResults_[float]()))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
def test_context_api(self):
# Capture miscellaneous functions not yet tested.
model_value = AbstractValue.Make("Hello")
class TrivialSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self._DeclareContinuousState(1)
self._DeclareDiscreteState(2)
self._DeclareAbstractState(model_value.Clone())
system = TrivialSystem()
context = system.CreateDefaultContext()
self.assertTrue(
context.get_state() is context.get_mutable_state())
self.assertTrue(
context.get_continuous_state_vector() is
context.get_mutable_continuous_state_vector())
self.assertTrue(
context.get_discrete_state_vector() is
context.get_mutable_discrete_state_vector())
self.assertEquals(context.get_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.assertEquals(
context.get_abstract_state(0).get_value(), model_value.get_value())
# Check AbstractValues API.
values = context.get_abstract_state()
self.assertEquals(values.size(), 1)
self.assertEquals(
values.get_value(0).get_value(), model_value.get_value())
self.assertEquals(
values.get_mutable_value(0).get_value(), model_value.get_value())
values.CopyFrom(values.Clone())
# - Check diagram context accessors.
builder = DiagramBuilder()
builder.AddSystem(system)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
# Existence check.
self.assertTrue(
diagram.GetMutableSubsystemState(system, context) is not None)
self.assertTrue(
diagram.GetMutableSubsystemContext(system, context) is not None)
def __init__(self, hand_controller, hand_plant):
LeafSystem.__init__(self)
self.hand_controller = hand_controller
self.hand_plant = hand_plant
self.n_cf = len(hand_controller.grasp_points)
self._data = []
self._sample_times = np.empty((0, 1))
self.shut_up = False
# Contact results
# self.DeclareInputPort('contact_results', PortDataType.kAbstractValued,
# hand_plant.contact_results_output_port().size())
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(mut.ContactResults()))
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_rgb(self):
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_0").get_index(),
AbstractValue.Make(self.pc))
self.context.FixInputPort(
self.pc_concat.GetInputPort("point_cloud_CiSi_1").get_index(),
AbstractValue.Make(self.pc))
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)
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_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_eval(self):
model_value = AbstractValue.Make("Hello World")
system = PassThrough(copy.copy(model_value))
context = system.CreateDefaultContext()
fixed = context.FixInputPort(0, 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_subscriber_wait_for_message(self):
"""Ensures that `WaitForMessage` threading works in a Python workflow
that is not threaded."""
# N.B. This will fail with `threading`. See below for using
# `multithreading`.
lcm = DrakeLcm("memq://")
lcm.StartReceiveThread()
sub = mut.LcmSubscriberSystem.Make("TEST_LOOP", header_t, lcm)
value = AbstractValue.Make(header_t())
for i in range(3):
message = header_t()
message.utime = i
lcm.Publish("TEST_LOOP", message.encode())
sub.WaitForMessage(i, value)
self.assertEqual(value.get_value().utime, i)
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("memq://")
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_contact(self):
# PenetrationAsContactPair
point_pair = PenetrationAsPointPair()
self.assertTrue(isinstance(point_pair.id_A, GeometryId))
self.assertTrue(isinstance(point_pair.id_B, GeometryId))
self.assertTrue(point_pair.p_WCa.shape == (3, ))
self.assertTrue(point_pair.p_WCb.shape == (3, ))
self.assertTrue(isinstance(point_pair.depth, float))
# PointPairContactInfo
id_A = BodyIndex(0)
id_B = BodyIndex(1)
contact_info = PointPairContactInfo(bodyA_index=id_A,
bodyB_index=id_B,
f_Bc_W=np.array([0, 0, 1]),
p_WC=np.array([0, 0, 0]),
separation_speed=0,
slip_speed=0,
point_pair=point_pair)
self.assertTrue(isinstance(contact_info.bodyA_index(), BodyIndex))
self.assertTrue(isinstance(contact_info.bodyB_index(), BodyIndex))
self.assertTrue(contact_info.contact_force().shape == (3, ))
self.assertTrue(contact_info.contact_point().shape == (3, ))
self.assertTrue(isinstance(contact_info.slip_speed(), float))
self.assertIsInstance(contact_info.point_pair(),
PenetrationAsPointPair)
# ContactResults
contact_results = ContactResults()
contact_results.AddContactInfo(contact_info)
self.assertTrue(contact_results.num_contacts() == 1)
self.assertTrue(
isinstance(contact_results.contact_info(0), PointPairContactInfo))
# ContactResultsToLcmSystem
file_name = FindResourceOrThrow(
"drake/multibody/benchmarks/acrobot/acrobot.sdf")
plant = MultibodyPlant()
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
contact_results_to_lcm = ContactResultsToLcmSystem(plant)
context = contact_results_to_lcm.CreateDefaultContext()
context.FixInputPort(0, AbstractValue.Make(contact_results))
output = contact_results_to_lcm.AllocateOutput()
contact_results_to_lcm.CalcOutput(context, output)
result = output.get_data(0)
self.assertIsInstance(result, AbstractValue)
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 __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 __init__(self, station, control_period=0.005, print_period=0.5):
LeafSystem.__init__(self)
self.set_name("Iiwa Controller")
self.current_plan = None
# Stuff for iiwa control
self.nu = 7
self.print_period = print_period
self.last_print_time = -print_period
self.control_period = control_period
# create a multibodyplant containing the robot only, which is used for
# jacobian calculations.
self.plant_iiwa = station.get_controller_plant()
self.tree_iiwa = self.plant_iiwa.tree()
self.context_iiwa = self.plant_iiwa.CreateDefaultContext()
self.l7_frame = self.plant_iiwa.GetFrameByName('iiwa_link_7')
# Declare iiwa_position/torque_command publishing rate
self._DeclarePeriodicPublish(control_period)
# iiwa position input port
self.iiwa_position_input_port = \
self._DeclareInputPort(
"iiwa_position", PortDataType.kVectorValued, 7)
# iiwa velocity input port
self.iiwa_velocity_input_port = \
self._DeclareInputPort(
"iiwa_velocity", PortDataType.kVectorValued, 7)
# plan abstract input port
self.plan_input_port = \
self._DeclareAbstractInputPort(
"iiwa_plan", AbstractValue.Make(PlanBase()))
# position and torque command output port
# first 7 elements are position commands.
# last 7 elements are torque commands.
self.iiwa_position_command_output_port = \
self._DeclareVectorOutputPort("iiwa_position_and_torque_command",
BasicVector(self.nu*2), self._CalcIiwaCommand)
def test_deprecated_abstract_input_port(self):
# A system that takes a Value[object] on its input, and parses the
# input value's first element to a float on its output.
class ParseFloatSystem(LeafSystem_[float]):
def __init__(self):
LeafSystem_[float].__init__(self)
self._DeclareAbstractInputPort("in")
self._DeclareVectorOutputPort("out", BasicVector(1), self._Out)
def _Out(self, context, y_data):
py_obj = self.EvalAbstractInput(context, 0).get_value()[0]
y_data.SetAtIndex(0, float(py_obj))
system = ParseFloatSystem()
context = system.CreateDefaultContext()
output = system.AllocateOutput()
context.FixInputPort(0, AbstractValue.Make(["22.2"]))
system.CalcOutput(context, output)
self.assertEqual(output.get_vector_data(0).GetAtIndex(0), 22.2)
def test_abstract_io_port(self):
test = self
# N.B. Since this has trivial operations, we can test all scalar types.
for T in [float, AutoDiffXd, Expression]:
default_value = ("default", T(0.))
expected_input_value = ("input", T(np.pi))
expected_output_value = ("output", 2*T(np.pi))
class CustomAbstractSystem(LeafSystem_[T]):
def __init__(self):
LeafSystem_[T].__init__(self)
self.input_port = self.DeclareAbstractInputPort(
"in", AbstractValue.Make(default_value))
self.output_port = self.DeclareAbstractOutputPort(
"out",
lambda: AbstractValue.Make(default_value),
self.DoCalcAbstractOutput,
prerequisites_of_calc=set([
self.input_port.ticket()]))
def DoCalcAbstractOutput(self, context, y_data):
input_value = self.EvalAbstractInput(
context, 0).get_value()
# The allocator function will populate the output with
# the "input"
test.assertTupleEqual(input_value, expected_input_value)
y_data.set_value(expected_output_value)
test.assertTupleEqual(y_data.get_value(),
expected_output_value)
system = CustomAbstractSystem()
context = system.CreateDefaultContext()
self.assertEqual(context.num_input_ports(), 1)
context.FixInputPort(0, AbstractValue.Make(expected_input_value))
output = system.AllocateOutput()
self.assertEqual(output.num_ports(), 1)
with catch_drake_warnings(expected_count=1):
output.get_num_ports()
system.CalcOutput(context, output)
value = output.get_data(0)
self.assertEqual(value.get_value(), expected_output_value)
def test_parameters_api(self):
def compare(actual, expected):
self.assertEquals(type(actual), type(expected))
if isinstance(actual, VectorBase):
self.assertTrue(
np.allclose(actual.get_value(), expected.get_value()))
else:
self.assertEquals(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.assertEquals(params.num_numeric_parameters(), 1)
self.assertEquals(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 __init__(self, kuka_plans, gripper_setpoint_list, update_period=0.05):
LeafSystem.__init__(self)
self.set_name("Plan Scheduler")
assert len(kuka_plans) == len(gripper_setpoint_list)
# Add a zero order hold to hold the current position of the robot
kuka_plans.insert(
0, JointSpacePlanRelative(duration=3.0, delta_q=np.zeros(7)))
gripper_setpoint_list.insert(0, 0.055)
if len(kuka_plans) > 1:
# Insert to the beginning of plan_list a plan that moves the robot from its
# current position to plan_list[0].traj.value(0)
kuka_plans.insert(
1,
JointSpacePlanGoToTarget(
duration=6.0,
q_target=kuka_plans[1].traj.value(0).flatten()))
gripper_setpoint_list.insert(0, 0.055)
self.gripper_setpoint_list = gripper_setpoint_list
self.kuka_plans_list = kuka_plans
self.current_plan = None
self.current_gripper_setpoint = None
self.current_plan_idx = 0
# Output ports for plans and gripper setpoints
self.iiwa_plan_output_port = self._DeclareAbstractOutputPort(
"iiwa_plan", lambda: AbstractValue.Make(PlanBase()),
self._GetCurrentPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self._CalcForceLimitOutput)
self._DeclarePeriodicPublish(update_period)
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 __init__(self, update_period=0.05):
LeafSystem.__init__(self)
self.set_name("Plan Scheduler")
self.current_plan = None
self.current_gripper_setpoint = None
self.end_time = 0
# Output ports for plans and gripper setpoints
self.iiwa_plan_output_port = self._DeclareAbstractOutputPort(
"iiwa_plan", lambda: AbstractValue.Make(PlanBase()),
self._GetCurrentPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self._CalcForceLimitOutput)
self._DeclarePeriodicPublish(update_period)
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)
def CreateDefaultValue(self):
return AbstractValue.Make(self._lcm_type())
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,
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)
