Merge pull request 'Base RobotPawn + PilotComponent' (#56) from ft-pilot-so100 into main

Reviewed-on: #56

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/LuckyDataTransfer.Build.cs

@@ -29,7 +29,11 @@ public class LuckyDataTransfer : ModuleRules
 				"WebSockets",
 				"Json",
 				"JsonUtilities",
-				"ImageWriteQueue"
+				"ImageWriteQueue",
+				"FunctionalTesting",
+				"RenderCore",
+				"RHI",
+				"RHICore"
 				// ... add other public dependencies that you statically link with here ...
 			}
 			);
@@ -41,7 +45,8 @@ public class LuckyDataTransfer : ModuleRules
 				"CoreUObject",
 				"Engine",
 				"Slate",
-				"SlateCore", "ImageWriteQueue",
+				"SlateCore", 
+				"ImageWriteQueue"
 				// ... add private dependencies that you statically link with here ...	
 			}
 			);

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Private/LuckyDataTransferSubsystem.cpp

@@ -2,18 +2,45 @@
 
 
 #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 "JsonObjectConverter.h"
+#include "ReviewComments.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 "Virtualization/VirtualizationTypes.h"
 
 ULuckyDataTransferSubsystem::ULuckyDataTransferSubsystem()
 {
+	
 }
 
 void ULuckyDataTransferSubsystem::Initialize(FSubsystemCollectionBase& Collection)
 {
 	Super::Initialize(Collection);
+
+	if (UGameInstance* GI = GetWorld()->GetGameInstance())
+	{
+		
+	}
+
 }
 
 void ULuckyDataTransferSubsystem::Deinitialize()
@@ -123,6 +150,12 @@ FPayload ULuckyDataTransferSubsystem::InterpretData(const FString& Message)
 	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)
 {
 	if (OnCommandReady.IsBound())
@@ -137,6 +170,12 @@ bool ULuckyDataTransferSubsystem::MakeObservationPayload(const FObservationPaylo
 	return CreateJsonPayload_Observation(Data);
 }
 
+FString ULuckyDataTransferSubsystem::CreateCaptureSessionID()
+{
+	SessionID = FGuid::NewGuid().ToString().Left(5);
+	return SessionID;
+}
+
 bool ULuckyDataTransferSubsystem::CreateJsonPayload_Observation(const FObservationPayload& Data)
 {
 	bool bSuccess = false;
@@ -151,3 +190,98 @@ bool ULuckyDataTransferSubsystem::CreateJsonPayload_Observation(const FObservati
 
 	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;
+}

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Private/LuckySensorPawnBase.cpp

@@ -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);
+}
+

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Public/LuckyDataTransferSubsystem.h

@@ -4,7 +4,7 @@
 
 #include "CoreMinimal.h"
 #include "IWebSocket.h"
-//#include "LuckyWriteThread.h"
+#include "LuckySensorPawnBase.h"
 #include "ObservationData.h"
 #include "Subsystems/WorldSubsystem.h"
 #include "LuckyDataTransferSubsystem.generated.h"
@@ -108,6 +108,25 @@ public:
 	bool MakeObservationPayload(const FObservationPayload& Data);
 	//---------------------------------------------------------//
 
-protected:
-	//LuckyWriteThread* WriteThread = nullptr;
+
+	//---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);
+	//-------------------------------------------------------//
+	
 };

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Public/LuckySensorPawnBase.h

@@ -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;
+};

Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp

@@ -109,13 +109,6 @@ template <typename ComponentType> void AMujocoVolumeActor::AssignComponentsToArr
 	}
 }
 
-template <typename UserClass>
-void AMujocoVolumeActor::BindPostPhysicDelegate(UserClass* Object, void(UserClass::* Func)(float))
-{
-	PostPhysicUpdateDelegate.BindUObject(Object, Func);
-}
-
-
 void AMujocoVolumeActor::InitializeMujoco()
 {
 	InitializeMujocoScene_WithContactExclusion(TMap<FString, FString>{});
@@ -417,7 +410,7 @@ void AMujocoVolumeActor::Tick(float DeltaTime)
 		for (int32 SimStep = 0; SimStep < SimStepsPerGameFrame; ++SimStep)
 		{
 			mj_step(MujocoModel.Get(), MujocoData.Get());
-			PostPhysicUpdateDelegate.ExecuteIfBound(MujocoData->time);
+			PostPhysicStepDelegate.ExecuteIfBound(MujocoData->time);
 		}
 
 		for (int32 i = 1; i < BodyComponents.Num(); ++i)

Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h

@@ -114,7 +114,7 @@ public:
 	// ------- POST UPDATE -------
 	// ---------------------------
 private:
-	FPostPhysicUpdate PostPhysicUpdateDelegate;
+	FPostPhysicUpdate PostPhysicStepDelegate;
 public:	
 	/**
 	 * Register a delegate to be executed after mj_step, useful to fine control actuators 
@@ -122,7 +122,10 @@ public:
 	 * @param Func - The Function to call - takes a float as parameter which is the current simulation timestamp
 	 */
 	template<typename UserClass>
-	void BindPostPhysicDelegate(UserClass* Object, void (UserClass::*Func)(float));
+	void BindPostPhysicStepDelegate(UserClass* Object, void (UserClass::*Func)(float))
+	{
+		PostPhysicStepDelegate.BindUObject(Object, Func);
+	}
 	
 
 	// -------------------------

Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp

@@ -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
+}

Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotMultiRotorDrone.cpp

