diff --git a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset index b8c6bc78..9175c3b7 100644 Binary files a/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset and b/Content/Blueprint/RobotPawnActors/BP_mujokoSO_100.uasset differ diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp index 2db3f0ca..1ee82e60 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp @@ -14,6 +14,7 @@ void URobotPilotComponent::BeginPlay() // Reference owning robot RobotOwner = Cast(GetOwner()); + InitPilotComponent(); } void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickType, @@ -24,18 +25,13 @@ void URobotPilotComponent::TickComponent(float DeltaTime, enum ELevelTick TickTy void URobotPilotComponent::InitPilotComponent() { - if (RobotOwner.IsValid() && RobotOwner->PhysicSceneProxy.IsValid()) + if (RobotOwner.Get() && RobotOwner->PhysicSceneProxy.Get()) { - RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::AnimateActuators); + RobotOwner->PhysicSceneProxy->BindPostPhysicStepDelegate(this, &URobotPilotComponent::PostPhysicStepUpdate); } } -void URobotPilotComponent::StartAnimation(const FRobotActuators& NewAnimationTarget) +void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime) { - AnimTargetRobotActuators = NewAnimationTarget; -} - -void URobotPilotComponent::AnimateActuators(float SimulationTime) -{ - // Override in each dedicated RobotPilotComponent + // Overriden in each dedicated RobotPilotComponent } diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp index e9fb3645..66b7edb9 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp +++ b/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; } diff --git a/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp b/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp index 2fbebec2..2bbe3402 100644 --- a/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp +++ b/Source/LuckyWorldV2/Private/Robot/RobotPawn.cpp @@ -28,11 +28,11 @@ void ARobotPawn::InitPilotComponent() break; case ERobotsName::SO100Robot: - RobotPilotComponent = NewObject(GetOwner()); + RobotPilotComponent = NewObject(this); break; case ERobotsName::DJIDrone: - RobotPilotComponent = NewObject(GetOwner()); + RobotPilotComponent = NewObject(this); break; case ERobotsName::Luck_e: @@ -63,6 +63,5 @@ void ARobotPawn::InitPilotComponent() if (RobotPilotComponent) { RobotPilotComponent->RegisterComponent(); - RobotPilotComponent->InitPilotComponent(); } } diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h index fdebb7f1..12a1c4eb 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h @@ -25,22 +25,16 @@ public: virtual void BeginPlay() override; virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; virtual void InitPilotComponent(); + virtual void PostPhysicStepUpdate(const float SimulationTime); -private: +protected: // Child class need access // Only to easy access within the component - TWeakObjectPtr RobotOwner = nullptr; + TObjectPtr RobotOwner = nullptr; // ---------------- // ----- ANIM ----- // ---------------- -public: - virtual void StartAnimation(const FRobotActuators& NewAnimationTarget); - -private: - virtual void AnimateActuators(float SimulationTime); // Bound to the PhysicProxy post-update delegate - float AnimationDuration = 0.f; +protected: // Child class need access float AnimationStartTime = 0.f; - FRobotActuators CurrentRobotActuators; // This will be updated by the post-physic delegate - FRobotActuators AnimStartRobotActuators; - FRobotActuators AnimTargetRobotActuators; + float AnimationDuration = 0.f; }; diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h index 2859ea22..532e6915 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h +++ b/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 }; + + diff --git a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h index 93cf8a54..45ed1c72 100644 --- a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h +++ b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h @@ -25,7 +25,11 @@ public: ERobotsName RobotType = ERobotsName::None; // This value must be set in the pawn UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn - TWeakObjectPtr PhysicSceneProxy; + TObjectPtr PhysicSceneProxy; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) // TODO Remove UPROPERTY once we migrate physics proxy initialization from Pawn + TObjectPtr RobotActor; // My brain is bleeding facing the fact that we have 2 actors... + // ------------------- // ------ PILOT ------