Skip to content

Commit

Permalink
primitives: deprecate SignalLogger and friends
Browse files Browse the repository at this point in the history
Closes: RobotLocomotion#10228

This is step 4 of 4 to deprecate and replace SignalLogger and SignalLog
with a logging system that avoids threading hazards (SignalLogger) and
slow memory management (SignalLog).

The patch deprecates SignalLog, SignalLogger, and LogOutput, and
replaces them within Drake by VectorLog, VectorLogSink and
LogVectorOutput, respectively.
  • Loading branch information
rpoyner-tri committed Aug 10, 2021
1 parent 4b8680b commit 5217083
Show file tree
Hide file tree
Showing 31 changed files with 233 additions and 183 deletions.
1 change: 1 addition & 0 deletions bindings/pydrake/systems/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ drake_pybind_library(
"//bindings/pydrake:documentation_pybind",
"//bindings/pydrake/common:cpp_template_pybind",
"//bindings/pydrake/common:default_scalars_pybind",
"//bindings/pydrake/common:deprecation_pybind",
],
cc_srcs = ["primitives_py.cc"],
package_info = PACKAGE_INFO,
Expand Down
21 changes: 14 additions & 7 deletions bindings/pydrake/systems/primitives_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

#include "drake/bindings/pydrake/common/cpp_template_pybind.h"
#include "drake/bindings/pydrake/common/default_scalars_pybind.h"
#include "drake/bindings/pydrake/common/deprecation_pybind.h"
#include "drake/bindings/pydrake/documentation_pybind.h"
#include "drake/bindings/pydrake/pydrake_pybind.h"
#include "drake/systems/primitives/adder.h"
Expand Down Expand Up @@ -230,10 +231,14 @@ PYBIND11_MODULE(primitives, m) {
py::arg("min_value"), py::arg("max_value"),
doc.Saturation.ctor.doc_2args);

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
DefineTemplateClassWithDefault<SignalLogger<T>, LeafSystem<T>>(
m, "SignalLogger", GetPyParam<T>(), doc.SignalLogger.doc)
.def(py::init<int, int>(), py::arg("input_size"),
py::arg("batch_allocation_size") = 1000, doc.SignalLogger.ctor.doc)
m, "SignalLogger", GetPyParam<T>(), doc.SignalLogger.doc_deprecated)
.def(py_init_deprecated<SignalLogger<T>, int, int>(
doc.SignalLogger.doc_deprecated),
py::arg("input_size"), py::arg("batch_allocation_size") = 1000,
doc.SignalLogger.ctor.doc)
.def("set_publish_period", &SignalLogger<T>::set_publish_period,
py::arg("period"), doc.SignalLogger.set_publish_period.doc)
.def("set_forced_publish_only",
Expand All @@ -256,12 +261,14 @@ PYBIND11_MODULE(primitives, m) {
return_value_policy_for_scalar_type<T>(), doc.SignalLogger.data.doc)
.def("reset", &SignalLogger<T>::reset, doc.SignalLogger.reset.doc);

AddTemplateFunction(m, "LogOutput", &LogOutput<T>, GetPyParam<T>(),
py::arg("src"), py::arg("builder"),
AddTemplateFunction(m, "LogOutput",
WrapDeprecated(doc.LogOutput.doc_deprecated, &LogOutput<T>),
GetPyParam<T>(), py::arg("src"), py::arg("builder"),
// Keep alive, ownership: `return` keeps `builder` alive.
py::keep_alive<0, 2>(),
// See #11531 for why `py_rvp::reference` is needed.
py_rvp::reference, doc.LogOutput.doc);
py_rvp::reference, doc.LogOutput.doc_deprecated);
#pragma GCC diagnostic pop

DefineTemplateClassWithDefault<StateInterpolatorWithDiscreteDerivative<T>,
Diagram<T>>(m, "StateInterpolatorWithDiscreteDerivative",
Expand Down Expand Up @@ -616,7 +623,7 @@ PYBIND11_MODULE(primitives, m) {
py::arg("threshold") = std::nullopt, doc.IsObservable.doc);

// TODO(eric.cousineau): Add more systems as needed.
}
} // NOLINT(readability/fn_size)

} // namespace pydrake
} // namespace drake
6 changes: 3 additions & 3 deletions bindings/pydrake/systems/pyplot_visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
from warnings import warn

