Merge pull request 'FT - Update Data Structure for Data transfer' (#63) from Noah_DataTransfer_1.0 into main

Reviewed-on: #63

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Private/LuckyDataTransferSubsystem.cpp

@@ -17,6 +17,7 @@
 #include "JsonObjectConverter.h"
 #include "ReviewComments.h"
 #include "WebSocketsModule.h"
+#include "IWebSocket.h"
 #include "Kismet/KismetStringLibrary.h"
 #include "Camera/CameraActor.h"
 #include "Camera/CameraComponent.h"
@@ -55,7 +56,7 @@ void ULuckyDataTransferSubsystem::Deinitialize()
 
 void ULuckyDataTransferSubsystem::Internal_OpenWebsocket(const FString& URL, const FString& Protocol)
 {
-	const FString NewUrl = URL.IsEmpty() ? TEXT("ws://127.0.0.1:3000/ws") : URL;
+	const FString NewUrl = URL.IsEmpty() ? TEXT("ws://127.0.0.1:3000/world") : URL;
 	const FString NewProtocol = Protocol.IsEmpty() ? TEXT("ws") : Protocol;
 
 	UE_LOG(LogTemp, Warning, TEXT("Opening WebSocket URL: %s"), *NewUrl);
@@ -66,6 +67,9 @@ void ULuckyDataTransferSubsystem::Internal_OpenWebsocket(const FString& URL, con
 	}
 	
 	Socket = FWebSocketsModule::Get().CreateWebSocket(NewUrl);
+
+	if (Socket.IsValid()) UE_LOG(LogTemp, Warning, TEXT("socket Valid %s"), *NewUrl);
+	
 	Socket->Connect();
 
 	//Set up callbacks
@@ -80,8 +84,9 @@ void ULuckyDataTransferSubsystem::Callback_OnConnected()
 	if (OnSocketReady.IsBound())
 	{
 		OnSocketReady.Broadcast(true);
-		UE_LOG(LogTemp, VeryVerbose, TEXT("WebSocket connected successfully"));
 	}
+	
+	UE_LOG(LogTemp, Warning, TEXT("WebSocket connected successfully"));
 }
 
 void ULuckyDataTransferSubsystem::Callback_OnConnectionError(const FString& Error)
@@ -207,7 +212,7 @@ bool ULuckyDataTransferSubsystem::WriteImageToDisk(const FString& inPath, const
 		return false;
 	}
 	
-	FString Path = inPath.IsEmpty() ? TEXT("C:/LuckyRobotsImages") : inPath;
+	FString Path = inPath.IsEmpty() ? TEXT("../Saved/LuckyRobotsData") : inPath; // swap this for const path
 	
 	if (!SensorPawns.IsEmpty())
 	{

Plugins/LuckyDataTransfer/Source/LuckyDataTransfer/Public/ObservationData.h

@@ -58,6 +58,9 @@ public:
 
 	UPROPERTY(BlueprintReadWrite, Category = "Observation")
 	FString filePath = FString();
+	
+	UPROPERTY(BlueprintReadWrite, Category = "Observation")
+	FString timeStamp = FString();
 };
 
 USTRUCT(BlueprintType)
@@ -67,10 +70,14 @@ struct FObservationPayload
 
 public:
 	UPROPERTY(BlueprintReadWrite, Category = "Observation")
-	FString timeStamp = FString();
+	FString type = FString();
 
 	UPROPERTY(BlueprintReadWrite, Category = "Observation")
-	FString id = FString();
+	FString request_id = FString();
+
+		
+	UPROPERTY(BlueprintReadWrite, Category = "Observation")
+	FString timeStamp = FString();
 	
 	UPROPERTY(BlueprintReadWrite, Category = "Observation")
 	TMap<FString, float> ObservationState;

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

@@ -209,6 +209,46 @@ mjData_& AMujocoVolumeActor::GetMujocoData() const
 	return *MujocoData.Get();
 }
 
