Merge branch 'add_jerk_limits_to_speed_action' into 'main'

Add jerk limits to SpeedAction

See merge request !150

engine/src/Storyboard/MotionControlAction/SpeedAction_impl.cpp

@@ -11,12 +11,203 @@
 
 #include "Storyboard/MotionControlAction/SpeedAction_impl.h"
 
+#include <MantleAPI/Common/spline.h>
 #include <MantleAPI/Traffic/control_strategy.h>
 #include <MantleAPI/Traffic/entity_helper.h>
 
+#include <optional>
+
 #include "Utils/EntityUtils.h"
 #include "Utils/Logger.h"
 
+using Velocity = units::velocity::meters_per_second_t;
+using Acceleration = units::acceleration::meters_per_second_squared_t;
+using Jerk = units::jerk::meters_per_second_cubed_t;
+using units::literals::operator""_s;
+using units::literals::operator""_ms;
+using units::literals::operator""_mps;
+using units::literals::operator""_mps_cu;
+
+namespace detail
+{
+Velocity GetYOfVelocityPolynomial(
+    units::time::second_t x,
+    const std::tuple<units::unit_t<units::compound_unit<units::velocity::meters_per_second, units::inverse<units::cubed<units::time::second>>>>,
+                     units::unit_t<units::compound_unit<units::velocity::meters_per_second, units::inverse<units::squared<units::time::second>>>>,
+                     units::unit_t<units::compound_unit<units::velocity::meters_per_second, units::inverse<units::time::second>>>,
+                     units::unit_t<units::velocity::meters_per_second>>& polynomial)
+{
+  const auto& [a, b, c, d] = polynomial;
+  return x * (x * (a * x + b) + c) + d;
+}
+
+void AddLinearVelocitySplineSection(std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>>& spline_sections,
+                                    const Velocity& start_speed,
+                                    const Velocity& target_speed,
+                                    const Acceleration& acceleration)
+{
+  const bool is_first_section{spline_sections.empty()};
+  const auto linear_section_start_time{is_first_section ? 0.0_s : units::time::second_t{spline_sections[0].end_time}};
+  const auto linear_section_start_speed{is_first_section ? start_speed : GetYOfVelocityPolynomial(linear_section_start_time, spline_sections[0].polynomial)};
+  const auto linear_section_duration{units::math::abs(target_speed - linear_section_start_speed) / acceleration};
+
+  mantle_api::SplineSection<units::velocity::meters_per_second> linear_section;
+  linear_section.start_time = linear_section_start_time;
+  linear_section.end_time = linear_section_start_time + linear_section_duration;
+  auto& [a, b, c, d] = linear_section.polynomial;
+  c = units::math::copysign(acceleration, (target_speed - start_speed).value());
+  d = linear_section_start_speed;
+
+  spline_sections.push_back(linear_section);
+}
+
+void AddInitialSquaredAccelerationSplineSection(std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>>& spline_sections,
+                                                const Velocity& start_speed,
+                                                const Velocity& speed_difference,
+                                                const Acceleration& target_acceleration,
+                                                const Jerk& jerk_limit)
+{
+  mantle_api::SplineSection<units::velocity::meters_per_second> squared_deceleration_section;
+  squared_deceleration_section.start_time = 0.0_ms;
+  squared_deceleration_section.end_time = target_acceleration / jerk_limit;
+  auto& [a, b, c, d] = squared_deceleration_section.polynomial;
+  b = units::math::copysign(0.5 * jerk_limit, speed_difference);
+  d = start_speed;
+
+  spline_sections.push_back(squared_deceleration_section);
+}
+
+void AddFinalSquaredAccelerationSplineSection(std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>>& spline_sections,
+                                              const Velocity& start_speed,
+                                              const Velocity& target_speed,
+                                              const Acceleration& target_acceleration,
+                                              const Jerk& jerk_limit)
+{
+  mantle_api::SplineSection<units::velocity::meters_per_second> squared_acceleration_section;
+  auto& [_, b, c, d] = squared_acceleration_section.polynomial;
+  b = 0.5 * jerk_limit;
+  c = -1.0 * target_acceleration;
+
+  const auto squared_acceleration_section_duration{target_acceleration / jerk_limit};
+  const auto squared_acceleration_section_start_speed{target_speed - GetYOfVelocityPolynomial(squared_acceleration_section_duration, squared_acceleration_section.polynomial)};
+
+  const auto is_second_section{spline_sections.size() == 1};
+  const auto time_offset{is_second_section ? 0.0_ms : spline_sections.back().start_time};
+  const auto speed_offset{is_second_section ? start_speed : std::get<3>(spline_sections.back().polynomial)};
+
+  squared_acceleration_section.start_time = time_offset + (units::math::abs(squared_acceleration_section_start_speed - speed_offset) / target_acceleration);
+  squared_acceleration_section.end_time = squared_acceleration_section.start_time + squared_acceleration_section_duration;
+  d = squared_acceleration_section_start_speed;
+
+  spline_sections.back().end_time = squared_acceleration_section.start_time;
+  spline_sections.push_back(squared_acceleration_section);
+}
+
+void AddFinalSquaredDecelerationSplineSection(std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>>& spline_sections,
+                                              const Velocity& start_speed,
+                                              const Velocity& target_speed,
+                                              const Acceleration& target_acceleration,
+                                              const Jerk& jerk_limit)
+{
+  mantle_api::SplineSection<units::velocity::meters_per_second> squared_deceleration_section;
+  auto& [_, b, c, d] = squared_deceleration_section.polynomial;
+  b = -0.5 * jerk_limit;
+  c = target_acceleration;
+
+  const auto squared_deceleration_section_duration{target_acceleration / jerk_limit};
+  const auto squared_deceleration_section_start_speed{target_speed - GetYOfVelocityPolynomial(squared_deceleration_section_duration, squared_deceleration_section.polynomial)};
+
+  const auto is_second_section{spline_sections.size() == 1};
+  const auto time_offset{is_second_section ? 0.0_ms : spline_sections.back().start_time};
+  const auto speed_offset{is_second_section ? start_speed : std::get<3>(spline_sections.back().polynomial)};
+
+  squared_deceleration_section.start_time = time_offset + ((squared_deceleration_section_start_speed - speed_offset) / target_acceleration);
+  squared_deceleration_section.end_time = squared_deceleration_section.start_time + squared_deceleration_section_duration;
+  d = squared_deceleration_section_start_speed;
+
+  spline_sections.back().end_time = squared_deceleration_section.start_time;
+  spline_sections.push_back(squared_deceleration_section);
+}
+
+bool IsJerkLimitSet(units::jerk::meters_per_second_cubed_t jerk_limit)
+{
+  return jerk_limit() < std::numeric_limits<double>::max() && !mantle_api::AlmostEqual(jerk_limit(), 0.0);
+}
+
+std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> GetVelocitySplineSectionsAccordingToJerkLimits(
+    const std::optional<mantle_api::Performance>& performance,
+    const Velocity& start_speed,
+    const Velocity& target_speed,
+    const Acceleration& start_acceleration,
+    const Acceleration& target_acceleration)
+{
+  std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> spline_sections;
+
+  if (!performance.has_value())
+  {
+    AddLinearVelocitySplineSection(spline_sections, start_speed, target_speed, target_acceleration);
+    return spline_sections;
+  }
+
+  const auto speed_difference{target_speed - start_speed};
+
+  if (speed_difference > 0.0_mps)
+  {
+    if (IsJerkLimitSet(performance->max_acceleration_rate))
+    {
+      AddInitialSquaredAccelerationSplineSection(spline_sections, start_speed, speed_difference, target_acceleration, performance->max_acceleration_rate);
+    }
+
+    AddLinearVelocitySplineSection(spline_sections, start_speed, target_speed, target_acceleration);
+
+    if (IsJerkLimitSet(performance->max_deceleration_rate))
+    {
+      AddFinalSquaredDecelerationSplineSection(spline_sections, start_speed, target_speed, target_acceleration, performance->max_deceleration_rate);
+    }
+  }
+  else if (speed_difference < 0.0_mps)
+  {
+    if (IsJerkLimitSet(performance->max_deceleration_rate))
+    {
+      AddInitialSquaredAccelerationSplineSection(spline_sections, start_speed, speed_difference, target_acceleration, performance->max_deceleration_rate);
+    }
+
+    AddLinearVelocitySplineSection(spline_sections, start_speed, target_speed, target_acceleration);
+
+    if (IsJerkLimitSet(performance->max_acceleration_rate))
+    {
+      AddFinalSquaredAccelerationSplineSection(spline_sections, start_speed, target_speed, target_acceleration, performance->max_acceleration_rate);
+    }
+  }
+
+  return spline_sections;
+}
+
+std::optional<mantle_api::Performance> GetVehiclePerformance(const mantle_api::IEntity& entity)
+{
+  if (auto* properties = dynamic_cast<mantle_api::VehicleProperties*>(entity.GetProperties()))
+  {
+    const auto& entity_name{entity.GetName()};
+
+    if (properties->performance.max_acceleration_rate < 0.0_mps_cu)
+    {
+      OpenScenarioEngine::v1_2::Logger::Warning("SpeedAction: the \'maxAccelerationRate\' performance parameter of entity " + entity_name + " cannot be negative. Using positive value instead.");
+      properties->performance.max_acceleration_rate *= -1.0;
+    }
+
+    if (properties->performance.max_deceleration_rate < 0.0_mps_cu)
+    {
+      OpenScenarioEngine::v1_2::Logger::Warning("SpeedAction: the \'maxDecelerationRate\' performance parameter of entity " + entity_name + " cannot be negative. Using positive value instead.");
+      properties->performance.max_deceleration_rate *= -1.0;
+    }
+
+    return properties->performance;
+  }
+
+  return std::nullopt;
+}
+
+}  // namespace detail
 namespace OpenScenarioEngine::v1_2
 {
 void SpeedAction::SetControlStrategy()
@@ -75,57 +266,83 @@ mantle_api::MovementDomain SpeedAction::GetMovementDomain() const
   return mantle_api::MovementDomain::kLongitudinal;
 }
 
