From 88369fc18d9eee732c1a2ec1f51e0299a4c519bb Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Wed, 29 Mar 2017 17:21:53 -0400 Subject: [PATCH 1/3] Add lcm_vector_gen support for default values --- drake/tools/lcm_vector_gen.py | 100 ++++++++++++++++++++++----- drake/tools/named_vector.proto | 19 +++-- drake/tools/test/gen/sample.h | 9 ++- drake/tools/test/sample.named_vector | 2 + drake/tools/test/sample_test.cc | 4 ++ 5 files changed, 111 insertions(+), 23 deletions(-) diff --git a/drake/tools/lcm_vector_gen.py b/drake/tools/lcm_vector_gen.py index 078e593706f5..afedf92b94d4 100644 --- a/drake/tools/lcm_vector_gen.py +++ b/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(K::kNumCoordinates) { this->SetFromVector(VectorX::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(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,12 +142,17 @@ 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 = """ //@} @@ -115,14 +160,18 @@ def generate_do_clone(hh, context, _): 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. diff --git a/drake/tools/named_vector.proto b/drake/tools/named_vector.proto index 92ef5cf2bd10..77b0bc2c3309 100644 --- a/drake/tools/named_vector.proto +++ b/drake/tools/named_vector.proto @@ -2,10 +2,13 @@ syntax = "proto3"; package tools; -// NamedVector is a descriptor for a vector of elements, each of which has -// a uniquely-identifying name and a docstring. NamedVector is the input format -// for lcm_vector_gen.py, a code generator that produces LCM messages and -// System Framework data structures. +// NamedVector is a descriptor for a vector of elements, each of which has a +// uniquely-identifying name and docstring. Elements optionally also may have: +// - a default value +// - a documented unit of measure +// +// NamedVector is the input format for lcm_vector_gen.py, a code generator that +// produces LCM messages and System Framework data structures. message NamedVector { repeated NamedVectorElement element = 1; } @@ -14,7 +17,15 @@ message NamedVectorElement { // The short name of this element. Should typically contain only lowercase // a-z and underscores, since it appears in generated code. string name = 1; + // A free-text description of this element's purpose. Only appears in // comments. string doc = 2; + + // An optional default value for this element. + string default_value = 3; + + // An optional documentation string stating the units of this element. + // It should be just the units abbreviation, e.g., "m/s". + string doc_units = 4; } diff --git a/drake/tools/test/gen/sample.h b/drake/tools/test/gen/sample.h index 1a9f3e181c1b..812ea1a6e191 100644 --- a/drake/tools/test/gen/sample.h +++ b/drake/tools/test/gen/sample.h @@ -32,9 +32,12 @@ class Sample : public systems::BasicVector { /// An abbreviation for our row index constants. typedef SampleIndices K; - /// Default constructor. Sets all rows to zero. + /// Default constructor. Sets all rows to their default value. + /// @arg @c x defaults to 42.0 in units of m/s. + /// @arg @c two_word defaults to 0.0 in units of unknown. Sample() : systems::BasicVector(K::kNumCoordinates) { - this->SetFromVector(VectorX::Zero(K::kNumCoordinates)); + this->set_x(42.0); + this->set_two_word(0.0); } Sample* DoClone() const override { return new Sample; } @@ -42,10 +45,12 @@ class Sample : public systems::BasicVector { /// @name Getters and Setters //@{ /// Some coordinate + /// @note @c x is expressed in units of m/s. const T& x() const { return this->GetAtIndex(K::kX); } void set_x(const T& x) { this->SetAtIndex(K::kX, x); } /// A very long documentation string that will certainly flow across multiple /// lines of C++ + /// @note @c two_word is expressed in units of unknown. const T& two_word() const { return this->GetAtIndex(K::kTwoWord); } void set_two_word(const T& two_word) { this->SetAtIndex(K::kTwoWord, two_word); diff --git a/drake/tools/test/sample.named_vector b/drake/tools/test/sample.named_vector index 15f409fd098e..a93b082a76f1 100644 --- a/drake/tools/test/sample.named_vector +++ b/drake/tools/test/sample.named_vector @@ -1,6 +1,8 @@ element { name: "x" doc: "Some coordinate" + doc_units: "m/s" + default_value: "42.0" } element { name: "two_word" diff --git a/drake/tools/test/sample_test.cc b/drake/tools/test/sample_test.cc index 65a199d26caa..e8b191978177 100644 --- a/drake/tools/test/sample_test.cc +++ b/drake/tools/test/sample_test.cc @@ -28,6 +28,10 @@ GTEST_TEST(SampleTest, SimpleCoverage) { // Size. EXPECT_EQ(dut.size(), SampleIndices::kNumCoordinates); + // Default values. + EXPECT_EQ(dut.x(), 42.0); + EXPECT_EQ(dut.two_word(), 0.0); + // Accessors. dut.set_x(11.0); dut.set_two_word(22.0); From 0e1554804d67931a78755753cad905fc4032db89 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Wed, 29 Mar 2017 17:21:53 -0400 Subject: [PATCH 2/3] Port BicycleCarParameters to use named_vector defaults --- drake/automotive/bicycle_car.cc | 8 +------ .../bicycle_car_parameters.named_vector | 14 +++++++++++++ drake/automotive/gen/bicycle_car_parameters.h | 21 +++++++++++++++++-- 3 files changed, 34 insertions(+), 9 deletions(-) diff --git a/drake/automotive/bicycle_car.cc b/drake/automotive/bicycle_car.cc index a89a37b4cb96..fd31dee4fb4d 100644 --- a/drake/automotive/bicycle_car.cc +++ b/drake/automotive/bicycle_car.cc @@ -178,16 +178,10 @@ std::unique_ptr> BicycleCar::AllocateParameters() template void BicycleCar::SetDefaultParameters(const systems::LeafContext& context, systems::Parameters* params) const { - // Parameters representative of a Cadillac SRX (from Althoff & Dolan, 2014). auto p = dynamic_cast*>( 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()); } // These instantiations must match the API documentation in bicycle.h. diff --git a/drake/automotive/bicycle_car_parameters.named_vector b/drake/automotive/bicycle_car_parameters.named_vector index a75d8b605217..5db6c3078246 100644 --- a/drake/automotive/bicycle_car_parameters.named_vector +++ b/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" } diff --git a/drake/automotive/gen/bicycle_car_parameters.h b/drake/automotive/gen/bicycle_car_parameters.h index c01d12b8b9f4..d728928b1648 100644 --- a/drake/automotive/gen/bicycle_car_parameters.h +++ b/drake/automotive/gen/bicycle_car_parameters.h @@ -35,9 +35,20 @@ class BicycleCarParameters : public systems::BasicVector { /// 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(K::kNumCoordinates) { - this->SetFromVector(VectorX::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* DoClone() const override { @@ -47,21 +58,27 @@ class BicycleCarParameters : public systems::BasicVector { /// @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); } //@} From 0219cd8e4b8e525e091165135d510430a0407a58 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Wed, 29 Mar 2017 17:21:53 -0400 Subject: [PATCH 3/3] Port IdmPlannerParameters to use named_vector defaults --- drake/automotive/gen/idm_planner_parameters.h | 27 +++++++++++++++++-- drake/automotive/idm_planner.cc | 13 +-------- .../idm_planner_parameters.named_vector | 18 +++++++++++++ 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/drake/automotive/gen/idm_planner_parameters.h b/drake/automotive/gen/idm_planner_parameters.h index 7bc29a2a0a9c..1ae325803789 100644 --- a/drake/automotive/gen/idm_planner_parameters.h +++ b/drake/automotive/gen/idm_planner_parameters.h @@ -37,9 +37,24 @@ class IdmPlannerParameters : public systems::BasicVector { /// 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(K::kNumCoordinates) { - this->SetFromVector(VectorX::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* DoClone() const override { @@ -49,27 +64,34 @@ class IdmPlannerParameters : public systems::BasicVector { /// @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 { } /// 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); } diff --git a/drake/automotive/idm_planner.cc b/drake/automotive/idm_planner.cc index f1cf7cb77ba5..78e31b2178e8 100644 --- a/drake/automotive/idm_planner.cc +++ b/drake/automotive/idm_planner.cc @@ -43,19 +43,8 @@ const T IdmPlanner::Evaluate(const IdmPlannerParameters& params, template void IdmPlanner::SetDefaultParameters(IdmPlannerParameters* 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()); } // These instantiations must match the API documentation in idm_planner.h. diff --git a/drake/automotive/idm_planner_parameters.named_vector b/drake/automotive/idm_planner_parameters.named_vector index 2168fd444991..ffc589b91472 100644 --- a/drake/automotive/idm_planner_parameters.named_vector +++ b/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" }