from pydrake.systems.framework import LeafSystem, PublishEvent, TriggerType
from pydrake.systems.primitives import SignalLogger
from pydrake.systems.primitives import VectorLog
from pydrake.trajectories import Trajectory


Expand Down Expand Up @@ -116,7 +116,7 @@ def get_recording_as_animation(self, **kwargs):
def animate(self, log, resample=True, **kwargs):
"""
Args:
log: A reference to a pydrake.systems.primitives.SignalLogger
log: A reference to a pydrake.systems.primitives.VectorLog
or a pydrake.trajectories.Trajectory that contains the plant
state after running a simulation.
resample: Whether we should do a resampling operation to make the
Expand All @@ -125,7 +125,7 @@ def animate(self, log, resample=True, **kwargs):
matches the sample timestep of the log.
Additional kwargs are passed through to FuncAnimation.
"""
if isinstance(log, SignalLogger):
if isinstance(log, VectorLog):
t = log.sample_times()
x = log.data()

Expand Down
2 changes: 1 addition & 1 deletion bindings/pydrake/systems/test/lcm_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
from pydrake.lcm import DrakeLcm, Subscriber
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import BasicVector, DiagramBuilder, LeafSystem
from pydrake.systems.primitives import ConstantVectorSource, LogOutput
from pydrake.systems.primitives import ConstantVectorSource


# TODO(eric.cousieau): Move this to more generic code when another piece of
Expand Down
137 changes: 70 additions & 67 deletions bindings/pydrake/systems/test/primitives_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -113,74 +113,77 @@ def test_instantiations(self):

