From f45e0d7c08b6db2bd68e3c656bb850e4406f3f99 Mon Sep 17 00:00:00 2001 From: Axel Date: Mon, 1 Feb 2021 17:48:56 +0100 Subject: [PATCH] Added substep algorithm for chrono. Added initial UE4 terrain detection with raycast. --- LibCarla/source/carla/client/Vehicle.cpp | 4 +- LibCarla/source/carla/client/Vehicle.h | 2 +- .../source/carla/client/detail/Client.cpp | 4 +- LibCarla/source/carla/client/detail/Client.h | 4 +- .../source/carla/client/detail/Simulator.h | 4 +- PythonAPI/carla/source/libcarla/Actor.cpp | 2 +- .../Carla/Source/Carla/Server/CarlaServer.cpp | 6 +- .../ChronoMovementComponent.cpp | 82 +++++++++++++------ .../ChronoMovementComponent.h | 23 +++++- 9 files changed, 92 insertions(+), 39 deletions(-) diff --git a/LibCarla/source/carla/client/Vehicle.cpp b/LibCarla/source/carla/client/Vehicle.cpp index 51955e49f..6f145571e 100644 --- a/LibCarla/source/carla/client/Vehicle.cpp +++ b/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 diff --git a/LibCarla/source/carla/client/Vehicle.h b/LibCarla/source/carla/client/Vehicle.h index 28e3eb77c..80cc65f91 100644 --- a/LibCarla/source/carla/client/Vehicle.h +++ b/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: diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 188562f28..05793623b 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/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) { diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index d7cee3554..9e96da13d 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/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, diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index d89c4c5f9..61caad163 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/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); } /// @} diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index 9e9eaa28c..dd4760088 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/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)) ; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index ca702c716..bb7bfc2e8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/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 + cr::ActorId ActorId, + uint64_t MaxSubsteps, + float MaxSubstepDeltaTime) -> R { 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::Success(); }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.cpp index ec0496982..ea87b01b1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.cpp +++ b/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(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(&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(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); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.h index fc2081284..028ec0580 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/MovementComponents/ChronoMovementComponent.h +++ b/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 terrain; + std::shared_ptr 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;