WIP - Architecture for brute force animation

+ sequence look / extend / grab / drop

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