Introduce a diagram that implements an IDM model with one external agent. #4081

jadecastro · 2016-11-08T17:17:08Z

This supplants IdmWithTrajectoryAgent.

This change is

jadecastro · 2016-11-08T20:33:13Z

+@david-german-tri for platform review, please.
+@jwnimmer-tri for feature review, please.
Thanks!

jwnimmer-tri · 2016-11-09T04:54:20Z

Partial checkpoint.

Reviewed 10 of 15 files at r1.
Review status: 10 of 15 files reviewed at latest revision, 19 unresolved discussions.

drake/automotive/CMakeLists.txt, line 17 at r3 (raw file):

  linear_car.cc
  idm_planner.cc
  single_lane_ego_and_agent.cc

Please alpha-sort (interleave) these entries within the list of sources.

drake/automotive/idm_planner.cc, line 11 at r3 (raw file):

#include "drake/common/drake_assert.h"
#include "drake/common/drake_export.h"
#include "drake/common/symbolic_expression.h"

Unused.

drake/automotive/idm_planner.cc, line 46 at r3 (raw file):

  // Obtain the input/output structures we need to read from and write into.
  const auto input_ego = this->EvalVectorInput(context, 0);
  const auto input_agent = this->EvalVectorInput(context, 1);

Prefer const auto& to const auto in almost all cases (including the prior two lines). Or maybe don't even use auto here, for clarity.

drake/automotive/idm_planner.cc, line 70 at r3 (raw file):

  // TODO(jadecastro): the below assertion forbids
  // symbolic::Expressions with this construction.

BTW If you #include "symbolic_formula.h it should compile okay.

drake/automotive/idm_planner.cc, line 82 at r3 (raw file):

                      (input_agent->GetAtIndex(0) - input_ego->GetAtIndex(0) -
                       l_a),
                  2.0)));

This is somewhat unreadable. Consider ways to make it moreso, such as capturing "input_foo->GetAtIndex" into named local variables, and/or making the line wrapping match the math better.

drake/automotive/idm_planner.cc, line 89 at r3 (raw file):

    const systems::SystemPortDescriptor<T>& descriptor) const {
  return std::make_unique<systems::BasicVector<T>>(1 /* output vector size */);
}

If I'm not mistaken, the parent class will already take care of this for us, without writing out this override?

drake/automotive/idm_planner.h, line 12 at r3 (raw file):

/// IDM: Intelligent Driver Model:
///    https://en.wikipedia.org/wiki/Intelligent_driver_model
///

Please elaborate on the input and output structure, somewhere in this class's APIs (overview and/or methods).

drake/automotive/idm_planner.h, line 23 at r3 (raw file):

class IdmPlanner : public systems::LeafSystem<T> {
 public:
  explicit IdmPlanner(const T& v_0);

Missing API comment on what v_0 means.

drake/automotive/idm_planner.h, line 32 at r3 (raw file):

  void EvalOutput(const systems::Context<T>& context,
                  systems::SystemOutput<T>* output) const override;

Missing disable of copy and assignment.

drake/automotive/linear_car.cc, line 48 at r3 (raw file):

  systems::BasicVector<T>* const output_vector =
      output->GetMutableVectorData(0);
  DRAKE_ASSERT(output_vector != nullptr);

The prior two lines seem like dead code?

drake/automotive/linear_car.cc, line 64 at r3 (raw file):

  const systems::VectorBase<T>& context_state =
      context.get_continuous_state_vector();
  auto input = this->EvalVectorInput(context, 0);

BTW Prefer const auto& for read-only values. (Or maybe don't even use auto here?)

drake/automotive/linear_car.cc, line 79 at r3 (raw file):

std::unique_ptr<systems::ContinuousState<T>>
LinearCar<T>::AllocateContinuousState() const {
  const int size = systems::System<T>::get_output_port(0).get_size();

const int size = get_output_port().get_size(); is shorter and clearer, I think?

drake/automotive/linear_car.cc, line 81 at r3 (raw file):

  const int size = systems::System<T>::get_output_port(0).get_size();
  return std::make_unique<systems::ContinuousState<T>>(
      std::make_unique<systems::BasicVector<T>>(size));

I think this instead should be the ContinuousState constructor overload that declares our second-order structure? (Passing , 1, 1, 0 for nq, nv, nz.)

drake/automotive/linear_car.cc, line 88 at r3 (raw file):

    const systems::SystemPortDescriptor<T>& descriptor) const {
  return std::make_unique<systems::BasicVector<T>>(2);
}

If I'm not mistaken, the parent class will already take care of this for us, without writing out this override?

drake/automotive/linear_car.cc, line 91 at r3 (raw file):

// These instantiations must match the API documentation in
// linear_car.h.

BTW I think this fits on the prior line?

drake/automotive/linear_car.h, line 41 at r2 (raw file):

      const systems::Context<T>& context,
      systems::ContinuousState<T>* derivatives) const override;

Missing disable of copy and assignment.

drake/automotive/linear_car.h, line 28 at r3 (raw file):

  bool has_any_direct_feedthrough() const override { return false; }

