Merge pull request RobotLocomotion#4081 from jadecastro/generic-car-p…

…lanner

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

drake/automotive/BUILD

@@ -11,6 +11,7 @@ cc_library(
     srcs = [
         "gen/driving_command.cc",
         "gen/euler_floating_joint_state.cc",
+        "gen/idm_planner_parameters.cc",
         "gen/idm_with_trajectory_agent_parameters.cc",
         "gen/idm_with_trajectory_agent_state.cc",
         "gen/simple_car_config.cc",
@@ -19,6 +20,7 @@ cc_library(
     hdrs = [
         "gen/driving_command.h",
         "gen/euler_floating_joint_state.h",
+        "gen/idm_planner_parameters.cc",
         "gen/idm_with_trajectory_agent_parameters.h",
         "gen/idm_with_trajectory_agent_state.h",
         "gen/simple_car_config.h",
@@ -58,6 +60,20 @@ cc_library(
     deps = DEPS,
     linkstatic = 1)
 
+cc_library(
+    name = "linear_car",
+    srcs = ["linear_car.cc"],
+    hdrs = ["linear_car.h"],
+    deps = DEPS,
+    linkstatic = 1)
+
+cc_library(
+    name = "idm_planner",
+    srcs = ["idm_planner.cc"],
+    hdrs = ["idm_planner.h"],
+    deps = DEPS,
+    linkstatic = 1)
+
 cc_library(
     name = "idm_with_trajectory_agent",
     srcs = ["idm_with_trajectory_agent.cc"],
@@ -72,6 +88,13 @@ cc_library(
     deps = DEPS,
     linkstatic = 1)
 
+cc_library(
+    name = "single_lane_ego_and_agent",
+    srcs = ["single_lane_ego_and_agent.cc"],
+    hdrs = ["single_lane_ego_and_agent.h"],
+    deps = DEPS,
+    linkstatic = 1)
+
 cc_library(
     name = "trajectory_car",
     srcs = ["trajectory_car.cc"],

drake/automotive/CMakeLists.txt

@@ -13,8 +13,11 @@ add_library_with_exports(LIB_NAME drakeAutomotive SOURCE_FILES
   gen/idm_with_trajectory_agent_state.cc
   gen/simple_car_config.cc
   gen/simple_car_state.cc
+  idm_planner.cc
   idm_with_trajectory_agent.cc
+  linear_car.cc
   simple_car.cc
+  single_lane_ego_and_agent.cc
   trajectory_car.cc
   )
 target_link_libraries(drakeAutomotive
@@ -26,9 +29,12 @@ pods_install_libraries(drakeAutomotive)
 drake_install_headers(
   create_trajectory_params.h
   curve2.h
+  idm_planner.h
   idm_with_trajectory_agent.h
+  linear_car.h
   simple_car.h
   simple_car_to_euler_floating_joint.h
+  single_lane_ego_and_agent.h
   trajectory_car.h
   # N.B. The gen/*.h headers are installed by gen/CMakeLists.txt.
   )

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,105 @@
+#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/symbolic_formula.h"
+
+namespace drake {
+namespace automotive {
+
+template <typename T>
+IdmPlanner<T>::IdmPlanner(const T& v_ref) : v_ref_(v_ref) {
+  // The reference velocity must be strictly positive.
+  DRAKE_ASSERT(v_ref > 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>
+const systems::SystemPortDescriptor<T>& IdmPlanner<T>::get_ego_port() const {
+  return systems::System<T>::get_input_port(0);
+}
+
+template <typename T>
+const systems::SystemPortDescriptor<T>& IdmPlanner<T>::get_agent_port() const {
+  return systems::System<T>::get_input_port(1);
+}
+
+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 systems::BasicVector<T>* input_ego =
+      this->EvalVectorInput(context, this->get_ego_port().get_index());
+  const systems::BasicVector<T>* input_agent =
+      this->EvalVectorInput(context, this->get_agent_port().get_index());
+  systems::BasicVector<T>* const output_vector =
+      output->GetMutableVectorData(0);
+  DRAKE_ASSERT(output_vector != nullptr);
+
+  // TODO(jadecastro): Bake in David's new parameter definition API and
+  // remove `IdmWithTrajectoryAgent`.
+  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();
+
+  const T& x_ego = input_ego->GetAtIndex(0);
+  const T& v_ego = input_ego->GetAtIndex(1);
+  const T& x_agent = input_agent->GetAtIndex(0);
+  const T& v_agent = input_agent->GetAtIndex(1);
+
+  // Ensure that we are supplying the planner with sane parameters and
+  // input values.
+  DRAKE_DEMAND(a > 0.0);
+  DRAKE_DEMAND(b > 0.0);
+  DRAKE_DEMAND(x_agent > (l_a + x_ego));
+
+  output_vector->SetAtIndex(
+      0, a * (1.0 - pow(v_ego / v_ref_, delta) -
+              pow((s_0 + v_ego * time_headway +
+                   v_ego * (v_ego - v_agent) / (2 * sqrt(a * b))) /
+                      (x_agent - x_ego - l_a),
+                  2.0)));
+}
+
+// These instantiations must match the API documentation in
+// idm_planner.h.
+template class IdmPlanner<double>;
+template class IdmPlanner<drake::TaylorVarXd>;
+template class IdmPlanner<drake::symbolic::Expression>;
+
+}  // namespace automotive
+}  // namespace drake

drake/automotive/idm_planner.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "drake/systems/framework/leaf_system.h"
+
+namespace drake {
+namespace automotive {
+
+/// IdmPlanner -- an IDM (Intelligent Driver Model) planner.
+///
+/// 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
+///
+/// Inputs:
+///   0: @p x_ego ego car position (scalar) [m]
+///   1: @p v_ego ego car velocity (scalar) [m/s]
+///   2: @p x_agent agent car position (scalar) [m]
+///   3: @p v_agent agent car velocity (scalar) [m/s]
+/// Outputs:
+///   0: @p vdot_ego linear acceleration of the ego car (scalar) [m/s^2].
+template <typename T>
+class IdmPlanner : public systems::LeafSystem<T> {
+ public:
+  /// @p v_ref desired velocity of the ego car in units of m/s.
+  explicit IdmPlanner(const T& v_ref);
+  ~IdmPlanner() override;
+
+  /// Returns the port to the ego car input subvector.
+  const systems::SystemPortDescriptor<T>& get_ego_port() const;
+
+  /// Returns the port to the agent car input subvector.
+  const systems::SystemPortDescriptor<T>& get_agent_port() const;
+
+  // System<T> overrides.
+  // The output of this system is an algebraic relation of its inputs.
+  bool has_any_direct_feedthrough() const override { return true; }
+
+  void EvalOutput(const systems::Context<T>& context,
+                  systems::SystemOutput<T>* output) const override;
+
+  // Disable copy and assignment.
+  IdmPlanner(const IdmPlanner<T>&) = delete;
+  IdmPlanner& operator=(const IdmPlanner<T>&) = delete;
+  IdmPlanner(IdmPlanner<T>&&) = delete;
+  IdmPlanner& operator=(IdmPlanner<T>&&) = delete;
+
+ private:
+  const T v_ref_;  // 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