WIP - Architecture for brute force animation

+ sequence look / extend / grab / drop

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

@@ -14,6 +14,7 @@ void URobotPilotComponent::BeginPlay()
 
 	// Reference owning robot
 	RobotOwner = Cast<ARobotPawn>(GetOwner());
+	InitPilotComponent();
 }
 
 void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickType,
@@ -24,18 +25,13 @@ void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickTy
 
 void URobotPilotComponent::InitPilotComponent()
 {
-	if (RobotOwner.IsValid() && RobotOwner->PhysicSceneProxy.IsValid())
+	if (RobotOwner.Get() && RobotOwner->PhysicSceneProxy.Get())
 	{
-		RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::AnimateActuators);
+		RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate);
 	}
 }
 
-void URobotPilotComponent::StartAnimation(const FRobotActuators& NewAnimationTarget)
+void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime)
 {
-	AnimTargetRobotActuators = NewAnimationTarget;
+	// Overriden in each dedicated RobotPilotComponent
-}
-
-void URobotPilotComponent::AnimateActuators(float SimulationTime)
-{
-	// Override in each dedicated RobotPilotComponent
 }

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

@@ -1,5 +1,10 @@
 #include "Robot/PilotComponent/RobotPilotSO100Component.h"
 
+#include "Actors/MujocoVolumeActor.h"
+#include "Kismet/KismetMathLibrary.h"
+#include "Kismet/KismetSystemLibrary.h"
+#include "Robot/RobotPawn.h"
+
 URobotPilotSO100Component::URobotPilotSO100Component()
 {
 	
@@ -16,7 +21,177 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T
 	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
 }
 
-void URobotPilotSO100Component::StartAnimation(const FRobotActuators& NewAnimationTarget)
+void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn)
 {
-	// Super::StartAnimation(NewAnimationTarget);
+	// Set Base Values
+	AnimStartRobotActuators = GetCurrentActuatorsFromPhysicScene();
+	TargetTransform = TargetTransformIn;
+	NextAnimationState();
+}
+
+void URobotPilotSO100Component::PrintActuators() const
+{
+	const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = GetCurrentActuatorsFromPhysicScene();
+	UE_LOG(LogTemp, Display, TEXT("Actuators Rotation %f | Pitch %f | Elbow %f | WristPitch %f | WristRoll %f | Jaw %f"),
+		Rotation,
+		Pitch,
+		Elbow,
+		WristPitch,
+		WristRoll,
+		Jaw
+		);
+}
+
+FSo100Actuators URobotPilotSO100Component::GetCurrentActuatorsFromPhysicScene() const
+{
+	return FSo100Actuators
+	{
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Rotation),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Pitch),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Elbow),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristPitch),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristRoll),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Jaw),
+	};
+}
+
+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 && CurrentAnimationState < MaxAnimationState)
+		{
+			NextAnimationState();
+		}
+	}
+}
+
+void URobotPilotSO100Component::NextAnimationState()
+{
+	const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time;
+	AnimationStartTime = CurrentSimulationTime;
+
+	switch (CurrentAnimationState)
+	{
+		case 0:
+			return RotateToTarget();
+		case 1:
+			return CloseJaw();
+		case 2:
+			return MoveToDropZone();
+		case 3:
+			return OpenJaw();
+		case 4:
+			return OpenJaw();
+		default:
+			return BasePose();
+	}
+}
+
+void URobotPilotSO100Component::BasePose()
+{
+	CurrentAnimationState = 0;
+	AnimationDuration = 1.5f;
+	AnimTargetRobotActuators = ActuatorsRestPosition;
+}
+
+void URobotPilotSO100Component::RotateToTarget()
+{
+	CurrentAnimationState = 1;
+	AnimationDuration = 1.f;
+
+	// Compute Pivot Point World Location
+	const auto WorldTransform = RobotOwner->RobotActor->GetActorTransform();
+	const FVector PivotWorldLocation = WorldTransform.GetTranslation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
+	
+
+	// FVector::CosineAngle2D()
+	const auto Angle = UKismetMathLibrary::FindLookAtRotation(
+		WorldTransform.GetRotation().GetForwardVector(),
+		TargetTransform.GetLocation() - PivotWorldLocation
+		);
+
+	const auto Cosine = WorldTransform.GetRotation().GetForwardVector().CosineAngle2D(TargetTransform.GetLocation() - PivotWorldLocation);
+	
+	UE_LOG(LogTemp, Display, TEXT("Rotation %f, %f, %f"), Angle.Roll, Angle.Pitch, Angle.Yaw);
+	UE_LOG(LogTemp, Display, TEXT("Cosine %f"), Cosine);
+	
+	DrawDebugLine(
+		this->GetWorld(),
+		PivotWorldLocation + 100 * FVector::UpVector,
+		PivotWorldLocation,
+		FColor::Blue,
+		true);	
+}
+
+void URobotPilotSO100Component::CloseJaw()
+{
+	// Here we need overcurrent detection - not here actually
+}
+
+void URobotPilotSO100Component::MoveToDropZone()
+{
+	const auto DropZoneActuatorsSettings = FMath::RandBool() ? ActuatorsLeftDropZone : ActuatorsRightDropZone;
+	
+}
+
+void URobotPilotSO100Component::OpenJaw()
+{
+}
+
+bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) const
+{
+	const double AnimAlpha = FMath::Clamp((SimulationTime - AnimationStartTime) / AnimationDuration, 0., 1.);
+
+	// Stop the animation if we reached the target
+	if (AnimAlpha >= 1.) return true;
+
+	// Rotation
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Rotation, FMath::Lerp(
+		AnimStartRobotActuators.Rotation,
+		AnimTargetRobotActuators.Rotation,
+		AnimAlpha
+	));
+
+	// Pitch
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Pitch, FMath::Lerp(
+		AnimStartRobotActuators.Pitch,
+		AnimTargetRobotActuators.Pitch,
+		AnimAlpha
+	));
+
+	// Elbow
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Elbow, FMath::Lerp(
+		AnimStartRobotActuators.Elbow,
+		AnimTargetRobotActuators.Elbow,
+		AnimAlpha
+	));
+		
+	// WristPitch
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristPitch, FMath::Lerp(
+		AnimStartRobotActuators.WristPitch,
+		AnimTargetRobotActuators.WristPitch,
+		AnimAlpha
+	));
+
+	// WristRoll
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristRoll, FMath::Lerp(
+		AnimStartRobotActuators.WristRoll,
+		AnimTargetRobotActuators.WristRoll,
+		AnimAlpha
+	));
+
+	// Jaw
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Jaw, FMath::Lerp(
+		AnimStartRobotActuators.Jaw,
+		AnimTargetRobotActuators.Jaw,
+		AnimAlpha
+	));
+
+	return false;
 }

Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp

