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" }