@numpy_compare.check_nonsymbolic_types
def test_signal_logger(self, T):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder_[T]()
kValue = T(2.4)
source = builder.AddSystem(ConstantVectorSource_[T]([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator_[T](kSize))
logger_per_step = builder.AddSystem(SignalLogger_[T](kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))

# Add a redundant logger via the helper method.
if T == float:
logger_per_step_2 = LogOutput(
integrator.get_output_port(0), builder
with catch_drake_warnings(expected_count=3):
# Log the output of a simple diagram containing a constant
# source and an integrator.
builder = DiagramBuilder_[T]()
kValue = T(2.4)
source = builder.AddSystem(ConstantVectorSource_[T]([kValue]))
kSize = 1
integrator = builder.AddSystem(Integrator_[T](kSize))
logger_per_step = builder.AddSystem(SignalLogger_[T](kSize))
builder.Connect(source.get_output_port(0),
integrator.get_input_port(0))
builder.Connect(integrator.get_output_port(0),
logger_per_step.get_input_port(0))

# Add a redundant logger via the helper method.
if T == float:
logger_per_step_2 = LogOutput(
integrator.get_output_port(0), builder
)
else:
logger_per_step_2 = LogOutput[T](
integrator.get_output_port(0), builder
)

# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger_[T](kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))

diagram = builder.Build()
simulator = Simulator_[T](diagram)
kTime = 1.
simulator.AdvanceTo(kTime)

# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()

self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
numpy_compare.assert_allclose(
t[-1]*kValue, x[0, -1], atol=1e-15, rtol=0
)
else:
logger_per_step_2 = LogOutput[T](
integrator.get_output_port(0), builder
)

# Add a periodic logger
logger_periodic = builder.AddSystem(SignalLogger_[T](kSize))
kPeriod = 0.1
logger_periodic.set_publish_period(kPeriod)
builder.Connect(integrator.get_output_port(0),
logger_periodic.get_input_port(0))

diagram = builder.Build()
simulator = Simulator_[T](diagram)
kTime = 1.
simulator.AdvanceTo(kTime)

# Verify outputs of the every-step logger
t = logger_per_step.sample_times()
x = logger_per_step.data()

self.assertTrue(t.shape[0] > 2)
self.assertTrue(t.shape[0] == x.shape[1])
numpy_compare.assert_allclose(
t[-1]*kValue, x[0, -1], atol=1e-15, rtol=0
)
numpy_compare.assert_equal(x, logger_per_step_2.data())

# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)

logger_per_step.reset()

# Verify that t and x retain their values after systems are deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())
numpy_compare.assert_equal(x, logger_per_step_2.data())

# Verify outputs of the periodic logger
t = logger_periodic.sample_times()
x = logger_periodic.data()
# Should log exactly once every kPeriod, up to and including
# kTime.
self.assertTrue(t.shape[0] == np.floor(kTime / kPeriod) + 1.)

logger_per_step.reset()

# Verify that t and x retain their values after systems are
# deleted.
t_copy = t.copy()
x_copy = x.copy()
del builder
del integrator
del logger_periodic
del logger_per_step
del logger_per_step_2
del diagram
del simulator
del source
gc.collect()
self.assertTrue((t == t_copy).all())
self.assertTrue((x == x_copy).all())

def test_linear_affine_system(self):
# Just make sure linear system is spelled correctly.
Expand Down
6 changes: 3 additions & 3 deletions bindings/pydrake/systems/test/pyplot_visualizer_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import (
Context, DiagramBuilder, PortDataType, VectorSystem, kUseDefaultName)
from pydrake.systems.primitives import SignalLogger
from pydrake.systems.primitives import VectorLogSink
from pydrake.systems.pyplot_visualizer import PyPlotVisualizer
from pydrake.trajectories import PiecewisePolynomial

Expand Down Expand Up @@ -73,7 +73,7 @@ class TestPyplotVisualizer(unittest.TestCase):
def test_simple_visualizer(self):
builder = DiagramBuilder()
system = builder.AddSystem(SimpleContinuousTimeSystem())
logger = builder.AddSystem(SignalLogger(1))
logger = builder.AddSystem(VectorLogSink(1))
builder.Connect(system.get_output_port(0), logger.get_input_port(0))
visualizer = builder.AddSystem(TestVisualizer(1))
builder.Connect(system.get_output_port(0),
Expand All @@ -86,7 +86,7 @@ def test_simple_visualizer(self):
simulator = Simulator(diagram, context)
simulator.AdvanceTo(.1)

ani = visualizer.animate(logger, repeat=True)
ani = visualizer.animate(logger.FindLog(context), repeat=True)
self.assertIsInstance(ani, animation.FuncAnimation)

def test_trajectory(self):
Expand Down
4 changes: 2 additions & 2 deletions examples/acrobot/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@ drake_cc_binary(
"//examples/acrobot:spong_controller",
"//systems/analysis:simulator",
"//systems/framework:diagram_builder",
"//systems/primitives:signal_logger",
"//systems/primitives:vector_log_sink",
"@yaml_cpp",
],
)
Expand Down Expand Up @@ -225,7 +225,7 @@ drake_cc_binary(
"//systems/estimators:kalman_filter",
"//systems/framework:diagram",
"//systems/primitives:linear_system",
"//systems/primitives:signal_logger",
"//systems/primitives:vector_log_sink",
"//systems/sensors:rotary_encoders",
"@gflags",
],
Expand Down
20 changes: 11 additions & 9 deletions examples/acrobot/run_lqr_w_estimator.cc
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@
#include "drake/systems/framework/diagram.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/linear_system.h"
#include "drake/systems/primitives/signal_logger.h"
#include "drake/systems/primitives/vector_log_sink.h"
#include "drake/systems/sensors/rotary_encoders.h"

namespace drake {
Expand Down Expand Up @@ -95,9 +95,10 @@ int do_main() {
builder.Connect(controller->get_output_port(), observer->get_input_port(1));

// Log the true state and the estimated state.
auto x_logger = LogOutput(acrobot_w_encoder->get_output_port(1), &builder);
auto x_logger = LogVectorOutput(acrobot_w_encoder->get_output_port(1),
&builder);
x_logger->set_name("x_logger");
auto xhat_logger = LogOutput(observer->get_output_port(0), &builder);
auto xhat_logger = LogVectorOutput(observer->get_output_port(0), &builder);
xhat_logger->set_name("xhat_logger");

// Build the system/simulator.
Expand Down Expand Up @@ -131,22 +132,23 @@ int do_main() {
simulator.AdvanceTo(FLAGS_simulation_sec);

// Plot the results (launch call_python_client to see the plots).
const auto& x_log = x_logger->FindLog(simulator.get_context());
const auto& xhat_log = xhat_logger->FindLog(simulator.get_context());
using common::CallPython;
using common::ToPythonTuple;
CallPython("figure", 1);
CallPython("clf");
CallPython("plot", x_logger->sample_times(),
(x_logger->data().row(0).array() - M_PI)
CallPython("plot", x_log.sample_times(),
(x_log.data().row(0).array() - M_PI)
.matrix().transpose());
CallPython("plot", x_logger->sample_times(),
x_logger->data().row(1).transpose());
CallPython("plot", x_log.sample_times(), x_log.data().row(1).transpose());
CallPython("legend", ToPythonTuple("theta1 - PI", "theta2"));
CallPython("axis", "tight");

CallPython("figure", 2);
CallPython("clf");
CallPython("plot", x_logger->sample_times(),
(x_logger->data().array() - xhat_logger->data().array())
CallPython("plot", x_log.sample_times(),
(x_log.data().array() - xhat_log.data().array())
.matrix().transpose());
CallPython("ylabel", "error");
CallPython("legend", ToPythonTuple("theta1", "theta2", "theta1dot",
Expand Down
8 changes: 4 additions & 4 deletions examples/acrobot/spong_sim.cc
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
#include "drake/examples/acrobot/spong_controller.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/signal_logger.h"
#include "drake/systems/primitives/vector_log_sink.h"

using drake::examples::acrobot::AcrobotParams;
using drake::examples::acrobot::AcrobotPlant;
Expand Down Expand Up @@ -97,8 +97,8 @@ std::string Simulate(const YAML::Node& scenario_node) {

builder.Connect(plant->get_output_port(0), controller->get_input_port(0));
builder.Connect(controller->get_output_port(0), plant->get_input_port(0));
auto state_logger = LogOutput(plant->get_output_port(0), &builder);
state_logger->set_publish_period(scenario.tape_period);
auto state_logger = LogVectorOutput(plant->get_output_port(0), &builder,
scenario.tape_period);

auto diagram = builder.Build();
Simulator<double> simulator(*diagram);
Expand All @@ -120,7 +120,7 @@ std::string Simulate(const YAML::Node& scenario_node) {
simulator.AdvanceTo(scenario.t_final);

Output output;
output.x_tape = state_logger->data();
output.x_tape = state_logger->FindLog(simulator.get_context()).data();
drake::yaml::YamlWriteArchive writer;
writer.Accept(output);
// The EmitString call below saves a document like so:
Expand Down
8 changes: 4 additions & 4 deletions examples/acrobot/spong_sim.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
)
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder
from pydrake.systems.primitives import LogOutput
from pydrake.systems.primitives import LogVectorOutput

from drake.examples.acrobot.acrobot_io import load_scenario, save_output

Expand All @@ -28,8 +28,8 @@ def simulate(*, initial_state, controller_params, t_final, tape_period):

builder.Connect(plant.get_output_port(0), controller.get_input_port(0))
builder.Connect(controller.get_output_port(0), plant.get_input_port(0))
state_logger = LogOutput(plant.get_output_port(0), builder)
state_logger.set_publish_period(tape_period)
state_logger = LogVectorOutput(plant.get_output_port(0), builder,
tape_period)

diagram = builder.Build()
simulator = Simulator(diagram)
Expand All @@ -44,7 +44,7 @@ def simulate(*, initial_state, controller_params, t_final, tape_period):

simulator.AdvanceTo(t_final)

x_tape = state_logger.data()
x_tape = state_logger.FindLog(context).data()
return x_tape


Expand Down
Loading

0 comments on commit 5217083

Please sign in to comment.