Compare commits
12 Commits
devrim-pat
...
main
Author | SHA1 | Date | |
---|---|---|---|
dd26c58e6b | |||
|
3c413a1982 | ||
127c37ffd7 | |||
|
3e69ebc730 | ||
|
28fc3bc723 | ||
|
6d0a3505f8 | ||
|
73b0aa12bf | ||
|
42196c7680 | ||
|
c6f63317b6 | ||
470cac8f6a | |||
|
b3e0389675 | ||
6bef120e16 |
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -29,7 +29,11 @@ public class LuckyDataTransfer : ModuleRules
|
|||||||
"WebSockets",
|
"WebSockets",
|
||||||
"Json",
|
"Json",
|
||||||
"JsonUtilities",
|
"JsonUtilities",
|
||||||
"ImageWriteQueue"
|
"ImageWriteQueue",
|
||||||
|
"FunctionalTesting",
|
||||||
|
"RenderCore",
|
||||||
|
"RHI",
|
||||||
|
"RHICore"
|
||||||
// ... add other public dependencies that you statically link with here ...
|
// ... add other public dependencies that you statically link with here ...
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
@ -41,7 +45,8 @@ public class LuckyDataTransfer : ModuleRules
|
|||||||
"CoreUObject",
|
"CoreUObject",
|
||||||
"Engine",
|
"Engine",
|
||||||
"Slate",
|
"Slate",
|
||||||
"SlateCore", "ImageWriteQueue",
|
"SlateCore",
|
||||||
|
"ImageWriteQueue"
|
||||||
// ... add private dependencies that you statically link with here ...
|
// ... add private dependencies that you statically link with here ...
|
||||||
}
|
}
|
||||||
);
|
);
|
||||||
|
@ -2,18 +2,45 @@
|
|||||||
|
|
||||||
|
|
||||||
#include "LuckyDataTransferSubsystem.h"
|
#include "LuckyDataTransferSubsystem.h"
|
||||||
|
|
||||||
|
#include "AutomationBlueprintFunctionLibrary.h"
|
||||||
|
#include "ImageUtils.h"
|
||||||
|
#include "RenderingThread.h"
|
||||||
|
#include "RenderUtils.h"
|
||||||
|
#include "RenderGraphUtils.h"
|
||||||
|
#include "RHI.h"
|
||||||
|
#include "RHICommandList.h"
|
||||||
|
#include "ImageWriteQueue.h"
|
||||||
|
#include "ImageWriteTask.h"
|
||||||
|
#include "ImagePixelData.h"
|
||||||
#include "JsonUtilities.h"
|
#include "JsonUtilities.h"
|
||||||
#include "JsonObjectConverter.h"
|
#include "JsonObjectConverter.h"
|
||||||
|
#include "ReviewComments.h"
|
||||||
#include "WebSocketsModule.h"
|
#include "WebSocketsModule.h"
|
||||||
|
#include "Kismet/KismetStringLibrary.h"
|
||||||
|
#include "Camera/CameraActor.h"
|
||||||
|
#include "Camera/CameraComponent.h"
|
||||||
|
#include "Components/SceneCaptureComponent2D.h"
|
||||||
|
#include "Kismet/GameplayStatics.h"
|
||||||
|
#include "Kismet/KismetMathLibrary.h"
|
||||||
|
#include "Kismet/KismetRenderingLibrary.h"
|
||||||
#include "Slate/SceneViewport.h"
|
#include "Slate/SceneViewport.h"
|
||||||
|
#include "Virtualization/VirtualizationTypes.h"
|
||||||
|
|
||||||
ULuckyDataTransferSubsystem::ULuckyDataTransferSubsystem()
|
ULuckyDataTransferSubsystem::ULuckyDataTransferSubsystem()
|
||||||
{
|
{
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ULuckyDataTransferSubsystem::Initialize(FSubsystemCollectionBase& Collection)
|
void ULuckyDataTransferSubsystem::Initialize(FSubsystemCollectionBase& Collection)
|
||||||
{
|
{
|
||||||
Super::Initialize(Collection);
|
Super::Initialize(Collection);
|
||||||
|
|
||||||
|
if (UGameInstance* GI = GetWorld()->GetGameInstance())
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void ULuckyDataTransferSubsystem::Deinitialize()
|
void ULuckyDataTransferSubsystem::Deinitialize()
|
||||||
@ -123,6 +150,12 @@ FPayload ULuckyDataTransferSubsystem::InterpretData(const FString& Message)
|
|||||||
return Payload;
|
return Payload;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void IncomingMessage(const FString& Message)
|
||||||
|
{
|
||||||
|
const FString outMessage = Message.IsEmpty() ? TEXT("no valid message") : Message;
|
||||||
|
UE_LOG(LogTemp, Warning, TEXT("Incoming message: %s"), *outMessage);
|
||||||
|
}
|
||||||
|
|
||||||
void ULuckyDataTransferSubsystem::CommandReady(const FPayload& Payload)
|
void ULuckyDataTransferSubsystem::CommandReady(const FPayload& Payload)
|
||||||
{
|
{
|
||||||
if (OnCommandReady.IsBound())
|
if (OnCommandReady.IsBound())
|
||||||
@ -137,6 +170,12 @@ bool ULuckyDataTransferSubsystem::MakeObservationPayload(const FObservationPaylo
|
|||||||
return CreateJsonPayload_Observation(Data);
|
return CreateJsonPayload_Observation(Data);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
FString ULuckyDataTransferSubsystem::CreateCaptureSessionID()
|
||||||
|
{
|
||||||
|
SessionID = FGuid::NewGuid().ToString().Left(5);
|
||||||
|
return SessionID;
|
||||||
|
}
|
||||||
|
|
||||||
bool ULuckyDataTransferSubsystem::CreateJsonPayload_Observation(const FObservationPayload& Data)
|
bool ULuckyDataTransferSubsystem::CreateJsonPayload_Observation(const FObservationPayload& Data)
|
||||||
{
|
{
|
||||||
bool bSuccess = false;
|
bool bSuccess = false;
|
||||||
@ -150,4 +189,99 @@ bool ULuckyDataTransferSubsystem::CreateJsonPayload_Observation(const FObservati
|
|||||||
}
|
}
|
||||||
|
|
||||||
return bSuccess;
|
return bSuccess;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ULuckyDataTransferSubsystem::RegisterSensor(ALuckySensorPawnBase* Sensor)
|
||||||
|
{
|
||||||
|
if (IsValid(Sensor))
|
||||||
|
{
|
||||||
|
SensorPawns.Add(Sensor);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool ULuckyDataTransferSubsystem::WriteImageToDisk(const FString& inPath, const double Timestamp, const FString& Comment)
|
||||||
|
{
|
||||||
|
if (SessionID.IsEmpty())
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Warning, TEXT("The Capture Session ID is empty"));
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
FString Path = inPath.IsEmpty() ? TEXT("C:/LuckyRobotsImages") : inPath;
|
||||||
|
|
||||||
|
if (!SensorPawns.IsEmpty())
|
||||||
|
{
|
||||||
|
for (const ALuckySensorPawnBase* Sensor : SensorPawns)
|
||||||
|
{
|
||||||
|
if (IsValid(Sensor) && Sensor->SensorInfo.bActive && Sensor->GetCamera() && Sensor->GetCaptureComponent())
|
||||||
|
{
|
||||||
|
FString Robot = TEXT("Robot_Name");
|
||||||
|
|
||||||
|
FString Episode = SessionID;
|
||||||
|
|
||||||
|
ENQUEUE_RENDER_COMMAND(ReadPixelsAsync)([Sensor, Path, Timestamp, Comment, Episode](FRHICommandListImmediate& RHICmdList)
|
||||||
|
{
|
||||||
|
FTextureResource* Resource = Sensor->RenderTarget->GetResource();
|
||||||
|
FRHITexture* ResourceRHI = Resource->GetTexture2DRHI();
|
||||||
|
|
||||||
|
TArray<FColor> OutPixels;
|
||||||
|
|
||||||
|
if (ensure(ResourceRHI))
|
||||||
|
{
|
||||||
|
OutPixels.SetNum(Resource->GetSizeX() * Resource->GetSizeY());
|
||||||
|
RHICmdList.ReadSurfaceData(
|
||||||
|
ResourceRHI,
|
||||||
|
FIntRect(0, 0, Resource->GetSizeX(), Resource->GetSizeY()),
|
||||||
|
OutPixels,
|
||||||
|
FReadSurfaceDataFlags()
|
||||||
|
);
|
||||||
|
|
||||||
|
UE_LOG(LogTemp, Warning, TEXT("Logged pixels: %d"), OutPixels.Num())
|
||||||
|
|
||||||
|
FImageWriteTask* ImageTask = new FImageWriteTask();
|
||||||
|
|
||||||
|
if (Path.Right(1) == "/")
|
||||||
|
{
|
||||||
|
//Path = Path.Left(Path.Len() - 1);
|
||||||
|
}
|
||||||
|
|
||||||
|
FString Robot = TEXT("Robot_Name");
|
||||||
|
|
||||||
|
const FString Filename = FString::Printf(
|
||||||
|
TEXT("%s/%s/%s/%s/%s_%s_%s_%d"),
|
||||||
|
*Path,
|
||||||
|
*Robot,
|
||||||
|
*Episode,
|
||||||
|
*Sensor->SensorInfo.Name,
|
||||||
|
*Robot,
|
||||||
|
*Sensor->SensorInfo.Name,
|
||||||
|
*Comment,
|
||||||
|
FMath::Floor<int32>(Timestamp * 1000)
|
||||||
|
);
|
||||||
|
|
||||||
|
//UE_LOG(LogTemp, Warning, TEXT("Evan requested a longer string describing the inner workings of the following string which describes in great detail the file path for the image you've just written to disk. It is: %s"), *Filename);
|
||||||
|
|
||||||
|
ImageTask->Format = EImageFormat::PNG;
|
||||||
|
ImageTask->Filename = Filename;
|
||||||
|
ImageTask->PixelData = MakeUnique<TImagePixelData<FColor>>(FIntPoint(Sensor->RenderTarget->GetSurfaceWidth(), Sensor->RenderTarget->GetSurfaceHeight()), TArray64<FColor>(OutPixels));
|
||||||
|
ImageTask->bOverwriteFile = true;
|
||||||
|
ImageTask->CompressionQuality = (int32)EImageCompressionQuality::Default;
|
||||||
|
|
||||||
|
//Add to write queue (async)
|
||||||
|
FModuleManager::LoadModuleChecked<IImageWriteQueueModule>("ImageWriteQueue").GetWriteQueue().Enqueue(TUniquePtr<IImageWriteTaskBase>(ImageTask));
|
||||||
|
|
||||||
|
ImageTask->OnCompleted = [](bool bSuccess) {
|
||||||
|
UE_LOG(LogTemp, Warning, TEXT("Image write completed: %s"), bSuccess ? TEXT("Success") : TEXT("Failed"));
|
||||||
|
};
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
}
|
}
|
@ -0,0 +1,57 @@
|
|||||||
|
// Fill out your copyright notice in the Description page of Project Settings.
|
||||||
|
|
||||||
|
|
||||||
|
#include "LuckySensorPawnBase.h"
|
||||||
|
|
||||||
|
#include "Camera/CameraComponent.h"
|
||||||
|
#include "Components/SceneCaptureComponent2D.h"
|
||||||
|
#include "Engine/TextureRenderTarget2D.h"
|
||||||
|
#include "GameFramework/SpringArmComponent.h"
|
||||||
|
|
||||||
|
|
||||||
|
// Sets default values
|
||||||
|
ALuckySensorPawnBase::ALuckySensorPawnBase()
|
||||||
|
{
|
||||||
|
// Set this pawn to call Tick() every frame. You can turn this off to improve performance if you don't need it.
|
||||||
|
PrimaryActorTick.bCanEverTick = true;
|
||||||
|
|
||||||
|
SpringArm = CreateDefaultSubobject<USpringArmComponent>(TEXT("SpringArm"));
|
||||||
|
SpringArm->bDoCollisionTest = false;
|
||||||
|
RootComponent = SpringArm;
|
||||||
|
|
||||||
|
Camera = CreateDefaultSubobject<UCameraComponent>(TEXT("Camera"));
|
||||||
|
Camera->SetupAttachment(SpringArm);
|
||||||
|
Camera->SetRelativeTransform(FTransform());
|
||||||
|
|
||||||
|
CaptureComponent = CreateDefaultSubobject<USceneCaptureComponent2D>(TEXT("CameraComponent"));
|
||||||
|
CaptureComponent->SetupAttachment(RootComponent);
|
||||||
|
|
||||||
|
|
||||||
|
RenderTarget = CreateDefaultSubobject<UTextureRenderTarget2D>(TEXT("RenderTarget"));
|
||||||
|
RenderTarget->UpdateResourceImmediate(true);
|
||||||
|
RenderTarget->ResizeTarget(1, 1);
|
||||||
|
|
||||||
|
|
||||||
|
CaptureComponent->CaptureSource = SCS_FinalColorLDR;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called when the game starts or when spawned
|
||||||
|
void ALuckySensorPawnBase::BeginPlay()
|
||||||
|
{
|
||||||
|
CaptureComponent->TextureTarget = RenderTarget;
|
||||||
|
|
||||||
|
Super::BeginPlay();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called every frame
|
||||||
|
void ALuckySensorPawnBase::Tick(float DeltaTime)
|
||||||
|
{
|
||||||
|
Super::Tick(DeltaTime);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Called to bind functionality to input
|
||||||
|
void ALuckySensorPawnBase::SetupPlayerInputComponent(UInputComponent* PlayerInputComponent)
|
||||||
|
{
|
||||||
|
Super::SetupPlayerInputComponent(PlayerInputComponent);
|
||||||
|
}
|
||||||
|
|
@ -4,7 +4,7 @@
|
|||||||
|
|
||||||
#include "CoreMinimal.h"
|
#include "CoreMinimal.h"
|
||||||
#include "IWebSocket.h"
|
#include "IWebSocket.h"
|
||||||
//#include "LuckyWriteThread.h"
|
#include "LuckySensorPawnBase.h"
|
||||||
#include "ObservationData.h"
|
#include "ObservationData.h"
|
||||||
#include "Subsystems/WorldSubsystem.h"
|
#include "Subsystems/WorldSubsystem.h"
|
||||||
#include "LuckyDataTransferSubsystem.generated.h"
|
#include "LuckyDataTransferSubsystem.generated.h"
|
||||||
@ -107,7 +107,26 @@ public:
|
|||||||
UFUNCTION(BlueprintCallable, Category = "Websocket")
|
UFUNCTION(BlueprintCallable, Category = "Websocket")
|
||||||
bool MakeObservationPayload(const FObservationPayload& Data);
|
bool MakeObservationPayload(const FObservationPayload& Data);
|
||||||
//---------------------------------------------------------//
|
//---------------------------------------------------------//
|
||||||
|
|
||||||
|
|
||||||
|
//---Image Capture----------------------------------------//
|
||||||
|
UFUNCTION(BlueprintCallable, Category = "Websocket")
|
||||||
|
FString CreateCaptureSessionID();
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadOnly, Category = "Websocket")
|
||||||
|
FString SessionID;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, Category = "Capture")
|
||||||
|
TMap<FString, ACameraActor*> ActiveCameras;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, Category = "Capture")
|
||||||
|
TArray<ALuckySensorPawnBase*> SensorPawns;
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable, Category = "Capture")
|
||||||
|
void RegisterSensor(ALuckySensorPawnBase* Sensor);
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable, Meta = (AutoCreateRefTerm = "Path, Comment"), Category = "Capture")
|
||||||
|
bool WriteImageToDisk(const FString& Path, const double Timestamp, const FString& Comment);
|
||||||
|
//-------------------------------------------------------//
|
||||||
|
|
||||||
protected:
|
|
||||||
//LuckyWriteThread* WriteThread = nullptr;
|
|
||||||
};
|
};
|
||||||
|
@ -0,0 +1,75 @@
|
|||||||
|
// Fill out your copyright notice in the Description page of Project Settings.
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "CoreMinimal.h"
|
||||||
|
#include "Camera/CameraActor.h"
|
||||||
|
#include "GameFramework/Pawn.h"
|
||||||
|
#include "Components/SceneCaptureComponent2D.h"
|
||||||
|
#include "Engine/TextureRenderTarget2D.h"
|
||||||
|
#include "GameFramework/SpringArmComponent.h"
|
||||||
|
#include "LuckySensorPawnBase.generated.h"
|
||||||
|
|
||||||
|
USTRUCT(BlueprintType)
|
||||||
|
struct FSensorInfo
|
||||||
|
{
|
||||||
|
GENERATED_USTRUCT_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
FString Name = FString();
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
FVector Anchor = FVector::ZeroVector;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
float Distance = 0.f;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
FRotator Rotation = FRotator::ZeroRotator;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
bool bActive = false;
|
||||||
|
};
|
||||||
|
|
||||||
|
UCLASS()
|
||||||
|
class LUCKYDATATRANSFER_API ALuckySensorPawnBase : public APawn
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
// Sets default values for this pawn's properties
|
||||||
|
ALuckySensorPawnBase();
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
FSensorInfo SensorInfo = FSensorInfo();
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable, Category="Sensor")
|
||||||
|
UCameraComponent* GetCamera() const { return Camera; }
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
UTextureRenderTarget2D* RenderTarget = nullptr;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
USceneCaptureComponent2D* CaptureComponent = nullptr;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
USpringArmComponent* SpringArm = nullptr;
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable, Category="Sensor")
|
||||||
|
USceneCaptureComponent2D* GetCaptureComponent() const { return CaptureComponent; }
|
||||||
|
|
||||||
|
protected:
|
||||||
|
// Called when the game starts or when spawned
|
||||||
|
virtual void BeginPlay() override;
|
||||||
|
|
||||||
|
UPROPERTY(BlueprintReadWrite, EditAnywhere, Category="Sensor")
|
||||||
|
UCameraComponent* Camera = nullptr;
|
||||||
|
|
||||||
|
public:
|
||||||
|
// Called every frame
|
||||||
|
virtual void Tick(float DeltaTime) override;
|
||||||
|
|
||||||
|
// Called to bind functionality to input
|
||||||
|
virtual void SetupPlayerInputComponent(class UInputComponent* PlayerInputComponent) override;
|
||||||
|
};
|
@ -0,0 +1,37 @@
|
|||||||
|
#include "Robot/PilotComponent/RobotPilotComponent.h"
|
||||||
|
|
||||||
|
#include "Actors/MujocoVolumeActor.h"
|
||||||
|
#include "Robot/RobotPawn.h"
|
||||||
|
|
||||||
|
URobotPilotComponent::URobotPilotComponent()
|
||||||
|
{
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotComponent::BeginPlay()
|
||||||
|
{
|
||||||
|
Super::BeginPlay();
|
||||||
|
|
||||||
|
// Reference owning robot
|
||||||
|
RobotOwner = Cast<ARobotPawn>(GetOwner());
|
||||||
|
InitPilotComponent();
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||||
|
FActorComponentTickFunction* ThisTickFunction)
|
||||||
|
{
|
||||||
|
Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotComponent::InitPilotComponent()
|
||||||
|
{
|
||||||
|
if (RobotOwner.Get() && RobotOwner->PhysicsSceneProxy.Get())
|
||||||
|
{
|
||||||
|
RobotOwner->PhysicsSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime)
|
||||||
|
{
|
||||||
|
// Overriden in each dedicated RobotPilotComponent
|
||||||
|
}
|
@ -0,0 +1,5 @@
|
|||||||
|
#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h"
|
||||||
|
|
||||||
|
URobotPilotMultiRotorDrone::URobotPilotMultiRotorDrone()
|
||||||
|
{
|
||||||
|
}
|
@ -0,0 +1,349 @@
|
|||||||
|
#include "Robot/PilotComponent/RobotPilotSO100Component.h"
|
||||||
|
|
||||||
|
#include "Actors/MujocoVolumeActor.h"
|
||||||
|
#include "Kismet/KismetMathLibrary.h"
|
||||||
|
#include "Kismet/KismetSystemLibrary.h"
|
||||||
|
#include "Robot/RobotPawn.h"
|
||||||
|
#include "Serialization/ShaderKeyGenerator.h"
|
||||||
|
|
||||||
|
URobotPilotSO100Component::URobotPilotSO100Component()
|
||||||
|
{
|
||||||
|
PrimaryComponentTick.bCanEverTick = true;
|
||||||
|
PrimaryComponentTick.bStartWithTickEnabled = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::BeginPlay()
|
||||||
|
{
|
||||||
|
Super::BeginPlay();
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType,
|
||||||
|
FActorComponentTickFunction* ThisTickFunction)
|
||||||
|
{
|
||||||
|
// Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
|
||||||
|
// const auto Joints = GetCurrentJointsFromPhysicsScene();
|
||||||
|
// const auto Controls = GetCurrentControlsFromPhysicScene();
|
||||||
|
// UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn)
|
||||||
|
{
|
||||||
|
// Set Base Values
|
||||||
|
TargetTransform = TargetTransformIn;
|
||||||
|
NextAnimationState();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::PrintCurrentActuators() const
|
||||||
|
{
|
||||||
|
PrintActuators(GetCurrentControlsFromPhysicScene());
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::PrintActuators(FSo100Actuators Actuators)
|
||||||
|
{
|
||||||
|
const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = Actuators;
|
||||||
|
UE_LOG(LogTemp, Display, TEXT("Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"),
|
||||||
|
Rotation,
|
||||||
|
Pitch,
|
||||||
|
Elbow,
|
||||||
|
WristPitch,
|
||||||
|
WristRoll,
|
||||||
|
Jaw
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::DisableAnim()
|
||||||
|
{
|
||||||
|
AnimationStartTime = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
|
||||||
|
{
|
||||||
|
return FSo100Actuators
|
||||||
|
{
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Rotation),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Pitch),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Elbow),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristPitch),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_WristRoll),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetActuatorValue(Actuator_Jaw),
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicsScene() const
|
||||||
|
{
|
||||||
|
// Due to Mujoco import plugin not renaming joint differently from controller and Unreal refusing to have 2 variables with the same name
|
||||||
|
// Joints get a "1" appended to their name -> refacto so it's "_Joint"
|
||||||
|
return FSo100Actuators
|
||||||
|
{
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")),
|
||||||
|
RobotOwner->PhysicsSceneProxy->GetJointValue(FString(Actuator_Jaw).Append("1"))
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
float URobotPilotSO100Component::GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B)
|
||||||
|
{
|
||||||
|
float DeltaSum = 0;
|
||||||
|
|
||||||
|
DeltaSum += FMath::Abs(A.Rotation) - FMath::Abs(B.Rotation);
|
||||||
|
DeltaSum += FMath::Abs(A.Pitch) - FMath::Abs(B.Pitch);
|
||||||
|
DeltaSum += FMath::Abs(A.Elbow) - FMath::Abs(B.Elbow);
|
||||||
|
DeltaSum += FMath::Abs(A.WristPitch) - FMath::Abs(B.WristPitch);
|
||||||
|
DeltaSum += FMath::Abs(A.WristRoll) - FMath::Abs(B.WristRoll);
|
||||||
|
DeltaSum += FMath::Abs(A.Jaw) - FMath::Abs(B.Jaw);
|
||||||
|
|
||||||
|
return DeltaSum;
|
||||||
|
}
|
||||||
|
|
||||||
|
double URobotPilotSO100Component::GetControlJointDeltaForActuator(FString ActuatorName) const
|
||||||
|
{
|
||||||
|
auto const Control = RobotOwner->PhysicsSceneProxy->GetActuatorValue(ActuatorName);
|
||||||
|
auto const Joint = RobotOwner->PhysicsSceneProxy->GetJointValue(ActuatorName.Append("1"));
|
||||||
|
return Control -Joint;
|
||||||
|
}
|
||||||
|
|
||||||
|
FSo100Actuators URobotPilotSO100Component::LerpActuators(
|
||||||
|
const FSo100Actuators& A,
|
||||||
|
const FSo100Actuators& B,
|
||||||
|
const float Alpha
|
||||||
|
)
|
||||||
|
{
|
||||||
|
return FSo100Actuators{
|
||||||
|
FMath::Lerp( A.Rotation, B.Rotation, Alpha),
|
||||||
|
FMath::Lerp( A.Pitch, B.Pitch, Alpha),
|
||||||
|
FMath::Lerp( A.Elbow, B.Elbow, Alpha),
|
||||||
|
FMath::Lerp( A.WristPitch, B.WristPitch, Alpha),
|
||||||
|
FMath::Lerp( A.WristRoll, B.WristRoll, Alpha),
|
||||||
|
FMath::Lerp( A.Jaw, B.Jaw, Alpha),
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
|
||||||
|
{
|
||||||
|
// Do anything that should be done after a mj_step update
|
||||||
|
|
||||||
|
if (AnimationStartTime > 0)
|
||||||
|
{
|
||||||
|
// Could be moved to if statement, but it becomes confusing - so let's keep a variable
|
||||||
|
const bool bHasFinishedAnimation = AnimateActuators(SimulationTime);
|
||||||
|
|
||||||
|
if (bHasFinishedAnimation)
|
||||||
|
{
|
||||||
|
// AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
|
||||||
|
NextAnimationState();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::NextAnimationState()
|
||||||
|
{
|
||||||
|
const float CurrentPhysicsEngineSceneTime = RobotOwner->PhysicsSceneProxy->GetMujocoData().time;
|
||||||
|
AnimationStartTime = CurrentPhysicsEngineSceneTime;
|
||||||
|
AnimStartRobotActuators = GetCurrentJointsFromPhysicsScene();
|
||||||
|
|
||||||
|
switch (CurrentAnimationState)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
return RotateToTarget();
|
||||||
|
case 1:
|
||||||
|
return MoveToTarget();
|
||||||
|
case 2:
|
||||||
|
return CloseJaw();
|
||||||
|
case 3:
|
||||||
|
return MoveToDropZone();
|
||||||
|
case 4:
|
||||||
|
return OpenJaw();
|
||||||
|
default:
|
||||||
|
return BasePose();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::BasePose()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> BasePose"));
|
||||||
|
CurrentAnimationState = 0;
|
||||||
|
AnimationDuration = 1.5f;
|
||||||
|
AnimTargetRobotActuators = ActuatorsRestPosition;
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::RotateToTarget()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> RotateToTarget"));
|
||||||
|
|
||||||
|
if (TargetTransform.GetLocation() == FVector::ZeroVector)
|
||||||
|
{
|
||||||
|
AnimationStartTime = 0.f;
|
||||||
|
CurrentAnimationState = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute Pivot Point World Location
|
||||||
|
const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||||
|
const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||||
|
|
||||||
|
// Find Yaw Rotation in degree - cache it for distance to target computation
|
||||||
|
RotationToTarget = UKismetMathLibrary::FindLookAtRotation(
|
||||||
|
PivotWorldLocation,
|
||||||
|
TargetTransform.GetLocation()
|
||||||
|
);
|
||||||
|
|
||||||
|
// reduce/increase Yaw to not have the fixed jaw colliding with the shape - TODO use middle of the jaw instead of the wall of the jaw
|
||||||
|
const auto Dot = FVector::DotProduct(RotationToTarget.Quaternion().GetForwardVector(), WorldTransform.GetRotation().GetForwardVector());
|
||||||
|
const auto Mod = .1 * (Dot > 0 ? 1 : -1);
|
||||||
|
|
||||||
|
// Convert to radians
|
||||||
|
const auto ActuatorRotation = RotationToTarget.Yaw * (1+Mod) / 180.0f * -PI; // Looks like we are not in the same referential hence the -PI instead of PI !
|
||||||
|
|
||||||
|
// Start the animation
|
||||||
|
AnimTargetRobotActuators = AnimStartRobotActuators;
|
||||||
|
AnimTargetRobotActuators.Rotation = ActuatorRotation;
|
||||||
|
CurrentAnimationState = 1;
|
||||||
|
AnimationDuration = .7f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::MoveToTarget()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToTarget"));
|
||||||
|
|
||||||
|
// Get Pivot World
|
||||||
|
const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
|
||||||
|
const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
|
||||||
|
|
||||||
|
// Rotate Jaw offset towards target
|
||||||
|
// Get pure 2d rotation
|
||||||
|
RotationToTarget.Pitch = 0;
|
||||||
|
RotationToTarget.Roll = 0;
|
||||||
|
const auto JawOffsetToPivot = JawOffset - PivotOffset;
|
||||||
|
const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation;
|
||||||
|
const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation());
|
||||||
|
|
||||||
|
const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.);
|
||||||
|
|
||||||
|
// Set the target actuators values
|
||||||
|
AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene();
|
||||||
|
AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend);
|
||||||
|
|
||||||
|
|
||||||
|
AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation;
|
||||||
|
PrintActuators(AnimTargetRobotActuators);
|
||||||
|
|
||||||
|
// Start the animation
|
||||||
|
CurrentAnimationState = 2;
|
||||||
|
AnimationDuration = 2.f;
|
||||||
|
|
||||||
|
DrawDebugLine(
|
||||||
|
this->GetWorld(),
|
||||||
|
JawPositionWorld,
|
||||||
|
JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance,
|
||||||
|
FColor::Green,
|
||||||
|
true
|
||||||
|
);
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::CloseJaw()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> CloseJaw"));
|
||||||
|
|
||||||
|
// Here we need overcurrent detection - not here actually
|
||||||
|
AnimTargetRobotActuators.Jaw = ClosedJaw;
|
||||||
|
|
||||||
|
// Reset TargetTransform
|
||||||
|
TargetTransform = FTransform();
|
||||||
|
|
||||||
|
// Start the animation
|
||||||
|
bDetectOverCurrent = true;
|
||||||
|
CurrentAnimationState = 3;
|
||||||
|
AnimationDuration = 2.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::MoveToDropZone()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> MoveToDropZone"));
|
||||||
|
|
||||||
|
AnimTargetRobotActuators = ActuatorsDropZone;
|
||||||
|
AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.);
|
||||||
|
AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicsScene().Jaw;
|
||||||
|
CurrentAnimationState = 4;
|
||||||
|
AnimationDuration = 3.f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void URobotPilotSO100Component::OpenJaw()
|
||||||
|
{
|
||||||
|
UE_LOG(LogTemp, Log, TEXT("Animate -> OpenJaw"));
|
||||||
|
|
||||||
|
AnimTargetRobotActuators.Jaw = OpenedJaw;
|
||||||
|
CurrentAnimationState = 5;
|
||||||
|
AnimationDuration = 0.6f;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
|
||||||
|
{
|
||||||
|
const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.);
|
||||||
|
|
||||||
|
// Need to wait for the joints to be in the right position before switching to next animation
|
||||||
|
const auto DeltaSum = GetDeltaSumBetweenActuatorValues(AnimTargetRobotActuators, GetCurrentJointsFromPhysicsScene());
|
||||||
|
|
||||||
|
// Alternative Animation completion event - checking for over-current
|
||||||
|
if (bDetectOverCurrent && GetControlJointDeltaForActuator(Actuator_Jaw) > OverCurrentThreshold)
|
||||||
|
{
|
||||||
|
bDetectOverCurrent = false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, DeltaSum);
|
||||||
|
|
||||||
|
// Stop the animation if we reached the target
|
||||||
|
if (AnimAlpha >= 1. && DeltaSum <= .001) return true;
|
||||||
|
|
||||||
|
// Rotation
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.Rotation,
|
||||||
|
AnimTargetRobotActuators.Rotation,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
// Pitch
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.Pitch,
|
||||||
|
AnimTargetRobotActuators.Pitch,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
// Elbow
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.Elbow,
|
||||||
|
AnimTargetRobotActuators.Elbow,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
// WristPitch
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.WristPitch,
|
||||||
|
AnimTargetRobotActuators.WristPitch,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
// WristRoll
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.WristRoll,
|
||||||
|
AnimTargetRobotActuators.WristRoll,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
// Jaw
|
||||||
|
RobotOwner->PhysicsSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
|
||||||
|
AnimStartRobotActuators.Jaw,
|
||||||
|
AnimTargetRobotActuators.Jaw,
|
||||||
|
AnimAlpha
|
||||||
|
));
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
67
Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp
Normal file
67
Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
#include "Robot/RobotPawn.h"
|
||||||
|
|
||||||
|
#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h"
|
||||||
|
#include "Robot/PilotComponent/RobotPilotSO100Component.h"
|
||||||
|
|
||||||
|
ARobotPawn::ARobotPawn()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void ARobotPawn::BeginPlay()
|
||||||
|
{
|
||||||
|
Super::BeginPlay();
|
||||||
|
InitRobot(); // TODO Maybe move to GameInstance to control when we initialize the robot completely
|
||||||
|
}
|
||||||
|
|
||||||
|
void ARobotPawn::InitRobot()
|
||||||
|
{
|
||||||
|
InitPilotComponent();
|
||||||
|
// Other initialization tasks
|
||||||
|
}
|
||||||
|
|
||||||
|
void ARobotPawn::InitPilotComponent()
|
||||||
|
{
|
||||||
|
// Initialize pilot component based on robot type
|
||||||
|
switch (RobotType)
|
||||||
|
{
|
||||||
|
case ERobotsName::None:
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ERobotsName::SO100Robot:
|
||||||
|
RobotPilotComponent = NewObject<URobotPilotSO100Component>(this);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ERobotsName::DJIDrone:
|
||||||
|
RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(this);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case ERobotsName::Luck_e:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::Stretch:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::LuckyDrone:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::ArmLucky:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::UnitreeG1:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::StretchRobotV1:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::PandaArmRobot:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::PuralinkRobot:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::UnitreeGo2:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::RevoluteRobot:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
case ERobotsName::BostonSpotRobot:
|
||||||
|
break; // TODO or remove from enum
|
||||||
|
}
|
||||||
|
|
||||||
|
// Register if this Robot has a Pilot Component
|
||||||
|
if (RobotPilotComponent)
|
||||||
|
{
|
||||||
|
RobotPilotComponent->RegisterComponent();
|
||||||
|
}
|
||||||
|
}
|
@ -0,0 +1,40 @@
|
|||||||
|
#pragma once
|
||||||
|
#include "CoreMinimal.h"
|
||||||
|
#include "RobotPilotComponent.generated.h"
|
||||||
|
|
||||||
|
USTRUCT(BlueprintType)
|
||||||
|
struct FRobotActuators
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
// Do we need a prent struct?
|
||||||
|
// What will be in common?
|
||||||
|
};
|
||||||
|
|
||||||
|
class ARobotPawn;
|
||||||
|
|
||||||
|
UCLASS(Blueprintable)
|
||||||
|
class LUCKYWORLDV2_API URobotPilotComponent : public UActorComponent
|
||||||
|
|
||||||
|
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
URobotPilotComponent();
|
||||||
|
|
||||||
|
virtual void BeginPlay() override;
|
||||||
|
virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
|
||||||
|
virtual void InitPilotComponent();
|
||||||
|
virtual void PostPhysicStepUpdate(const float SimulationTime);
|
||||||
|
|
||||||
|
protected: // Child class need access
|
||||||
|
// Only to easy access within the component
|
||||||
|
TObjectPtr<ARobotPawn> RobotOwner = nullptr;
|
||||||
|
|
||||||
|
// ----------------
|
||||||
|
// ----- ANIM -----
|
||||||
|
// ----------------
|
||||||
|
protected: // Child class need access
|
||||||
|
float AnimationStartTime = 0.f;
|
||||||
|
float AnimationDuration = 0.f;
|
||||||
|
};
|
@ -0,0 +1,13 @@
|
|||||||
|
#pragma once
|
||||||
|
#include "CoreMinimal.h"
|
||||||
|
#include "Robot/PilotComponent/RobotPilotComponent.h"
|
||||||
|
#include "RobotPilotMultiRotorDrone.generated.h"
|
||||||
|
|
||||||
|
UCLASS(Blueprintable)
|
||||||
|
class LUCKYWORLDV2_API URobotPilotMultiRotorDrone : public URobotPilotComponent
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
URobotPilotMultiRotorDrone();
|
||||||
|
};
|
@ -0,0 +1,149 @@
|
|||||||
|
#pragma once
|
||||||
|
#include "CoreMinimal.h"
|
||||||
|
#include "Robot/PilotComponent/RobotPilotComponent.h"
|
||||||
|
#include "RobotPilotSO100Component.generated.h"
|
||||||
|
|
||||||
|
USTRUCT(BlueprintType)
|
||||||
|
struct FSo100Actuators
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double Rotation = 0.;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double Pitch = 0.;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double Elbow = 0.;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double WristPitch = 0.;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double WristRoll = 0.;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
double Jaw = 0.;
|
||||||
|
};
|
||||||
|
|
||||||
|
UCLASS(Blueprintable)
|
||||||
|
class LUCKYWORLDV2_API URobotPilotSO100Component : public URobotPilotComponent
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
URobotPilotSO100Component();
|
||||||
|
|
||||||
|
virtual void BeginPlay() override;
|
||||||
|
virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable)
|
||||||
|
void SetTarget(const FTransform& TargetTransformIn);
|
||||||
|
private:
|
||||||
|
FTransform TargetTransform;
|
||||||
|
|
||||||
|
//---------------------
|
||||||
|
//------- DEBUG -------
|
||||||
|
//---------------------
|
||||||
|
public:
|
||||||
|
UFUNCTION(BlueprintCallable)
|
||||||
|
void PrintCurrentActuators() const;
|
||||||
|
static void PrintActuators(FSo100Actuators Actuators);
|
||||||
|
|
||||||
|
UFUNCTION(BlueprintCallable)
|
||||||
|
void DisableAnim();
|
||||||
|
|
||||||
|
private:
|
||||||
|
// SO100 Controls by name
|
||||||
|
FString Actuator_Rotation = FString("Rotation");
|
||||||
|
FString Actuator_Pitch = FString("Pitch");
|
||||||
|
FString Actuator_Elbow = FString("Elbow");
|
||||||
|
FString Actuator_WristPitch = FString("Wrist_Pitch");
|
||||||
|
FString Actuator_WristRoll = FString("Wrist_Roll");
|
||||||
|
FString Actuator_Jaw = FString("Jaw");
|
||||||
|
|
||||||
|
// SO100 Static Variables
|
||||||
|
FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
|
||||||
|
FVector JawOffset = FVector{23, 2, 9};
|
||||||
|
float MaxReach = 20.757929; // fixed_jaw_pad_3 ForwardVectorLength Delta between Rest and MaxExtend
|
||||||
|
|
||||||
|
// Actuators Joints and Controls are expressed in doubles
|
||||||
|
double ClosedJaw = 0.18;
|
||||||
|
double OpenedJaw = -2.0;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Query the physic proxy on the RobotOwner to get the SO100 actuators values
|
||||||
|
* @return
|
||||||
|
*/
|
||||||
|
FSo100Actuators GetCurrentControlsFromPhysicScene() const;
|
||||||
|
FSo100Actuators GetCurrentJointsFromPhysicsScene() const;
|
||||||
|
double GetControlJointDeltaForActuator(FString ActuatorName) const;
|
||||||
|
static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B);
|
||||||
|
static FSo100Actuators LerpActuators(const FSo100Actuators& A, const FSo100Actuators& B, const float Alpha);
|
||||||
|
|
||||||
|
// Called after every physic step
|
||||||
|
virtual void PostPhysicStepUpdate(const float SimulationTime) override;
|
||||||
|
bool AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate
|
||||||
|
FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate
|
||||||
|
FSo100Actuators AnimStartRobotActuators;
|
||||||
|
FSo100Actuators AnimTargetRobotActuators;
|
||||||
|
FRotator RotationToTarget = FRotator::ZeroRotator;
|
||||||
|
|
||||||
|
// ------------------------
|
||||||
|
// ----- OVER-CURRENT -----
|
||||||
|
// ------------------------
|
||||||
|
bool bDetectOverCurrent = false;
|
||||||
|
const float OverCurrentThreshold = 0.15;
|
||||||
|
|
||||||
|
// Quick and dirty sequence of moves
|
||||||
|
// -1 -> Start Game, extended
|
||||||
|
// 0 -> Retract - base pose
|
||||||
|
// 1 -> Rotate towards target
|
||||||
|
// 2 -> Move to target
|
||||||
|
// 3 -> Close the jaw
|
||||||
|
// 4 -> Go to drop zone
|
||||||
|
// 5 -> open jaw
|
||||||
|
int32 CurrentAnimationState = -1;
|
||||||
|
void NextAnimationState();
|
||||||
|
|
||||||
|
void BasePose();
|
||||||
|
void RotateToTarget();
|
||||||
|
void MoveToTarget();
|
||||||
|
void CloseJaw();
|
||||||
|
void MoveToDropZone();
|
||||||
|
void OpenJaw();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// Here let's write the code trying to match with Constantin class
|
||||||
|
// After both classes have been designed around specific needs, see what can be migrated in the parent class and update both children
|
||||||
|
|
||||||
|
FSo100Actuators ActuatorsRestPosition {
|
||||||
|
0.,
|
||||||
|
-1.54,
|
||||||
|
3.105,
|
||||||
|
-1.5,
|
||||||
|
1.47,
|
||||||
|
-1.39
|
||||||
|
};
|
||||||
|
|
||||||
|
FSo100Actuators ActuatorsMaxExtendPosition {
|
||||||
|
0.0000001,
|
||||||
|
0.081027,
|
||||||
|
-0.01707,
|
||||||
|
-0.075,
|
||||||
|
1.469020,
|
||||||
|
-1.389073
|
||||||
|
};
|
||||||
|
|
||||||
|
FSo100Actuators ActuatorsDropZone {
|
||||||
|
PI / 2,
|
||||||
|
-2.17,
|
||||||
|
0.805,
|
||||||
|
1.345,
|
||||||
|
1.61,
|
||||||
|
0
|
||||||
|
};
|
||||||
|
};
|
||||||
|
|
||||||
|
|
41
Source/LuckyWorldV2/Public/Robot/RobotPawn.h
Normal file
41
Source/LuckyWorldV2/Public/Robot/RobotPawn.h
Normal file
@ -0,0 +1,41 @@
|
|||||||
|
#pragma once
|
||||||
|
#include "CoreMinimal.h"
|
||||||
|
#include "SharedDef.h"
|
||||||
|
#include "RobotPawn.generated.h"
|
||||||
|
|
||||||
|
class AMujocoVolumeActor;
|
||||||
|
class URobotPilotComponent;
|
||||||
|
|
||||||
|
// Enum of bots
|
||||||
|
|
||||||
|
UCLASS(Blueprintable)
|
||||||
|
class LUCKYWORLDV2_API ARobotPawn : public APawn // Should be an actor?
|
||||||
|
{
|
||||||
|
GENERATED_BODY()
|
||||||
|
|
||||||
|
public:
|
||||||
|
ARobotPawn();
|
||||||
|
|
||||||
|
virtual void BeginPlay() override;
|
||||||
|
|
||||||
|
// TODO Called by GameInstance after robot has been spawned
|
||||||
|
void InitRobot();
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
ERobotsName RobotType = ERobotsName::None; // This value must be set in the pawn
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn
|
||||||
|
TObjectPtr<AMujocoVolumeActor> PhysicsSceneProxy;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn
|
||||||
|
TObjectPtr<AActor> RobotActor; // My brain is bleeding facing the fact that we have 2 actors...
|
||||||
|
|
||||||
|
|
||||||
|
// -------------------
|
||||||
|
// ------ PILOT ------
|
||||||
|
// -------------------
|
||||||
|
UPROPERTY(EditAnywhere, BlueprintReadWrite)
|
||||||
|
URobotPilotComponent* RobotPilotComponent = nullptr;
|
||||||
|
UFUNCTION(BlueprintCallable)
|
||||||
|
void InitPilotComponent(); // This should have Robot type as parameter?
|
||||||
|
};
|
Loading…
x
Reference in New Issue
Block a user