Skip to content

Commit

Permalink
systems/cleanup: Change context.FixInputPort() to port.FixValue()
Browse files Browse the repository at this point in the history
  • Loading branch information
mpetersen94 committed Jan 6, 2020
1 parent afcd04c commit 915990a
Show file tree
Hide file tree
Showing 9 changed files with 28 additions and 28 deletions.
2 changes: 1 addition & 1 deletion systems/analysis/test/lyapunov_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions systems/controllers/dynamic_programming.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down
3 changes: 2 additions & 1 deletion systems/controllers/linear_model_predictive_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
2 changes: 1 addition & 1 deletion systems/estimators/test/kalman_filter_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down
4 changes: 2 additions & 2 deletions systems/estimators/test/luenberger_observer_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
18 changes: 9 additions & 9 deletions systems/lcm/test/lcm_log_playback_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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> {
Expand Down Expand Up @@ -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);
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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());
Expand Down
10 changes: 6 additions & 4 deletions systems/trajectory_optimization/direct_collocation.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}
}

Expand Down Expand Up @@ -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.
Expand Down
5 changes: 3 additions & 2 deletions systems/trajectory_optimization/direct_transcription.cc
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 915990a

Please sign in to comment.