  /// Returns the input port.

Please elaborate on the input structure (either here, or class overview). (It's a single scalar acceleration.)

drake/automotive/linear_car.h, line 31 at r3 (raw file):

  const systems::SystemPortDescriptor<T>& get_input_port() const;

  /// Returns the output port.

Please elaborate on the output structure (either here, or class overview). (It's two scalars; first is x, second is v.)

drake/automotive/single_lane_ego_and_agent.h, line 62 at r3 (raw file):

  const LinearCar<T>* get_ego_car_system() { return ego_car_; }
  const LinearCar<T>* get_agent_car_system() { return agent_car_; }

Missing disable of copy and assignment.

Comments from Reviewable

jwnimmer-tri · 2016-11-09T15:05:27Z

Partial checkpoint.

Reviewed 2 of 15 files at r1.
Review status: 12 of 15 files reviewed at latest revision, 30 unresolved discussions, some commit checks failed.

a discussion (no related file):
What's the plan for idm_with_trajectory_agent? Remove it in an immediately-next PR?

drake/automotive/idm_planner.cc, line 10 at r3 (raw file):

#include "drake/automotive/gen/idm_planner_parameters.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_export.h"

META (for all classes in the PR)... in new code, do not #include "drake/common/drake_export.h" and do not mention DRAKE_EXPORT in the instantiations at the end of the file. They have no effect, and are being globally removed per #3973.

drake/automotive/single_lane_ego_and_agent.cc, line 13 at r3 (raw file):

    : systems::Diagram<T>() {
  // The reference velocity must be strictly positive.
  DRAKE_ASSERT(v_0 > 0);

BTW Consider DRAKE_DEMAND so that even release builds have this (cheap, only-once) check during construction.

drake/automotive/single_lane_ego_and_agent.cc, line 21 at r3 (raw file):

  ego_car_ = builder.AddSystem(std::make_unique<LinearCar<T>>());

  value_ = builder.AddSystem(std::make_unique<systems::ConstantVectorSource<T>>(

Unclear why value_ is a member field instead of a local variable.

drake/automotive/single_lane_ego_and_agent.cc, line 23 at r3 (raw file):

  value_ = builder.AddSystem(std::make_unique<systems::ConstantVectorSource<T>>(
      a_agent /* acceleration of the agent */));
  planner_ = builder.AddSystem(std::make_unique<IdmPlanner<T>>(

Unclear why planner_ is a member field instead of a local variable.

drake/automotive/single_lane_ego_and_agent.cc, line 28 at r3 (raw file):

  builder.Connect(*planner_, *ego_car_);
  builder.Connect(*value_, *agent_car_);
  builder.Connect(ego_car_->get_output_port(), planner_->get_input_port(0));

Magic number 0.

drake/automotive/single_lane_ego_and_agent.cc, line 29 at r3 (raw file):

  builder.Connect(*value_, *agent_car_);
  builder.Connect(ego_car_->get_output_port(), planner_->get_input_port(0));
  builder.Connect(agent_car_->get_output_port(), planner_->get_input_port(1));

Magic number 1.

drake/automotive/single_lane_ego_and_agent.cc, line 39 at r3 (raw file):

template <typename T>
bool SingleLaneEgoAndAgent<T>::has_any_direct_feedthrough() const {
  return false;

BTW Add a TODO that Diagram should be able to compute this, once the sparsity matrix is available?

drake/automotive/single_lane_ego_and_agent.h, line 42 at r3 (raw file):

/// They are already available to link against in libdrakeSystemFramework.
/// No other values for T are currently supported.
/// @ingroup primitive_systems

This seems like the wrong group for this System.

drake/automotive/single_lane_ego_and_agent.h, line 60 at r3 (raw file):

  /// Getters for the ego and agent car systems.
  const LinearCar<T>* get_ego_car_system() { return ego_car_; }

Missing object-const ... get_ego_car_system() const { ....
Also on the line below.

drake/automotive/single_lane_ego_and_agent.h, line 67 at r3 (raw file):

  LinearCar<T>* agent_car_ = nullptr;
  IdmPlanner<T>* planner_ = nullptr;
  systems::ConstantVectorSource<T>* value_ = nullptr;

If possible, add const prior to all of these lines (to make the types be const). (I think it should work.)

Comments from Reviewable

jwnimmer-tri · 2016-11-09T16:10:46Z

Partial checkpoint.

Reviewed 1 of 15 files at r1, 1 of 1 files at r2.
Review status: 14 of 15 files reviewed at latest revision, 40 unresolved discussions, some commit checks failed.

drake/automotive/test/idm_planner_test.cc, line 20 at r3 (raw file):

  }

  double get_v_0_() const { return v_0_; }

The trailing underscore in the method name here is weird, and possibly defective (though I didn't go read the cppguide to check for sure).

drake/automotive/test/idm_planner_test.cc, line 26 at r3 (raw file):

    auto input_ego = std::make_unique<systems::BasicVector<double>>(2);
    auto input_agent = std::make_unique<systems::BasicVector<double>>(2);
    input_ego->SetAtIndex(0, x_ego);      // x_ego

BTW I'm not sure these comments are adding much value.

drake/automotive/test/idm_planner_test.cc, line 33 at r3 (raw file):

                                  std::move(input_ego)));
    context_->SetInputPort(1, std::make_unique<systems::FreestandingInputPort>(
                                  std::move(input_agent)));

FYI #4041 offers SetInputPortToConstantValue sugar, which would make this method much more elegant. Perhaps keep an eye on when it merges and take advantage of it here?

drake/automotive/test/idm_planner_test.cc, line 44 at r3 (raw file):

};

TEST_F(IdmPlannerTest, Topology) {

Missing attribute-checking coverage of the second input port?

drake/automotive/test/idm_planner_test.cc, line 50 at r3 (raw file):

  EXPECT_EQ(systems::kInputPort, input_descriptor.get_face());
  // EXPECT_EQ(IdmPlannerInputIndices::kNumCoordinates,
  //          input_descriptor.get_size());

Cannot have commented-out code.

drake/automotive/test/idm_planner_test.cc, line 88 at r3 (raw file):

       pow((s_0 + v_e * time_headway + v_e * (v_e - v_a) / (2 * sqrt(a * b))) /
               (x_a - x_e - l_a),
           2.0));

Is repeating the formula here truly the best way to unit test? Would a literal value for expected_output (possibly with a few sets of input values) be superior?

drake/automotive/test/linear_car_test.cc, line 36 at r3 (raw file):

    return std::unique_ptr<systems::FreestandingInputPort>(
        new systems::FreestandingInputPort(std::move(data)));
  }

BTW Same thing on #4041 making this go away.

drake/automotive/test/linear_car_test.cc, line 69 at r3 (raw file):

  auto result = output_->get_vector_data(0);

  // Verify that the starting input is zero.

Typo "starting output" is zero, I think?

drake/automotive/test/linear_car_test.cc, line 73 at r3 (raw file):

  EXPECT_EQ(0.0, result->GetAtIndex(0));
  EXPECT_EQ(0.0, result->GetAtIndex(1));
}

I'm not sure what this Input test is trying to show that the subsequent Output test isn't already doing?

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 21 at r3 (raw file):

    std::unique_ptr<systems::BasicVector<T>> data) {
  return make_unique<systems::FreestandingInputPort>(std::move(data));
}

BTW Same thing on #4041 making this go away.

Comments from Reviewable

jwnimmer-tri · 2016-11-09T16:27:07Z

First pass done.

Reviewed 1 of 1 files at r3.
Review status: all files reviewed at latest revision, 44 unresolved discussions, some commit checks failed.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 45 at r3 (raw file):

  }

  // intialize the diagram with v_0_ego = 50, vdot_agent = 2.7

cappunc

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 58 at r3 (raw file):

  const auto& output_ego_car_pos = dut_.get_output_ports().at(0);
  const auto& output_agent_car_pos = dut_.get_output_ports().at(0);

One of the two prior lines has a magic number typo.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 144 at r3 (raw file):

  EXPECT_EQ(2.7, derivatives_agent->GetAtIndex(1));

  // Now verify behavior at a new set of parameters.

Not sure "parameters" is the best term to explain the new state values?

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 155 at r3 (raw file):

  // Expected state derivatives wrt. the given states.
  EXPECT_EQ(25.0, derivatives_ego->GetAtIndex(0));
  EXPECT_NEAR(-411.914474, derivatives_ego->GetAtIndex(1), 1e-6);

FYI Thus squishing the life out of the ego car occupants at 42 G's deceleration. ;)

Comments from Reviewable

jwnimmer-tri · 2016-11-11T00:02:31Z

@jadecastro Closed by accident?

jadecastro · 2016-11-11T00:15:03Z

Yes I force-pushed from no brach to the same remote, and apparently
git doesn't like that. The "reopen pull request" button is greyed so I may
have to open a fresh PR. I'll take a look this evening.

jwnimmer-tri · 2016-11-11T00:21:55Z

Ouch, sorry!

jadecastro · 2016-11-11T14:06:32Z

Review status: all files reviewed at latest revision, 44 unresolved discussions, some commit checks failed.

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

What's the plan for idm_with_trajectory_agent? Remove it in an immediately-next PR?

I was waiting on https://github.com//pull/3991 to land. I think the best thing to do is to make a new PR when this one lands to purposefully make the switchover to `single_lane_ego_and_agent` and in the process delete `idm_with_trajectory_agent`, if that's okay.

drake/automotive/CMakeLists.txt, line 17 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Please alpha-sort (interleave) these entries within the list of sources.

Done.

drake/automotive/idm_planner.cc, line 10 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

META (for all classes in the PR)... in new code, do not #include "drake/common/drake_export.h" and do not mention DRAKE_EXPORT in the instantiations at the end of the file. They have no effect, and are being globally removed per #3973.

Thanks. I didn't realize that we were moving away from this. I'll keep it in mind for this and future PRs.

drake/automotive/idm_planner.cc, line 11 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Unused.

Done.

drake/automotive/idm_planner.cc, line 46 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Prefer const auto& to const auto in almost all cases (including the prior two lines). Or maybe don't even use auto here, for clarity.

Done.

drake/automotive/idm_planner.cc, line 70 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW If you #include "symbolic_formula.h it should compile okay.

Ah, indeed. That makes things nice. I've made changes to all affected parts of the code to reflect that.

BTW - that also changes single_lane_ego_and_agent.cc, however to enable symbolic::Expression there I also had to add that scalar type to constant_vector_source.cc. PTAL.

drake/automotive/idm_planner.cc, line 82 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This is somewhat unreadable. Consider ways to make it moreso, such as capturing "input_foo->GetAtIndex" into named local variables, and/or making the line wrapping match the math better.

Done.

drake/automotive/idm_planner.cc, line 89 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If I'm not mistaken, the parent class will already take care of this for us, without writing out this override?

Done.

drake/automotive/idm_planner.h, line 12 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Please elaborate on the input and output structure, somewhere in this class's APIs (overview and/or methods).

Done.

drake/automotive/idm_planner.h, line 23 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing API comment on what v_0 means.

Done. I've also changed `v_0` to `v_ref` for clarity.

drake/automotive/idm_planner.h, line 32 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing disable of copy and assignment.

Done.

drake/automotive/linear_car.cc, line 48 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The prior two lines seem like dead code?

Done.

drake/automotive/linear_car.cc, line 64 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Prefer const auto& for read-only values. (Or maybe don't even use auto here?)

Done.

drake/automotive/linear_car.cc, line 79 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

const int size = get_output_port().get_size(); is shorter and clearer, I think?

Done.

drake/automotive/linear_car.cc, line 81 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I think this instead should be the ContinuousState constructor overload that declares our second-order structure? (Passing , 1, 1, 0 for nq, nv, nz.)

Done.

drake/automotive/linear_car.cc, line 88 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If I'm not mistaken, the parent class will already take care of this for us, without writing out this override?

Done.

drake/automotive/linear_car.cc, line 91 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW I think this fits on the prior line?

Done.

drake/automotive/linear_car.h, line 41 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing disable of copy and assignment.

Done.

drake/automotive/linear_car.h, line 28 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Please elaborate on the input structure (either here, or class overview). (It's a single scalar acceleration.)

Done.

drake/automotive/linear_car.h, line 31 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Please elaborate on the output structure (either here, or class overview). (It's two scalars; first is x, second is v.)

Done.

drake/automotive/single_lane_ego_and_agent.cc, line 13 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Consider DRAKE_DEMAND so that even release builds have this (cheap, only-once) check during construction.

Thanks for the tip. Done.

drake/automotive/single_lane_ego_and_agent.cc, line 21 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Unclear why value_ is a member field instead of a local variable.

Done.

drake/automotive/single_lane_ego_and_agent.cc, line 23 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Unclear why planner_ is a member field instead of a local variable.

Done.

drake/automotive/single_lane_ego_and_agent.cc, line 28 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Magic number 0.

Done. I've assigned these variables (`inport_ego`, `inport_agent`).

drake/automotive/single_lane_ego_and_agent.cc, line 29 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Magic number 1.

Done.

drake/automotive/single_lane_ego_and_agent.cc, line 39 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Add a TODO that Diagram should be able to compute this, once the sparsity matrix is available?

Done.

drake/automotive/single_lane_ego_and_agent.h, line 42 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This seems like the wrong group for this System.

Done.

drake/automotive/single_lane_ego_and_agent.h, line 60 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing object-const ... get_ego_car_system() const { ....
Also on the line below.

Done.

drake/automotive/single_lane_ego_and_agent.h, line 62 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing disable of copy and assignment.

Done.

drake/automotive/single_lane_ego_and_agent.h, line 67 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

If possible, add const prior to all of these lines (to make the types be const). (I think it should work.)

Done.

drake/automotive/test/idm_planner_test.cc, line 20 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The trailing underscore in the method name here is weird, and possibly defective (though I didn't go read the cppguide to check for sure).

Done.

drake/automotive/test/idm_planner_test.cc, line 26 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW I'm not sure these comments are adding much value.

Done.

drake/automotive/test/idm_planner_test.cc, line 33 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

FYI #4041 offers SetInputPortToConstantValue sugar, which would make this method much more elegant. Perhaps keep an eye on when it merges and take advantage of it here?

Ok. I have added a TODO.

drake/automotive/test/idm_planner_test.cc, line 44 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Missing attribute-checking coverage of the second input port?

Done.

drake/automotive/test/idm_planner_test.cc, line 50 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Cannot have commented-out code.

Done.

drake/automotive/test/idm_planner_test.cc, line 88 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Is repeating the formula here truly the best way to unit test? Would a literal value for expected_output (possibly with a few sets of input values) be superior?

I'm not sure what you mean.

drake/automotive/test/linear_car_test.cc, line 36 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Same thing on #4041 making this go away.

Ok, I've added a TODO.

drake/automotive/test/linear_car_test.cc, line 69 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Typo "starting output" is zero, I think?

Removed.

drake/automotive/test/linear_car_test.cc, line 73 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I'm not sure what this Input test is trying to show that the subsequent Output test isn't already doing?

Ok, I've removed it.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 21 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW Same thing on #4041 making this go away.

Done. Removed `MakeInput` completely.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 45 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

cappunc

Done.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 58 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

One of the two prior lines has a magic number typo.

Done.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 144 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Not sure "parameters" is the best term to explain the new state values?

Done.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 155 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

FYI Thus squishing the life out of the ego car occupants at 42 G's deceleration. ;)

:) No crash test dummies were harmed in the making of this unit test.

Comments from Reviewable

jwnimmer-tri · 2016-11-11T14:44:02Z

Somehow github and reviewable are confused about the symbolic PR coming in as-new here. I don't know how you can fix that though. Is this feature branch fully merged up to master?

Review status: 4 of 27 files reviewed at latest revision, 1 unresolved discussion.

Comments from Reviewable

jadecastro · 2016-11-11T14:46:05Z

Yeah, I noticed those files have hitched a ride in this PR. Strange - all I did was rebase against master. Let me see if I can disentangle the mess. If not, I'll just submit a new PR.

Review status: 4 of 27 files reviewed at latest revision, 1 unresolved discussion.

Comments from Reviewable

jwnimmer-tri · 2016-11-11T14:46:54Z

Given the discussions in flight I would prefer to keep this PR if we can. I should be able to reasonably ignore the changes that were from the other PR (that I also reviewed).

Review status: 4 of 27 files reviewed at latest revision, 1 unresolved discussion.

Comments from Reviewable

jadecastro · 2016-11-11T14:49:57Z

Ok, I'll try to remove those from the PR (strange they don't show on any of my personal branch commits, when I select New Pull Request). Failing that, I'll keep them and keep this PR open.

Update: Fixed (woohoo!)

Review status: 4 of 27 files reviewed at latest revision, 1 unresolved discussion.

Comments from Reviewable

jadecastro · 2016-11-11T14:53:48Z

Review status: 4 of 27 files reviewed at latest revision, 1 unresolved discussion.

drake/automotive/test/idm_planner_test.cc, line 88 at r3 (raw file):

Previously, jadecastro (Jonathan DeCastro) wrote…

I'm not sure what you mean.

Are you suggesting I inline the parameters, or pre-compute the entire equation, and add comments?

Comments from Reviewable

jwnimmer-tri · 2016-11-12T00:00:27Z

Review status: all files reviewed at latest revision, 2 unresolved discussions, some commit checks failed.

drake/automotive/test/idm_planner_test.cc, line 88 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Yeah, I started to write a reply but don't have a great grasp of it yet. I will try to get some thoughts down later this evening, but I'm off today.

I guess I was imagining more of a table of hand-computed cases. For a tuple of (a, b, s_0, ... x_a, v_a) literals, the expected output a_e is blah.

So like

{
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},  // but really some other numbers
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},  // but really some other numbers
}

And then run through all of those cases in a loop.

The way it stands now, only one set of numbers is ever tested, and for all we know the expected_output is some silly value because of some typo in (both copies of) the big formula.

I guess I'm not 100% clear on what kinds of testing here are important to us here though, so I'll leave it to your discretion.

Comments from Reviewable

jadecastro · 2016-11-13T19:05:28Z

I've updated the input ports to use the named-getter features, and have addressed the unit test comment.

BTW, it seems that a recent merge broke the ability to use symbolic::Expression within diagrams. I believe this is the only system using it, so perhaps that is why it passed. I've removed the symbolic::Expression scalar type from single_lane_ego_and_agent for now, and have marked it with a TODO.

Review status: 12 of 17 files reviewed at latest revision, 1 unresolved discussion.

drake/automotive/linear_car.h, line 61 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

FYI Having direct T members is covered in bees because of Eigen alignment, but we can leave fix that for another day (since almost nobody else in System 2.0 gets is right, either).

Okay, I'll leave it for now. Maybe a 2f2, since I'm not really sure how to fix this.

drake/automotive/test/idm_planner_test.cc, line 88 at r3 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

I guess I was imagining more of a table of hand-computed cases. For a tuple of (a, b, s_0, ... x_a, v_a) literals, the expected output a_e is blah.

So like
{
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},  // but really some other numbers
 {1, 3, 1, 0.1, 4, 4.5, 6.3, 4.2, 12.7, 13.1},  // but really some other numbers
}
And then run through all of those cases in a loop.

The way it stands now, only one set of numbers is ever tested, and for all we know the expected_output is some silly value because of some typo in (both copies of) the big formula.

I guess I'm not 100% clear on what kinds of testing here are important to us here though, so I'll leave it to your discretion.

Thanks. I've re-worked the unit test to test for expected behaviors at different input configurations. I can expand to include the effects of parameters, but I think I will do that once I've included the new parameter definition features.

Comments from Reviewable

david-german-tri · 2016-11-15T22:05:08Z

Reviewed 4 of 15 files at r1, 2 of 12 files at r4, 5 of 10 files at r7, 3 of 5 files at r8, 2 of 2 files at r9.
Review status: all files reviewed at latest revision, 17 unresolved discussions.

drake/automotive/CMakeLists.txt, line 1 at r9 (raw file):

add_subdirectory(gen)

BTW do you want to update the BUILD files too?

drake/automotive/CMakeLists.txt, line 29 at r9 (raw file):

  )
pods_install_libraries(drakeAutomotive)
drake_install_headers(

Do you need to install the new headers?

drake/automotive/idm_planner.cc, line 57 at r9 (raw file):

  // Obtain the input/output structures we need to read from and write into.
  const systems::BasicVector<T>* input_ego =
    this->EvalVectorInput(context, this->get_ego_port().get_index());

indentation is wrong; run clang-format?

drake/automotive/idm_planner.h, line 43 at r9 (raw file):

  // System<T> overrides.
  // The output of this system is an algbraic relation of its inputs.

btw nit: algebraic

drake/automotive/linear_car.cc, line 10 at r9 (raw file):

#include "drake/common/drake_assert.h"
#include "drake/common/symbolic_expression.h"
#include "drake/common/autodiff_overloads.h"

alphabetize

drake/automotive/linear_car.cc, line 24 at r9 (raw file):

                          2,  // Two outputs: x, v.
                          systems::kContinuousSampling);
  this->DeclareContinuousState(2);  // Two states: x, v.

Did you consider using one of the multi-argument versions? This seems like second-order state to me, and moreover it would be pretty nice to have named accessors/mutators.

drake/automotive/linear_car.cc, line 68 at r9 (raw file):

  DRAKE_ASSERT(derivatives_state != nullptr);

  derivatives_state->SetAtIndex(0, context_state.GetAtIndex(1));

BTW, you can also say (*derivatives)[0] = foo; if you like. (Ditto below.)

drake/automotive/linear_car.cc, line 68 at r9 (raw file):

  DRAKE_ASSERT(derivatives_state != nullptr);

  derivatives_state->SetAtIndex(0, context_state.GetAtIndex(1));

BTW consider commenting these as "velocity" and "acceleration" respectively

drake/automotive/simple_car_gen.sh, line 17 at r9 (raw file):

gen_lcm_and_vector "euler floating joint state" x y z roll pitch yaw
gen_lcm_and_vector "idm with trajectory agent state" x_e v_e x_a v_a a_a
gen_vector_yaml "idm with trajectory agent parameters" $drake/automotive/idm_with_trajectory_agent_parameters.yaml

This is redundant now, right? Do you plan to delete it in a subsequent PR?

drake/automotive/single_lane_ego_and_agent.cc, line 46 at r9 (raw file):

// TODO(jadecastro): Leave this for Diagram to infer based on sparsity
// matrix, once implemented.

Wait, why is this necessary? Diagram does already try to infer direct-feedthrough.

drake/automotive/test/idm_planner_test.cc, line 9 at r9 (raw file):

#include "drake/automotive/gen/idm_planner_parameters.h"

#include "gtest/gtest.h"

This goes above the drake includes.

https://google.github.io/styleguide/cppguide.html#Names_and_Order_of_Includes

drake/automotive/test/idm_planner_test.cc, line 35 at r9 (raw file):

    const double v_agent = state[3];

    auto input_ego = std::make_unique<systems::BasicVector<double>>(2);

BTW, you could just use BasicVector<double>::Make here

drake/automotive/test/idm_planner_test.cc, line 86 at r9 (raw file):

  std::vector<double> state;

  // Test case #1: Set the initial states such that the agent and ego

BTW consider splitting these up into separate TEST_Fs.

drake/automotive/test/linear_car_test.cc, line 36 at r9 (raw file):

      std::unique_ptr<systems::BasicVector<double>> data) {
    // TODO(jadecastro): Take advantage of
    // `SetInputPortToConstantValue` in #4041.

There is now FixInputPort. Example here:

drake/drake/systems/framework/test/diagram_context_test.cc

Line 66 in c4618e6

context_->FixInputPort(0, BasicVector<double>::Make({128}));

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 9 at r9 (raw file):

#include "drake/systems/framework/system_input.h"

#include "gtest/gtest.h"

goes above drake

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 141 at r9 (raw file):

  dut_.EvalOutput(*context_, output_.get());

  // Expected state derivatives wrt. the given states.

Here and below, comments giving some kind of intuition for the results would be useful.

drake/systems/framework/primitives/constant_vector_source.cc, line 9 at r9 (raw file):

#include "drake/systems/framework/basic_vector.h"
#include "drake/systems/framework/leaf_context.h"
#include "drake/common/symbolic_formula.h"

alphabetize

Comments from Reviewable

jadecastro · 2016-11-17T00:52:19Z

Review status: 8 of 18 files reviewed at latest revision, 17 unresolved discussions.

drake/automotive/CMakeLists.txt, line 1 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

BTW do you want to update the BUILD files too?

Done.

drake/automotive/CMakeLists.txt, line 29 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

Do you need to install the new headers?

Done.

drake/automotive/idm_planner.cc, line 57 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

indentation is wrong; run clang-format?

Done, but it didn't change the formatting.

drake/automotive/idm_planner.h, line 43 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

btw nit: algebraic

Done.

drake/automotive/linear_car.cc, line 10 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

alphabetize

Done.

drake/automotive/linear_car.cc, line 24 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

Did you consider using one of the multi-argument versions? This seems like second-order state to me, and moreover it would be pretty nice to have named accessors/mutators.

Done. Converted to second-order state and am using named accessors.

drake/automotive/linear_car.cc, line 68 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

BTW consider commenting these as "velocity" and "acceleration" respectively

Done.

drake/automotive/linear_car.cc, line 68 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

BTW, you can also say (*derivatives)[0] = foo; if you like. (Ditto below.)

Done.

drake/automotive/simple_car_gen.sh, line 17 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

This is redundant now, right? Do you plan to delete it in a subsequent PR?

Yes it's redundant. And will be removed in the next PR.

drake/automotive/single_lane_ego_and_agent.cc, line 46 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

Wait, why is this necessary? Diagram does already try to infer direct-feedthrough.

Thanks. At some point I think it wasn't working, but it clearly does now. Done.

drake/automotive/test/idm_planner_test.cc, line 9 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

This goes above the drake includes.

https://google.github.io/styleguide/cppguide.html#Names_and_Order_of_Includes

Done.

drake/automotive/test/idm_planner_test.cc, line 35 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

BTW, you could just use BasicVector<double>::Make here

Done.

drake/automotive/test/idm_planner_test.cc, line 86 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

BTW consider splitting these up into separate TEST_Fs.

Done.

drake/automotive/test/linear_car_test.cc, line 36 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

There is now FixInputPort. Example here:

drake/drake/systems/framework/test/diagram_context_test.cc

Line 66 in c4618e6

context_->FixInputPort(0, BasicVector<double>::Make({128}));

Done. I've removed the "TODO"s to use `SetInputPortToConstantValue`, since `FixInputPort` seems to do the trick?

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 9 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

goes above drake

Done.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 141 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

Here and below, comments giving some kind of intuition for the results would be useful.

Done.

drake/systems/framework/primitives/constant_vector_source.cc, line 9 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

alphabetize

Done.

Comments from Reviewable

jwnimmer-tri · 2016-11-17T16:23:29Z

Reviewed 4 of 5 files at r8, 11 of 11 files at r10.
Review status: all files reviewed at latest revision, 19 unresolved discussions.

drake/automotive/single_lane_ego_and_agent.cc, line 60 at r10 (raw file):

}

// TODO(jadecastro): Introduce symbolic::Expression scalar type.

BTW This already seems done below?

drake/automotive/test/idm_planner_test.cc, line 26 at r10 (raw file):