-void SpeedAction::SetSpline(const mantle_api::IEntity &entity,
-                            mantle_api::SplineSection<units::velocity::meters_per_second> spline_section,
-                            units::velocity::meters_per_second_t target_speed)
+void SpeedAction::SetSpline(mantle_api::UniqueId entity_id,
+                            Velocity target_speed,
+                            const std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>>& spline_sections)
 {
-  std::get<3>(spline_section.polynomial) = entity.GetVelocity().Length();
-  auto control_strategy = std::make_shared<mantle_api::FollowVelocitySplineControlStrategy>();
+  auto control_strategy{std::make_shared<mantle_api::FollowVelocitySplineControlStrategy>()};
   control_strategy->default_value = target_speed;
-  control_strategy->velocity_splines.push_back(std::move(spline_section));
-  mantle.environment->UpdateControlStrategies(entity.GetUniqueId(), {control_strategy});
+  control_strategy->velocity_splines = spline_sections;
+  mantle.environment->UpdateControlStrategies(entity_id, {control_strategy});
 }
 
 void SpeedAction::SetLinearVelocitySplineControlStrategy(const std::string &actor)
 {
   auto &entity = EntityUtils::GetEntityByName(mantle.environment, actor);
+  const auto start_speed{entity.GetVelocity().Length()};
   const auto target_speed = values.GetSpeedActionTarget();
 
   mantle_api::SplineSection<units::velocity::meters_per_second> spline_section;
-  spline_section.start_time = mantle_api::Time(0);
+  spline_section.start_time = 0.0_ms;
+
+  if (mantle_api::AlmostEqual(start_speed, target_speed))
+  {
+    spline_section.end_time = 0.0_ms;
+    std::get<3>(spline_section.polynomial) = start_speed;
+    SetSpline(entity.GetUniqueId(), target_speed, {spline_section});
+    return;
+  }
   if (values.speedActionDynamics.transitionDynamics.dimension == mantle_api::Dimension::kTime)
   {
-    const auto duration = units::time::second_t(values.speedActionDynamics.transitionDynamics.value);
+    const units::time::second_t duration{values.speedActionDynamics.transitionDynamics.value};
+    auto target_acceleration{(target_speed - start_speed) / duration};
+
     spline_section.end_time = duration;
-    std::get<2>(spline_section.polynomial) = (target_speed - entity.GetVelocity().Length()) / duration;
-    SetSpline(entity, spline_section, target_speed);
+    std::get<2>(spline_section.polynomial) = target_acceleration;
+    std::get<3>(spline_section.polynomial) = start_speed;
+    SetSpline(entity.GetUniqueId(), target_speed, {spline_section});
     return;
   }
   if (values.speedActionDynamics.transitionDynamics.dimension == mantle_api::Dimension::kRate)
   {
-    const auto acceleration = units::acceleration::meters_per_second_squared_t(values.speedActionDynamics.transitionDynamics.value);
-    spline_section.end_time = units::math::abs(target_speed - entity.GetVelocity().Length()) / acceleration;
-    std::get<2>(spline_section.polynomial) = units::math::copysign(acceleration, target_speed - entity.GetVelocity().Length());
-    SetSpline(entity, spline_section, target_speed);
+    const auto performance{detail::GetVehiclePerformance(entity)};
+    const Acceleration target_acceleration{std::abs(values.speedActionDynamics.transitionDynamics.value)};
+
+    if (values.speedActionDynamics.followingMode.has_value() && (values.speedActionDynamics.followingMode.value() == FollowingMode::kFollow))
+    {
+      const auto spline_sections{detail::GetVelocitySplineSectionsAccordingToJerkLimits(performance,
+                                                                                        start_speed,
+                                                                                        target_speed,
+                                                                                        entity.GetAcceleration().Length(),
+                                                                                        target_acceleration)};
+
+      SetSpline(entity.GetUniqueId(), target_speed, spline_sections);
+    }
+    else
+    {
+      spline_section.end_time = units::math::abs(target_speed - start_speed) / target_acceleration;
+      std::get<2>(spline_section.polynomial) = units::math::copysign(target_acceleration, target_speed - start_speed);
+      std::get<3>(spline_section.polynomial) = start_speed;
+      SetSpline(entity.GetUniqueId(), target_speed, {spline_section});
+    }
+
     return;
   }
   if (values.speedActionDynamics.transitionDynamics.dimension == mantle_api::Dimension::kDistance)
   {
-    const auto start_speed = entity.GetVelocity().Length().value();
-
     // Distance has to be converted to time, since spline is expected over time, not distance
     //  distance = time(v_init + v_delta/2) = time(v_init + v_end)/2 => time = 2*distance/(v_init + v_end)
-    spline_section.end_time
-      = units::time::second_t((2 * values.speedActionDynamics.transitionDynamics.value)/(target_speed.value() + start_speed));
+    const units::time::second_t duration{(2 * values.speedActionDynamics.transitionDynamics.value) / (target_speed.value() + start_speed.value())};
 
     // v_end = v_init + acceleration*time, where time = 2*distance/(v_init + v_end) then acceleration = (v_end -
     // v_init) * (v_end + v_init) / 2* distance
-    auto rate = units::acceleration::meters_per_second_squared_t((target_speed.value() - start_speed) *
-                                                                 (target_speed.value() + start_speed) /
-                                                                 (2 * values.speedActionDynamics.transitionDynamics.value));
-    std::get<2>(spline_section.polynomial) = units::math::copysign(
-      (units::acceleration::meters_per_second_squared_t(rate)), target_speed - entity.GetVelocity().Length());
-    SetSpline(entity, spline_section, target_speed);
+    Acceleration target_acceleration{(target_speed.value() - start_speed.value()) * (target_speed.value() + start_speed.value()) /
+                                     (2 * values.speedActionDynamics.transitionDynamics.value)};
+
+    spline_section.end_time = duration;
+    std::get<2>(spline_section.polynomial) = units::math::copysign(target_acceleration, target_speed - entity.GetVelocity().Length());
+    std::get<3>(spline_section.polynomial) = start_speed;
+    SetSpline(entity.GetUniqueId(), target_speed, {spline_section});
     return;
   }
   throw std::runtime_error("Dimension must be either kTime, kRate, or kDistance");

engine/src/Storyboard/MotionControlAction/SpeedAction_impl.h

@@ -29,9 +29,9 @@ public:
   [[nodiscard]] mantle_api::MovementDomain GetMovementDomain() const override;
 
 private:
-  void SetSpline(const mantle_api::IEntity &entity,
-                 mantle_api::SplineSection<units::velocity::meters_per_second> spline_section,
-                 units::velocity::meters_per_second_t target_speed);
+  void SetSpline(mantle_api::UniqueId entity_id,
+                 units::velocity::meters_per_second_t target_speed,
+                 const std::vector<mantle_api::SplineSection<units::velocity::meters_per_second>> &spline_sections);
   void SetLinearVelocitySplineControlStrategy(const std::string &actor);
 };
 

