WIP - Architecture for brute force animation

+ sequence look / extend / grab / drop

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