Skip to content

Commit

Permalink
Basic caching infrastructure.
Browse files Browse the repository at this point in the history
Rearchitect fixed input ports to support invalidation.
Simplified SystemOutput.
Get rid of unnecessary data structures in diagram context.
Moved FixInputPort to ContextBase.
Convert spring-mass test to take advantage of cached derivatives.
Modify depth sensor to use the cache.
Add validate step in context allocation.
Rename Freestanding -> Fixed.
Merge changes from PRs: dependency tracker, cache, cache entry,
  cloneable DiagramContinuous/DiscreteState, connect base classes,
  output port allocator context parameter removal, PR RobotLocomotion#8623,
  input port PR RobotLocomotion#8760, diagram output PR RobotLocomotion#8857, output port PR RobotLocomotion#8920.
  • Loading branch information
sherm1 committed Jun 19, 2018
1 parent 51f5829 commit 5522b48
Show file tree
Hide file tree
Showing 52 changed files with 1,706 additions and 870 deletions.
1 change: 0 additions & 1 deletion bindings/pydrake/systems/framework_py_semantics.cc
Original file line number Diff line number Diff line change
Expand Up @@ -178,7 +178,6 @@ void DefineFrameworkPySemantics(py::module m) {

auto system_output = DefineTemplateClassWithDefault<SystemOutput<T>>(
m, "SystemOutput", GetPyParam<T>());
DefClone(&system_output);
system_output
.def("get_num_ports", &SystemOutput<T>::get_num_ports)
.def("get_data", &SystemOutput<T>::get_data,
Expand Down
4 changes: 3 additions & 1 deletion bindings/pydrake/systems/framework_py_systems.cc
Original file line number Diff line number Diff line change
Expand Up @@ -274,7 +274,9 @@ struct Impl {
py::arg("input_port"), py::arg("output_port"))
// Context.
.def("CreateDefaultContext", &System<T>::CreateDefaultContext)
.def("AllocateOutput", &System<T>::AllocateOutput)
.def("AllocateOutput",
overload_cast_explicit<unique_ptr<SystemOutput<T>>>(
&System<T>::AllocateOutput))
.def(
"EvalVectorInput",
[](const System<T>* self, const Context<T>& arg1, int arg2) {
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/systems/rgbd_camera_example.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
# Allocate context and render.
context = camera.CreateDefaultContext()
context.FixInputPort(0, BasicVector(x))
output = camera.AllocateOutput(context)
output = camera.AllocateOutput()
camera.CalcOutput(context, output)

# Get images from computed output.
Expand Down
4 changes: 2 additions & 2 deletions bindings/pydrake/systems/test/custom_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,7 +103,7 @@ def test_adder_execution(self):
system = self._create_adder_system()
context = system.CreateDefaultContext()
self._fix_adder_inputs(context)
output = system.AllocateOutput(context)
output = system.AllocateOutput()
self.assertEqual(output.get_num_ports(), 1)
system.CalcOutput(context, output)
value = output.get_vector_data(0).get_value()
Expand Down Expand Up @@ -354,7 +354,7 @@ def _DoCalcAbstractOutput(self, context, y_data):

self.assertEqual(context.get_num_input_ports(), 1)
context.FixInputPort(0, AbstractValue.Make(expected_input_value))
output = system.AllocateOutput(context)
output = system.AllocateOutput()
self.assertEqual(output.get_num_ports(), 1)
system.CalcOutput(context, output)
value = output.get_data(0)
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/systems/test/general_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -145,7 +145,7 @@ def test_simulator_ctor(self):

def check_output(context):
# Check number of output ports and value for a given context.
output = system.AllocateOutput(context)
output = system.AllocateOutput()
self.assertEqual(output.get_num_ports(), 1)
system.CalcOutput(context, output)
if T == float:
Expand Down
12 changes: 6 additions & 6 deletions bindings/pydrake/systems/test/primitives_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -156,7 +156,7 @@ def test_vector_pass_through(self):
system = PassThrough(model_value.size())
context = system.CreateDefaultContext()
context.FixInputPort(0, model_value)
output = system.AllocateOutput(context)
output = system.AllocateOutput()
input_eval = system.EvalVectorInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
Expand All @@ -168,7 +168,7 @@ def test_abstract_pass_through(self):
system = PassThrough(model_value)
context = system.CreateDefaultContext()
context.FixInputPort(0, model_value)
output = system.AllocateOutput(context)
output = system.AllocateOutput()
input_eval = system.EvalAbstractInput(context, 0)
compare_value(self, input_eval, model_value)
system.CalcOutput(context, output)
Expand All @@ -183,7 +183,7 @@ def test_gain(self):

for system in systems:
context = system.CreateDefaultContext()
output = system.AllocateOutput(context)
output = system.AllocateOutput()

def mytest(input, expected):
context.FixInputPort(0, BasicVector(input))
Expand All @@ -197,7 +197,7 @@ def mytest(input, expected):
def test_saturation(self):
system = Saturation((0., -1., 3.), (1., 2., 4.))
context = system.CreateDefaultContext()
output = system.AllocateOutput(context)
output = system.AllocateOutput()

def mytest(input, expected):
context.FixInputPort(0, BasicVector(input))
Expand All @@ -212,7 +212,7 @@ def test_wrap_to_system(self):
system = WrapToSystem(2)
system.set_interval(1, 1., 2.)
context = system.CreateDefaultContext()
output = system.AllocateOutput(context)
output = system.AllocateOutput()

def mytest(input, expected):
context.FixInputPort(0, BasicVector(input))
Expand All @@ -238,7 +238,7 @@ def test_multiplexer(self):
port_size = sum([len(vec) for vec in case['data']])
self.assertEqual(mux.get_output_port(0).size(), port_size)
context = mux.CreateDefaultContext()
output = mux.AllocateOutput(context)
output = mux.AllocateOutput()
num_ports = len(case['data'])
self.assertEqual(context.get_num_input_ports(), num_ports)
for j, vec in enumerate(case['data']):
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/systems/test/rendering_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,7 @@ def test_pose_aggregator(self):

# - CalcOutput.
context = aggregator.CreateDefaultContext()
output = aggregator.AllocateOutput(context)
output = aggregator.AllocateOutput()

p1 = [0, 1, 2]
pose1 = PoseVector()
Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/systems/test/test_util_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -180,7 +180,7 @@ PYBIND11_MODULE(test_util, m) {
state.CopyToVector() + dt * state_dot.CopyToVector());
}
// Calculate output.
auto output = system.AllocateOutput(*context);
auto output = system.AllocateOutput();
system.CalcOutput(*context, output.get());
return output;
});
Expand Down
6 changes: 3 additions & 3 deletions bindings/pydrake/test/automotive_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -139,7 +139,7 @@ def test_pure_pursuit(self):
lane = rg.junction(0).segment(0).lane(0)
pure_pursuit = PurePursuitController()
context = pure_pursuit.CreateDefaultContext()
output = pure_pursuit.AllocateOutput(context)
output = pure_pursuit.AllocateOutput()

# Fix the inputs.
ld_value = framework.AbstractValue.Make(
Expand Down Expand Up @@ -189,7 +189,7 @@ def test_idm_controller(self):
road_position_strategy=RoadPositionStrategy.kExhaustiveSearch,
period_sec=0.)
context = idm.CreateDefaultContext()
output = idm.AllocateOutput(context)
output = idm.AllocateOutput()

# Fix the inputs.
pose_vector1 = PoseVector()
Expand Down Expand Up @@ -235,7 +235,7 @@ def test_simple_car(self):
simple_car = SimpleCar()
simulator = Simulator(simple_car)
context = simulator.get_mutable_context()
output = simple_car.AllocateOutput(context)
output = simple_car.AllocateOutput()

# Fix the input.
command = DrivingCommand()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@ void QpOutputTranslatorSystem::CalcActuationTorques(
// Output:
auto out_vector = output->get_mutable_value();

// TODO(sherm1) This computation should be cached so it will be evaluated
// when first requested and then available for re-use.
out_vector = robot_.B.transpose() * qp_output->dof_torques();

DRAKE_ASSERT(out_vector.size() == robot_.get_num_actuators());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,6 @@ class QpOutputTranslatorSystem : public systems::LeafSystem<double> {
* selection matrix that maps the actuator indices to the generalized
* coordinate indices.
*/
// TODO(sherm1) This should be cached so it doesn't need to be recomputed.
void CalcActuationTorques(const systems::Context<double>& context,
systems::BasicVector<double>* output) const;

Expand Down
10 changes: 5 additions & 5 deletions systems/framework/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -295,6 +295,7 @@ drake_cc_library(
srcs = ["output_port_value.cc"],
hdrs = ["output_port_value.h"],
deps = [
":framework_common",
":value",
":value_checker",
":vector",
Expand All @@ -310,7 +311,6 @@ drake_cc_library(
hdrs = ["context.h"],
deps = [
":context_base",
":output_port_value",
":parameters",
":state",
"//common:default_scalars",
Expand All @@ -324,15 +324,14 @@ drake_cc_library(
srcs = ["leaf_context.cc"],
hdrs = ["leaf_context.h"],
deps = [
":cache_and_dependency_tracker",
":context",
":parameters",
":vector",
"//common:default_scalars",
"//common:essential",
],
)

# TODO(sherm1) This should be "input_port".
drake_cc_library(
name = "input_port_descriptor",
srcs = [
Expand Down Expand Up @@ -381,6 +380,7 @@ drake_cc_library(
":event_collection",
":input_port_descriptor",
":output_port",
":output_port_value",
":system_base",
":system_constraint",
":system_scalar_converter",
Expand All @@ -400,7 +400,7 @@ drake_cc_library(
srcs = ["leaf_output_port.cc"],
hdrs = ["leaf_output_port.h"],
deps = [
":framework_common",
":leaf_context",
":output_port",
":value",
":vector",
Expand Down Expand Up @@ -600,7 +600,6 @@ drake_cc_googletest(
drake_cc_googletest(
name = "cache_test",
deps = [
":cache_and_dependency_tracker",
":context_base",
"//common:essential",
"//systems/framework/test_utilities",
Expand Down Expand Up @@ -782,6 +781,7 @@ drake_cc_googletest(
deps = [
":output_port_value",
"//common:essential",
"//systems/framework/test_utilities",
],
)

Expand Down
28 changes: 19 additions & 9 deletions systems/framework/cache.cc
Original file line number Diff line number Diff line change
Expand Up @@ -79,19 +79,29 @@ CacheEntryValue& Cache::CreateNewCacheEntryValue(
nullptr /* no value yet */));
CacheEntryValue& value = *store_[index];

// Allocate a DependencyTracker for this cache entry. Note that a pointer
// to the new CacheEntryValue is retained so must have a lifetime matching
// the tracker. That requires that the Cache and DependencyGraph are contained
// in the same Context.
DependencyTracker& tracker = trackers->CreateNewDependencyTracker(
ticket,
"cache " + description,
&value);
// If no well-known DependencyTracker has been created for this cache entry,
// create a new one.
DependencyTracker* tracker{};
if (trackers->has_tracker(ticket)) {
// This should only have been done for built-in trackers.
DRAKE_DEMAND(ticket < internal::kNextAvailableTicket);
tracker = &trackers->get_mutable_tracker(ticket);
tracker->set_cache_entry_value(&value);
} else {
// Allocate a DependencyTracker for this cache entry. Note that a pointer
// to the new CacheEntryValue is retained so must have a lifetime matching
// the tracker. That requires that the Cache and DependencyGraph are
// contained in the same Context.
tracker = &trackers->CreateNewDependencyTracker(
ticket,
"cache " + description,
&value);
}

// Subscribe to prerequisites (trackers must already exist).
for (auto prereq : prerequisites) {
auto& prereq_tracker = trackers->get_mutable_tracker(prereq);
tracker.SubscribeToPrerequisite(&prereq_tracker);
tracker->SubscribeToPrerequisite(&prereq_tracker);
}
return value;
}
Expand Down
Loading

0 comments on commit 5522b48

Please sign in to comment.