From 306d3016f6634fbb5dddf579a6f47cf45f4d9aa7 Mon Sep 17 00:00:00 2001 From: Jb win Date: Fri, 2 May 2025 22:48:49 +0700 Subject: [PATCH] FT - Update an Object in the Mujoco Scene at runtime + Target with a Body name + Update Location and Rotation --- .../Private/Actors/MujocoVolumeActor.cpp | 40 +++++++++++++++++++ .../Public/Actors/MujocoVolumeActor.h | 5 ++- 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp b/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp index 54e44206..c3e9d9b3 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Private/Actors/MujocoVolumeActor.cpp +++ b/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) diff --git a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h b/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h index 18190bf5..831fa7b0 100644 --- a/Plugins/LuckyMujoco/Source/LuckyMujoco/Public/Actors/MujocoVolumeActor.h +++ b/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 ------- // ---------------------------