From 45490051cd545a142c808fe30067047cf338fa04 Mon Sep 17 00:00:00 2001 From: Jb win Date: Sat, 3 May 2025 01:45:06 +0700 Subject: [PATCH] WIP - Configure Episode Series Launch --- .../Private/Actors/MujocoStaticMeshActor.cpp | 6 + .../Public/Actors/MujocoStaticMeshActor.h | 5 + .../Private/Episode/EpisodeSubSystem.cpp | 114 +++++++++++++++--- .../PilotComponent/RobotPilotComponent.cpp | 5 + .../RobotPilotSO100Component.cpp | 13 +- .../Public/Episode/EpisodeSubSystem.h | 41 ++++++- .../PilotComponent/RobotPilotComponent.h | 1 + .../PilotComponent/RobotPilotSO100Component.h | 10 +- Source/LuckyWorldV2/Public/Robot/RobotPawn.h | 2 +- 9 files changed, 173 insertions(+), 24 deletions(-) diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoStaticMeshActor.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoStaticMeshActor.cpp index f9816e0e..823b7caf 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoStaticMeshActor.cpp +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoStaticMeshActor.cpp @@ -10,6 +10,12 @@ AMujocoStaticMeshActor::AMujocoStaticMeshActor() PrimaryActorTick.bCanEverTick = false; } +void AMujocoStaticMeshActor::BeginPlay() +{ + Super::BeginPlay(); + MainActorBody = Cast(this->GetComponentByClass(UMujocoBodyComponent::StaticClass())); +} + #if WITH_EDITOR void AMujocoStaticMeshActor::PostDuplicate(EDuplicateMode::Type DuplicateMode) diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoStaticMeshActor.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoStaticMeshActor.h index 0f39d9e2..bfa0675c 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoStaticMeshActor.h +++ b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoStaticMeshActor.h @@ -4,6 +4,7 @@ #include "Engine/StaticMeshActor.h" #include "MujocoStaticMeshActor.generated.h" +class UMujocoBodyComponent; /** * */ @@ -15,9 +16,13 @@ class LUCKYMUJOCO_API AMujocoStaticMeshActor : public AActor public: AMujocoStaticMeshActor(); + virtual void BeginPlay() override; + #if WITH_EDITOR virtual void PreDuplicate(FObjectDuplicationParameters& DupParams) override; virtual void PostDuplicate(EDuplicateMode::Type DuplicateMode) override; #endif + + TObjectPtr MainActorBody; }; diff --git a/Source/LuckyWorldV2/Private/Episode/EpisodeSubSystem.cpp b/Source/LuckyWorldV2/Private/Episode/EpisodeSubSystem.cpp index 0bef06f0..3547d80a 100644 --- a/Source/LuckyWorldV2/Private/Episode/EpisodeSubSystem.cpp +++ b/Source/LuckyWorldV2/Private/Episode/EpisodeSubSystem.cpp @@ -1,4 +1,10 @@ #include "Episode/EpisodeSubSystem.h" +#include "Actors/MujocoStaticMeshActor.h" +#include "Actors/MujocoVolumeActor.h" +#include "Kismet/GameplayStatics.h" +#include "Robot/RobotPawn.h" +#include "Robot/PilotComponent/RobotPilotComponent.h" +#include "Robot/PilotComponent/RobotPilotSO100Component.h" UEpisodeSubSystem::UEpisodeSubSystem() @@ -18,9 +24,40 @@ void UEpisodeSubSystem::Deinitialize() void UEpisodeSubSystem::Tick(float DeltaTime) { // if capture has started - if (!bIsCapturing) return; + if (!bIsCapturing || CapturedEpisodes >= EpisodesCount) return; - // Flow that we need to figure out + // Here we are capturing the data, running an episode + + // Noah + // Configure the DataTransfer -> Use CurrentRobot->Cameras + // Start the Capture + // Make specs for JB to add API on the robot data + + // JB + // Check status of the episode + // object near base location ? + // Get Robot right zone + // Get Robot left zone + // Move Object at Transform + // Robot->Grab object at Location + Deliver at Zone left/right + // Start Episode new episode - increase counter + + // Check the distance of Object to BaseLocation + // EpisodeTargetObject->MainActorBody->GetComponentTransform(); + // Order the robot to go fetch an object + + // Both of us once we finished our own tasks and synced + // Add the timestamp debug component on the scene to check if the rendered frame and the data are in sync + + // Tick + // Get object location + compute velocity + // if velocity == 0 -> set Time since velocity 0 + // if velocity == 0 for time > trigger restart time + // check object location - is in drop zone? + // if in drop zone -> set success + + // Check ObjectLocation + // If Velocity // ProceduralSceneController - is object spawned? -> If not spawn it // if object spawned && !robot.hasTarget -> Robot -> SetTarget @@ -29,23 +66,62 @@ void UEpisodeSubSystem::Tick(float DeltaTime) // How to reset the episode? } -void UEpisodeSubSystem::StartNewEpisodesSeries(int32 EpisodesCount) +void UEpisodeSubSystem::StartNewEpisodesSeries(const int32 EpisodesCountIn) { - // Noah - // Configure the DataTransfer -> Use CurrentRobot->Cameras - // Start the Capture - // Make specs for JB to add API on the robot data - - // JB - // Find the current Robot in Scene - // Store as CurrentRobot - // Order the robot to go fetch an object - - // Both of us once we finished our own tasks and synced - // - Spawn the shape at random locations -> might need another controller like ProceduralSceneSubSystem? - // - Add the timestamp debug component on the scene to check if the rendered frame and the data are in sync - - // TODO Which Tick is responsible of the scene capture? - + // Robot and Exercise + FindEpisodeObjectFromScene(); + FindRobotPawnFromScene(); + EpisodesCount = EpisodesCountIn; bIsCapturing = true; + CurrentObjectBaseLocation = CurrentRobot->RobotPilotComponent->GetReachableTransform(); + + // Data + ConfigureDataCapture(); +} + +void UEpisodeSubSystem::StartEpisode() +{ + +} + +void UEpisodeSubSystem::FindEpisodeObjectFromScene() +{ + TArray MujocoObjects; + UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), AMujocoStaticMeshActor::StaticClass(), MujocoObjects); + if (MujocoObjects.IsValidIndex(0) && Cast(MujocoObjects[0])) + { + EpisodeTargetObject = Cast(MujocoObjects[0]); + } +} + +void UEpisodeSubSystem::FindRobotPawnFromScene() +{ + TArray RobotPawns; + UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), ARobotPawn::StaticClass(), RobotPawns); + if (RobotPawns.IsValidIndex(0) && Cast(RobotPawns[0])) + { + CurrentRobot = Cast(RobotPawns[0]); + } +} + +void UEpisodeSubSystem::ConfigureDataCapture() +{ + // Noah's space of artistic expression +} + +void UEpisodeSubSystem::CreatePayload() +{ + // CurrentRobot->Cameras + // CurrentRobot -> Tell JB what he should expose on the RobotPawn + const auto TimeStamp = CurrentRobot->PhysicsSceneProxy->GetMujocoData().time; + const auto So100PilotCmp = Cast(CurrentRobot->RobotPilotComponent); + const auto Joints = So100PilotCmp->GetCurrentControlsFromPhysicScene(); + + // JB + // Here I need this specific data +} + +void UEpisodeSubSystem::SendEpisodeData() +{ + // Send the Data } diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp index 173d3bab..aaa93f83 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotComponent.cpp @@ -35,3 +35,8 @@ void URobotPilotComponent::PostPhysicStepUpdate(const float SimulationTime) { // Overriden in each dedicated RobotPilotComponent } + +FTransform URobotPilotComponent::GetReachableTransform() +{ + return FTransform::Identity; +} diff --git a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp index 70cc16c1..aa72e25b 100644 --- a/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp +++ b/Source/LuckyWorldV2/Private/Robot/PilotComponent/RobotPilotSO100Component.cpp @@ -23,6 +23,17 @@ void URobotPilotSO100Component::TickComponent(float DeltaTime, enum ELevelTick T // UE_LOG(LogTemp, Log, TEXT("Joint: %f - Control: %f - Delta: %f"), Joints.Jaw, Controls.Jaw, Controls.Jaw - Joints.Jaw); } +FTransform URobotPilotSO100Component::GetReachableTransform() +{ + const auto RobotTransform = RobotOwner->GetActorTransform(); + const float Yaw = MaxYaw * FMath::RandRange(0., 1.) * (FMath::RandBool() ? 1 : -1); + const float Range = MaxRange * FMath::RandRange(0., 1.); + const FRotator RandomRotator = FRotator{0,0,Yaw}; + const FVector RandomLocation = RandomRotator.RotateVector(RobotTransform.GetRotation().GetForwardVector() * Range); + const FRotator RandomRotation = UKismetMathLibrary::MakeRotFromXZ(RandomLocation- RobotTransform.GetLocation(), FVector::UpVector); + return FTransform(RandomRotation, RandomLocation); +} + void URobotPilotSO100Component::SetTarget(const FTransform& TargetTransformIn) { @@ -221,7 +232,7 @@ void URobotPilotSO100Component::MoveToTarget() const auto JawPositionWorld = RotationToTarget.RotateVector(JawOffsetToPivot) + PivotWorldLocation; const auto Distance = FVector::Distance(JawPositionWorld, TargetTransform.GetLocation()); - const auto AlphaExtend = FMath::Clamp(Distance / MaxReach, 0., 1.); + const auto AlphaExtend = FMath::Clamp(Distance / MaxRange, 0., 1.); // Set the target actuators values AnimTargetRobotActuators = GetCurrentJointsFromPhysicsScene(); diff --git a/Source/LuckyWorldV2/Public/Episode/EpisodeSubSystem.h b/Source/LuckyWorldV2/Public/Episode/EpisodeSubSystem.h index d19cc54b..89dae2ff 100644 --- a/Source/LuckyWorldV2/Public/Episode/EpisodeSubSystem.h +++ b/Source/LuckyWorldV2/Public/Episode/EpisodeSubSystem.h @@ -4,6 +4,7 @@ #include "EpisodeSubSystem.generated.h" +class AMujocoStaticMeshActor; class ARobotPawn; UCLASS() @@ -25,11 +26,49 @@ public: /** * Called by the UI when pressing the "Capture" button */ - void StartNewEpisodesSeries(int32 EpisodesCount); + void StartNewEpisodesSeries(int32 EpisodesCountIn); + + private: + // --------------------- + // ------- FLOW -------- + // --------------------- + void StartEpisode(); + + + // --------------------- + // ------- ROBOT ------- + // --------------------- + + // The state of capture - if true we should call the scene capture and data transfer bool bIsCapturing = false; + int32 EpisodesCount = 0; + int32 CapturedEpisodes = 0; + + void FindEpisodeObjectFromScene(); + void FindRobotPawnFromScene(); UPROPERTY() TObjectPtr CurrentRobot; + + // The object that will serve for the episode + TObjectPtr EpisodeTargetObject; + FTransform CurrentObjectBaseLocation = FTransform::Identity; + FTransform CurrentObjectTargetLocation = FTransform::Identity; + + + // -------------------- + // ------- DATA ------- + // -------------------- + + // Noah here add anything you need + void ConfigureDataCapture(); + + void CreatePayload(); + + void SendEpisodeData(); }; + + + diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h index 12a1c4eb..f4aadadd 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotComponent.h @@ -26,6 +26,7 @@ public: virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; virtual void InitPilotComponent(); virtual void PostPhysicStepUpdate(const float SimulationTime); + virtual FTransform GetReachableTransform(); protected: // Child class need access // Only to easy access within the component diff --git a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h index c63095bc..a5425621 100644 --- a/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h +++ b/Source/LuckyWorldV2/Public/Robot/PilotComponent/RobotPilotSO100Component.h @@ -36,7 +36,10 @@ public: virtual void BeginPlay() override; virtual void TickComponent(float DeltaTime, enum ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + virtual FTransform GetReachableTransform() override; + + UFUNCTION(BlueprintCallable) void SetTarget(const FTransform& TargetTransformIn); private: @@ -65,8 +68,9 @@ private: // 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 - + float MaxRange = 20.757929; // fixed_jaw_pad_3 ForwardVectorLength Delta between Rest and MaxExtend + float MaxYaw = 150.f; + // Actuators Joints and Controls are expressed in doubles double ClosedJaw = 0.18; double OpenedJaw = -2.0; @@ -75,7 +79,9 @@ private: * Query the physic proxy on the RobotOwner to get the SO100 actuators values * @return */ +public: FSo100Actuators GetCurrentControlsFromPhysicScene() const; +private: FSo100Actuators GetCurrentJointsFromPhysicsScene() const; double GetControlJointDeltaForActuator(FString ActuatorName) const; static float GetDeltaSumBetweenActuatorValues(const FSo100Actuators& A, const FSo100Actuators& B); diff --git a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h index eacbb89d..05e7fc9e 100644 --- a/Source/LuckyWorldV2/Public/Robot/RobotPawn.h +++ b/Source/LuckyWorldV2/Public/Robot/RobotPawn.h @@ -40,7 +40,7 @@ public: URobotPilotComponent* RobotPilotComponent = nullptr; UFUNCTION(BlueprintCallable) void InitPilotComponent(); // This should have Robot type as parameter? - + // --------------------- // ------ SENSORS ------