  void SetInputValue(const std::vector<double>& state) {
    unsigned int state_size =

Don't use unsigned per cppguide & code_style_guide.

Comments from Reviewable

jadecastro · 2016-11-17T16:31:48Z

Review status: 16 of 45 files reviewed at latest revision, 19 unresolved discussions.

drake/automotive/single_lane_ego_and_agent.cc, line 60 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

BTW This already seems done below?

Thanks. Done.

drake/automotive/test/idm_planner_test.cc, line 26 at r10 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Don't use unsigned per cppguide & code_style_guide.

Done.

Comments from Reviewable

david-german-tri · 2016-11-17T23:10:42Z

+(status: curate commits before merging)

Review status: 16 of 18 files reviewed at latest revision, 17 unresolved discussions.

Comments from Reviewable

david-german-tri · 2016-11-17T23:14:08Z

Reviewed 9 of 11 files at r10, 1 of 29 files at r11, 1 of 1 files at r12.
Review status: all files reviewed at latest revision, 4 unresolved discussions.

drake/automotive/CMakeLists.txt, line 1 at r9 (raw file):

Previously, jadecastro (Jonathan DeCastro) wrote…

Done.

test/BUILD also?

drake/automotive/idm_planner.cc, line 57 at r9 (raw file):

Previously, jadecastro (Jonathan DeCastro) wrote…

Done, but it didn't change the formatting.

Looks like it did to me. Anyhow, it's correct now.

drake/automotive/test/idm_planner_test.cc, line 71 at r12 (raw file):

}

// Test case #1: Set the initial states such that the agent and ego

BTW, the ordinals don't add much

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 141 at r9 (raw file):

Previously, jadecastro (Jonathan DeCastro) wrote…

Done.

s/maps//?

Comments from Reviewable

jadecastro · 2016-11-18T01:26:41Z

Great - I'll be sure to curate everything before merging.

Review status: 16 of 19 files reviewed at latest revision, 3 unresolved discussions.

drake/automotive/CMakeLists.txt, line 1 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

test/BUILD also?

Done.

drake/automotive/idm_planner.cc, line 57 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

Looks like it did to me. Anyhow, it's correct now.

I notice it now :)