@@ -0,0 +1,5 @@
+#include "Robot/PilotComponent/RobotPilotMultiRotorDrone.h"
+
+URobotPilotMultiRotorDrone::URobotPilotMultiRotorDrone()
+{
+}

Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp

@@ -0,0 +1,363 @@
+#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;
+}
+
+void URobotPilotSO100Component::DebugUpdatePos()
+{
+	const int JointId = RobotOwner->PhysicsSceneProxy->GetJointId("MujocoBody_6"); 
+	auto MjData = RobotOwner->PhysicsSceneProxy->GetMujocoData();
+	auto MjModel = RobotOwner->PhysicsSceneProxy->GetMujocoModel();
+	// mj_name2id(MjModel, mjOBJ_JOINT, "");
+	
+	if (JointId)
+	{
+		auto adx = MjModel.jnt_qposadr[JointId];
+		mjtNum* body_pos = MjData.xpos + 3 * JointId;
+		UE_LOG(LogTemp, Log, TEXT("JointId %i - pos.x %f - pos.y %f - pos.z %f"), JointId, body_pos[0], body_pos[1], body_pos[2]);
+	}
+}
+
+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;
+}

Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp

@@ -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();
+	}
+}

Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h

@@ -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;
+};

Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotMultiRotorDrone.h

@@ -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();
+};

Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h

@@ -0,0 +1,152 @@
+#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();
+
+	UFUNCTION(BlueprintCallable)
+	void DebugUpdatePos();
+	
+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
+	};
+};
+
+

Source/LuckyWorldV2/Public/Robot/RobotPawn.h

@@ -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?  
+};

win_build.bat

@@ -6,6 +6,38 @@ set "UE_PATH=C:\Program Files\UE_5.5"
 set "PROJECT_PATH=%~dp0"
 set "UAT_PATH=%UE_PATH%\Engine\Build\BatchFiles\RunUAT.bat"
 
+
+:: Check if MuJoCo DLL exists in bin directory
+if not exist "%MUJOCO_BIN_PATH%\mujoco.dll" (
+    echo Error: MuJoCo DLL not found at %MUJOCO_BIN_PATH%\mujoco.dll
+    echo Please ensure the MuJoCo DLL is properly built and placed in the correct location.
+    pause
+    exit /b 1
+)
+
+:: Copy MuJoCo DLL to the plugin binaries directory if it doesn't exist there
+if not exist "%MUJOCO_DLL_PATH%\mujoco.dll" (
+    echo Copying MuJoCo DLL to plugin binaries directory...
+    copy "%MUJOCO_BIN_PATH%\mujoco.dll" "%MUJOCO_DLL_PATH%\"
+    if errorlevel 1 (
+        echo Failed to copy MuJoCo DLL
+        pause
+        exit /b 1
+    )
+)
+
+:: Add UE and MuJoCo paths to PATH environment variable
+set "PATH=%UE_PATH%\Engine\Binaries\Win64;%MUJOCO_DLL_PATH%;%MUJOCO_BIN_PATH%;%PATH%"
+
+:: Set the DLL search path for the current process
+set "PATH=%PATH%;%MUJOCO_DLL_PATH%;%MUJOCO_BIN_PATH%"
+
+:: Run the Unreal Editor
+:: echo Starting Unreal Editor...
+:: start "" "%UE_PATH%\Engine\Binaries\Win64\UnrealEditor.exe" "%PROJECT_PATH%LuckyWorldV2.uproject"
+
+endlocal 
+
 :: Build and package command
 "%UAT_PATH%" ^
     -ScriptsForProject="%PROJECT_PATH%LuckyWorldV2.uproject" ^