FT - SO100-PilotComponent

+ Grab Sequence Retrac->Find Target->Rotate towards target->Extend->Grab->Drop
+ Can work with any target on the right side (left side bugged - I know why)

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

@@ -41,40 +41,64 @@ public:
 	void SetTarget(const FTransform& TargetTransformIn);
 private:
 	FTransform TargetTransform;
-	
+
+	//---------------------
+	//------- DEBUG -------
+	//---------------------
 public:
 	UFUNCTION(BlueprintCallable)
-	void PrintActuators() const;
+	void PrintCurrentActuators() const;
+	static void PrintActuators(FSo100Actuators Actuators);
 
+	UFUNCTION(BlueprintCallable)
+	void DisableAnim();
+	
 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");
+	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 GetCurrentActuatorsFromPhysicScene() const;
+	FSo100Actuators GetCurrentControlsFromPhysicScene() const;
+	FSo100Actuators GetCurrentJointsFromPhysicScene() 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) const; // Bound to the PhysicProxy post-update delegate
+	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 goal
+	// 1 -> Rotate towards target
 	// 2 -> Move to target
 	// 3 -> Close the jaw
 	// 4 -> Go to drop zone
@@ -85,6 +109,7 @@ private:
 
 	void BasePose();
 	void RotateToTarget();
+	void MoveToTarget();
 	void CloseJaw();
 	void MoveToDropZone();
 	void OpenJaw();
@@ -104,32 +129,23 @@ private:
 	};
 
 	FSo100Actuators ActuatorsMaxExtendPosition {
-		0.,
-		0.,
-		0.,
-		0.,
-		-1.56,
-		-1.045
+		0.0000001,
+		0.081027,
+		-0.01707,
+		-0.075,
+		1.469020,
+		-1.389073
 	};
 	
-	FSo100Actuators ActuatorsLeftDropZone {
-		0.,
-		0.,
-		0.,
-		0.,
-		-1.56,
-		-1.045
+	FSo100Actuators ActuatorsDropZone {
+		PI / 2,
+		-2.17,
+		0.805,
+		1.345,
+		1.61,
+		0
 	};
 
-	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