@@ -28,11 +28,11 @@ void ARobotPawn::InitPilotComponent()
 		break;
 		
 	case ERobotsName::SO100Robot:
-		RobotPilotComponent = NewObject<URobotPilotSO100Component>(GetOwner());
+		RobotPilotComponent = NewObject<URobotPilotSO100Component>(this);
 		break;
 
 	case ERobotsName::DJIDrone:
-		RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(GetOwner());
+		RobotPilotComponent = NewObject<URobotPilotMultiRotorDrone>(this);
 		break;
 	
 	case ERobotsName::Luck_e:
@@ -63,6 +63,5 @@ void ARobotPawn::InitPilotComponent()
 	if (RobotPilotComponent)
 	{
 		RobotPilotComponent->RegisterComponent();
-		RobotPilotComponent->InitPilotComponent();
 	}
 }

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

@@ -25,22 +25,16 @@ public:
 	virtual void BeginPlay() override;
 	virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
 	virtual void InitPilotComponent();
+	virtual void PostPhysicStepUpdate(const float SimulationTime);
 	
-private:
+protected: // Child class need access
 	// Only to easy access within the component
-	TWeakObjectPtr<ARobotPawn> RobotOwner = nullptr;
+	TObjectPtr<ARobotPawn> RobotOwner = nullptr;
 
 	// ----------------
 	// ----- ANIM -----
 	// ----------------
-public:
+protected: // Child class need access
-	virtual void StartAnimation(const FRobotActuators& NewAnimationTarget);
-	
-private:
-	virtual void AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate
-	float AnimationDuration = 0.f;
 	float AnimationStartTime = 0.f;
-	FRobotActuators CurrentRobotActuators; // This will be updated by the post-physic delegate 
+	float AnimationDuration = 0.f;
-	FRobotActuators AnimStartRobotActuators;
-	FRobotActuators AnimTargetRobotActuators;
 };

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

@@ -4,26 +4,26 @@
 #include "RobotPilotSO100Component.generated.h"
 
 USTRUCT(BlueprintType)
