Add a single-lane linear car leaf system

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

drake/automotive/CMakeLists.txt

@@ -16,6 +16,9 @@ add_library_with_exports(LIB_NAME drakeAutomotive SOURCE_FILES
   idm_with_trajectory_agent.cc
   simple_car.cc
   trajectory_car.cc
+  linear_car.cc
+  idm_planner.cc
+  single_lane_ego_and_agent.cc
   )
 target_link_libraries(drakeAutomotive
   drakeCommon

drake/automotive/gen/idm_planner_parameters.cc

@@ -0,0 +1,18 @@
+#include "drake/automotive/gen/idm_planner_parameters.h"
+
+// GENERATED FILE DO NOT EDIT
+// See drake/tools/lcm_vector_gen.py.
+
+namespace drake {
+namespace automotive {
+
+const int IdmPlannerParametersIndices::kNumCoordinates;
+const int IdmPlannerParametersIndices::kA;
+const int IdmPlannerParametersIndices::kB;
+const int IdmPlannerParametersIndices::kS0;
+const int IdmPlannerParametersIndices::kTimeHeadway;
+const int IdmPlannerParametersIndices::kDelta;
+const int IdmPlannerParametersIndices::kLA;
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/gen/idm_planner_parameters.h

@@ -0,0 +1,69 @@
+#pragma once
+
+// GENERATED FILE DO NOT EDIT
+// See drake/tools/lcm_vector_gen.py.
+
+#include <stdexcept>
+#include <string>
+
+#include <Eigen/Core>
+
+#include "drake/common/drake_export.h"
+#include "drake/systems/framework/basic_vector.h"
+
+namespace drake {
+namespace automotive {
+
+/// Describes the row indices of a IdmPlannerParameters.
+struct DRAKE_EXPORT IdmPlannerParametersIndices {
+  /// The total number of rows (coordinates).
+  static const int kNumCoordinates = 6;
+
+  // The index of each individual coordinate.
+  static const int kA = 0;
+  static const int kB = 1;
+  static const int kS0 = 2;
+  static const int kTimeHeadway = 3;
+  static const int kDelta = 4;
+  static const int kLA = 5;
+};
+
+/// Specializes BasicVector with specific getters and setters.
+template <typename T>
+class IdmPlannerParameters : public systems::BasicVector<T> {
+ public:
+  // An abbreviation for our row index constants.
+  typedef IdmPlannerParametersIndices K;
+
+  /// Default constructor.  Sets all rows to zero.
+  IdmPlannerParameters() : systems::BasicVector<T>(K::kNumCoordinates) {
+    this->SetFromVector(VectorX<T>::Zero(K::kNumCoordinates));
+  }
+
+  /// @name Getters and Setters
+  //@{
+  // max acceleration
+  const T a() const { return this->GetAtIndex(K::kA); }
+  void set_a(const T& a) { this->SetAtIndex(K::kA, a); }
+  // comfortable braking deceleration
+  const T b() const { return this->GetAtIndex(K::kB); }
+  void set_b(const T& b) { this->SetAtIndex(K::kB, b); }
+  // minimum desired net distance
+  const T s_0() const { return this->GetAtIndex(K::kS0); }
+  void set_s_0(const T& s_0) { this->SetAtIndex(K::kS0, s_0); }
+  // desired time headway to vehicle in front
+  const T time_headway() const { return this->GetAtIndex(K::kTimeHeadway); }
+  void set_time_headway(const T& time_headway) {
+    this->SetAtIndex(K::kTimeHeadway, time_headway);
+  }
+  // free-road exponent
+  const T delta() const { return this->GetAtIndex(K::kDelta); }
+  void set_delta(const T& delta) { this->SetAtIndex(K::kDelta, delta); }
+  // length of leading car
+  const T l_a() const { return this->GetAtIndex(K::kLA); }
+  void set_l_a(const T& l_a) { this->SetAtIndex(K::kLA, l_a); }
+  //@}
+};
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/idm_planner.cc

@@ -0,0 +1,97 @@
+#include "drake/automotive/idm_planner.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include "drake/automotive/gen/idm_planner_parameters.h"
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_export.h"
+#include "drake/common/symbolic_expression.h"
+
+namespace drake {
+namespace automotive {
+
+template <typename T>
+IdmPlanner<T>::IdmPlanner(const T& v_0) : v_0_(v_0) {
+
+  // The reference velocity must be strictly positive.
+  DRAKE_ASSERT(v_0 > 0);
+
+  // Declare the ego car input port.
+  this->DeclareInputPort(systems::kVectorValued,
+                         2,  // Size of the ego car output vector.
+                         systems::kContinuousSampling);
+  // Declare the agent car input port.
+  this->DeclareInputPort(systems::kVectorValued,
+                         2,  // Size of the agent car output vector.
+                         systems::kContinuousSampling);
+  // Declare the output port.
+  this->DeclareOutputPort(systems::kVectorValued,
+                          1,  // We have a single linear acceleration value.
+                          systems::kContinuousSampling);
+}
+
+template <typename T>
+IdmPlanner<T>::~IdmPlanner() {}
+
+template <typename T>
+void IdmPlanner<T>::EvalOutput(const systems::Context<T>& context,
+                               systems::SystemOutput<T>* output) const {
+  DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(context));
+  DRAKE_ASSERT_VOID(systems::System<T>::CheckValidOutput(output));
+
+  // 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);
+  systems::BasicVector<T>* const output_vector =
+      output->GetMutableVectorData(0);
+  DRAKE_ASSERT(output_vector != nullptr);
+
+  // TODO: bake in David's parameters definition stuff.
+  IdmPlannerParameters<T> params;
+  params.set_a(T(1.0));             // max acceleration.
+  params.set_b(T(3.0));             // comfortable braking deceleration.
+  params.set_s_0(T(1.0));           // minimum desired net distance.
+  params.set_time_headway(T(0.1));  // desired time headway to vehicle in front.
+  params.set_delta(T(4.0));         // recommended choice of free-road exponent.
+  params.set_l_a(T(4.5));           // length of leading car.
+
+  const T& a = params.a();
+  const T& b = params.b();
+  const T& s_0 = params.s_0();
+  const T& time_headway = params.time_headway();
+  const T& delta = params.delta();
+  const T& l_a = params.l_a();
+
+  // @p a and @p b must be positive.
+  // TODO: the below assertion forbids symbolic::Expressions with this
+  // construction.
+  DRAKE_ASSERT( a > 0.0 );
+  DRAKE_ASSERT( b > 0.0 );
+
+  output_vector->SetAtIndex(
+      0, a * (1.0 - pow(input_ego->GetAtIndex(1) / v_0_, delta) -
+              pow((s_0 + input_ego->GetAtIndex(1) * time_headway +
+                   input_ego->GetAtIndex(1) *
+                       (input_ego->GetAtIndex(1) - input_agent->GetAtIndex(1)) /
+                       (2 * sqrt(a * b))) /
+                      (input_agent->GetAtIndex(0) - input_ego->GetAtIndex(0) -
+                       l_a),
+                  2.0)));
+}
+
+template <typename T>
+std::unique_ptr<systems::BasicVector<T>> IdmPlanner<T>::AllocateOutputVector(
+    const systems::SystemPortDescriptor<T>& descriptor) const {
+  return std::make_unique<systems::BasicVector<T>>(1 /* output vector size */);
+}
+
+// These instantiations must match the API documentation in
+// idm_planner.h.
+template class DRAKE_EXPORT IdmPlanner<double>;
+template class DRAKE_EXPORT IdmPlanner<drake::TaylorVarXd>;
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/idm_planner.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace automotive {
+
+/// IdmPlanner -- an IDM (Intelligent Driver Modal)[1].
+///
+/// [1] IDM: Intelligent Driver Model:
+///    https://en.wikipedia.org/wiki/Intelligent_driver_model
+///
+/// Instantiated templates for the following kinds of T's are provided:
+/// - double
+/// - drake::TaylorVarXd
+/// - drake::symbolic::Expression
+///
+/// They are already available to link against in libdrakeAutomotive.
+///
+/// @ingroup automotive_systems
+template <typename T>
+class IdmPlanner : public systems::LeafSystem<T> {
+ public:
+  IdmPlanner(const T& v_0);
+  ~IdmPlanner() override;
+
+  /// The output of this system is an algbraic relation of its inputs.
+  bool has_any_direct_feedthrough() const override { return true; }
+
+  // System<T> overrides
+  void EvalOutput(const systems::Context<T>& context,
+                  systems::SystemOutput<T>* output) const override;
+
+ protected:
+  std::unique_ptr<systems::BasicVector<T>> AllocateOutputVector(
+      const systems::SystemPortDescriptor<T>& descriptor) const override;
+
+ private:
+  const T v_0_;  // Desired vehicle velocity.
+};
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/idm_planner_parameters.yaml

@@ -0,0 +1,19 @@
+---
+-
+     name: a
+     doc: max acceleration
+-
+     name: b
+     doc: comfortable braking deceleration
+-
+     name: s_0
+     doc: minimum desired net distance
+-
+     name: time_headway
+     doc: desired time headway to vehicle in front
+-
+     name: delta
+     doc: free-road exponent
+-
+     name: l_a
+     doc: length of leading car

drake/automotive/linear_car.cc

@@ -0,0 +1,122 @@
+#include "drake/automotive/linear_car.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <Eigen/Geometry>
+
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_export.h"
+#include "drake/common/eigen_autodiff_types.h"
+#include "drake/common/symbolic_expression.h"
+#include "drake/math/autodiff_overloads.h"
+//#include "drake/systems/framework/leaf_context.h"
+
+namespace drake {
+namespace automotive {
+
+template <typename T>
+LinearCar<T>::LinearCar() {
+  this->DeclareInputPort(systems::kVectorValued,
+                         1, //LinearCarInputIndices::kNumCoordinates,
+                         systems::kContinuousSampling);
+  this->DeclareOutputPort(systems::kVectorValued,
+                          2, //LinearCarStateIndices::kNumCoordinates,
+                          systems::kContinuousSampling);
+}
+
+template <typename T>
+LinearCar<T>::~LinearCar() {}
+
+template <typename T>
+const systems::SystemPortDescriptor<T>& LinearCar<T>::get_input_port() const {
+  return systems::System<T>::get_input_port(0);
+}
+
+template <typename T>
+const systems::SystemPortDescriptor<T>& LinearCar<T>::get_output_port() const {
+  return systems::System<T>::get_output_port(0);
+}
+
+template <typename T>
+void LinearCar<T>::EvalOutput(
+    const systems::Context<T>& context,
+    systems::SystemOutput<T>* output) const {
+  DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(context));
+  DRAKE_ASSERT_VOID(systems::System<T>::CheckValidOutput(output));
+
+  // Obtain the structure we need to write into.
+  systems::BasicVector<T>* const output_vector =
+      output->GetMutableVectorData(0);
+  DRAKE_ASSERT(output_vector != nullptr);
+
+  // TODO(david-german-tri): Remove this copy by allowing output ports to be
+  // mere pointers to state variables (or cache lines).
+  //output_vector->get_mutable_value() =
+  //    context.get_continuous_state()->CopyToVector();
+  this->GetMutableOutputVector(output, 0) =
+    this->CopyContinuousStateVector(context);
+}
+
+template <typename T>
+void LinearCar<T>::EvalTimeDerivatives(
+    const systems::Context<T>& context,
+    systems::ContinuousState<T>* derivatives) const {
+  DRAKE_ASSERT_VOID(systems::System<T>::CheckValidContext(context));
+  DRAKE_ASSERT(derivatives != nullptr);
+
+  // Obtain the state.
+  const systems::VectorBase<T>& context_state =
+      context.get_continuous_state_vector();
+  //const LinearCarInput<T>* const input =
+  //    dynamic_cast<const LinearCarInput<T>*>(this->EvalVectorInput(context, 0));
+  auto input = this->EvalVectorInput(context, 0);
+  //DRAKE_ASSERT(input != nullptr);
+  //const LinearCarState<T>* const state =
+  //    dynamic_cast<const LinearCarState<T>*>(&context_state);
+  //DRAKE_ASSERT(state != nullptr);
+
+  // Obtain the structure we need to write into.
+  systems::VectorBase<T>* const derivatives_state =
+      derivatives->get_mutable_vector();
+  DRAKE_ASSERT(derivatives_state != nullptr);
+  //LinearCarState<T>* const new_derivatives =
+  //    dynamic_cast<LinearCarState<T>*>(derivatives_state);
+  //DRAKE_ASSERT(new_derivatives != nullptr);
+
+  // Declare the state derivatives (consistent with linear_car_state.cc).
+  //new_derivatives->set_x(state->v());
+  //new_derivatives->set_v(input->vdot());
+  derivatives_state->SetAtIndex(0, context_state.GetAtIndex(1));
+  derivatives_state->SetAtIndex(1, input->GetAtIndex(0));
+}
+
+template <typename T>
+std::unique_ptr<systems::ContinuousState<T>>
+LinearCar<T>::AllocateContinuousState() const {
+  //auto state = std::make_unique<LinearCarState<T>>();
+  // Define the initial state values.
+  //state->set_x(T{0.0});
+  //state->set_v(T{0.0});
+  const int size = systems::System<T>::get_output_port(0).get_size();
+  //DRAKE_ASSERT(systems::System<T>::get_input_port(0).get_size() == size);
+  return std::make_unique<systems::ContinuousState<T>>(
+      std::make_unique<systems::BasicVector<T>>(size));
+}
+
+template <typename T>
+std::unique_ptr<systems::BasicVector<T>>
+LinearCar<T>::AllocateOutputVector(
+    const systems::SystemPortDescriptor<T>& descriptor) const {
+  //return std::make_unique<LinearCarState<T>>();
+  return std::make_unique<systems::BasicVector<T>>(2);
+}
+
+// These instantiations must match the API documentation in
+// idm_with_trajectory_agent.h.
+template class DRAKE_EXPORT LinearCar<double>;
+template class DRAKE_EXPORT LinearCar<drake::TaylorVarXd>;
+template class DRAKE_EXPORT LinearCar<drake::symbolic::Expression>;
+
+}  // namespace automotive
+}  // namespace drake