Added substep algorithm for chrono. Added initial UE4 terrain detection with raycast.

LibCarla/source/carla/client/Vehicle.cpp

@@ -96,8 +96,8 @@ namespace client {
     GetEpisode().Lock()->UseCarSimRoad(*this, enabled);
   }
 
-  void Vehicle::EnableChronoPhysics() {
-    GetEpisode().Lock()->EnableChronoPhysics(*this);
+  void Vehicle::EnableChronoPhysics(uint64_t MaxSubsteps, float MaxSubstepDeltaTime) {
+    GetEpisode().Lock()->EnableChronoPhysics(*this, MaxSubsteps, MaxSubstepDeltaTime);
   }
 
 } // namespace client

LibCarla/source/carla/client/Vehicle.h

@@ -93,7 +93,7 @@ namespace client {
     /// Enables the use of CarSim internal road definition instead of unreal's
     void UseCarSimRoad(bool enabled);
 
-    void EnableChronoPhysics();
+    void EnableChronoPhysics(uint64_t MaxSubsteps, float MaxSubstepDeltaTime);
 
   private:
 

LibCarla/source/carla/client/detail/Client.cpp

@@ -338,8 +338,8 @@ namespace detail {
     _pimpl->AsyncCall("use_carsim_road", vehicle, enabled);
   }
 
-  void Client::EnableChronoPhysics(rpc::ActorId vehicle) {
-    _pimpl->AsyncCall("enable_chrono_physics", vehicle);
+  void Client::EnableChronoPhysics(rpc::ActorId vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime) {
+    _pimpl->AsyncCall("enable_chrono_physics", vehicle, MaxSubsteps, MaxSubstepDeltaTime);
   }
 
   void Client::ApplyControlToWalker(rpc::ActorId walker, const rpc::WalkerControl &control) {

LibCarla/source/carla/client/detail/Client.h

@@ -220,7 +220,9 @@ namespace detail {
         bool enabled);
 
     void EnableChronoPhysics(
-        rpc::ActorId vehicle);
+        rpc::ActorId vehicle,
+        uint64_t MaxSubsteps,
+        float MaxSubstepDeltaTime);
 
     void ApplyControlToWalker(
         rpc::ActorId walker,

LibCarla/source/carla/client/detail/Simulator.h

@@ -454,8 +454,8 @@ namespace detail {
       _client.UseCarSimRoad(vehicle.GetId(), enabled);
     }
 
-    void EnableChronoPhysics(Vehicle &vehicle) {
-      _client.EnableChronoPhysics(vehicle.GetId());
+    void EnableChronoPhysics(Vehicle &vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime) {
+      _client.EnableChronoPhysics(vehicle.GetId(), MaxSubsteps, MaxSubstepDeltaTime);
     }
 
     /// @}

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -143,7 +143,7 @@ void export_actor() {
       .def("get_traffic_light", &cc::Vehicle::GetTrafficLight)
       .def("enable_carsim", &cc::Vehicle::EnableCarSim, (arg("simfile_path") = ""))
       .def("use_carsim_road", &cc::Vehicle::UseCarSimRoad, (arg("enabled")))
-      .def("enable_chrono_physics", &cc::Vehicle::EnableChronoPhysics)
+      .def("enable_chrono_physics", &cc::Vehicle::EnableChronoPhysics, (arg("max_substeps")=10, arg("max_substep_delta_time")=0.01))
       .def(self_ns::str(self_ns::self))
   ;
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp

@@ -1134,7 +1134,9 @@ void FCarlaServer::FPimpl::BindActions()
   };
 
   BIND_SYNC(enable_chrono_physics) << [this](
-      cr::ActorId ActorId) -> R<void>
+      cr::ActorId ActorId,
+      uint64_t MaxSubsteps,
+      float MaxSubstepDeltaTime) -> R<void>
   {
     REQUIRE_CARLA_EPISODE();
     auto ActorView = Episode->FindActor(ActorId);
@@ -1147,7 +1149,7 @@ void FCarlaServer::FPimpl::BindActions()
     {
       RESPOND_ERROR("unable to set chrono physics: not actor is not a vehicle");
     }
-    UChronoMovementComponent::CreateChronoMovementComponent(Vehicle);
+    UChronoMovementComponent::CreateChronoMovementComponent(Vehicle, MaxSubsteps, MaxSubstepDeltaTime);
     return R<void>::Success();
   };
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.cpp

@@ -14,10 +14,12 @@ using namespace chrono::vehicle::hmmwv;
 #endif
 
 void UChronoMovementComponent::CreateChronoMovementComponent(
-    ACarlaWheeledVehicle* Vehicle)
+    ACarlaWheeledVehicle* Vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime)
 {
   #ifdef WITH_CHRONO
   UChronoMovementComponent* ChronoMovementComponent = NewObject<UChronoMovementComponent>(Vehicle);
+  ChronoMovementComponent->MaxSubsteps = MaxSubsteps;
+  ChronoMovementComponent->MaxSubstepDeltaTime = MaxSubstepDeltaTime;
   ChronoMovementComponent->RegisterComponent();
   Vehicle->SetCarlaMovementComponent(ChronoMovementComponent);
   #else
@@ -28,16 +30,35 @@ void UChronoMovementComponent::CreateChronoMovementComponent(
 #ifdef WITH_CHRONO
 
 constexpr double CMTOM = 0.01;
-ChVector<> UE4LocationToChrono(FVector& Location)
+ChVector<> UE4LocationToChrono(const FVector& Location)
 {
   return CMTOM*ChVector<>(Location.Y, Location.Z, -Location.X);
 }
 constexpr double MTOCM = 100;
-FVector ChronoToUE4Location(ChVector<>& position)
+FVector ChronoToUE4Location(const ChVector<>& position)
 {
   return MTOCM*FVector(-position.z(), position.x(), position.y());
 }
 
+UERayCastTerrain::UERayCastTerrain(UWorld* World, ChVehicle* Vehicle)
+    : UEWorld(World), ChronoVehicle(Vehicle) {}
+
+double UERayCastTerrain::GetHeight(double x, double y) const
+{
+  double z = ChronoVehicle->GetVehiclePos().z();
+  FVector Location = ChronoToUE4Location({x, y, z});
+  carla::log_warning("GetHeight:", x, y, z);
+  return 0;
+}
+ChVector<> UERayCastTerrain::GetNormal(double x, double y) const
+{
+  return ChVector<>(0,1,0);
+}
+float UERayCastTerrain::GetCoefficientFriction(double x, double y) const
+{
+  return 1;
+}
+
 void UChronoMovementComponent::BeginPlay()
 {
   Super::BeginPlay();
@@ -45,7 +66,7 @@ void UChronoMovementComponent::BeginPlay()
   DisableUE4VehiclePhysics();
 
   // // // Chrono system
-  sys.Set_G_acc(ChVector<>(0, 0, 0));
+  sys.Set_G_acc(ChVector<>(0, -9.8, 0));
   sys.SetSolverType(ChSolver::Type::BARZILAIBORWEIN);
   sys.SetSolverMaxIterations(150);
   sys.SetMaxPenetrationRecoverySpeed(4.0);
@@ -66,12 +87,8 @@ void UChronoMovementComponent::BeginPlay()
   my_hmmwv.Initialize();
 
   // Create the terrain
-  terrain = chrono_types::make_shared<RigidTerrain>(&sys);
-  auto patch = terrain->AddPatch(ChCoordsys<>(ChVector<>(0, 0, 0), QUNIT), ChVector<>(200, 100, -10));
-  patch->SetContactFrictionCoefficient(0.9f);
-  patch->SetContactRestitutionCoefficient(0.01f);
-  patch->SetContactMaterialProperties(2e7f, 0.3f);
-  terrain->Initialize();
+  terrain = chrono_types::make_shared<UERayCastTerrain>(GetWorld(), &my_hmmwv.GetVehicle());
+
   carla::log_warning("ChronoBeginPlay");
 }
 
@@ -84,7 +101,6 @@ void UChronoMovementComponent::ProcessControl(FVehicleControl &Control)
   double Brake = Control.Brake + Control.bHandBrake;
 
   my_hmmwv.Synchronize(Time, {Throttle, Steering, Brake}, *terrain.get());
-  terrain->Synchronize(Time);
   carla::log_warning("ChronoProcessControl");
 }
 
@@ -92,23 +108,33 @@ void UChronoMovementComponent::TickComponent(float DeltaTime,
       ELevelTick TickType,
       FActorComponentTickFunction* ThisTickFunction)
 {
-
-  // Maximum delta time for the simulation to be stable
-  // TODO: make this a customizable value
-  const float MaxDeltaTime = 0.002;
-  if (DeltaTime > MaxDeltaTime)
+  carla::log_warning("DeltaTime:", DeltaTime);
+  if (DeltaTime > MaxSubstepDeltaTime)
   {
-    uint64_t NumberSubSteps = FGenericPlatformMath::FloorToInt(DeltaTime/MaxDeltaTime);
-    carla::log_warning("Number of chrono substeps:", NumberSubSteps);
-    for (uint64_t i = 0; i < NumberSubSteps; ++i)
+    uint64_t NumberSubSteps = FGenericPlatformMath::FloorToInt(DeltaTime/MaxSubstepDeltaTime);
+    if (NumberSubSteps < MaxSubsteps)
     {
-      AdvanceChronoSimulation(MaxDeltaTime);
+      for (uint64_t i = 0; i < NumberSubSteps; ++i)
+      {
+        AdvanceChronoSimulation(MaxSubstepDeltaTime);
+      }
+      float RemainingTime = DeltaTime - NumberSubSteps*MaxSubstepDeltaTime;
+      AdvanceChronoSimulation(RemainingTime);
+      carla::log_warning("NumberSubSteps:", NumberSubSteps);
+    }
+    else
+    {
+      double SubDelta = DeltaTime / MaxSubsteps;
+      for (uint64_t i = 0; i < MaxSubsteps; ++i)
+      {
+        AdvanceChronoSimulation(SubDelta);
+      }
+      carla::log_warning("MaxSubsteps limit, SubDelta:", SubDelta);
     }
-    float RemainingTime = DeltaTime - NumberSubSteps*MaxDeltaTime;
-    AdvanceChronoSimulation(RemainingTime);
   }
   else
   {
+    carla::log_warning("Single step");
     AdvanceChronoSimulation(DeltaTime);
   }
 
@@ -119,15 +145,21 @@ void UChronoMovementComponent::TickComponent(float DeltaTime,
   carla::log_warning("Time:", Time);
   carla::log_warning("vehicle pos (", VehiclePos.x(), VehiclePos.y(), VehiclePos.z(), ")");
   carla::log_warning("vehicle rot (", VehicleRot.e1(), VehicleRot.e2(), VehicleRot.e3(), VehicleRot.e0(), ")");
-  CarlaVehicle->SetActorLocation(ChronoToUE4Location(VehiclePos));
-  CarlaVehicle->SetActorRotation(FQuat(VehicleRot.e1(), VehicleRot.e2(), VehicleRot.e3(), VehicleRot.e0()));
+  FVector NewLocation = ChronoToUE4Location(VehiclePos);
+  if(NewLocation.ContainsNaN())
+  {
+    carla::log_warning("Error: Chrono vehicle position contains NaN");
+    UDefaultMovementComponent::CreateDefaultMovementComponent(CarlaVehicle);
+    return;
+  }
+  CarlaVehicle->SetActorLocation(NewLocation);
+  // CarlaVehicle->SetActorRotation(FQuat(VehicleRot.e1(), VehicleRot.e2(), VehicleRot.e3(), VehicleRot.e0()));
 
   carla::log_warning("ChronoTick");
 }
 
 void UChronoMovementComponent::AdvanceChronoSimulation(float StepSize)
 {
-  terrain->Advance(StepSize);
   my_hmmwv.Advance(StepSize);
   sys.DoStepDynamics(StepSize);
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.h

@@ -15,7 +15,7 @@
 
 #include "chrono/physics/ChSystemNSC.h"
 #include "chrono_vehicle/ChVehicleModelData.h"
-#include "chrono_vehicle/terrain/RigidTerrain.h"
+#include "chrono_vehicle/ChTerrain.h"
 #include "chrono_vehicle/driver/ChDataDriver.h"
 #include "chrono_models/vehicle/hmmwv/HMMWV.h"
 
@@ -24,6 +24,20 @@
 
 #include "ChronoMovementComponent.generated.h"
 
+#ifdef WITH_CHRONO
+class UERayCastTerrain : public chrono::vehicle::ChTerrain
+{
+  UWorld* UEWorld;
+  chrono::vehicle::ChVehicle* ChronoVehicle;
+public:
+  UERayCastTerrain(UWorld* World, chrono::vehicle::ChVehicle* Vehicle);
+
+  double GetHeight(double x, double y) const override;
+  chrono::ChVector<> GetNormal(double x, double y) const override;
+  float GetCoefficientFriction(double x, double y) const;
+};
+#endif
+
 UCLASS(Blueprintable, meta=(BlueprintSpawnableComponent) )
 class CARLA_API UChronoMovementComponent : public UBaseCarlaMovementComponent
 {
@@ -32,13 +46,16 @@ class CARLA_API UChronoMovementComponent : public UBaseCarlaMovementComponent
 #ifdef WITH_CHRONO
   chrono::ChSystemNSC sys;
   chrono::vehicle::hmmwv::HMMWV_Full my_hmmwv;
-  std::shared_ptr<chrono::vehicle::RigidTerrain> terrain;
+  std::shared_ptr<UERayCastTerrain> terrain;
 #endif
 
+  uint64_t MaxSubsteps = 10;
+  float MaxSubstepDeltaTime = 0.01;
+
 public:
 
 
-  static void CreateChronoMovementComponent(ACarlaWheeledVehicle* Vehicle);
+  static void CreateChronoMovementComponent(ACarlaWheeledVehicle* Vehicle, uint64_t MaxSubsteps, float MaxSubstepDeltaTime);
 
   #ifdef WITH_CHRONO
   virtual void BeginPlay() override;