-struct FSo100Actuators : public FRobotActuators
+struct FSo100Actuators
 {
 	GENERATED_BODY()
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float Rotation = 0.f;
+	double Rotation = 0.;
 	
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float Pitch = 0.f;
+	double Pitch = 0.;
 
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float Elbow = 0.f;
+	double Elbow = 0.;
 
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float WristPitch = 0.f;
+	double WristPitch = 0.;
 
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float WristRoll = 0.f;
+	double WristRoll = 0.;
 	
 	UPROPERTY(EditAnywhere, BlueprintReadWrite)
-	float Jaw = 0.f;
+	double Jaw = 0.;
 };
 
 UCLASS(Blueprintable)
@@ -31,20 +31,120 @@ class LUCKYWORLDV2_API URobotPilotSO100Component : public URobotPilotComponent
 {
 	GENERATED_BODY()
 
-	public:
+public:
 	URobotPilotSO100Component();
 	
 	virtual void BeginPlay() override;
 	virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
 
 	UFUNCTION(BlueprintCallable)
-	virtual void StartAnimation(const FRobotActuators& NewAnimationTarget) override;
+	void SetTarget(const FTransform& TargetTransformIn);
+private:
+	FTransform TargetTransform;
+	
+public:
+	UFUNCTION(BlueprintCallable)
+	void PrintActuators() const;
+
+private:
+	// SO100 Controls by name
+	FString Ctrl_Rotation = FString("Rotation");
+	FString Ctrl_Pitch = FString("Pitch");
+	FString Ctrl_Elbow = FString("Elbow");
+	FString Ctrl_WristPitch = FString("Wrist_Pitch");
+	FString Ctrl_WristRoll = FString("Wrist_Roll");
+	FString Ctrl_Jaw = FString("Jaw");
+
+	// SO100 Static Variables
+	FVector PivotOffset = FVector{-0.000030, 4.520021, 1.650041};
+	
+	/**
+	 * Query the physic proxy on the RobotOwner to get the SO100 actuators values
+	 * @return 
+	 */
+	FSo100Actuators GetCurrentActuatorsFromPhysicScene() const;
+	
+	// Called after every physic step
+	virtual void PostPhysicStepUpdate(const float SimulationTime) override;
+	bool AnimateActuators(float SimulationTime) const; // Bound to the PhysicProxy post-update delegate
+	FSo100Actuators CurrentRobotActuators; // This will be updated by the post-physic delegate 
+	FSo100Actuators AnimStartRobotActuators;
+	FSo100Actuators AnimTargetRobotActuators;
+
+	// Quick and dirty sequence of moves
+	// -1 -> Start Game, extended
+	// 0 -> Retract - base pose
+	// 1 -> Rotate towards goal
+	// 2 -> Move to target
+	// 3 -> Close the jaw
+	// 4 -> Go to drop zone
+	// 5 -> open jaw
+	int32 CurrentAnimationState = -1;
+	int32 MaxAnimationState = 5;
+	void NextAnimationState();
+
+	void BasePose();
+	void RotateToTarget();
+	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.,
+		0.,
+		0.,
+		0.,
+		-1.56,
+		-1.045
+	};
+	
+	FSo100Actuators ActuatorsLeftDropZone {
+		0.,
+		0.,
+		0.,
+		0.,
+		-1.56,
+		-1.045
+	};
+
+	FSo100Actuators ActuatorsRightDropZone {
+		0.,
+		0.,
+		0.,
+		0.,
+		-1.56,
+		-1.045
+	};
 	
 	// Tick where it can have targets
 	// Open Claw (ClawIndex)
 	// Presets to move certain joints into certain positions -> HardCode
 	// Move the arm myself JB + Capture LOG
+
+	// Set the target in world absolute <- get actor of class with tag
+	// Compute target relative
+	// Compute distance to target
+	// compute rotation - direction to target
+	// lerp values to go grab
+	// close jaw
+	// select drop zone
+	// move to drop zone (pick bool)
+	// open jaw
+	// assess success or failure
 };
+
+

Source/LuckyWorldV2/Public/Robot/RobotPawn.h

@@ -25,7 +25,11 @@ public:
 	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
-	TWeakObjectPtr<AMujocoVolumeActor> PhysicSceneProxy;
+	TObjectPtr<AMujocoVolumeActor> PhysicSceneProxy;
+
+	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 ------