FT - Update an Object in the Mujoco Scene at runtime

+ Target with a Body name
+ Update Location and Rotation

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

@@ -109,7 +109,10 @@ public:
 	 * @return mjData_ - Full access to mujoco scene options and data 
 	 */
 	mjData_& GetMujocoData() const;
-	
+
+	UFUNCTION(BlueprintCallable, Category = "Mujoco")
+	void UpdateGeomPosition(const FString BodyName, const FVector& NewPosition, const FQuat& NewRotation);
+
 	// ---------------------------
 	// ------- POST UPDATE -------
 	// ---------------------------