Merge pull request #5655 from jwnimmer-tri/tools-named_vector-defaults

Add lcm_vector_gen support for defaults; use it in Bicycle and Idm

drake/automotive/bicycle_car.cc

@@ -178,16 +178,10 @@ std::unique_ptr<systems::Parameters<T>> BicycleCar<T>::AllocateParameters()
 template <typename T>
 void BicycleCar<T>::SetDefaultParameters(const systems::LeafContext<T>& context,
                                       systems::Parameters<T>* params) const {
-  // Parameters representative of a Cadillac SRX (from Althoff & Dolan, 2014).
   auto p = dynamic_cast<BicycleCarParameters<T>*>(
       params->get_mutable_numeric_parameter(0));
   DRAKE_DEMAND(p != nullptr);
-  p->set_mass(T(2278.));  // Mass [kg].
-  p->set_lf(T(1.292));    // Distance from center of mass to front axle [m].
-  p->set_lr(T(1.515));    // Distance from center of mass to rear axle [m].
-  p->set_Iz(T(3210.));    // Moment of inertia about the yaw-axis [kg m^2].
-  p->set_Cf(T(10.8e4));   // Cornering stiffness (front) [N / rad].
-  p->set_Cr(T(10.8e4));   // Cornering stiffness (rear) [N / rad].
+  p->SetFrom(BicycleCarParameters<T>());
 }
 
 // These instantiations must match the API documentation in bicycle.h.

drake/automotive/bicycle_car_parameters.named_vector

@@ -1,24 +1,38 @@
+# The default values contained below are representative of a Cadillac SRX (from
+# Althoff & Dolan, 2014).
 element {
      name: "mass"
      doc: "mass"
+     doc_units: "kg"
+     default_value: "2278.0"
 }
 element {
      name: "lf"
      doc: "distance from the center of mass to the front axle"
+     doc_units: "m"
+     default_value: "1.292"
 }
 element {
      name: "lr"
      doc: "distance from the center of mass to the rear axle"
+     doc_units: "m"
+     default_value: "1.515"
 }
 element {
      name: "Iz"
      doc: "moment of inertia about the yaw-axis"
+     doc_units: "kg m^2"
+     default_value: "3210.0"
 }
 element {
      name: "Cf"
      doc: "cornering stiffness (front)"
+     doc_units: "N / rad"
+     default_value: "10.8e4"
 }
 element {
      name: "Cr"
      doc: "cornering stiffness (rear)"
+     doc_units: "N / rad"
+     default_value: "10.8e4"
 }

drake/automotive/gen/bicycle_car_parameters.h

