FIX - left side was not working

+ Removed useless var
+ added logs
+ cycle back to initial RestPosition

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

@@ -133,9 +133,9 @@ void URobotPilotSO100Component::PostPhysicStepUpdate(const float SimulationTime)
 		// 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)
+		if (bHasFinishedAnimation)
 		{
-			AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
+			// AnimationStartTime = 0.; // Only for debug, can be left here but useless in normal operation mode
 			NextAnimationState();
 		}
 	}
@@ -159,8 +159,6 @@ void URobotPilotSO100Component::NextAnimationState()
 			return MoveToDropZone();
 		case 4:
 			return OpenJaw();
-		case 5:
-			return OpenJaw();
 		default:
 			return BasePose();
 	}
@@ -168,14 +166,23 @@ void URobotPilotSO100Component::NextAnimationState()
 
 void URobotPilotSO100Component::BasePose()
 {
+	UE_LOG(LogTemp, Log, TEXT("Animate -> BasePose"));
 	CurrentAnimationState = 0;
-	AnimationDuration = 1.f;
+	AnimationDuration = 1.5f;
 	AnimTargetRobotActuators = ActuatorsRestPosition;
-	// AnimTargetRobotActuators = ActuatorsMaxExtendPosition;
 }
 
 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);
@@ -186,10 +193,12 @@ void URobotPilotSO100Component::RotateToTarget()
 		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
-	// 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; 
+	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;
@@ -200,6 +209,8 @@ void URobotPilotSO100Component::RotateToTarget()
 
 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);
@@ -212,16 +223,7 @@ void URobotPilotSO100Component::MoveToTarget()
 	const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation;
 	const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation());
 
-	DrawDebugLine(
-		this->GetWorld(),
-		JawPositionWorld,
-		JawPositionWorld + FVector::UpVector * 100,
-		FColor::Blue,
-		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();
@@ -233,7 +235,7 @@ void URobotPilotSO100Component::MoveToTarget()
 	
 	// Start the animation 	
 	CurrentAnimationState = 2;
-	AnimationDuration = 1.f;
+	AnimationDuration = 2.f;
 	
 	DrawDebugLine(
 		this->GetWorld(),
@@ -246,17 +248,24 @@ void URobotPilotSO100Component::MoveToTarget()
 
 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 = 1.6f;
+	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 = GetCurrentJointsFromPhysicScene().Jaw;
@@ -266,6 +275,8 @@ void URobotPilotSO100Component::MoveToDropZone()
 
 void URobotPilotSO100Component::OpenJaw()
 {
+	UE_LOG(LogTemp, Log, TEXT("Animate -> OpenJaw"));
+
 	AnimTargetRobotActuators.Jaw = OpenedJaw;
 	CurrentAnimationState = 5;
 	AnimationDuration = 0.6f;
@@ -286,7 +297,7 @@ bool URobotPilotSO100Component::AnimateActuators(const float SimulationTime)
 	}
 
 	
-	// UE_LOG(LogTemp, Log, TEXT("AnimationAlpha: %f - Delta: %f"), AnimAlpha, Delta);
+	// 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;

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

@@ -104,7 +104,6 @@ private:
 	// 4 -> Go to drop zone
 	// 5 -> open jaw
 	int32 CurrentAnimationState = -1;
-	int32 MaxAnimationState = 5;
 	void NextAnimationState();
 
 	void BasePose();