From 87da1336e10e3dcadfeb59cc6e1b242c26c9ae38 Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Mon, 16 Dec 2019 16:32:04 -0500 Subject: [PATCH 1/5] systems/controllers: Change context.FixInputPort() to port.FixValue() --- .../test/dynamic_programming_test.cc | 2 +- .../test/inverse_dynamics_controller_test.cc | 38 +++++++++---------- .../controllers/test/inverse_dynamics_test.cc | 18 +++------ ...linear_model_predictive_controller_test.cc | 8 ++-- .../test/linear_quadratic_regulator_test.cc | 2 +- .../controllers/test/pid_controller_test.cc | 15 ++++---- 6 files changed, 36 insertions(+), 47 deletions(-) diff --git a/systems/controllers/test/dynamic_programming_test.cc b/systems/controllers/test/dynamic_programming_test.cc index 5f7ce0a4da0b..77a2dbcd037b 100644 --- a/systems/controllers/test/dynamic_programming_test.cc +++ b/systems/controllers/test/dynamic_programming_test.cc @@ -54,7 +54,7 @@ GTEST_TEST(FittedValueIterationTest, SingleIntegrator) { auto context = policy->CreateDefaultContext(); auto output = policy->get_output_port().Allocate(); for (const double x : state_grid[0]) { - context->FixInputPort(0, Vector1d{x}); + policy->get_input_port().FixValue(context.get(), x); policy->get_output_port().Calc(*context, output.get()); double y = output->get_value>()[0]; EXPECT_EQ(y, (x < 0) - (x > 0)); // implements -sgn(x). diff --git a/systems/controllers/test/inverse_dynamics_controller_test.cc b/systems/controllers/test/inverse_dynamics_controller_test.cc index 46ff2af3a8b5..35a6ffe889b0 100644 --- a/systems/controllers/test/inverse_dynamics_controller_test.cc +++ b/systems/controllers/test/inverse_dynamics_controller_test.cc @@ -51,27 +51,23 @@ GTEST_TEST(InverseDynamicsControllerTest, TestTorque) { vd_r << 1, 2, 3, 4, 5, 6, 7; // Connects inputs. - auto state_input = std::make_unique>( - robot_plant.num_positions() + robot_plant.num_velocities()); - state_input->get_mutable_value() << q, v; - - auto reference_state_input = std::make_unique>( - robot_plant.num_positions() + robot_plant.num_velocities()); - reference_state_input->get_mutable_value() << q_r, v_r; - - auto reference_acceleration_input = - std::make_unique>(robot_plant.num_velocities()); - reference_acceleration_input->get_mutable_value() << vd_r; - - inverse_dynamics_context->FixInputPort( - dut->get_input_port_estimated_state().get_index(), - std::move(state_input)); - inverse_dynamics_context->FixInputPort( - dut->get_input_port_desired_state().get_index(), - std::move(reference_state_input)); - inverse_dynamics_context->FixInputPort( - dut->get_input_port_desired_acceleration().get_index(), - std::move(reference_acceleration_input)); + VectorX state_input(robot_plant.num_positions() + + robot_plant.num_velocities()); + state_input << q, v; + + VectorX reference_state_input(robot_plant.num_positions() + + robot_plant.num_velocities()); + reference_state_input << q_r, v_r; + + VectorX reference_acceleration_input(robot_plant.num_velocities()); + reference_acceleration_input << vd_r; + + dut->get_input_port_estimated_state().FixValue(inverse_dynamics_context.get(), + state_input); + dut->get_input_port_desired_state().FixValue(inverse_dynamics_context.get(), + reference_state_input); + dut->get_input_port_desired_acceleration().FixValue( + inverse_dynamics_context.get(), reference_acceleration_input); // Sets integrated position error. VectorX q_int(dim); diff --git a/systems/controllers/test/inverse_dynamics_test.cc b/systems/controllers/test/inverse_dynamics_test.cc index 24e7a3c5c843..ae85f9b9a424 100644 --- a/systems/controllers/test/inverse_dynamics_test.cc +++ b/systems/controllers/test/inverse_dynamics_test.cc @@ -76,20 +76,14 @@ class InverseDynamicsTest : public ::testing::Test { vd_d = acceleration_desired; } - auto state_input = make_unique>( - num_positions() + num_velocities()); - state_input->get_mutable_value() << position, velocity; - inverse_dynamics_context_->FixInputPort( - inverse_dynamics_->get_input_port_estimated_state().get_index(), - std::move(state_input)); + VectorXd state_input(num_positions() + num_velocities()); + state_input << position, velocity; + inverse_dynamics_->get_input_port_estimated_state().FixValue( + inverse_dynamics_context_.get(), state_input); if (!inverse_dynamics_->is_pure_gravity_compensation()) { - auto vd_input = - make_unique>(num_velocities()); - vd_input->get_mutable_value() << vd_d; - inverse_dynamics_context_->FixInputPort( - inverse_dynamics_->get_input_port_desired_acceleration().get_index(), - std::move(vd_input)); + inverse_dynamics_->get_input_port_desired_acceleration().FixValue( + inverse_dynamics_context_.get(), vd_d); } // Hook input of the expected size. diff --git a/systems/controllers/test/linear_model_predictive_controller_test.cc b/systems/controllers/test/linear_model_predictive_controller_test.cc index 4d7ea7a10a28..5a60495cac1d 100644 --- a/systems/controllers/test/linear_model_predictive_controller_test.cc +++ b/systems/controllers/test/linear_model_predictive_controller_test.cc @@ -39,7 +39,7 @@ class TestMpcWithDoubleIntegrator : public ::testing::Test { std::unique_ptr> system_context = system->CreateDefaultContext(); - system_context->FixInputPort(0, u0); + system->get_input_port().FixValue(system_context.get(), u0); system_context->get_mutable_discrete_state(0).SetFromVector(x0); dut_.reset(new LinearModelPredictiveController( @@ -72,7 +72,7 @@ TEST_F(TestMpcWithDoubleIntegrator, TestAgainstInfiniteHorizonSolution) { const Eigen::Matrix x0 = Eigen::Vector2d::Ones(); auto context = dut_->CreateDefaultContext(); - context->FixInputPort(0, BasicVector::Make(x0(0), x0(1))); + dut_->get_input_port(0).FixValue(context.get(), x0); std::unique_ptr> output = dut_->AllocateOutput(); dut_->CalcOutput(*context, output.get()); @@ -144,7 +144,7 @@ class TestMpcWithCubicSystem : public ::testing::Test { auto context = system_->CreateDefaultContext(); // Set nominal input to zero. - context->FixInputPort(0, Vector1d::Constant(0.)); + system_->get_input_port(0).FixValue(context.get(), 0.); // Set the nominal state. BasicVector& x = @@ -239,7 +239,7 @@ GTEST_TEST(TestMpcConstructor, ThrowIfRNotStrictlyPositiveDefinite) { std::make_unique>(A, B, C, D, 1.); std::unique_ptr> context = system->CreateDefaultContext(); - context->FixInputPort(0, BasicVector::Make(0., 0.)); + system->get_input_port().FixValue(context.get(), Eigen::Vector2d::Zero()); const Eigen::Matrix2d Q = Eigen::Matrix2d::Identity(); diff --git a/systems/controllers/test/linear_quadratic_regulator_test.cc b/systems/controllers/test/linear_quadratic_regulator_test.cc index 4eae6c1128b0..be1217c3b580 100644 --- a/systems/controllers/test/linear_quadratic_regulator_test.cc +++ b/systems/controllers/test/linear_quadratic_regulator_test.cc @@ -89,7 +89,7 @@ void TestLQRAffineSystemAgainstKnownSolution( Eigen::VectorXd x0 = Eigen::VectorXd::Zero(n); Eigen::VectorXd u0 = Eigen::VectorXd::Zero(m); - context->FixInputPort(0, u0); + sys.get_input_port().FixValue(context.get(), u0); if (sys.time_period() == 0.0) { context->get_mutable_continuous_state().SetFromVector(x0); } else { diff --git a/systems/controllers/test/pid_controller_test.cc b/systems/controllers/test/pid_controller_test.cc index 61831f3e071f..c582d7e01de6 100644 --- a/systems/controllers/test/pid_controller_test.cc +++ b/systems/controllers/test/pid_controller_test.cc @@ -28,14 +28,13 @@ class PidControllerTest : public ::testing::Test { derivatives_ = controller_.AllocateTimeDerivatives(); // State: - auto vec0 = std::make_unique>(port_size_ * 2); - vec0->get_mutable_value().setZero(); - context_->FixInputPort(0, std::move(vec0)); + VectorX vec0 = VectorX::Zero(port_size_ * 2); + controller_.get_input_port(0).FixValue(context_.get(), vec0); // Desired state: - auto vec1 = std::make_unique>(port_size_ * 2); - vec1->get_mutable_value() << error_signal_, error_rate_signal_; - context_->FixInputPort(1, std::move(vec1)); + VectorX vec1(port_size_ * 2); + vec1 << error_signal_, error_rate_signal_; + controller_.get_input_port(1).FixValue(context_.get(), vec1); } const int port_size_{3}; @@ -224,9 +223,9 @@ GTEST_TEST(PidIOProjectionTest, Test) { VectorX integral = VectorX::Random(num_controlled_q); // State: - context->FixInputPort(0, std::make_unique>(x)); + dut.get_input_port(0).FixValue(context.get(), x); // Desired state: - context->FixInputPort(1, std::make_unique>(x_d)); + dut.get_input_port(1).FixValue(context.get(), x_d); // Integral: dut.set_integral_value(context.get(), integral); From ec9063579d8a537100e98c65db4ff377a29ad60c Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Mon, 16 Dec 2019 17:41:27 -0500 Subject: [PATCH 2/5] systems/primitives: Change context.FixInputPort() to port.FixValue() --- systems/primitives/linear_system.cc | 6 ++-- systems/primitives/linear_system.h | 2 +- systems/primitives/test/adder_test.cc | 16 +++++------ systems/primitives/test/affine_system_test.cc | 6 ++-- .../test/barycentric_system_test.cc | 4 +-- systems/primitives/test/demultiplexer_test.cc | 13 ++------- .../test/discrete_derivative_test.cc | 3 +- .../test/discrete_time_delay_test.cc | 4 +-- .../test/first_order_low_pass_filter_test.cc | 5 ++-- .../primitives/test/gain_scalartype_test.cc | 8 ++---- systems/primitives/test/gain_test.cc | 6 +--- systems/primitives/test/linear_system_test.cc | 28 +++++++++---------- systems/primitives/test/multiplexer_test.cc | 11 ++++---- .../test/pass_through_scalartype_test.cc | 5 +--- systems/primitives/test/pass_through_test.cc | 7 ++--- systems/primitives/test/sine_test.cc | 9 ++---- .../test/symbolic_vector_system_test.cc | 11 ++++---- .../test/trajectory_affine_system_test.cc | 8 ++---- .../test/trajectory_linear_system_test.cc | 8 ++---- .../primitives/test/wrap_to_system_test.cc | 2 +- 20 files changed, 66 insertions(+), 96 deletions(-) diff --git a/systems/primitives/linear_system.cc b/systems/primitives/linear_system.cc index 3a3d62e0aae2..1f8954869c87 100644 --- a/systems/primitives/linear_system.cc +++ b/systems/primitives/linear_system.cc @@ -148,10 +148,8 @@ std::unique_ptr> DoFirstOrderTaylorApproximation( auto autodiff_args = math::initializeAutoDiffTuple(x0, u0); if (input_port) { - auto input_vector = std::make_unique>(num_inputs); - input_vector->SetFromVector(std::get<1>(autodiff_args)); - autodiff_context->FixInputPort(input_port->get_index(), - std::move(input_vector)); + VectorX input_vector = std::get<1>(autodiff_args); + input_port->FixValue(autodiff_context.get(), input_vector); } Eigen::MatrixXd A(num_states, num_states), B(num_states, num_inputs); diff --git a/systems/primitives/linear_system.h b/systems/primitives/linear_system.h index 44d5555e353a..5bebdfe1b78d 100644 --- a/systems/primitives/linear_system.h +++ b/systems/primitives/linear_system.h @@ -180,7 +180,7 @@ class TimeVaryingLinearSystem : public TimeVaryingAffineSystem { /// @note All _vector_ inputs in the system must be connected, either to the /// output of some upstream System within a Diagram (e.g., if system is a /// reference to a subsystem in a Diagram), or to a constant value using, e.g. -/// `context->FixInputPort(0,default_input)`. Any _abstract_ inputs in the +/// `port.FixValue(context, default_input)`. Any _abstract_ inputs in the /// system must be unconnected (the port must be both optional and unused). /// /// @note The inputs, states, and outputs of the returned system are NOT the diff --git a/systems/primitives/test/adder_test.cc b/systems/primitives/test/adder_test.cc index 5a068d3a1b67..4731dd727215 100644 --- a/systems/primitives/test/adder_test.cc +++ b/systems/primitives/test/adder_test.cc @@ -21,14 +21,14 @@ class AdderTest : public ::testing::Test { void SetUp() override { adder_.reset(new Adder(2 /* inputs */, 3 /* size */)); context_ = adder_->CreateDefaultContext(); - input0_.reset(new BasicVector(3 /* size */)); - input1_.reset(new BasicVector(3 /* size */)); + input0_ = Eigen::VectorXd(3 /* size */); + input1_ = Eigen::VectorXd(3 /* size */); } std::unique_ptr> adder_; std::unique_ptr> context_; - std::unique_ptr> input0_; - std::unique_ptr> input1_; + Eigen::VectorXd input0_; + Eigen::VectorXd input1_; }; // Tests that the system exports the correct topology. @@ -52,10 +52,10 @@ TEST_F(AdderTest, Topology) { TEST_F(AdderTest, AddTwoVectors) { // Hook up two inputs of the expected size. ASSERT_EQ(2, context_->num_input_ports()); - input0_->get_mutable_value() << 1, 2, 3; - input1_->get_mutable_value() << 4, 5, 6; - context_->FixInputPort(0, std::move(input0_)); - context_->FixInputPort(1, std::move(input1_)); + input0_ << 1, 2, 3; + input1_ << 4, 5, 6; + adder_->get_input_port(0).FixValue(context_.get(), input0_); + adder_->get_input_port(1).FixValue(context_.get(), input1_); Eigen::Vector3d expected(5, 7, 9); EXPECT_EQ(expected, adder_->get_output_port().Eval(*context_)); diff --git a/systems/primitives/test/affine_system_test.cc b/systems/primitives/test/affine_system_test.cc index 4fadf51643ca..f5123e7a6a27 100644 --- a/systems/primitives/test/affine_system_test.cc +++ b/systems/primitives/test/affine_system_test.cc @@ -185,7 +185,7 @@ GTEST_TEST(DiscreteAffineSystemTest, DiscreteTime) { context->get_mutable_discrete_state(0).SetFromVector(x0); double u0 = 29; - context->FixInputPort(0, Vector1d::Constant(u0)); + system.get_input_port().FixValue(context.get(), u0); auto update = system.AllocateDiscreteVariables(); system.CalcDiscreteVariableUpdates(*context, update.get()); @@ -251,7 +251,7 @@ GTEST_TEST(SimpleTimeVaryingAffineSystemTest, EvalTest) { auto context = sys.CreateDefaultContext(); context->SetTime(t); context->get_mutable_continuous_state_vector().SetFromVector(x); - context->FixInputPort(0, BasicVector::Make(42.0)); + sys.get_input_port().FixValue(context.get(), 42.0); auto derivs = sys.AllocateTimeDerivatives(); sys.CalcTimeDerivatives(*context, derivs.get()); @@ -270,7 +270,7 @@ GTEST_TEST(SimpleTimeVaryingAffineSystemTest, DiscreteEvalTest) { auto context = sys.CreateDefaultContext(); context->SetTime(t); context->get_mutable_discrete_state().get_mutable_vector().SetFromVector(x); - context->FixInputPort(0, BasicVector::Make(42.0)); + sys.get_input_port().FixValue(context.get(), 42.0); auto updates = sys.AllocateDiscreteVariables(); sys.CalcDiscreteVariableUpdates(*context, updates.get()); diff --git a/systems/primitives/test/barycentric_system_test.cc b/systems/primitives/test/barycentric_system_test.cc index b0e11a8f7e6e..582ee67e032e 100644 --- a/systems/primitives/test/barycentric_system_test.cc +++ b/systems/primitives/test/barycentric_system_test.cc @@ -45,8 +45,8 @@ GTEST_TEST(BarycentricSystemTest, MatrixGain) { for (double x : {-1., -.5, .3, .7}) { for (double y : {-.24, .4, .8}) { test = Vector2d{x, y}; - gain_context->FixInputPort(0, test); - bary_context->FixInputPort(0, test); + gain.get_input_port().FixValue(gain_context.get(), test); + bary_sys.get_input_port().FixValue(bary_context.get(), test); const auto& gain_output = gain.get_output_port().Eval(*gain_context); const auto& bary_output = bary_sys.get_output_port().Eval(*bary_context); diff --git a/systems/primitives/test/demultiplexer_test.cc b/systems/primitives/test/demultiplexer_test.cc index 5377f53d1501..43d7b147b399 100644 --- a/systems/primitives/test/demultiplexer_test.cc +++ b/systems/primitives/test/demultiplexer_test.cc @@ -23,12 +23,10 @@ class DemultiplexerTest : public ::testing::Test { void SetUp() override { demux_ = make_unique>(3 /* size */); context_ = demux_->CreateDefaultContext(); - input_ = make_unique>(3 /* size */); } std::unique_ptr> demux_; std::unique_ptr> context_; - std::unique_ptr> input_; }; // Tests that the input signal gets demultiplexed into its individual @@ -39,10 +37,9 @@ TEST_F(DemultiplexerTest, DemultiplexVector) { ASSERT_EQ(1, context_->num_input_ports()); ASSERT_EQ(1, demux_->num_input_ports()); Eigen::Vector3d input_vector(1.0, 3.14, 2.18); - input_->get_mutable_value() << input_vector; // Hook input of the expected size. - context_->FixInputPort(0, std::move(input_)); + demux_->get_input_port(0).FixValue(context_.get(), input_vector); // The number of output ports must match the size of the input vector. ASSERT_EQ(input_vector.size(), demux_->num_output_ports()); @@ -66,7 +63,6 @@ GTEST_TEST(OutputSize, SizeDifferentFromOne) { auto demux = make_unique>( kInputSize /* size */, kOutputSize /* output_ports_size */); auto context = demux->CreateDefaultContext(); - auto input = make_unique>(kInputSize /* size */); // Checks that the number of input ports in the system and in the context // are consistent. @@ -74,10 +70,9 @@ GTEST_TEST(OutputSize, SizeDifferentFromOne) { ASSERT_EQ(1, demux->num_input_ports()); Eigen::VectorXd input_vector = Eigen::VectorXd::LinSpaced(kInputSize, 1.0, 6.0); - input->get_mutable_value() << input_vector; // Hook input of the expected size. - context->FixInputPort(0, std::move(input)); + demux->get_input_port(0).FixValue(context.get(), input_vector); // The number of output ports must equal the size of the input port divided // by the size of the output ports provided in the constructor, in this case @@ -109,7 +104,6 @@ GTEST_TEST(VectorizedOutputSize, SizeDifferentFromOne) { auto demux = make_unique>( kOutputPortsSizes /* output_ports_sizes */); auto context = demux->CreateDefaultContext(); - auto input = make_unique>(kInputSize /* size */); // Checks that the number of input ports in the system and in the context // are consistent. @@ -117,10 +111,9 @@ GTEST_TEST(VectorizedOutputSize, SizeDifferentFromOne) { ASSERT_EQ(1, demux->num_input_ports()); Eigen::VectorXd input_vector = Eigen::VectorXd::LinSpaced(kInputSize, 1.0, 6.0); - input->get_mutable_value() << input_vector; // Hook input of the expected size. - context->FixInputPort(0, std::move(input)); + demux->get_input_port(0).FixValue(context.get(), input_vector); // The number of output ports must equal the length of the vector // kOutputPortsSizes. diff --git a/systems/primitives/test/discrete_derivative_test.cc b/systems/primitives/test/discrete_derivative_test.cc index 134d60371476..c5b5ce5ad4cb 100644 --- a/systems/primitives/test/discrete_derivative_test.cc +++ b/systems/primitives/test/discrete_derivative_test.cc @@ -100,8 +100,7 @@ GTEST_TEST(StateInterpolatorWithDiscreteDerivativeTest, BasicTest) { auto context = position_to_state.CreateDefaultContext(); const Eigen::Vector2d current_position(0.643, 0.821); - context->FixInputPort(position_to_state.get_input_port().get_index(), - current_position); + position_to_state.get_input_port().FixValue(context.get(), current_position); // Use setter that zeros the initial velocity output: const Eigen::Vector2d last_position(0.123, 0.456); diff --git a/systems/primitives/test/discrete_time_delay_test.cc b/systems/primitives/test/discrete_time_delay_test.cc index 8e7cf7b6952b..276cd7197168 100644 --- a/systems/primitives/test/discrete_time_delay_test.cc +++ b/systems/primitives/test/discrete_time_delay_test.cc @@ -307,8 +307,8 @@ class SymbolicDiscreteTimeDelayTest : public ::testing::Test { // Initialize the context with symbolic variables. context_ = delay_->CreateDefaultContext(); - context_->FixInputPort( - 0, BasicVector::Make(symbolic::Variable("u0"))); + delay_->get_input_port().FixValue( + context_.get(), symbolic::Expression(symbolic::Variable("u0"))); auto& xd = context_->get_mutable_discrete_state(0); for (int ii = 0; ii < (kBuffer + 1); ii++) { xd[ii] = symbolic::Variable("x" + std::to_string(ii)); diff --git a/systems/primitives/test/first_order_low_pass_filter_test.cc b/systems/primitives/test/first_order_low_pass_filter_test.cc index 8c79342cf684..4fa7b4ac5ee8 100644 --- a/systems/primitives/test/first_order_low_pass_filter_test.cc +++ b/systems/primitives/test/first_order_low_pass_filter_test.cc @@ -68,7 +68,8 @@ TEST_F(FirstOrderLowPassFilterTest, Topology) { TEST_F(FirstOrderLowPassFilterTest, Output) { SetUpSingleTimeConstantFilter(); ASSERT_EQ(1, context_->num_input_ports()); - context_->FixInputPort(0, BasicVector::Make({1.0, 2.0, 3.0})); + filter_->get_input_port().FixValue(context_.get(), + Vector3(1.0, 2.0, 3.0)); ASSERT_EQ(1, filter_->num_output_ports()); @@ -85,7 +86,7 @@ TEST_F(FirstOrderLowPassFilterTest, Derivatives) { SetUpMultipleTimeConstantsFilter(); ASSERT_EQ(1, context_->num_input_ports()); Vector3 u({1.0, 2.0, 3.0}); // The input signal. - context_->FixInputPort(0, u); + filter_->get_input_port().FixValue(context_.get(), u); // Sets a more interesting (non-zero) state. Vector3 z_expected(-1.0, 2.0, 3.5); diff --git a/systems/primitives/test/gain_scalartype_test.cc b/systems/primitives/test/gain_scalartype_test.cc index 44ccdd29b57c..c1a6849047ab 100644 --- a/systems/primitives/test/gain_scalartype_test.cc +++ b/systems/primitives/test/gain_scalartype_test.cc @@ -41,7 +41,6 @@ GTEST_TEST(GainScalarTypeTest, AutoDiff) { const double kGain = 2.0; auto gain = make_unique>(kGain /* gain */, 3 /* size */); auto context = gain->CreateDefaultContext(); - auto input = make_unique>(3 /* size */); // Sets the input values. VectorX input_vector(3); @@ -52,9 +51,7 @@ GTEST_TEST(GainScalarTypeTest, AutoDiff) { input_vector(1).derivatives() << 0, 0; // Constant input. input_vector(2).derivatives() << 0, 1; // Second independent variable. - input->get_mutable_value() << input_vector; - - context->FixInputPort(0, std::move(input)); + gain->get_input_port().FixValue(context.get(), input_vector); const auto& output_vector = gain->get_output_port().Eval(*context); @@ -101,10 +98,9 @@ TEST_F(SymbolicGainTest, VectorThroughGainSystem) { Eigen::Matrix input_vector( drake::symbolic::Expression{1.0}, drake::symbolic::Expression{3.14}, drake::symbolic::Expression{2.18}); - input_->get_mutable_value() << input_vector; // Hook input of the expected size. - context_->FixInputPort(0, move(input_)); + gain_->get_input_port(0).FixValue(context_.get(), input_vector); // Checks that the number of output ports in the Gain system and the // SystemOutput are consistent. diff --git a/systems/primitives/test/gain_test.cc b/systems/primitives/test/gain_test.cc index 2e00eca60c16..af33576dd157 100644 --- a/systems/primitives/test/gain_test.cc +++ b/systems/primitives/test/gain_test.cc @@ -27,18 +27,14 @@ void TestGainSystem(const Gain& gain_system, // Verifies that Gain allocates no state variables in the context. EXPECT_EQ(0, context->num_continuous_states()); - auto input = - make_unique>(gain_system.get_gain_vector().size()); // Checks that the number of input ports in the Gain system and the Context // are consistent. ASSERT_EQ(1, gain_system.num_input_ports()); ASSERT_EQ(1, context->num_input_ports()); - input->get_mutable_value() << input_vector; - // Hook input of the expected size. - context->FixInputPort(0, std::move(input)); + gain_system.get_input_port().FixValue(context.get(), input_vector); // Checks the output. ASSERT_EQ(1, gain_system.num_output_ports()); diff --git a/systems/primitives/test/linear_system_test.cc b/systems/primitives/test/linear_system_test.cc index f3f58c770278..2ee9e0e5f9f8 100644 --- a/systems/primitives/test/linear_system_test.cc +++ b/systems/primitives/test/linear_system_test.cc @@ -57,7 +57,7 @@ GTEST_TEST(LinearSystemTestWithEmptyPort, EmptyPort) { // when the empty vector port is unconnected. auto context = dut.CreateDefaultContext(); auto derivatives = dut.AllocateTimeDerivatives(); - context->FixInputPort(0, Vector1d(0)); + dut.get_input_port().FixValue(context.get(), 0.); DRAKE_EXPECT_NO_THROW(dut.CalcTimeDerivatives(*context, derivatives.get())); } @@ -229,7 +229,7 @@ class TestLinearizeFromAffine : public ::testing::Test { // A,B,C,D matrices. TEST_F(TestLinearizeFromAffine, ContinuousAtEquilibrium) { auto context = continuous_system_->CreateDefaultContext(); - context->FixInputPort(0, Vector1d::Constant(u0_)); + continuous_system_->get_input_port().FixValue(context.get(), u0_); // Set x0 to the actual equilibrium point. context->get_mutable_continuous_state_vector().SetFromVector( @@ -259,7 +259,7 @@ TEST_F(TestLinearizeFromAffine, ContinuousAtEquilibrium) { // not at equilibrium returns the original A,B,C,D matrices and affine terms. TEST_F(TestLinearizeFromAffine, ContinuousAtNonEquilibrium) { auto context = continuous_system_->CreateDefaultContext(); - context->FixInputPort(0, Vector1d::Constant(u0_)); + continuous_system_->get_input_port().FixValue(context.get(), u0_); context->get_mutable_continuous_state_vector().SetFromVector(x0_); // This Context is not an equilibrium point. @@ -287,7 +287,7 @@ TEST_F(TestLinearizeFromAffine, ContinuousAtNonEquilibrium) { TEST_F(TestLinearizeFromAffine, DiscreteAtEquilibrium) { auto context = discrete_system_->CreateDefaultContext(); - context->FixInputPort(0, Vector1d::Constant(u0_)); + discrete_system_->get_input_port().FixValue(context.get(), u0_); // Set x0 to the actual equilibrium point. systems::BasicVector& xd = @@ -319,7 +319,7 @@ TEST_F(TestLinearizeFromAffine, DiscreteAtEquilibrium) { // at equilibrium returns the original A,B,C,D matrices and affine terms. TEST_F(TestLinearizeFromAffine, DiscreteAtNonEquilibrium) { auto context = discrete_system_->CreateDefaultContext(); - context->FixInputPort(0, Vector1d::Constant(u0_)); + discrete_system_->get_input_port().FixValue(context.get(), u0_); systems::BasicVector& xd = context->get_mutable_discrete_state().get_mutable_vector(); xd.SetFromVector(x0_); @@ -423,11 +423,11 @@ GTEST_TEST(TestLinearize, LinearizingWithMixedInputs) { // Now check with the vector-valued input port connected but the abstract // input port not yet connected. - context->FixInputPort(0, Vector1(0.0)); + system.get_input_port(0).FixValue(context.get(), 0.0); DRAKE_EXPECT_NO_THROW(Linearize(system, *context)); // Now check with the abstract input port connected. - context->FixInputPort(1, Value>()); + system.get_input_port(1).FixValue(context.get(), std::vector()); DRAKE_EXPECT_NO_THROW(Linearize(system, *context)); } @@ -596,9 +596,9 @@ TEST_F(LinearSystemSymbolicTest, MakeLinearSystemException4) { TEST_F(LinearSystemTest, LinearizeSystemWithParameters) { examples::pendulum::PendulumPlant pendulum; auto context = pendulum.CreateDefaultContext(); - auto input = std::make_unique>(); - input->set_tau(0.0); - context->FixInputPort(0, std::move(input)); + auto input = examples::pendulum::PendulumInput(); + input.set_tau(0.0); + pendulum.get_input_port().FixValue(context.get(), input); examples::pendulum::PendulumState& state = dynamic_cast&>( context->get_mutable_continuous_state_vector()); @@ -644,7 +644,7 @@ GTEST_TEST(LinearizeTest, NoState) { // Check that I get back the original system (continuous time). const AffineSystem sys(A, B, f0, C, D, y0); auto context = sys.CreateDefaultContext(); - context->FixInputPort(0, Eigen::Vector2d{7, 8}); + sys.get_input_port().FixValue(context.get(), Eigen::Vector2d{7, 8}); const auto taylor_sys = FirstOrderTaylorApproximation(sys, *context); EXPECT_TRUE(CompareMatrices(taylor_sys->A(), A, tol)); @@ -657,7 +657,7 @@ GTEST_TEST(LinearizeTest, NoState) { // Check that I get back the original system (discrete time). const AffineSystem sys2(A, B, f0, C, D, y0, 0.1); auto context2 = sys2.CreateDefaultContext(); - context2->FixInputPort(0, Eigen::Vector2d{7, 8}); + sys2.get_input_port().FixValue(context2.get(), Eigen::Vector2d{7, 8}); const auto taylor_sys2 = FirstOrderTaylorApproximation(sys2, *context2); EXPECT_TRUE(CompareMatrices(taylor_sys2->A(), A, tol)); @@ -764,8 +764,8 @@ void TestMimo(bool is_discrete) { MimoSystem sys(is_discrete); auto context = sys.CreateDefaultContext(); // System is linear, so input and state values don't matter. - context->FixInputPort(0, Vector1d::Zero()); - context->FixInputPort(1, Eigen::Vector3d::Zero()); + sys.get_input_port(0).FixValue(context.get(), 0.0); + sys.get_input_port(1).FixValue(context.get(), Eigen::Vector3d::Zero()); const double tol = 1e-8; diff --git a/systems/primitives/test/multiplexer_test.cc b/systems/primitives/test/multiplexer_test.cc index fd7cd6c5e0f0..14831c242ea4 100644 --- a/systems/primitives/test/multiplexer_test.cc +++ b/systems/primitives/test/multiplexer_test.cc @@ -47,9 +47,10 @@ TEST_F(MultiplexerTest, Basic) { ASSERT_EQ(6, mux_->get_output_port(0).size()); // Provide input data. - context_->FixInputPort(0, BasicVector::Make({11.0, 22.0})); - context_->FixInputPort(1, BasicVector::Make({21.0})); - context_->FixInputPort(2, BasicVector::Make({31.0, 32.0, 33.0})); + mux_->get_input_port(0).FixValue(context_.get(), Eigen::Vector2d(11.0, 22.0)); + mux_->get_input_port(1).FixValue(context_.get(), 21.0); + mux_->get_input_port(2).FixValue(context_.get(), + Eigen::Vector3d(31.0, 32.0, 33.0)); // Confirm output data. const auto& value = mux_->get_output_port(0).Eval(*context_); @@ -89,8 +90,8 @@ TEST_F(MultiplexerTest, ModelVectorConstructor) { ASSERT_EQ(2, mux_->get_output_port(0).size()); // Confirm that the vector is truly MyVector2d. - context_->FixInputPort(0, {0.0}); - context_->FixInputPort(1, {0.0}); + mux_->get_input_port(0).FixValue(context_.get(), 0.0); + mux_->get_input_port(1).FixValue(context_.get(), 0.0); ASSERT_NO_THROW(mux_->get_output_port(0).Eval(*context_)); } diff --git a/systems/primitives/test/pass_through_scalartype_test.cc b/systems/primitives/test/pass_through_scalartype_test.cc index 2ded1b8edc3a..bad34578d1cd 100644 --- a/systems/primitives/test/pass_through_scalartype_test.cc +++ b/systems/primitives/test/pass_through_scalartype_test.cc @@ -29,7 +29,6 @@ GTEST_TEST(PassThroughScalarTypeTest, AutoDiff) { // Set a PassThrough system with input and output of size 3. auto buffer = make_unique>(3 /* size */); auto context = buffer->CreateDefaultContext(); - auto input = make_unique>(3 /* size */); // Sets the input values. Vector3 input_vector(1.0, 3.14, 2.18); @@ -39,9 +38,7 @@ GTEST_TEST(PassThroughScalarTypeTest, AutoDiff) { input_vector(1).derivatives() << 0, 1, 0; // Second independent variable. input_vector(2).derivatives() << 0, 0, 1; // Third independent variable. - input->get_mutable_value() << input_vector; - - context->FixInputPort(0, std::move(input)); + buffer->get_input_port().FixValue(context.get(), input_vector); // The expected output value equals the input. Vector3 expected; diff --git a/systems/primitives/test/pass_through_test.cc b/systems/primitives/test/pass_through_test.cc index b742653e7de0..8f8fcccb847d 100644 --- a/systems/primitives/test/pass_through_test.cc +++ b/systems/primitives/test/pass_through_test.cc @@ -66,11 +66,10 @@ TEST_P(PassThroughTest, VectorThroughPassThroughSystem) { // Hook input of the expected size. if (!is_abstract_) { - context_->FixInputPort( - 0, std::make_unique>(input_value_)); + pass_through_->get_input_port(0).FixValue(context_.get(), input_value_); } else { - context_->FixInputPort( - 0, AbstractValue::Make(SimpleAbstractType(input_value_))); + pass_through_->get_input_port(0).FixValue(context_.get(), + SimpleAbstractType(input_value_)); } // Checks that the number of output ports in the system and in the diff --git a/systems/primitives/test/sine_test.cc b/systems/primitives/test/sine_test.cc index a9ba3efae9dd..853778de94c1 100644 --- a/systems/primitives/test/sine_test.cc +++ b/systems/primitives/test/sine_test.cc @@ -63,9 +63,6 @@ void TestSineSystem(const Sine& sine_system, // Loop over the input vectors and check that the Sine system outputs match // the expected outputs. for (int i = 0; i < input_vectors.cols(); i++) { - auto input = - make_unique>( - sine_system.amplitude_vector().size()); ASSERT_EQ(1, sine_system.num_input_ports()); ASSERT_EQ(1, context->num_input_ports()); @@ -73,11 +70,11 @@ void TestSineSystem(const Sine& sine_system, // inputs (i.e., each column in the input_vectors matrix represents a // sampling instant. The number of rows in input_vectors should match the // size of the input port). - ASSERT_EQ(input->get_mutable_value().size(), input_vectors.rows()); + ASSERT_EQ(sine_system.amplitude_vector().size(), input_vectors.rows()); // Initialize the input and associate it with the context. - input->get_mutable_value() << input_vectors.col(i); - context->FixInputPort(0, std::move(input)); + sine_system.get_input_port(0).FixValue(context.get(), + input_vectors.col(i)); // Check the Sine output. ASSERT_EQ(3, sine_system.num_output_ports()); diff --git a/systems/primitives/test/symbolic_vector_system_test.cc b/systems/primitives/test/symbolic_vector_system_test.cc index 465a2ac995f4..1ef3648b03ca 100644 --- a/systems/primitives/test/symbolic_vector_system_test.cc +++ b/systems/primitives/test/symbolic_vector_system_test.cc @@ -214,7 +214,7 @@ TEST_F(SymbolicVectorSystemTest, ScalarPassThrough) { EXPECT_EQ(system.get_output_port().size(), 1); EXPECT_TRUE(system.HasDirectFeedthrough(0, 0)); - context->FixInputPort(0, Vector1d{0.12}); + system.get_input_port().FixValue(context.get(), 0.12); EXPECT_TRUE(CompareMatrices(system.get_output_port() .template Eval>(*context) .CopyToVector(), @@ -327,7 +327,7 @@ TEST_F(SymbolicVectorSystemTest, VectorPassThrough) { EXPECT_EQ(system.get_output_port().size(), 2); EXPECT_TRUE(system.HasDirectFeedthrough(0, 0)); - context->FixInputPort(0, Vector2d{0.12, 0.34}); + system.get_input_port().FixValue(context.get(), Vector2d{0.12, 0.34}); EXPECT_TRUE(CompareMatrices(system.get_output_port() .template Eval>(*context) .CopyToVector(), @@ -453,7 +453,7 @@ TEST_F(SymbolicVectorSystemTest, ContinuousTimeSymbolic) { context->SetTime(tc_); context->SetContinuousState(xc_); - context->FixInputPort(0, uc_); + system->get_input_port().FixValue(context.get(), uc_); context->get_mutable_numeric_parameter(0).SetFromVector(pc_); const auto& xdot = system->EvalTimeDerivatives(*context).get_vector(); @@ -598,7 +598,7 @@ TEST_F(SymbolicVectorSystemTest, DiscreteTimeSymbolic) { context->SetTime(tc_); context->get_mutable_discrete_state_vector().SetFromVector(xc_); - context->FixInputPort(0, uc_); + system->get_input_port().FixValue(context.get(), uc_); context->get_mutable_numeric_parameter(0).SetFromVector(pc_); auto discrete_variables = system->AllocateDiscreteVariables(); @@ -664,7 +664,8 @@ class SymbolicVectorSystemAutoDiffXdTest : public SymbolicVectorSystemTest { void SetContext(const VectorX& txup) { context_->SetTime(txup[0]); context_->SetContinuousState(txup.segment<1>(1)); - context_->FixInputPort(0, txup.segment<2>(2)); + autodiff_system_->get_input_port(0).FixValue(context_.get(), + txup.segment<2>(2)); context_->get_mutable_numeric_parameter(0).SetFromVector(txup.tail<2>()); } diff --git a/systems/primitives/test/trajectory_affine_system_test.cc b/systems/primitives/test/trajectory_affine_system_test.cc index c93b687d631e..4e978de4fd31 100644 --- a/systems/primitives/test/trajectory_affine_system_test.cc +++ b/systems/primitives/test/trajectory_affine_system_test.cc @@ -31,7 +31,6 @@ class TrajectoryAffineSystemTest data_.kDiscreteTimeStep); } context_ = dut_->CreateDefaultContext(); - input_vector_ = make_unique>(2 /* size */); continuous_state_ = &context_->get_mutable_continuous_state(); discrete_state_ = &context_->get_mutable_discrete_state(); derivatives_ = dut_->AllocateTimeDerivatives(); @@ -45,7 +44,6 @@ class TrajectoryAffineSystemTest ContinuousState* continuous_state_{nullptr}; DiscreteValues* discrete_state_{nullptr}; - unique_ptr> input_vector_; unique_ptr> derivatives_; unique_ptr> updates_; @@ -87,8 +85,7 @@ TEST_P(TrajectoryAffineSystemTest, KnotPointConsistency) { TEST_P(TrajectoryAffineSystemTest, DiscreteUpdates) { // Sets the context's input port. Eigen::Vector2d u(7, 42); - input_vector_->get_mutable_value() << u; - context_->FixInputPort(0, std::move(input_vector_)); + dut_->get_input_port().FixValue(context_.get(), u); EXPECT_NE(derivatives_, nullptr); EXPECT_NE(updates_, nullptr); @@ -123,8 +120,7 @@ TEST_P(TrajectoryAffineSystemTest, DiscreteUpdates) { TEST_P(TrajectoryAffineSystemTest, Output) { // Sets the context's input port. Eigen::Vector2d u(5.6, -10.1); - input_vector_->get_mutable_value() << u; - context_->FixInputPort(0, std::move(input_vector_)); + dut_->get_input_port().FixValue(context_.get(), u); // Sets the state. Eigen::Vector2d x(0.8, -22.1); diff --git a/systems/primitives/test/trajectory_linear_system_test.cc b/systems/primitives/test/trajectory_linear_system_test.cc index 38a6ebbc8f28..a35d1b739ace 100644 --- a/systems/primitives/test/trajectory_linear_system_test.cc +++ b/systems/primitives/test/trajectory_linear_system_test.cc @@ -30,7 +30,6 @@ class TrajectoryLinearSystemTest : public ::testing::TestWithParam { } context_ = dut_->CreateDefaultContext(); - input_vector_ = make_unique>(2 /* size */); continuous_state_ = &context_->get_mutable_continuous_state(); discrete_state_ = &context_->get_mutable_discrete_state(); derivatives_ = dut_->AllocateTimeDerivatives(); @@ -44,7 +43,6 @@ class TrajectoryLinearSystemTest : public ::testing::TestWithParam { ContinuousState* continuous_state_{nullptr}; DiscreteValues* discrete_state_{nullptr}; - unique_ptr> input_vector_; unique_ptr> derivatives_; unique_ptr> updates_; @@ -76,8 +74,7 @@ TEST_P(TrajectoryLinearSystemTest, KnotPointConsistency) { TEST_P(TrajectoryLinearSystemTest, Derivatives) { // Sets the context's input port. Eigen::Vector2d u(7, 42); - input_vector_->get_mutable_value() << u; - context_->FixInputPort(0, std::move(input_vector_)); + dut_->get_input_port().FixValue(context_.get(), u); EXPECT_NE(derivatives_, nullptr); EXPECT_NE(updates_, nullptr); @@ -114,8 +111,7 @@ TEST_P(TrajectoryLinearSystemTest, Derivatives) { TEST_P(TrajectoryLinearSystemTest, Output) { // Sets the context's input port. Eigen::Vector2d u(5.6, -10.1); - input_vector_->get_mutable_value() << u; - context_->FixInputPort(0, std::move(input_vector_)); + dut_->get_input_port().FixValue(context_.get(), u); // Sets the state. Eigen::Vector2d x(0.8, -22.1); diff --git a/systems/primitives/test/wrap_to_system_test.cc b/systems/primitives/test/wrap_to_system_test.cc index ad0e55b57e10..c33f6e51b1d9 100644 --- a/systems/primitives/test/wrap_to_system_test.cc +++ b/systems/primitives/test/wrap_to_system_test.cc @@ -19,7 +19,7 @@ void CheckOutput(const System& dut, const Eigen::Ref>& input, const Eigen::Ref>& expected_output) { auto context = dut.CreateDefaultContext(); - context->FixInputPort(0, input); + dut.get_input_port(0).FixValue(context.get(), input); EXPECT_TRUE(CompareMatrices(dut.get_output_port(0).Eval(*context), expected_output, 1e-12)); } From fb0e58123bad82955c118b9996702fd2ac8a3e56 Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Mon, 16 Dec 2019 18:11:07 -0500 Subject: [PATCH 3/5] systems/sensors: Change context.FixInputPort() to port.FixValue() --- .../sensors/test/image_to_lcm_image_array_t_test.cc | 13 +++---------- systems/sensors/test/image_writer_test.cc | 7 ++----- .../sensors/test/lcm_image_array_to_images_test.cc | 4 +--- systems/sensors/test/optitrack_sender_test.cc | 5 +---- systems/sensors/test/rotary_encoders_test.cc | 10 +++++----- 5 files changed, 12 insertions(+), 27 deletions(-) diff --git a/systems/sensors/test/image_to_lcm_image_array_t_test.cc b/systems/sensors/test/image_to_lcm_image_array_t_test.cc index 201afaa6f913..46539dff9c72 100644 --- a/systems/sensors/test/image_to_lcm_image_array_t_test.cc +++ b/systems/sensors/test/image_to_lcm_image_array_t_test.cc @@ -30,17 +30,10 @@ robotlocomotion::image_array_t SetUpInputAndOutput( const InputPort& label_image_input_port = dut->label_image_input_port(); - auto color_image_value = std::make_unique>(color_image); - auto depth_image_value = std::make_unique>(depth_image); - auto label_image_value = std::make_unique>(label_image); - std::unique_ptr> context = dut->CreateDefaultContext(); - context->FixInputPort(color_image_input_port.get_index(), - std::move(color_image_value)); - context->FixInputPort(depth_image_input_port.get_index(), - std::move(depth_image_value)); - context->FixInputPort(label_image_input_port.get_index(), - std::move(label_image_value)); + color_image_input_port.FixValue(context.get(), color_image); + depth_image_input_port.FixValue(context.get(), depth_image); + label_image_input_port.FixValue(context.get(), label_image); return dut->image_array_t_msg_output_port(). Eval(*context); diff --git a/systems/sensors/test/image_writer_test.cc b/systems/sensors/test/image_writer_test.cc index e1f10a5ba423..65b01d26debe 100644 --- a/systems/sensors/test/image_writer_test.cc +++ b/systems/sensors/test/image_writer_test.cc @@ -263,8 +263,7 @@ class ImageWriterTest : public ::testing::Test { port_name, path.string(), period, start_time); auto events = writer.AllocateCompositeEventCollection(); auto context = writer.AllocateContext(); - context->FixInputPort(port.get_index(), - AbstractValue::Make>(image)); + port.FixValue(context.get(), image); context->SetTime(0.); writer.CalcNextUpdateTime(*context, events.get()); @@ -622,9 +621,7 @@ TEST_F(ImageWriterTest, SingleConfiguredPort) { std::logic_error, ".*InputPort.* is not connected"); // Confirms that a valid publish increments the counter. - context->FixInputPort( - port.get_index(), - AbstractValue::Make(test_image())); + port.FixValue(context.get(), test_image()); const std::string expected_name = tester.MakeFileName( tester.port_format(port.get_index()), pixel_type, context->get_time(), diff --git a/systems/sensors/test/lcm_image_array_to_images_test.cc b/systems/sensors/test/lcm_image_array_to_images_test.cc index 4f810403279e..c6f1aa72cbde 100644 --- a/systems/sensors/test/lcm_image_array_to_images_test.cc +++ b/systems/sensors/test/lcm_image_array_to_images_test.cc @@ -37,9 +37,7 @@ void DecodeImageArray( LcmImageArrayToImages* dut, const image_array_t& lcm_images, ImageRgba8U* color_image, ImageDepth32F* depth_image) { std::unique_ptr> context = dut->CreateDefaultContext(); - auto input_value = std::make_unique>(lcm_images); - context->FixInputPort(dut->image_array_t_input_port().get_index(), - std::move(input_value)); + dut->image_array_t_input_port().FixValue(context.get(), lcm_images); *color_image = dut->color_image_output_port().Eval(*context); *depth_image = dut->depth_image_output_port().Eval(*context); diff --git a/systems/sensors/test/optitrack_sender_test.cc b/systems/sensors/test/optitrack_sender_test.cc index 353b530ef6f4..a78dd8ba4382 100644 --- a/systems/sensors/test/optitrack_sender_test.cc +++ b/systems/sensors/test/optitrack_sender_test.cc @@ -43,12 +43,9 @@ GTEST_TEST(OptitrackSenderTest, OptitrackLcmSenderTest) { EXPECT_EQ(pose_vector.value(frame_id).translation()[1], ty); EXPECT_EQ(pose_vector.value(frame_id).translation()[2], tz); - std::unique_ptr input( - new Value>(pose_vector)); - auto context = dut.CreateDefaultContext(); auto output = dut.AllocateOutput(); - context->FixInputPort(0 /* input port ID*/, std::move(input)); + dut.get_input_port(0).FixValue(context.get(), pose_vector); dut.CalcUnrestrictedUpdate(*context, &context->get_mutable_state()); dut.CalcOutput(*context, output.get()); diff --git a/systems/sensors/test/rotary_encoders_test.cc b/systems/sensors/test/rotary_encoders_test.cc index 3d2656deaed9..0b3e90b7298e 100644 --- a/systems/sensors/test/rotary_encoders_test.cc +++ b/systems/sensors/test/rotary_encoders_test.cc @@ -42,7 +42,7 @@ GTEST_TEST(TestEncoders, QuantizeOnly) { floor(angle(j) * ticks_per_radian(j)) / ticks_per_radian(j); } - context->FixInputPort(0, angle); + encoders.get_input_port().FixValue(context.get(), angle); encoders.CalcOutput(*context, output.get()); EXPECT_TRUE(CompareMatrices(desired_measurement, @@ -72,7 +72,7 @@ GTEST_TEST(TestEncoders, SelectorOnly) { angle = Eigen::Vector4d::Random(); desired_measurement = angle.segment(1, 2); - context->FixInputPort(0, angle); + encoders.get_input_port().FixValue(context.get(), angle); encoders.CalcOutput(*context, output.get()); EXPECT_TRUE(CompareMatrices(desired_measurement, @@ -104,7 +104,7 @@ GTEST_TEST(TestEncoders, QuantizationAndSelector) { srand(42); for (int i = 0; i < 10; i++) { angle = Eigen::Vector4d::Random(); - context->FixInputPort(0, angle); + encoders.get_input_port().FixValue(context.get(), angle); using std::floor; using std::ceil; @@ -147,7 +147,7 @@ GTEST_TEST(TestEncoders, CalibrationOffsets) { srand(42); for (int i = 0; i < 10; i++) { angle = Eigen::Vector2d::Random(); - context->FixInputPort(0, angle); + encoders.get_input_port().FixValue(context.get(), angle); angle -= offsets; using std::floor; @@ -188,7 +188,7 @@ GTEST_TEST(TestEncoders, ScalarConversion) { symbolic::Variable("u2"), symbolic::Variable("u3") }; - context->FixInputPort(0, input); + dut.get_input_port().FixValue(context.get(), input); // Obtain the symbolic outputs. auto outputs = dut.AllocateOutput(); From afcd04cff4044c8cf2fc25e3c964aba5dbc7327a Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Mon, 16 Dec 2019 18:11:30 -0500 Subject: [PATCH 4/5] systems/rendering: Change context.FixInputPort() to port.FixValue() --- ...ultibody_position_to_geometry_pose_test.cc | 2 +- .../rendering/test/pose_aggregator_test.cc | 51 ++++++++++--------- .../test/pose_bundle_to_draw_message_test.cc | 2 +- .../test/render_pose_to_geometry_pose_test.cc | 2 +- 4 files changed, 30 insertions(+), 27 deletions(-) diff --git a/systems/rendering/test/multibody_position_to_geometry_pose_test.cc b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc index 27785f5b50d9..88fb601ee909 100644 --- a/systems/rendering/test/multibody_position_to_geometry_pose_test.cc +++ b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc @@ -31,7 +31,7 @@ GTEST_TEST(MultibodyPositionToGeometryPoseTest, InputOutput) { const Eigen::VectorXd position = Eigen::VectorXd::LinSpaced(mbp.num_positions(), 0.123, 0.456); - context->FixInputPort(dut.get_input_port().get_index(), position); + dut.get_input_port().FixValue(context.get(), position); const auto& output = dut.get_output_port().Eval>(*context); diff --git a/systems/rendering/test/pose_aggregator_test.cc b/systems/rendering/test/pose_aggregator_test.cc index a66448b195ab..008b9562e412 100644 --- a/systems/rendering/test/pose_aggregator_test.cc +++ b/systems/rendering/test/pose_aggregator_test.cc @@ -91,32 +91,32 @@ TEST_F(PoseAggregatorTest, CompositeAggregation) { generic_input.set_name(1, "Mycroft"); generic_input.set_pose(1, Isometry3d(translation_1)); generic_input.set_model_instance_id(1, kMycroftModelInstanceId); - context_->FixInputPort(0, AbstractValue::Make(generic_input)); + aggregator_.get_input_port(0).FixValue(context_.get(), generic_input); // Set an arbitrary translation in the first PoseVector input. - auto pose_vec1 = std::make_unique>(); - pose_vec1->set_translation(Eigen::Translation(0.5, 1.5, 2.5)); + PoseVector pose_vec1 = PoseVector(); + pose_vec1.set_translation(Eigen::Translation(0.5, 1.5, 2.5)); // TODO(russt): Consider implementing ToAutoDiffXd for BasicVector? - auto autodiff_pose_vec1 = std::make_unique>(); - autodiff_pose_vec1->set_value( - pose_vec1->get_value().template cast()); - context_->FixInputPort(1, std::move(pose_vec1)); + PoseVector autodiff_pose_vec1 = PoseVector(); + autodiff_pose_vec1.set_value( + pose_vec1.get_value().template cast()); + aggregator_.get_input_port(1).FixValue(context_.get(), pose_vec1); // Set some numbers in the corresponding FrameVelocity input. - auto frame_vel1 = std::make_unique>(); - frame_vel1->get_mutable_value() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; - auto autodiff_frame_vel1 = std::make_unique>(); - autodiff_frame_vel1->set_value( - frame_vel1->get_value().template cast()); - context_->FixInputPort(2, std::move(frame_vel1)); + FrameVelocity frame_vel1 = FrameVelocity(); + frame_vel1.get_mutable_value() << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0; + FrameVelocity autodiff_frame_vel1 = FrameVelocity(); + autodiff_frame_vel1.set_value( + frame_vel1.get_value().template cast()); + aggregator_.get_input_port(2).FixValue(context_.get(), frame_vel1); // Set an arbitrary rotation in the second PoseVector input. - auto pose_vec2 = std::make_unique>(); - pose_vec2->set_rotation(Eigen::Quaternion(0.5, 0.5, 0.5, 0.5)); - auto autodiff_pose_vec2 = std::make_unique>(); - autodiff_pose_vec2->set_value( - pose_vec2->get_value().template cast()); - context_->FixInputPort(3, std::move(pose_vec2)); + PoseVector pose_vec2 = PoseVector(); + pose_vec2.set_rotation(Eigen::Quaternion(0.5, 0.5, 0.5, 0.5)); + PoseVector autodiff_pose_vec2 = PoseVector(); + autodiff_pose_vec2.set_value( + pose_vec2.get_value().template cast()); + aggregator_.get_input_port(3).FixValue(context_.get(), pose_vec2); aggregator_.CalcOutput(*context_, output_.get()); @@ -167,11 +167,14 @@ TEST_F(PoseAggregatorTest, CompositeAggregation) { auto autodiff_context = autodiff_aggregator->CreateDefaultContext(); autodiff_context->SetTimeStateAndParametersFrom(*context_); - autodiff_context->FixInputPort( - 0, AbstractValue::Make(ToAutoDiffXd(generic_input))); - autodiff_context->FixInputPort(1, std::move(autodiff_pose_vec1)); - autodiff_context->FixInputPort(2, std::move(autodiff_frame_vel1)); - autodiff_context->FixInputPort(3, std::move(autodiff_pose_vec2)); + autodiff_aggregator->get_input_port(0).FixValue(autodiff_context.get(), + ToAutoDiffXd(generic_input)); + autodiff_aggregator->get_input_port(1).FixValue(autodiff_context.get(), + autodiff_pose_vec1); + autodiff_aggregator->get_input_port(2).FixValue(autodiff_context.get(), + autodiff_frame_vel1); + autodiff_aggregator->get_input_port(3).FixValue(autodiff_context.get(), + autodiff_pose_vec2); auto autodiff_output = autodiff_aggregator->AllocateOutput(); autodiff_aggregator->CalcOutput(*autodiff_context, autodiff_output.get()); diff --git a/systems/rendering/test/pose_bundle_to_draw_message_test.cc b/systems/rendering/test/pose_bundle_to_draw_message_test.cc index 694427526d93..f5ed142a23d4 100644 --- a/systems/rendering/test/pose_bundle_to_draw_message_test.cc +++ b/systems/rendering/test/pose_bundle_to_draw_message_test.cc @@ -27,7 +27,7 @@ GTEST_TEST(PoseBundleToDrawMessageTest, Conversion) { PoseBundleToDrawMessage converter; auto context = converter.AllocateContext(); - context->FixInputPort(0, AbstractValue::Make(bundle)); + converter.get_input_port(0).FixValue(context.get(), bundle); auto output = converter.AllocateOutput(); converter.CalcOutput(*context, output.get()); diff --git a/systems/rendering/test/render_pose_to_geometry_pose_test.cc b/systems/rendering/test/render_pose_to_geometry_pose_test.cc index 400c045f1f61..cc3c991ef300 100644 --- a/systems/rendering/test/render_pose_to_geometry_pose_test.cc +++ b/systems/rendering/test/render_pose_to_geometry_pose_test.cc @@ -21,7 +21,7 @@ GTEST_TEST(RenderPoseToGeometryPoseTest, InputOutput) { const Eigen::Translation3d translation(1.0, 2.0, 3.0); const PoseVector input(rotation, translation); auto context = dut.CreateDefaultContext(); - context->FixInputPort(dut.get_input_port().get_index(), input); + dut.get_input_port().FixValue(context.get(), input); const auto& output = dut.get_output_port().Eval>( From 915990aca4c73a5424f3302649862e65fea8a2d3 Mon Sep 17 00:00:00 2001 From: mpetersen94 Date: Mon, 6 Jan 2020 13:25:09 -0500 Subject: [PATCH 5/5] systems/cleanup: Change context.FixInputPort() to port.FixValue() --- systems/analysis/test/lyapunov_test.cc | 2 +- systems/controllers/dynamic_programming.cc | 4 ++-- .../linear_model_predictive_controller.h | 3 ++- systems/estimators/test/kalman_filter_test.cc | 2 +- .../test/luenberger_observer_test.cc | 4 ++-- systems/lcm/test/lcm_log_playback_test.cc | 18 +++++++++--------- .../test/spring_mass_system_test.cc | 8 ++------ .../direct_collocation.cc | 10 ++++++---- .../direct_transcription.cc | 5 +++-- 9 files changed, 28 insertions(+), 28 deletions(-) diff --git a/systems/analysis/test/lyapunov_test.cc b/systems/analysis/test/lyapunov_test.cc index 38e0e73d60d3..7ef719d75104 100644 --- a/systems/analysis/test/lyapunov_test.cc +++ b/systems/analysis/test/lyapunov_test.cc @@ -71,7 +71,7 @@ VectorX pendulum_bases(const VectorX& x) { GTEST_TEST(LyapunovTest, PendulumSampleBasedLyapunov) { examples::pendulum::PendulumPlant pendulum; auto context = pendulum.CreateDefaultContext(); - context->FixInputPort(0, Vector1d{0.0}); + pendulum.get_input_port().FixValue(context.get(), 0.0); Eigen::VectorXd q_samples = Eigen::VectorXd::LinSpaced(31, -1.5 * M_PI, 1.5 * M_PI); diff --git a/systems/controllers/dynamic_programming.cc b/systems/controllers/dynamic_programming.cc index 676aa72f7132..ee2a4e4c96cc 100644 --- a/systems/controllers/dynamic_programming.cc +++ b/systems/controllers/dynamic_programming.cc @@ -86,7 +86,7 @@ FittedValueIteration( cost[input].resize(num_states); input_mesh.get_mesh_point(input, &input_vec); - context.FixInputPort(0, input_vec); + system.get_input_port(0).FixValue(&context, input_vec); for (int state = 0; state < num_states; state++) { context.SetTime(0.0); @@ -216,7 +216,7 @@ Eigen::VectorXd LinearProgrammingApproximateDynamicProgramming( Eigen::VectorXd state_vec(state_size); Eigen::VectorXd next_state_vec(state_size); for (int input = 0; input < num_inputs; input++) { - context.FixInputPort(0, input_samples.col(input)); + system.get_input_port(0).FixValue(&context, input_samples.col(input)); for (int state = 0; state < num_states; state++) { context.SetTime(0.0); state_vec = state_samples.col(state); diff --git a/systems/controllers/linear_model_predictive_controller.h b/systems/controllers/linear_model_predictive_controller.h index 3b94bdf9b9f7..e7bbe511b8a0 100644 --- a/systems/controllers/linear_model_predictive_controller.h +++ b/systems/controllers/linear_model_predictive_controller.h @@ -60,7 +60,8 @@ class LinearModelPredictiveController : public LeafSystem { /// of dimension num_inputs. /// @pre base_context must have discrete states set as appropriate for the /// given @p model. The input must also be initialized via - /// `base_context->FixInputPort(0, u0)`, or otherwise initialized via Diagram. + /// `input_port.FixValue(base_context, u0)`, or otherwise initialized via + /// Diagram. LinearModelPredictiveController( std::unique_ptr> model, std::unique_ptr> base_context, diff --git a/systems/estimators/test/kalman_filter_test.cc b/systems/estimators/test/kalman_filter_test.cc index 0d81d59bb25a..7529cea46ce4 100644 --- a/systems/estimators/test/kalman_filter_test.cc +++ b/systems/estimators/test/kalman_filter_test.cc @@ -47,7 +47,7 @@ GTEST_TEST(TestKalman, DoubleIntegrator) { // Should get the same result, but as an affine system. auto sys = std::make_unique>(A, B, C, D); auto context = sys->CreateDefaultContext(); - context->FixInputPort(0, Eigen::Matrix::Zero()); + sys->get_input_port().FixValue(context.get(), 0.0); context->get_mutable_continuous_state().SetFromVector( Eigen::Vector2d::Zero()); auto filter = diff --git a/systems/estimators/test/luenberger_observer_test.cc b/systems/estimators/test/luenberger_observer_test.cc index 302760c7881c..5f2cf7244bf5 100644 --- a/systems/estimators/test/luenberger_observer_test.cc +++ b/systems/estimators/test/luenberger_observer_test.cc @@ -62,8 +62,8 @@ GTEST_TEST(LuenbergerObserverTest, ErrorDynamics) { Eigen::Vector3d xhatdot = A * xhat + B * u + L * (y - C * xhat - D * u); - context->FixInputPort(0, y); - context->FixInputPort(1, u); + observer->get_input_port(0).FixValue(context.get(), y); + observer->get_input_port(1).FixValue(context.get(), u); context->get_mutable_continuous_state_vector().SetFromVector(xhat); observer->CalcTimeDerivatives(*context, derivatives.get()); diff --git a/systems/lcm/test/lcm_log_playback_test.cc b/systems/lcm/test/lcm_log_playback_test.cc index 1612c02d03ad..931a4b21be60 100644 --- a/systems/lcm/test/lcm_log_playback_test.cc +++ b/systems/lcm/test/lcm_log_playback_test.cc @@ -16,14 +16,14 @@ namespace { const int kDim = 1; void SetInput(double time, const std::string& name, double val, - Context* context) { + const InputPort& input_port, Context* context) { lcmt_drake_signal msg; msg.dim = kDim; msg.val.resize(kDim, val); msg.coord.resize(kDim, name); msg.timestamp = time * 1e6; context->SetTime(time); - context->FixInputPort(0, AbstractValue::Make(msg)); + input_port.FixValue(context, msg); } class DummySys : public LeafSystem { @@ -92,26 +92,26 @@ void GenerateLog() { auto pub1 = LcmPublisherSystem::Make("Ch1", &w_log); auto context1 = pub1->CreateDefaultContext(); - SetInput(0.1, "Ch0", 1, context0.get()); + SetInput(0.1, "Ch0", 1, pub0->get_input_port(), context0.get()); pub0->Publish(*context0); - SetInput(0.22, "Ch1", 2, context1.get()); + SetInput(0.22, "Ch1", 2, pub1->get_input_port(), context1.get()); pub1->Publish(*context1); // Testing multiple messages sent to the same channel at the same time. // Only the last one should be visible from the Subscriber's point of view. - SetInput(0.3, "Ch0", 3, context0.get()); + SetInput(0.3, "Ch0", 3, pub0->get_input_port(), context0.get()); pub0->Publish(*context0); - SetInput(0.3, "Ch0", 4, context0.get()); + SetInput(0.3, "Ch0", 4, pub0->get_input_port(), context0.get()); pub0->Publish(*context0); - SetInput(0.3, "Ch0", 5, context0.get()); + SetInput(0.3, "Ch0", 5, pub0->get_input_port(), context0.get()); pub0->Publish(*context0); // Testing sending a message to a different channel at the same time. - SetInput(0.3, "Ch1", 6, context1.get()); + SetInput(0.3, "Ch1", 6, pub1->get_input_port(), context1.get()); pub1->Publish(*context1); - SetInput(0.4, "Ch1", 7, context1.get()); + SetInput(0.4, "Ch1", 7, pub1->get_input_port(), context1.get()); pub1->Publish(*context1); } diff --git a/systems/plants/spring_mass_system/test/spring_mass_system_test.cc b/systems/plants/spring_mass_system/test/spring_mass_system_test.cc index 1eacf5f9c35d..314dd1b7313b 100644 --- a/systems/plants/spring_mass_system/test/spring_mass_system_test.cc +++ b/systems/plants/spring_mass_system/test/spring_mass_system_test.cc @@ -187,17 +187,13 @@ TEST_F(SpringMassSystemTest, DynamicsWithExternalForce) { // Asserts exactly one input for this case expecting an external force. ASSERT_EQ(1, context_->num_input_ports()); - // Creates a vector holding the data entry to the supplied input force. - auto force_vector = make_unique>(1 /* size */); - // Sets the input force. const double kExternalForce = 1.0; - force_vector->get_mutable_value() << kExternalForce; // Creates a free standing input port not actually connected to the output of - // another system but that has its own data in force_vector. + // another system but that has its own data. // This is done in order to be able to test this system standalone. - context_->FixInputPort(0, std::move(force_vector)); + system_->get_input_port(0).FixValue(context_.get(), kExternalForce); InitializeState(0.1, 0.1); // Displacement 0.1m, velocity 0.1m/sec. system_->CalcTimeDerivatives(*context_, system_derivatives_.get()); diff --git a/systems/trajectory_optimization/direct_collocation.cc b/systems/trajectory_optimization/direct_collocation.cc index 94bdca3121d3..d853eda91cbb 100644 --- a/systems/trajectory_optimization/direct_collocation.cc +++ b/systems/trajectory_optimization/direct_collocation.cc @@ -64,8 +64,9 @@ DirectCollocationConstraint::DirectCollocationConstraint( } // Provide a fixed value for the input port and keep an alias around. - input_port_value_ = &context_->FixInputPort( - input_port_->get_index(), system_->AllocateInputVector(*input_port_)); + input_port_value_ = &input_port_->FixValue( + context_.get(), + system_->AllocateInputVector(*input_port_)->get_value()); } } @@ -169,8 +170,9 @@ DirectCollocation::DirectCollocation( if (input_port_) { // Allocate the input port and keep an alias around. - input_port_value_ = &context_->FixInputPort( - input_port_->get_index(), system_->AllocateInputVector(*input_port_)); + input_port_value_ = &input_port_->FixValue( + context_.get(), + system_->AllocateInputVector(*input_port_)->get_value()); } // Add the dynamic constraints. diff --git a/systems/trajectory_optimization/direct_transcription.cc b/systems/trajectory_optimization/direct_transcription.cc index d600c237d13c..4d25bf273b67 100644 --- a/systems/trajectory_optimization/direct_transcription.cc +++ b/systems/trajectory_optimization/direct_transcription.cc @@ -330,8 +330,9 @@ void DirectTranscription::AddAutodiffDynamicConstraints( } // Provide a fixed value for the input port and keep an alias around. - input_port_value_ = &context_->FixInputPort( - input_port_->get_index(), system_->AllocateInputVector(*input_port_)); + input_port_value_ = &input_port_->FixValue( + context_.get(), + system_->AllocateInputVector(*input_port_)->get_value()); } // For N-1 timesteps, add a constraint which depends on the knot