Merge pull request RobotLocomotion#12535 from mpetersen94/fix_11341/s…

…ystems

systems: Change context.FixInputPort() to port.FixValue()

systems/analysis/test/lyapunov_test.cc

@@ -71,7 +71,7 @@ VectorX<T> pendulum_bases(const VectorX<T>& x) {
 GTEST_TEST(LyapunovTest, PendulumSampleBasedLyapunov) {
   examples::pendulum::PendulumPlant<double> 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);

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);

systems/controllers/linear_model_predictive_controller.h

@@ -60,7 +60,8 @@ class LinearModelPredictiveController : public LeafSystem<T> {
   /// 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<systems::System<double>> model,
       std::unique_ptr<systems::Context<double>> base_context,

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<BasicVector<double>>()[0];
     EXPECT_EQ(y, (x < 0) - (x > 0));  // implements -sgn(x).

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<BasicVector<double>>(
-      robot_plant.num_positions() + robot_plant.num_velocities());
-  state_input->get_mutable_value() << q, v;
-
-  auto reference_state_input = std::make_unique<BasicVector<double>>(
-      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<BasicVector<double>>(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<double> state_input(robot_plant.num_positions() +
+                              robot_plant.num_velocities());
+  state_input << q, v;
+
+  VectorX<double> reference_state_input(robot_plant.num_positions() +
+                                        robot_plant.num_velocities());
+  reference_state_input << q_r, v_r;
+
+  VectorX<double> 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<double> q_int(dim);

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<BasicVector<double>>(
-        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<BasicVector<double>>(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.

systems/controllers/test/linear_model_predictive_controller_test.cc

@@ -39,7 +39,7 @@ class TestMpcWithDoubleIntegrator : public ::testing::Test {
 
     std::unique_ptr<Context<double>> 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<double>(
@@ -72,7 +72,7 @@ TEST_F(TestMpcWithDoubleIntegrator, TestAgainstInfiniteHorizonSolution) {
   const Eigen::Matrix<double, 2, 1> x0 = Eigen::Vector2d::Ones();
 
   auto context = dut_->CreateDefaultContext();
-  context->FixInputPort(0, BasicVector<double>::Make(x0(0), x0(1)));
+  dut_->get_input_port(0).FixValue(context.get(), x0);
   std::unique_ptr<SystemOutput<double>> 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<double>& x =
@@ -239,7 +239,7 @@ GTEST_TEST(TestMpcConstructor, ThrowIfRNotStrictlyPositiveDefinite) {
       std::make_unique<LinearSystem<double>>(A, B, C, D, 1.);
 
   std::unique_ptr<Context<double>> context = system->CreateDefaultContext();
-  context->FixInputPort(0, BasicVector<double>::Make(0., 0.));
+  system->get_input_port().FixValue(context.get(), Eigen::Vector2d::Zero());
 
   const Eigen::Matrix2d Q = Eigen::Matrix2d::Identity();
 

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 {

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<BasicVector<double>>(port_size_ * 2);
-    vec0->get_mutable_value().setZero();
-    context_->FixInputPort(0, std::move(vec0));
+    VectorX<double> vec0 = VectorX<double>::Zero(port_size_ * 2);
+    controller_.get_input_port(0).FixValue(context_.get(), vec0);
 
     // Desired state:
-    auto vec1 = std::make_unique<BasicVector<double>>(port_size_ * 2);
-    vec1->get_mutable_value() << error_signal_, error_rate_signal_;
-    context_->FixInputPort(1, std::move(vec1));
+    VectorX<double> 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<double> integral = VectorX<double>::Random(num_controlled_q);
 
   // State:
-  context->FixInputPort(0, std::make_unique<BasicVector<double>>(x));
+  dut.get_input_port(0).FixValue(context.get(), x);
   // Desired state:
-  context->FixInputPort(1, std::make_unique<BasicVector<double>>(x_d));
+  dut.get_input_port(1).FixValue(context.get(), x_d);
   // Integral:
   dut.set_integral_value(context.get(), integral);
 

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<LinearSystem<double>>(A, B, C, D);
   auto context = sys->CreateDefaultContext();
-  context->FixInputPort(0, Eigen::Matrix<double, 1, 1>::Zero());
+  sys->get_input_port().FixValue(context.get(), 0.0);
   context->get_mutable_continuous_state().SetFromVector(
       Eigen::Vector2d::Zero());
   auto filter =

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());

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<double>* context) {
+              const InputPort<double>& input_port, Context<double>* 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<lcmt_drake_signal>(msg));
+  input_port.FixValue(context, msg);
 }
 
 class DummySys : public LeafSystem<double> {
@@ -92,26 +92,26 @@ void GenerateLog() {
   auto pub1 = LcmPublisherSystem::Make<lcmt_drake_signal>("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);
 }
 

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<BasicVector<double>>(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());

systems/primitives/linear_system.cc

@@ -148,10 +148,8 @@ std::unique_ptr<AffineSystem<double>> DoFirstOrderTaylorApproximation(
 
   auto autodiff_args = math::initializeAutoDiffTuple(x0, u0);
   if (input_port) {
-    auto input_vector = std::make_unique<BasicVector<AutoDiffXd>>(num_inputs);
-    input_vector->SetFromVector(std::get<1>(autodiff_args));
-    autodiff_context->FixInputPort(input_port->get_index(),
-                                   std::move(input_vector));
+    VectorX<AutoDiffXd> 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);

systems/primitives/linear_system.h

@@ -180,7 +180,7 @@ class TimeVaryingLinearSystem : public TimeVaryingAffineSystem<T> {
 /// @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

systems/primitives/test/adder_test.cc

@@ -21,14 +21,14 @@ class AdderTest : public ::testing::Test {
   void SetUp() override {
     adder_.reset(new Adder<double>(2 /* inputs */, 3 /* size */));
     context_ = adder_->CreateDefaultContext();
-    input0_.reset(new BasicVector<double>(3 /* size */));
-    input1_.reset(new BasicVector<double>(3 /* size */));
+    input0_ = Eigen::VectorXd(3 /* size */);
+    input1_ = Eigen::VectorXd(3 /* size */);
   }
 
   std::unique_ptr<Adder<double>> adder_;
   std::unique_ptr<Context<double>> context_;
-  std::unique_ptr<BasicVector<double>> input0_;
-  std::unique_ptr<BasicVector<double>> 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_));

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<double>::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<double>::Make(42.0));
+  sys.get_input_port().FixValue(context.get(), 42.0);
 
   auto updates = sys.AllocateDiscreteVariables();
   sys.CalcDiscreteVariableUpdates(*context, updates.get());

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);