WIP - Configure Episode Series Launch

Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoStaticMeshActor.cpp

@@ -10,6 +10,12 @@ AMujocoStaticMeshActor::AMujocoStaticMeshActor()
 	PrimaryActorTick.bCanEverTick = false;
 }
 
+void AMujocoStaticMeshActor::BeginPlay()
+{
+	Super::BeginPlay();
+	MainActorBody = Cast<UMujocoBodyComponent>(this->GetComponentByClass(UMujocoBodyComponent::StaticClass()));
+}
+
 #if WITH_EDITOR
 
 void AMujocoStaticMeshActor::PostDuplicate(EDuplicateMode::Type DuplicateMode)

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

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<AActor*> MujocoObjects;
+	UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), AMujocoStaticMeshActor::StaticClass(), MujocoObjects);
+	if (MujocoObjects.IsValidIndex(0) && Cast<AMujocoStaticMeshActor>(MujocoObjects[0]))
+	{
+		EpisodeTargetObject = Cast<AMujocoStaticMeshActor>(MujocoObjects[0]);		
+	}
+}
+
+void UEpisodeSubSystem::FindRobotPawnFromScene()
+{
+	TArray<AActor*> RobotPawns;
+	UGameplayStatics::GetAllActorsOfClass(this->GetWorld(), ARobotPawn::StaticClass(), RobotPawns);
+	if (RobotPawns.IsValidIndex(0) && Cast<ARobotPawn>(RobotPawns[0]))
+	{
+		CurrentRobot = Cast<ARobotPawn>(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<URobotPilotSO100Component>(CurrentRobot->RobotPilotComponent);
+	const auto Joints = So100PilotCmp->GetCurrentControlsFromPhysicScene();
+
+	// JB
+	// Here I need this specific data
+}
+
+void UEpisodeSubSystem::SendEpisodeData()
+{
+	// Send the Data
 }

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

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();

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<ARobotPawn> CurrentRobot;
+
+	// The object that will serve for the episode
+	TObjectPtr<AMujocoStaticMeshActor> EpisodeTargetObject;
+	FTransform CurrentObjectBaseLocation = FTransform::Identity;
+	FTransform CurrentObjectTargetLocation = FTransform::Identity;
+
+
+	// --------------------
+	// ------- DATA -------
+	// --------------------
+
+	// Noah here add anything you need
+	void ConfigureDataCapture();
+
+	void CreatePayload();
+
+	void SendEpisodeData();
 };
+
+
+

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

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

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