From 906b4658d5a28054d834e51c7677a9033b46f863 Mon Sep 17 00:00:00 2001 From: Blyron <53337103+Blyron@users.noreply.github.com> Date: Wed, 2 Nov 2022 16:13:22 +0100 Subject: [PATCH] Aaron/terramechanics performance (#5900) * Added local frame option. Fixed inputs for NN * Add Textures * Changed texture to grayscale RGB and added check for 'air' particles * Updated 2k and 4k textures enable parameter for select res texture for deformation * Fix issue where trying to access TMap when loading * Use contains functions instead of checking num of elemnts on tmap to make a secure access * Used ParticlesHeightmap optimization and clear some log * Fixed number of particles inputs. Removed debug output. Fixed mutex creating cpu stalls * Decoupling visualization with gamethread * Make work optimisations * Disable deformation visualization * Updated collisions parameters, collisions itself are not updated * Disable Collisions vehicle with landscape * Add in pytorch output wheel normal and put them into UE4 and apply to wheels * Added factors based on distance, never negative, check if there are particles * Fix compilation error * Removed unused code and normals related code * Fixed compiling error Co-authored-by: Axel --- LibCarla/source/carla/pytorch/pytorch.cpp | 27 +++----- LibCarla/source/carla/pytorch/pytorch.h | 3 - .../Source/Carla/MapGen/LargeMapManager.h | 2 + .../Vehicle/CustomTerrainPhysicsComponent.cpp | 66 ++++--------------- .../Vehicle/CustomTerrainPhysicsComponent.h | 4 +- 5 files changed, 24 insertions(+), 78 deletions(-) diff --git a/LibCarla/source/carla/pytorch/pytorch.cpp b/LibCarla/source/carla/pytorch/pytorch.cpp index c932502ab..9076d84e7 100644 --- a/LibCarla/source/carla/pytorch/pytorch.cpp +++ b/LibCarla/source/carla/pytorch/pytorch.cpp @@ -66,8 +66,7 @@ namespace learning { WheelOutput GetWheelTensorOutput( const at::Tensor &particle_forces, - const at::Tensor &wheel_forces, - const at::Tensor &wheel_normal) { + const at::Tensor &wheel_forces ) { WheelOutput result; const float* wheel_forces_data = wheel_forces.data_ptr(); result.wheel_forces_x = wheel_forces_data[0]; @@ -76,10 +75,6 @@ namespace learning { result.wheel_torque_x = wheel_forces_data[3]; result.wheel_torque_y = wheel_forces_data[4]; result.wheel_torque_z = wheel_forces_data[5]; - const float* wheel_normal_data = wheel_normal.data_ptr(); - result.wheel_normal_x = wheel_normal_data[0]; - result.wheel_normal_y = wheel_normal_data[1]; - result.wheel_normal_z = wheel_normal_data[2]; const float* particle_forces_data = particle_forces.data_ptr(); int num_dimensions = 3; int num_particles = particle_forces.sizes()[0]; @@ -207,13 +202,13 @@ namespace learning { std::vector Tensors = Output.toTuple()->elements(); _output.wheel0 = GetWheelTensorOutput( - Tensors[0].toTensor().cpu(), Tensors[4].toTensor().cpu(), Tensors[8].toTensor().cpu()); + Tensors[0].toTensor().cpu(), Tensors[4].toTensor().cpu() ); _output.wheel1 = GetWheelTensorOutput( - Tensors[1].toTensor().cpu(), Tensors[5].toTensor().cpu(), Tensors[9].toTensor().cpu()); + Tensors[1].toTensor().cpu(), Tensors[5].toTensor().cpu() ); _output.wheel2 = GetWheelTensorOutput( - Tensors[2].toTensor().cpu(), Tensors[6].toTensor().cpu(), Tensors[10].toTensor().cpu()); + Tensors[2].toTensor().cpu(), Tensors[6].toTensor().cpu() ); _output.wheel3 = GetWheelTensorOutput( - Tensors[3].toTensor().cpu(), Tensors[7].toTensor().cpu(), Tensors[11].toTensor().cpu()); + Tensors[3].toTensor().cpu(), Tensors[7].toTensor().cpu() ); } void NeuralModel::ForwardDynamic() { @@ -279,17 +274,13 @@ namespace learning { std::vector Tensors = Output.toTuple()->elements(); _output.wheel0 = GetWheelTensorOutput( - Tensors[0].toTensor().cpu(), Tensors[4].toTensor().cpu(), - Tensors[8].toTensor().cpu()); + Tensors[0].toTensor().cpu(), Tensors[4].toTensor().cpu() ); _output.wheel1 = GetWheelTensorOutput( - Tensors[1].toTensor().cpu(), Tensors[5].toTensor().cpu(), - Tensors[9].toTensor().cpu()); + Tensors[1].toTensor().cpu(), Tensors[5].toTensor().cpu() ); _output.wheel2 = GetWheelTensorOutput( - Tensors[2].toTensor().cpu(), Tensors[6].toTensor().cpu(), - Tensors[10].toTensor().cpu()); + Tensors[2].toTensor().cpu(), Tensors[6].toTensor().cpu() ); _output.wheel3 = GetWheelTensorOutput( - Tensors[3].toTensor().cpu(), Tensors[7].toTensor().cpu(), - Tensors[11].toTensor().cpu()); + Tensors[3].toTensor().cpu(), Tensors[7].toTensor().cpu() ); } Outputs& NeuralModel::GetOutputs() { diff --git a/LibCarla/source/carla/pytorch/pytorch.h b/LibCarla/source/carla/pytorch/pytorch.h index f5bb79962..6ee3f8e6d 100644 --- a/LibCarla/source/carla/pytorch/pytorch.h +++ b/LibCarla/source/carla/pytorch/pytorch.h @@ -47,9 +47,6 @@ namespace learning { float wheel_torque_x = 0; float wheel_torque_y = 0; float wheel_torque_z = 0; - float wheel_normal_x = 0; - float wheel_normal_y = 0; - float wheel_normal_z = 0; std::vector _particle_forces; }; struct Outputs { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h index d4a514131..263d85990 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h @@ -201,6 +201,8 @@ public: protected: + void RemoveLandscapeCollisionIfHaveTerraMechanics(ULevel* InLevel); + void UpdateTilesState(); void RemovePendingActorsToRemove(); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.cpp index bf88926e9..2b758408e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.cpp @@ -1862,11 +1862,6 @@ void UCustomTerrainPhysicsComponent::RunNNPhysicsSimulation( DrawTiles(GetWorld(), SparseMap.GetIntersectingTiles(BboxWheel3), BboxWheel3.Center.Z); } - //UE_LOG(LogCarla, Log, TEXT("Found %d particles in wheel 0 %s"), ParticlesWheel0.size(), *WheelPosition0.ToString()); - //UE_LOG(LogCarla, Log, TEXT("Found %d particles in wheel 1 %s"), ParticlesWheel1.size(), *WheelPosition1.ToString()); - //UE_LOG(LogCarla, Log, TEXT("Found %d particles in wheel 2 %s"), ParticlesWheel2.size(), *WheelPosition2.ToString()); - //UE_LOG(LogCarla, Log, TEXT("Found %d particles in wheel 3 %s"), ParticlesWheel3.size(), *WheelPosition3.ToString()); - TArray ParticlePos0, ParticleVel0, ParticlePos1, ParticleVel1, ParticlePos2, ParticleVel2, ParticlePos3, ParticleVel3; TArray WheelPos0, WheelOrient0, WheelLinVel0, WheelAngVel0; @@ -2041,27 +2036,6 @@ void UCustomTerrainPhysicsComponent::RunNNPhysicsSimulation( Output.wheel3.wheel_torque_y, Output.wheel3.wheel_torque_z))); } - - { - TArray WheelsNormals; - WheelsNormals.Add(FVector( - Output.wheel0.wheel_normal_x, - Output.wheel0.wheel_normal_y, - Output.wheel0.wheel_normal_z)); - WheelsNormals.Add(FVector( - Output.wheel1.wheel_normal_x, - Output.wheel1.wheel_normal_y, - Output.wheel1.wheel_normal_z)); - WheelsNormals.Add(FVector( - Output.wheel2.wheel_normal_x, - Output.wheel2.wheel_normal_y, - Output.wheel2.wheel_normal_z)); - WheelsNormals.Add(FVector( - Output.wheel3.wheel_normal_x, - Output.wheel3.wheel_normal_y, - Output.wheel3.wheel_normal_z)); - AddForcesToVehicleWheels(Vehicle, WheelsNormals); - } #endif } @@ -2135,8 +2109,6 @@ void UCustomTerrainPhysicsComponent::OnLevelAddedToWorld(ULevel* InLevel, UWorld CurrentComponent->SetCollisionEnabled( ECollisionEnabled::Type::NoCollision ); } - - } } } @@ -2312,38 +2284,22 @@ void UCustomTerrainPhysicsComponent::UpdateTilesHeightMapsInRadius(FDVector Posi } } -void UCustomTerrainPhysicsComponent::AddForcesToVehicleWheels(ACarlaWheeledVehicle *Vehicle, TArray WheelsNormals ) +void UCustomTerrainPhysicsComponent::AddForceToSingleWheel( USkeletalMeshComponent* SkeletalMeshComponent, FVector WheelPosition, FVector WheelNormalForce ) { - USkeletalMeshComponent* SKMesh = Cast( Vehicle->GetRootComponent() ); - - UE_LOG(LogCarla, Warning, TEXT("Normal0 %f"), WheelsNormals[0].Z ); - - AddForceToSingleWheel( SKMesh, FName(TEXT("Wheel_Front_Left")) , WheelsNormals[0] ); - AddForceToSingleWheel( SKMesh, FName(TEXT("Wheel_Front_Right")), WheelsNormals[1] ); - AddForceToSingleWheel( SKMesh, FName(TEXT("Wheel_Rear_Left")), WheelsNormals[2] ); - AddForceToSingleWheel( SKMesh, FName(TEXT("Wheel_Rear_Left")), WheelsNormals[3] ); -} - -void UCustomTerrainPhysicsComponent::AddForceToSingleWheel( USkeletalMeshComponent* SkeletalMeshComponent, FName WheelName, FVector WheelNormalForce ) -{ - FVector WheelLocation = SkeletalMeshComponent->GetBoneLocation(WheelName, EBoneSpaces::Type::WorldSpace); - WheelLocation = UEFrameToSI(WheelLocation); - FVector WheelBottomLocation = WheelLocation - FVector(0,0, 0.337); - float OriginalHeight = SparseMap.GetHeight(WheelLocation); + FVector WheelBottomLocation = WheelPosition - FVector(0,0, 0.337); + float OriginalHeight = SparseMap.GetHeight(WheelPosition); float FloorHeight = OriginalHeight - UEFrameToSI(TerrainDepth); + + if( WheelNormalForce.Size() == 0 ){ + WheelNormalForce = FVector::UpVector; + } - if( WheelBottomLocation.Z < FloorHeight ) - { - SkeletalMeshComponent->AddForceToAllBodiesBelow( WheelNormalForce * NormalForceIntensity, WheelName, false, true ); + float ForceFactor = ( WheelBottomLocation.Z - OriginalHeight ) / ( FloorHeight - OriginalHeight ); + if( ForceFactor < 0){ + ForceFactor = 0; } - else if( WheelBottomLocation.Z < OriginalHeight ) - { - SkeletalMeshComponent->AddForceToAllBodiesBelow( WheelNormalForce * NormalForceIntensity, WheelName, false, true ); - } - else - { - } + SkeletalMeshComponent->AddForceAtLocationLocal(WheelPosition, SIToUEFrame(WheelNormalForce) * ( ForceFactor * NormalForceIntensity) ); } void UCustomTerrainPhysicsComponent::ApplyForcesToVehicle( diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.h index f0c6e5de8..acd12562e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CustomTerrainPhysicsComponent.h @@ -379,8 +379,8 @@ private: void FlagTilesToRedoOrderedContainer(const std::vector& Particles); void UpdateTilesHeightMapsInRadius(FDVector Position, uint32 Rad ); - void AddForcesToVehicleWheels(ACarlaWheeledVehicle *Vehicle, TArray WheelsNormals); - void AddForceToSingleWheel(USkeletalMeshComponent* SkeletalMeshComponent, FName WheelName, FVector WheelNormalForce); + void AddForceToSingleWheel(USkeletalMeshComponent* SkeletalMeshComponent, + FVector WheelPosition, FVector WheelNormalForce); UPROPERTY(EditAnywhere) TArray ForcesToApply;