engine/src/Utils/EntityCreator.cpp

@@ -80,11 +80,25 @@ void EntityCreator::CreateVehicle(std::shared_ptr<NET_ASAM_OPENSCENARIO::v1_2::I
   FillBoundingBoxProperties(properties, vehicle->GetBoundingBox(), vehicle->GetName());
   FillGenericProperties(properties, vehicle->GetProperties()->GetProperties());
 
+  const auto& performance{vehicle->GetPerformance()};
+
+  properties.performance.max_speed = units::velocity::meters_per_second_t(performance->GetMaxSpeed());
   properties.performance.max_acceleration =
-      units::acceleration::meters_per_second_squared_t(vehicle->GetPerformance()->GetMaxAcceleration());
+      units::acceleration::meters_per_second_squared_t(performance->GetMaxAcceleration());
   properties.performance.max_deceleration =
-      units::acceleration::meters_per_second_squared_t(vehicle->GetPerformance()->GetMaxDeceleration());
-  properties.performance.max_speed = units::velocity::meters_per_second_t(vehicle->GetPerformance()->GetMaxSpeed());
+      units::acceleration::meters_per_second_squared_t(performance->GetMaxDeceleration());
+
+  if (performance->IsSetMaxAccelerationRate())
+  {
+    properties.performance.max_acceleration_rate =
+        units::jerk::meters_per_second_cubed_t(performance->GetMaxAccelerationRate());
+  }
+
+  if (performance->IsSetMaxDecelerationRate())
+  {
+    properties.performance.max_deceleration_rate =
+        units::jerk::meters_per_second_cubed_t(performance->GetMaxDecelerationRate());
+  }
 
   FillAxleProperties(properties.front_axle, vehicle->GetAxles()->GetFrontAxle(), vehicle->GetBoundingBox(), name);
   FillAxleProperties(properties.rear_axle, vehicle->GetAxles()->GetRearAxle(), vehicle->GetBoundingBox(), name);