drake/automotive/test/idm_planner_test.cc, line 71 at r12 (raw file):

Previously, david-german-tri (David German) wrote…

BTW, the ordinals don't add much

Removed.

drake/automotive/test/single_lane_ego_and_agent_test.cc, line 141 at r9 (raw file):

Previously, david-german-tri (David German) wrote…

s/maps//?

Fixed.

Comments from Reviewable

Add diagram composing an ego car, a planner and an agent car Add a unit test for the diagram Add unit test for the standalone IDM planner leaf system Fix bad-cast issues, perform clang-format, cleanup, documentation

Minor updates.

Address review comments

david-german-tri · 2016-11-18T17:15:00Z

Reviewed 3 of 3 files at r13.
Review status: all files reviewed at latest revision, all discussions resolved.

Comments from Reviewable

jwnimmer-tri · 2016-11-18T18:20:41Z

Reviewed 1 of 29 files at r11, 1 of 1 files at r12, 3 of 3 files at r13.
Review status: all files reviewed at latest revision, all discussions resolved.

Comments from Reviewable

jwnimmer-tri · 2016-11-18T18:20:48Z

Review status: all files reviewed at latest revision, all discussions resolved.

Comments from Reviewable

Purge IdmWithTrajectoryAgent (replaced with diagram in #4081)

jadecastro added status: do not merge status: do not review status: curate commits before merging labels Nov 8, 2016

jadecastro changed the title ~~Introduce a diagram that implements a two-car IDM model~~ Introduce a diagram that implements a IDM car model with one agent. Nov 8, 2016

jadecastro changed the title ~~Introduce a diagram that implements a IDM car model with one agent.~~ Introduce a diagram that implements an IDM model with one agent. Nov 8, 2016

jadecastro changed the title ~~Introduce a diagram that implements an IDM model with one agent.~~ Introduce a diagram that implements an IDM model with one external agent. Nov 8, 2016

jadecastro force-pushed the generic-car-planner branch from 0050f05 to b6ff775 Compare November 8, 2016 18:20

jadecastro removed status: do not merge status: do not review labels Nov 8, 2016

jwnimmer-tri assigned david-german-tri and jwnimmer-tri Nov 8, 2016

jadecastro force-pushed the generic-car-planner branch from 0745dae to 5323dfe Compare November 9, 2016 00:43

jadecastro closed this Nov 10, 2016

jadecastro force-pushed the generic-car-planner branch from 5323dfe to 39641a6 Compare November 10, 2016 23:25

jadecastro reopened this Nov 11, 2016

jadecastro force-pushed the generic-car-planner branch from 7bc8225 to 7d55814 Compare November 11, 2016 15:17

jadecastro force-pushed the generic-car-planner branch from fe9fb77 to 5a63654 Compare November 13, 2016 18:55

jadecastro force-pushed the generic-car-planner branch 2 times, most recently from 2f5b0fa to 5981cb3 Compare November 15, 2016 12:48

jadecastro force-pushed the generic-car-planner branch from 11b98d6 to 344327d Compare November 17, 2016 00:51

jadecastro force-pushed the generic-car-planner branch 4 times, most recently from aa824a0 to e25ff08 Compare November 17, 2016 01:50

jadecastro force-pushed the generic-car-planner branch 2 times, most recently from fb3620b to 21ac54b Compare November 17, 2016 17:42

david-german-tri added the status: curate commits before merging label Nov 17, 2016

jadecastro added 3 commits November 17, 2016 23:44

Add a single-lane linear car leaf system

1deba34

Add diagram composing an ego car, a planner and an agent car Add a unit test for the diagram Add unit test for the standalone IDM planner leaf system Fix bad-cast issues, perform clang-format, cleanup, documentation

Style guide

da616c5

Minor updates.

clang-format

26a687b

Address review comments

jadecastro force-pushed the generic-car-planner branch from 75b4d65 to 26a687b Compare November 18, 2016 04:48

jadecastro removed the status: curate commits before merging label Nov 18, 2016

jadecastro merged commit 211f49a into RobotLocomotion:master Nov 18, 2016

jadecastro added a commit that referenced this pull request Nov 19, 2016

Merge pull request #4172 from jadecastro/remove-monolithic-idm

8118d3d

Purge IdmWithTrajectoryAgent (replaced with diagram in #4081)

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Introduce a diagram that implements an IDM model with one external agent. #4081

Introduce a diagram that implements an IDM model with one external agent. #4081

jadecastro commented Nov 8, 2016 •

edited by david-german-tri

Loading

jadecastro commented Nov 8, 2016 •

edited

Loading

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016 •

edited

Loading

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016 •

edited

Loading

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 12, 2016

jadecastro commented Nov 13, 2016

david-german-tri commented Nov 15, 2016

jadecastro commented Nov 17, 2016

jwnimmer-tri commented Nov 17, 2016

jadecastro commented Nov 17, 2016

david-german-tri commented Nov 17, 2016

david-german-tri commented Nov 17, 2016

jadecastro commented Nov 18, 2016

david-german-tri commented Nov 18, 2016

jwnimmer-tri commented Nov 18, 2016

jwnimmer-tri commented Nov 18, 2016

Introduce a diagram that implements an IDM model with one external agent. #4081

Introduce a diagram that implements an IDM model with one external agent. #4081

Conversation

jadecastro commented Nov 8, 2016 • edited by david-german-tri Loading

jadecastro commented Nov 8, 2016 • edited Loading

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 9, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016 • edited Loading

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 11, 2016

jadecastro commented Nov 11, 2016 • edited Loading

jadecastro commented Nov 11, 2016

jwnimmer-tri commented Nov 12, 2016

jadecastro commented Nov 13, 2016

david-german-tri commented Nov 15, 2016

jadecastro commented Nov 17, 2016

jwnimmer-tri commented Nov 17, 2016

jadecastro commented Nov 17, 2016

david-german-tri commented Nov 17, 2016

david-german-tri commented Nov 17, 2016

jadecastro commented Nov 18, 2016

david-german-tri commented Nov 18, 2016

jwnimmer-tri commented Nov 18, 2016

jwnimmer-tri commented Nov 18, 2016

jadecastro commented Nov 8, 2016 •

edited by david-german-tri

Loading

jadecastro commented Nov 8, 2016 •

edited

Loading

jadecastro commented Nov 11, 2016 •

edited

Loading

jadecastro commented Nov 11, 2016 •

edited

Loading