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/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp

@@ -4,10 +4,12 @@
 #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()
@@ -18,20 +20,30 @@ void URobotPilotSO100Component::BeginPlay()
 void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick TickType,
 	FActorComponentTickFunction* ThisTickFunction)
 {
-	Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+	// Super::TickComponent(DeltaTime, TickType, ThisTickFunction);
+	// const auto Joints = GetCurrentJointsFromPhysicScene();
+	// 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
-	AnimStartRobotActuators = GetCurrentActuatorsFromPhysicScene();
 	TargetTransform = TargetTransformIn;
 	NextAnimationState();
 }
 
-void URobotPilotSO100Component::PrintActuators() const
+
+
+void URobotPilotSO100Component::PrintCurrentActuators() const
 {
-	const auto [Rotation, Pitch, Elbow, WristPitch, WristRoll, Jaw] = GetCurrentActuatorsFromPhysicScene();
+	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,
@@ -42,16 +54,73 @@ void URobotPilotSO100Component::PrintActuators() const
 		);
 }
 
-FSo100Actuators URobotPilotSO100Component::GetCurrentActuatorsFromPhysicScene() const
+void URobotPilotSO100Component::DisableAnim()
+{
+	AnimationStartTime = 0;
+}
+
+FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
 {
 	return FSo100Actuators
 	{
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Rotation),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Rotation),
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Pitch),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Pitch),
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Elbow),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Elbow),
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristPitch),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristPitch),
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_WristRoll),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_WristRoll),
-		RobotOwner->PhysicSceneProxy->GetActuatorValue(Ctrl_Jaw),
+		RobotOwner->PhysicSceneProxy->GetActuatorValue(Actuator_Jaw),
+	};
+}
+
+FSo100Actuators URobotPilotSO100Component::GetCurrentJointsFromPhysicScene() 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->PhysicSceneProxy->GetJointValue(FString(Actuator_Rotation).Append("1")),
+		RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Pitch).Append("1")),
+		RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_Elbow).Append("1")),
+		RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristPitch).Append("1")),
+		RobotOwner->PhysicSceneProxy->GetJointValue(FString(Actuator_WristRoll).Append("1")),
+		RobotOwner->PhysicSceneProxy->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->PhysicSceneProxy->GetActuatorValue(ActuatorName);
+	auto const Joint = RobotOwner->PhysicSceneProxy->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),
 	};
 }
 
@@ -59,13 +128,14 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
 {
 	// Do anything that should be done after a mj_step update
 
-	if (AnimationStartTime >= 0)
+	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)
 		{
+			AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
 			NextAnimationState();
 		}
 	}
@@ -75,19 +145,22 @@ void URobotPilotSO100Component::NextAnimationState()
 {
 	const float CurrentSimulationTime = RobotOwner->PhysicSceneProxy->GetMujocoData().time;
 	AnimationStartTime = CurrentSimulationTime;
+	AnimStartRobotActuators = GetCurrentJointsFromPhysicScene();
 
 	switch (CurrentAnimationState)
 	{
 		case 0:
 			return RotateToTarget();
 		case 1:
-			return CloseJaw();
+			return MoveToTarget();
 		case 2:
-			return MoveToDropZone();
+			return CloseJaw();
 		case 3:
-			return OpenJaw();
+			return MoveToDropZone();
 		case 4:
 			return OpenJaw();
+		case 5:
+			return OpenJaw();
 		default:
 			return BasePose();
 	}
@@ -96,98 +169,165 @@ void URobotPilotSO100Component::NextAnimationState()
 void URobotPilotSO100Component::BasePose()
 {
 	CurrentAnimationState = 0;
-	AnimationDuration = 1.5f;
+	AnimationDuration = 1.f;
 	AnimTargetRobotActuators = ActuatorsRestPosition;
+	// AnimTargetRobotActuators = ActuatorsMaxExtendPosition;
 }
 
 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);
+	const FVector PivotWorldLocation = WorldTransform.GetLocation() + WorldTransform.GetRotation().RotateVector(PivotOffset);
 