+void AMujocoVolumeActor::UpdateGeomPosition(const FString BodyName, const FVector& NewPosition, const FQuat& NewRotation)
+{
+	// Step 1: Get body ID
+	const int Body_ID = mj_name2id(MujocoModel.Get(), mjOBJ_BODY, TCHAR_TO_ANSI(*BodyName));
+	if (Body_ID < 0) {
+		UE_LOG(LogTemp, Error, TEXT("Body not found: %s"), *BodyName);
+		return;
+	}
+
+	// Step 2: Get the joint ID (assuming one joint per body)
+	const int Joint_Adr = MujocoModel->body_jntadr[Body_ID];
+	if (MujocoModel->jnt_type[Joint_Adr] != mjJNT_FREE) {
+		UE_LOG(LogTemp, Error, TEXT("Body '%s' does not have a free joint."), *BodyName);
+		return;
+	}
+
+	// Step 3: Get qpos and qvel addresses
+	const int Qpos_Adr = MujocoModel->jnt_qposadr[Joint_Adr];
+	const int Qvel_Adr = MujocoModel->jnt_dofadr[Joint_Adr];
+
+	// Step 4: Convert position and rotation
+	MujocoData->qpos[Qpos_Adr + 0] = NewPosition.X / 100.f;       // X
+	MujocoData->qpos[Qpos_Adr + 1] = -NewPosition.Y / 100.f;      // Y (flip for Unreal Z-up)
+	MujocoData->qpos[Qpos_Adr + 2] = NewPosition.Z / 100.f;       // Z
+
+	// Unreal (X, Y, Z, W) → MuJoCo (W, X, Y, Z)
+	MujocoData->qpos[Qpos_Adr + 3] = NewRotation.W;
+	MujocoData->qpos[Qpos_Adr + 4] = NewRotation.X;
+	MujocoData->qpos[Qpos_Adr + 5] = NewRotation.Y;
+	MujocoData->qpos[Qpos_Adr + 6] = NewRotation.Z;
+
+	// Step 5: Zero velocity
+	for (int i = 0; i < 6; i++) {
+		MujocoData->qvel[Qvel_Adr + i] = 0.0;
+	}
+
+	// Step 6: Update MuJoCo state
+	mj_forward(MujocoModel.Get(), MujocoData.Get());
+}
+
 void AMujocoVolumeActor::SetActuatorValue(const FString& ActuatorName, double Value)
 {
 	if (MujocoModel)

Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h

@@ -110,6 +110,9 @@ public:
 	 */
 	mjData_& GetMujocoData() const;
 
+	UFUNCTION(BlueprintCallable, Category = "Mujoco")
+	void UpdateGeomPosition(const FString BodyName, const FVector& NewPosition, const FQuat& NewRotation);
+
 	// ---------------------------
 	// ------- POST UPDATE -------
 	// ---------------------------

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

@@ -59,20 +59,6 @@ void URobotPilotSO100Component::DisableAnim()
 	AnimationStartTime = 0;
 }
 
-void URobotPilotSO100Component::DebugUpdatePos()
-{
-	const int JointId = RobotOwner->PhysicsSceneProxy->GetJointId("MujocoBody_6"); 
-	auto MjData = RobotOwner->PhysicsSceneProxy->GetMujocoData();
-	auto MjModel = RobotOwner->PhysicsSceneProxy->GetMujocoModel();
-	// mj_name2id(MjModel, mjOBJ_JOINT, "");
-	
-	if (JointId)
-	{
-		auto adx = MjModel.jnt_qposadr[JointId];
-		mjtNum* body_pos = MjData.xpos + 3 * JointId;
-		UE_LOG(LogTemp, Log, TEXT("JointId %i - pos.x %f - pos.y %f - pos.z %f"), JointId, body_pos[0], body_pos[1], body_pos[2]);
-	}
-}
 
 FSo100Actuators URobotPilotSO100Component::GetCurrentControlsFromPhysicScene() const
 {

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

@@ -53,9 +53,6 @@ public:
 	UFUNCTION(BlueprintCallable)
 	void DisableAnim();
 
-	UFUNCTION(BlueprintCallable)
-	void DebugUpdatePos();
-	
 private:
 	// SO100 Controls by name
 	FString Actuator_Rotation = FString("Rotation");