@@ -35,9 +35,20 @@ class BicycleCarParameters : public systems::BasicVector<T> {
   /// An abbreviation for our row index constants.
   typedef BicycleCarParametersIndices K;
 
-  /// Default constructor.  Sets all rows to zero.
+  /// Default constructor.  Sets all rows to their default value:
+  /// @arg @c mass defaults to 2278.0 in units of kg.
+  /// @arg @c lf defaults to 1.292 in units of m.
+  /// @arg @c lr defaults to 1.515 in units of m.
+  /// @arg @c Iz defaults to 3210.0 in units of kg m^2.
+  /// @arg @c Cf defaults to 10.8e4 in units of N / rad.
+  /// @arg @c Cr defaults to 10.8e4 in units of N / rad.
   BicycleCarParameters() : systems::BasicVector<T>(K::kNumCoordinates) {
-    this->SetFromVector(VectorX<T>::Zero(K::kNumCoordinates));
+    this->set_mass(2278.0);
+    this->set_lf(1.292);
+    this->set_lr(1.515);
+    this->set_Iz(3210.0);
+    this->set_Cf(10.8e4);
+    this->set_Cr(10.8e4);
   }
 
   BicycleCarParameters<T>* DoClone() const override {
@@ -47,21 +58,27 @@ class BicycleCarParameters : public systems::BasicVector<T> {
   /// @name Getters and Setters
   //@{
   /// mass
+  /// @note @c mass is expressed in units of kg.
   const T& mass() const { return this->GetAtIndex(K::kMass); }
   void set_mass(const T& mass) { this->SetAtIndex(K::kMass, mass); }
   /// distance from the center of mass to the front axle
+  /// @note @c lf is expressed in units of m.
   const T& lf() const { return this->GetAtIndex(K::kLf); }
   void set_lf(const T& lf) { this->SetAtIndex(K::kLf, lf); }
   /// distance from the center of mass to the rear axle
+  /// @note @c lr is expressed in units of m.
   const T& lr() const { return this->GetAtIndex(K::kLr); }
   void set_lr(const T& lr) { this->SetAtIndex(K::kLr, lr); }
   /// moment of inertia about the yaw-axis
+  /// @note @c Iz is expressed in units of kg m^2.
   const T& Iz() const { return this->GetAtIndex(K::kIz); }
   void set_Iz(const T& Iz) { this->SetAtIndex(K::kIz, Iz); }
   /// cornering stiffness (front)
+  /// @note @c Cf is expressed in units of N / rad.
   const T& Cf() const { return this->GetAtIndex(K::kCf); }
   void set_Cf(const T& Cf) { this->SetAtIndex(K::kCf, Cf); }
   /// cornering stiffness (rear)
+  /// @note @c Cr is expressed in units of N / rad.
   const T& Cr() const { return this->GetAtIndex(K::kCr); }
   void set_Cr(const T& Cr) { this->SetAtIndex(K::kCr, Cr); }
   //@}

drake/automotive/gen/idm_planner_parameters.h

@@ -37,9 +37,24 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
   /// An abbreviation for our row index constants.
   typedef IdmPlannerParametersIndices K;
 
-  /// Default constructor.  Sets all rows to zero.
+  /// Default constructor.  Sets all rows to their default value:
+  /// @arg @c v_ref defaults to 10.0 in units of m/s.
+  /// @arg @c a defaults to 1.0 in units of m/s^2.
+  /// @arg @c b defaults to 3.0 in units of m/s^2.
+  /// @arg @c s_0 defaults to 1.0 in units of m.
+  /// @arg @c time_headway defaults to 0.1 in units of s.
+  /// @arg @c delta defaults to 4.0 in units of dimensionless.
+  /// @arg @c bloat_diameter defaults to 4.5 in units of m.
+  /// @arg @c distance_lower_limit defaults to 1e-2 in units of m.
   IdmPlannerParameters() : systems::BasicVector<T>(K::kNumCoordinates) {
-    this->SetFromVector(VectorX<T>::Zero(K::kNumCoordinates));
+    this->set_v_ref(10.0);
+    this->set_a(1.0);
+    this->set_b(3.0);
+    this->set_s_0(1.0);
+    this->set_time_headway(0.1);
+    this->set_delta(4.0);
+    this->set_bloat_diameter(4.5);
+    this->set_distance_lower_limit(1e-2);
   }
 
   IdmPlannerParameters<T>* DoClone() const override {
@@ -49,27 +64,34 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
   /// @name Getters and Setters
   //@{
   /// desired velocity in free traffic
+  /// @note @c v_ref is expressed in units of m/s.
   const T& v_ref() const { return this->GetAtIndex(K::kVRef); }
   void set_v_ref(const T& v_ref) { this->SetAtIndex(K::kVRef, v_ref); }
   /// max acceleration
+  /// @note @c a is expressed in units of m/s^2.
   const T& a() const { return this->GetAtIndex(K::kA); }
   void set_a(const T& a) { this->SetAtIndex(K::kA, a); }
   /// comfortable braking deceleration
+  /// @note @c b is expressed in units of m/s^2.
   const T& b() const { return this->GetAtIndex(K::kB); }
   void set_b(const T& b) { this->SetAtIndex(K::kB, b); }
   /// minimum desired net distance
+  /// @note @c s_0 is expressed in units of m.
   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
+  /// @note @c time_headway is expressed in units of s.
   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
+  /// @note @c delta is expressed in units of dimensionless.
   const T& delta() const { return this->GetAtIndex(K::kDelta); }
   void set_delta(const T& delta) { this->SetAtIndex(K::kDelta, delta); }
   /// diameter of circle about the vehicle's pose that encloses its physical
   /// footprint
+  /// @note @c bloat_diameter is expressed in units of m.
   const T& bloat_diameter() const {
     return this->GetAtIndex(K::kBloatDiameter);
   }
@@ -78,6 +100,7 @@ class IdmPlannerParameters : public systems::BasicVector<T> {
   }
   /// lower saturation bound on net distance to prevent near-singular IDM
   /// solutions
+  /// @note @c distance_lower_limit is expressed in units of m.
   const T& distance_lower_limit() const {
     return this->GetAtIndex(K::kDistanceLowerLimit);
   }

drake/automotive/idm_planner.cc

@@ -43,19 +43,8 @@ const T IdmPlanner<T>::Evaluate(const IdmPlannerParameters<T>& params,
 
 template <typename T>
 void IdmPlanner<T>::SetDefaultParameters(IdmPlannerParameters<T>* idm_params) {
-  // Default values from https://en.wikipedia.org/wiki/Intelligent_driver_model.
   DRAKE_DEMAND(idm_params != nullptr);
-  idm_params->set_v_ref(10.);  // desired velocity in free traffic [m/s].
-  idm_params->set_a(T(1.));    // max acceleration [m/s^2].
-  idm_params->set_b(T(3.));    // comfortable braking deceleration [m/s^2].
-  idm_params->set_s_0(T(1.));  // minimum desired net distance [m].
-  idm_params->set_time_headway(T(0.1));  // desired headway to lead vehicle [s].
-  idm_params->set_delta(T(4.));  // recommended choice of acceleration exponent.
-  idm_params->set_bloat_diameter(T(4.5));  // diameter of circle about the
-                                           // vehicle's pose that encloses its
-                                           // physical footprint.
-  idm_params->set_distance_lower_limit(T(1e-2));  // lower saturation bound on
-                                                  // the net distance.
+  idm_params->SetFrom(IdmPlannerParameters<T>());
 }
 
 // These instantiations must match the API documentation in idm_planner.h.

drake/automotive/idm_planner_parameters.named_vector

@@ -1,33 +1,51 @@
+# The default values contained below are from:
+#   https://en.wikipedia.org/wiki/Intelligent_driver_model
 element {
      name: "v_ref"
      doc: "desired velocity in free traffic"
+     doc_units: "m/s"
+     default_value: "10.0"
 }
 element {
      name: "a"
      doc: "max acceleration"
+     doc_units: "m/s^2"
+     default_value: "1.0"
 }
 element {
      name: "b"
      doc: "comfortable braking deceleration"
+     doc_units: "m/s^2"
+     default_value: "3.0"
 }
 element {
      name: "s_0"
      doc: "minimum desired net distance"
+     doc_units: "m"
+     default_value: "1.0"
 }
 element {
      name: "time_headway"
      doc: "desired time headway to vehicle in front"
+     doc_units: "s"
+     default_value: "0.1"
 }
 element {
      name: "delta"
      doc: "free-road exponent"
+     doc_units: "dimensionless"
+     default_value: "4.0"
 }
 element {
      name: "bloat_diameter"
      doc: "diameter of circle about the vehicle's pose that encloses its physical footprint"
+     doc_units: "m"
+     default_value: "4.5"
      # TODO(jadecastro): replace with vehicle geometry.
 }
 element {
      name: "distance_lower_limit"
      doc: "lower saturation bound on net distance to prevent near-singular IDM solutions"
+     doc_units: "m"
+     default_value: "1e-2"
 }

drake/tools/lcm_vector_gen.py

@@ -75,16 +75,56 @@ def generate_indices_storage(cc, caller_context, fields):
     put(cc, '', 1)
 
 
-DEFAULT_CTOR = """
+# One variant of a default constructor (all zeros).  (Depending on the
+# named_vector details, we will either use this variant or the subsequent one.)
+DEFAULT_CTOR_ZEROS = """
   /// Default constructor.  Sets all rows to zero.
   %(camel)s() : systems::BasicVector<T>(K::kNumCoordinates) {
     this->SetFromVector(VectorX<T>::Zero(K::kNumCoordinates));
   }
 """
-
-
-def generate_default_ctor(hh, context, _):
-    put(hh, DEFAULT_CTOR % context, 2)
+# A second variant of a default constructor (field-by-field setting).
+DEFAULT_CTOR_CUSTOM_BEGIN_API = """
+  /// Default constructor.  Sets all rows to their default value:
+"""
+DEFAULT_CTOR_CUSTOM_FIELD_API = """
+  /// @arg @c %(field)s defaults to %(default_value)s in units of %(doc_units)s.
+"""
+DEFAULT_CTOR_CUSTOM_BEGIN_BODY = """
+  %(camel)s() : systems::BasicVector<T>(K::kNumCoordinates) {
+"""
+DEFAULT_CTOR_CUSTOM_FIELD_BODY = """
+    this->set_%(field)s(%(default_value)s);
+"""
+DEFAULT_CTOR_CUSTOM_END = """
+}
+"""
+DEFAULT_CTOR_FIELD_DEFAULT_VALUE = '0.0'  # When not otherwise overridden.
+DEFAULT_CTOR_FIELD_UNKNOWN_DOC_UNITS = 'unknown'
+
+
+def generate_default_ctor(hh, caller_context, fields):
+    # If all defaults are 0.0 and unit-less, then emit the simple ctor.
+    if all([item['default_value'] == DEFAULT_CTOR_FIELD_DEFAULT_VALUE and
+            item['doc_units'] == DEFAULT_CTOR_FIELD_UNKNOWN_DOC_UNITS
+            for item in fields]):
+        put(hh, DEFAULT_CTOR_ZEROS % caller_context, 2)
+        return
+    # Otherwise, emit a customized ctor.
+    put(hh, DEFAULT_CTOR_CUSTOM_BEGIN_API % caller_context, 1)
+    for field in fields:
+        context = dict(caller_context)
+        context.update(field=field['name'])
+        context.update(default_value=field['default_value'])
+        context.update(doc_units=field['doc_units'])
+        put(hh, DEFAULT_CTOR_CUSTOM_FIELD_API % context, 1)
+    put(hh, DEFAULT_CTOR_CUSTOM_BEGIN_BODY % caller_context, 1)
+    for field in fields:
+        context = dict(caller_context)
+        context.update(field=field['name'])
+        context.update(default_value=field['default_value'])
+        put(hh, DEFAULT_CTOR_CUSTOM_FIELD_BODY % context, 1)
+    put(hh, DEFAULT_CTOR_CUSTOM_END % caller_context, 2)
 
 
 DO_CLONE = """
@@ -102,27 +142,36 @@ def generate_do_clone(hh, context, _):
   /// @name Getters and Setters
   //@{
 """
-ACCESSOR = """
-    /// %(doc)s
-    const T& %(field)s() const { return this->GetAtIndex(K::%(kname)s); }
-    void set_%(field)s(const T& %(field)s) {
-      this->SetAtIndex(K::%(kname)s, %(field)s);
-    }
+ACCESSOR_FIELD_DOC = """
+  /// %(doc)s
+"""
+ACCESSOR_FIELD_DOC_UNITS = """
+  /// @note @c %(field)s is expressed in units of %(doc_units)s.
+"""
+ACCESSOR_FIELD_METHODS = """
+  const T& %(field)s() const { return this->GetAtIndex(K::%(kname)s); }
+  void set_%(field)s(const T& %(field)s) {
+    this->SetAtIndex(K::%(kname)s, %(field)s);
+  }
 """
 ACCESSOR_END = """
   //@}
 """
 
 
 def generate_accessors(hh, caller_context, fields):
-    context = dict(caller_context)
-    put(hh, ACCESSOR_BEGIN % context, 1)
+    put(hh, ACCESSOR_BEGIN % caller_context, 1)
     for field in fields:
+        context = dict(caller_context)
         context.update(field=field['name'])
         context.update(kname=to_kname(field['name']))
         context.update(doc=field['doc'])
-        put(hh, ACCESSOR % context, 1)
-    put(hh, ACCESSOR_END % context, 2)
+        context.update(doc_units=field['doc_units'])
+        put(hh, ACCESSOR_FIELD_DOC % context, 1)
+        if context['doc_units'] != DEFAULT_CTOR_FIELD_UNKNOWN_DOC_UNITS:
+            put(hh, ACCESSOR_FIELD_DOC_UNITS % context, 1)
+        put(hh, ACCESSOR_FIELD_METHODS  % context, 1)
+    put(hh, ACCESSOR_END % caller_context, 2)
 
 
 IS_VALID_BEGIN = """
@@ -356,10 +405,27 @@ def generate_code(args):
         with open(args.named_vector_file, "r") as f:
             vec = named_vector_pb2.NamedVector()
             google.protobuf.text_format.Merge(f.read(), vec)
-            fields = [{'name': el.name, 'doc': el.doc} for el in vec.element]
+            fields = [{
+                'name': el.name,
+                'doc': el.doc,
+                'default_value': el.default_value,
+                'doc_units': el.doc_units,
+                } for el in vec.element]
     else:
         # Parse the field names from the command line.
-        fields = [{'name': x, 'doc': x} for x in args.fields]
+        fields = [{
+            'name': x,
+            'doc': x,
+            'default_value': '',
+            'doc_units': '',
+        } for x in args.fields]
+
+    # Default some field attributes if they are missing.
+    for item in fields:
+        if len(item['default_value']) == 0:
+            item['default_value'] = DEFAULT_CTOR_FIELD_DEFAULT_VALUE
+        if len(item['doc_units']) == 0:
+            item['doc_units'] = DEFAULT_CTOR_FIELD_UNKNOWN_DOC_UNITS
 
     # The context provides string substitutions for the C++ code blocks in the
     # literal strings throughout this program.