-
+	// Find Yaw Rotation in degree - cache it for distance to target computation
-	// FVector::CosineAngle2D()
+	RotationToTarget = UKismetMathLibrary::FindLookAtRotation(
-	const auto Angle = UKismetMathLibrary::FindLookAtRotation(
+		PivotWorldLocation,
-		WorldTransform.GetRotation().GetForwardVector(),
+		TargetTransform.GetLocation()
-		TargetTransform.GetLocation() - PivotWorldLocation
 		);
 	
-	const auto Cosine = WorldTransform.GetRotation().GetForwardVector().CosineAngle2D(TargetTransform.GetLocation() - PivotWorldLocation);
+	// Convert to radians
+	// Looks like we are not in the same referential hence the -1 !
+	// reduce Yaw to not have the fixed jaw colliding with the shape
+	const auto ActuatorRotation = RotationToTarget.Yaw * 0.9 / 180.0f * -PI; 
 
-	UE_LOG(LogTemp, Display, TEXT("Rotation %f, %f, %f"), Angle.Roll, Angle.Pitch, Angle.Yaw);
+	// Start the animation
-	UE_LOG(LogTemp, Display, TEXT("Cosine %f"), Cosine);
+	AnimTargetRobotActuators = AnimStartRobotActuators;
+	AnimTargetRobotActuators.Rotation = ActuatorRotation;
+	CurrentAnimationState = 1;
+	AnimationDuration = .7f;
+}
+
+void URobotPilotSO100Component::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());
 
 	DrawDebugLine(
 		this->GetWorld(),
-		PivotWorldLocation + 100 * FVector::UpVector,
+		JawPositionWorld,
-		PivotWorldLocation,
+		JawPositionWorld + FVector::UpVector * 100,
 		FColor::Blue,
-		true);	
+		true
+	);
+	
+	const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.);
+	// UE_LOG(LogTemp, Log, TEXT("Distance: %f - AlphaExtend: %f"), Distance, AlphaExtend);
+	
+	// Set the target actuators values 
+	AnimTargetRobotActuators = GetCurrentJointsFromPhysicScene();
+	AnimTargetRobotActuators = LerpActuators(ActuatorsRestPosition, ActuatorsMaxExtendPosition, AlphaExtend);
+
+	
+	AnimTargetRobotActuators.Rotation = AnimStartRobotActuators.Rotation;
+	PrintActuators(AnimTargetRobotActuators);
+	
+	// Start the animation 	
+	CurrentAnimationState = 2;
+	AnimationDuration = 1.f;
+	
+	DrawDebugLine(
+		this->GetWorld(),
+		JawPositionWorld,
+		JawPositionWorld + (TargetTransform.GetLocation() - JawPositionWorld).GetSafeNormal() * Distance,
+		FColor::Green,
+		true
+	);
 }
 
 void URobotPilotSO100Component::CloseJaw()
 {
 	// Here we need overcurrent detection - not here actually
+	AnimTargetRobotActuators.Jaw = ClosedJaw;
+
+	// Start the animation
+	bDetectOverCurrent = true;
+	CurrentAnimationState = 3;
+	AnimationDuration = 1.6f;
 }
 
 void URobotPilotSO100Component::MoveToDropZone()
 {
-	const auto DropZoneActuatorsSettings = FMath::RandBool() ? ActuatorsLeftDropZone : ActuatorsRightDropZone;
+	AnimTargetRobotActuators = ActuatorsDropZone;
-	
+	AnimTargetRobotActuators.Rotation = ActuatorsDropZone.Rotation * (FMath::RandBool() ? 1. : -1.); 
+	AnimTargetRobotActuators.Jaw = GetCurrentJointsFromPhysicScene().Jaw;
+	CurrentAnimationState = 4;
+	AnimationDuration = 3.f;
 }
 
 void URobotPilotSO100Component::OpenJaw()
 {
+	AnimTargetRobotActuators.Jaw = OpenedJaw;
+	CurrentAnimationState = 5;
+	AnimationDuration = 0.6f;
 }
 
-bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime) const
+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, GetCurrentJointsFromPhysicScene());
+
+	// 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, Delta);
+
 	// Stop the animation if we reached the target
-	if (AnimAlpha >= 1.) return true;
+	if (AnimAlpha >= 1. && DeltaSum <= .001) return true;
 
 	// Rotation
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Rotation, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Rotation, FMath::Lerp(
 		AnimStartRobotActuators.Rotation,
 		AnimTargetRobotActuators.Rotation,
 		AnimAlpha
 	));
 
 	// Pitch
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Pitch, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Pitch, FMath::Lerp(
 		AnimStartRobotActuators.Pitch,
 		AnimTargetRobotActuators.Pitch,
 		AnimAlpha
 	));
 
 	// Elbow
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Elbow, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Elbow, FMath::Lerp(
 		AnimStartRobotActuators.Elbow,
 		AnimTargetRobotActuators.Elbow,
 		AnimAlpha
 	));
 		
 	// WristPitch
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristPitch, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristPitch, FMath::Lerp(
 		AnimStartRobotActuators.WristPitch,
 		AnimTargetRobotActuators.WristPitch,
 		AnimAlpha
 	));
 
 	// WristRoll
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_WristRoll, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_WristRoll, FMath::Lerp(
 		AnimStartRobotActuators.WristRoll,
 		AnimTargetRobotActuators.WristRoll,
 		AnimAlpha
 	));
 
 	// Jaw
-	RobotOwner->PhysicSceneProxy->SetActuatorValue(Ctrl_Jaw, FMath::Lerp(
+	RobotOwner->PhysicSceneProxy->SetActuatorValue(Actuator_Jaw, FMath::Lerp(
 		AnimStartRobotActuators.Jaw,
 		AnimTargetRobotActuators.Jaw,
 		AnimAlpha

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

@@ -42,39 +42,63 @@ public:
 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 Actuator_Rotation = FString("Rotation");
-	FString Ctrl_Pitch = FString("Pitch");
+	FString Actuator_Pitch = FString("Pitch");
-	FString Ctrl_Elbow = FString("Elbow");
+	FString Actuator_Elbow = FString("Elbow");
-	FString Ctrl_WristPitch = FString("Wrist_Pitch");
+	FString Actuator_WristPitch = FString("Wrist_Pitch");
-	FString Ctrl_WristRoll = FString("Wrist_Roll");
+	FString Actuator_WristRoll = FString("Wrist_Roll");
-	FString Ctrl_Jaw = FString("Jaw");
+	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,30 +129,21 @@ private:
 	};
 
 	FSo100Actuators ActuatorsMaxExtendPosition {
-		0.,
+		0.0000001,
-		0.,
+		0.081027,
-		0.,
+		-0.01707,
-		0.,
+		-0.075,
-		-1.56,
+		1.469020,
-		-1.045
+		-1.389073
 	};
 	
-	FSo100Actuators ActuatorsLeftDropZone {
+	FSo100Actuators ActuatorsDropZone {
-		0.,
+		PI / 2,
-		0.,
+		-2.17,
-		0.,
+		0.805,
-		0.,
+		1.345,
-		-1.56,
+		1.61,
-		-1.045
+		0
-	};
-
-	FSo100Actuators ActuatorsRightDropZone {
-		0.,
-		0.,
-		0.,
-		0.,
-		-1.56,
-		-1.045
 	};
 
 	// Tick where it can have targets