From 5edcefd9b49ad071ccab6f776ffb6733e4e7a1a7 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:38 +0100 Subject: [PATCH 01/32] Resolve the step mode on the engine and make the worker thread-safe A per-mode strategy replaces the branching that had grown across live, direct and puppet stepping. The worker reads shadow atomics rather than UPROPERTYs, joins before teardown, publishes only when it actually advanced, and paces the live loop with a hybrid sleep instead of a full spin. --- .../URLab/Private/MuJoCo/Core/AMjManager.cpp | 335 ++++++++++++++++-- .../Private/MuJoCo/Core/MjArticulation.cpp | 7 +- .../Private/MuJoCo/Core/MjPhysicsEngine.cpp | 311 +++++++++++++--- .../URLab/Private/Replay/MjReplayManager.cpp | 5 +- Source/URLab/Public/MuJoCo/Core/AMjManager.h | 140 +++++++- .../Public/MuJoCo/Core/MjPhysicsEngine.h | 88 ++++- 6 files changed, 779 insertions(+), 107 deletions(-) diff --git a/Source/URLab/Private/MuJoCo/Core/AMjManager.cpp b/Source/URLab/Private/MuJoCo/Core/AMjManager.cpp index 1023d385..ea56e421 100644 --- a/Source/URLab/Private/MuJoCo/Core/AMjManager.cpp +++ b/Source/URLab/Private/MuJoCo/Core/AMjManager.cpp @@ -36,9 +36,15 @@ #include "Blueprint/UserWidget.h" #include "Transport/ZmqPublishTransport.h" #include "Transport/ZmqSubscribeTransport.h" -#include "Transport/ZmqRpcTransport.h" #include "Bridge/RpcDispatcher.h" -#include "Transport/SnapshotProducer.h" +#include "Bridge/BridgeServerConfig.h" +#include "Bridge/BridgeServerConfigUtils.h" +#include "State/MjMsgpackEncoder.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" +#include "State/MjObservationLevel.h" +#include "UserChannels/MjUserChannelComponent.h" +#include "Urdf/UrdfExporter.h" #include "Transport/ShmPublishTransport.h" #include "Transport/ShmRpcTransport.h" #include "MuJoCo/Core/MjSimulationState.h" @@ -74,7 +80,52 @@ void AAMjManager::PostCompile() { if (PhysicsEngine) PhysicsEngine->PostCompile(); + RefreshStateCaches(); +} + +void AAMjManager::RefreshStateCaches() +{ BuildEntityCache(); + // Rebuild the state-IR producer cache off the same trigger as the entity + // cache (initial compile + every recompile). Both run on the game thread. + StateCollector.Init(this); + StateCollector.RebuildProducerCacheGameThread(); + // Re-export the URDF(s) so the robot_description matches the fresh model. + ExportRobotDescriptions(); +} + +void AAMjManager::ExportRobotDescriptions() +{ + RobotDescriptions.Reset(); + if (!PhysicsEngine || !PhysicsEngine->m_model) + return; + + const mjModel* Model = PhysicsEngine->m_model; + const FString RootDir = FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("URLab"), TEXT("UrdfExport")); + const FUrdfExportConfig Cfg; + + for (AMjArticulation* Art : GetAllArticulations()) + { + if (!Art) + continue; + const FString RawName = Art->GetName(); + const FName Segment = FMjCanonicalName::ArtSegment(Art); + const FString SegmentStr = Segment.ToString(); + const FString OutDir = FPaths::Combine(RootDir, SegmentStr); + + const FUrdfModel Urdf = FUrdfExporter::ExportToDir( + Model, SegmentStr, RawName, OutDir, Cfg); + if (Urdf.Links.Num() == 0) + continue; + + RobotDescriptions.Add(Segment, Urdf.Xml); + for (const FString& W : Urdf.Warnings) + UE_LOG(LogURLab, Warning, TEXT("[URDF] %s: %s"), *SegmentStr, *W); + UE_LOG(LogURLab, Log, + TEXT("[URDF] %s: %d links, %d joints, %d meshes -> %s"), + *SegmentStr, Urdf.Links.Num(), Urdf.Joints.Num(), Urdf.MeshIds.Num(), + *FPaths::Combine(OutDir, TEXT("model.urdf"))); + } } void AAMjManager::BuildEntityCache() @@ -186,13 +237,32 @@ void AAMjManager::BeginPlay() // Cooked path: no editor subsystem. Manager owns its bridge and // brings up both RPC transports inline. Bridge owns all RPC // transports; manager only owns publish/subscribe streams. + FURLabBridgeServerConfig CookedConfig; + URLabBridgeServerConfigUtils::LoadFromIni(CookedConfig); + URLabBridgeServerConfigUtils::ApplyEnvAndCommandLineOverrides(CookedConfig); + BridgeServer = NewObject(this, TEXT("BridgeServer")); BridgeServer->SetOwnedByManager(true); - BridgeServer->Start(); // ZMQ on tcp://0.0.0.0:5559 - BridgeServer->EnsureShmBound(); // SHM under "live" + BridgeServer->SetInstanceConfig(CookedConfig); + const FString StepEndpoint = FString::Printf(TEXT("tcp://%s:%d"), + *CookedConfig.BindAddress, CookedConfig.StepPort); + BridgeServer->Start(StepEndpoint); + BridgeServer->EnsureShmBound(CookedConfig.InstanceId); } BridgeServer->RegisterManager(this); + // Bind external (ROS) transports if their module is present. No-op when + // URLabRos is not loaded (its factory hooks are unbound), so non-ROS builds + // and non-ROS users are unaffected. This is what starts in-process ROS + // publishing/services for a live session. + BridgeServer->EnsureExternalTransportsBound(); + + // State PUB binds the configured address + port so farm instances don't + // collide; single-editor defaults reproduce tcp://0.0.0.0:5555. + const FURLabBridgeServerConfig& NetConfig = BridgeServer->GetInstanceConfig(); + const FString StateEndpoint = FString::Printf(TEXT("tcp://%s:%d"), + *NetConfig.BindAddress, NetConfig.StatePort); + // Auto-create the streaming transports (PIE-only producers — they // tap PhysicsEngine pre/post-step callbacks). Bridge-style UObject // lifecycle: NewObject + SetOwningManager + TransportInit. Manager @@ -204,12 +274,13 @@ void AAMjManager::BeginPlay() this, TEXT("AutoZmqBroadcaster")); if (Broadcaster) { + Broadcaster->ZmqEndpoint = StateEndpoint; Broadcaster->SetOwningManager(this); if (Broadcaster->TransportInit()) { ManagerOwnedPublishTransports.Add(Broadcaster); UE_LOG(LogURLab, Log, - TEXT("[AAMjManager] Created UURLabZmqPublishTransport (tcp://0.0.0.0:5555)")); + TEXT("[AAMjManager] Created UURLabZmqPublishTransport (%s)"), *StateEndpoint); } } @@ -242,6 +313,10 @@ void AAMjManager::BeginPlay() // Compile via PhysicsEngine (also discovers ZMQ components in PreCompile) Compile(); + // The engine runs its own PostCompile (component PostSetup) inside Compile(); + // the manager's PostCompile shim is not on that path, so build the entity + + // producer caches the state IR reads here, after the articulation lists sync. + RefreshStateCaches(); if (NetworkManager) NetworkManager->UpdateCameraStreamingState(); @@ -300,37 +375,16 @@ void AAMjManager::BeginPlay() } }); - // Build the state_full snapshot once per physics step and fan it - // out to every IMjSnapshotPublisher (ZMQ PUB, SHM ring, ...). This - // is the only place BuildStateSnapshot runs per step. + // Build the state IR once per physics step, encode it to the canonical + // msgpack `state_full` snapshot, and fan the bytes out to every + // IMjSnapshotPublisher (ZMQ PUB, SHM ring, ...). This runs inside the + // engine's CallbackMutex, so the collector's persistent snapshot buffer + // never races an on-demand Collect from an RPC step reply. TWeakObjectPtr WeakSelf(this); PhysicsEngine->RegisterPostStepCallback( [WeakSelf](mjModel* m, mjData* d) { - AAMjManager* Self = WeakSelf.Get(); - if (!Self) - return; - - TArray Pubs; - { - FScopeLock Lock(&Self->SnapshotPublishersMutex); - Pubs.Reserve(Self->SnapshotPublishers.Num()); - for (const FRegisteredSnapshotPublisher& R : Self->SnapshotPublishers) - { - if (R.Publisher && R.Owner.IsValid()) - Pubs.Add(R.Publisher); - } - } - if (Pubs.Num() == 0) - return; - - FURLabRpcDispatcher* Disp = Self->GetStepDispatcher(); - const int64 StepIdx = Disp ? Disp->GetStepCounter() : 0; - TArray Buf = FMjSnapshotProducer::BuildStateSnapshot( - Self, m, d, StepIdx); - if (Buf.Num() == 0) - return; - for (IMjSnapshotPublisher* Pub : Pubs) - Pub->PublishSnapshot(Buf); + if (AAMjManager* Self = WeakSelf.Get()) + Self->FanOutStateSnapshot(m, d); }); PhysicsEngine->RunMujocoAsync(); @@ -395,6 +449,188 @@ void AAMjManager::UnregisterSnapshotPublisher(IMjSnapshotPublisher* Publisher) }); } +void AAMjManager::RegisterStateProducer(TScriptInterface Producer) +{ + UObject* Obj = Producer.GetObject(); + if (!Obj) + return; + { + FScopeLock Lock(&StateProducersMutex); + for (const TWeakObjectPtr& P : StateProducers) + { + if (P.Get() == Obj) + return; // already registered + } + StateProducers.Add(Obj); + } + StateCollector.MarkProducerCacheDirty(); +} + +void AAMjManager::UnregisterStateProducer(TScriptInterface Producer) +{ + UObject* Obj = Producer.GetObject(); + if (!Obj) + return; + { + FScopeLock Lock(&StateProducersMutex); + StateProducers.RemoveAll([Obj](const TWeakObjectPtr& P) { + return P.Get() == Obj; + }); + } + StateCollector.MarkProducerCacheDirty(); +} + +void AAMjManager::GetStateProducers(TArray>& Out) const +{ + FScopeLock Lock(&StateProducersMutex); + Out = StateProducers; +} + +namespace +{ +// The canonical art segment a user-channel component contributes under, or empty +// for scene scope. Mirrors the collector's producer scope resolution. +FString UserChannelScopeSegment(const UMjUserChannelComponent* Comp) +{ + if (!Comp) + return FString(); + AActor* Owner = Comp->GetOwner(); + if (const AMjArticulation* Art = Cast(Owner)) + return FMjCanonicalName::ArtSegment(Art).ToString(); + return FString(); +} +} // namespace + +bool AAMjManager::ApplyUserChannelInput(FName ArtOrNone, FName Channel, + const FMjUserChannel& Value) +{ + const FString TargetScope = ArtOrNone.IsNone() ? FString() : ArtOrNone.ToString(); + + TArray> Producers; + GetStateProducers(Producers); + + bool bApplied = false; + for (const TWeakObjectPtr& Weak : Producers) + { + UMjUserChannelComponent* Comp = Cast(Weak.Get()); + if (!Comp) + continue; + if (UserChannelScopeSegment(Comp) != TargetScope) + continue; + EMjUserChannelKind Declared; + if (!Comp->GetDeclaredInputKind(Channel, Declared)) + continue; + if (Comp->ApplyInput(Channel, Value)) + bApplied = true; + } + return bApplied; +} + +void AAMjManager::GetUserInputChannels(TArray& Out) const +{ + TArray> Producers; + GetStateProducers(Producers); + + for (const TWeakObjectPtr& Weak : Producers) + { + UMjUserChannelComponent* Comp = Cast(Weak.Get()); + if (!Comp) + continue; + const FString Scope = UserChannelScopeSegment(Comp); + TArray> Declared; + Comp->GetDeclaredInputChannels(Declared); + for (const TPair& Pair : Declared) + { + FMjUserInputChannelInfo Info; + Info.ArtSegment = Scope; + Info.Channel = Pair.Key; + Info.Kind = Pair.Value; + Out.Add(MoveTemp(Info)); + } + } +} + +void AAMjManager::RegisterStateConsumer(IMjStateConsumer* Consumer, UObject* OwnerObj) +{ + if (!Consumer || !OwnerObj) + return; + FScopeLock Lock(&StateConsumersMutex); + for (const FRegisteredStateConsumer& R : StateConsumers) + { + if (R.Consumer == Consumer) + return; // already registered + } + StateConsumers.Add({OwnerObj, Consumer}); +} + +void AAMjManager::UnregisterStateConsumer(IMjStateConsumer* Consumer) +{ + if (!Consumer) + return; + FScopeLock Lock(&StateConsumersMutex); + StateConsumers.RemoveAll([Consumer](const FRegisteredStateConsumer& R) { + return R.Consumer == Consumer; + }); +} + +void AAMjManager::FanOutStateSnapshot(mjModel* m, mjData* d) +{ + // Build the state IR once per physics step, encode it to the canonical + // msgpack `state_full` snapshot, and fan the bytes out to every + // IMjSnapshotPublisher (ZMQ PUB, SHM ring, ...). Registered IMjStateConsumers + // receive the same typed IR and run their own encoders. Runs inside the + // engine's CallbackMutex, so the collector's persistent snapshot buffer never + // races an on-demand Collect from an RPC step reply. + TArray Pubs; + { + FScopeLock Lock(&SnapshotPublishersMutex); + Pubs.Reserve(SnapshotPublishers.Num()); + for (const FRegisteredSnapshotPublisher& R : SnapshotPublishers) + { + if (R.Publisher && R.Owner.IsValid()) + Pubs.Add(R.Publisher); + } + } + + TArray Consumers; + { + FScopeLock Lock(&StateConsumersMutex); + Consumers.Reserve(StateConsumers.Num()); + for (const FRegisteredStateConsumer& R : StateConsumers) + { + if (R.Consumer && R.Owner.IsValid()) + Consumers.Add(R.Consumer); + } + } + + // bPublishersPaused gates the msgpack byte fan-out only: it is set on + // Direct / Puppet mode entry so the step reply is the sole delivery to the + // stepping client (no double-write). A typed consumer is a distinct sink, so + // it receives the IR every step in all modes regardless of the pause. + const bool bByteFanOut = Pubs.Num() > 0 + && !bPublishersPaused.load(std::memory_order_acquire); + + if (!bByteFanOut && Consumers.Num() == 0) + return; + + FURLabRpcDispatcher* Disp = GetStepDispatcher(); + const int64 StepIdx = Disp ? Disp->GetStepCounter() : 0; + const FMjStateSnapshot& Snap = StateCollector.Collect(m, d, StepIdx); + + for (IMjStateConsumer* Consumer : Consumers) + Consumer->ConsumeState(Snap); + + if (!bByteFanOut) + return; + + TArray Buf = FMjMsgpackEncoder::EncodeSnapshotBytes( + Snap, EObservationLevel::Standard); + if (Buf.Num() == 0) + return; + for (IMjSnapshotPublisher* Pub : Pubs) + Pub->PublishSnapshot(Buf); +} + void AAMjManager::EndPlay(const EEndPlayReason::Type EndPlayReason) { // Stop the physics async thread BEFORE Super::EndPlay so PostStep @@ -425,7 +661,12 @@ void AAMjManager::EndPlay(const EEndPlayReason::Type EndPlayReason) kShutdownTimeoutSec); } } - PhysicsEngine->ClearCallbacks(); + // Clearing callbacks takes CallbackMutex, which a wedged worker holds for + // its whole iteration. Only safe once the worker has provably exited; + // on the detach path the callbacks leak with the rest of the accepted + // leak rather than deadlock PIE-stop. + if (bAsyncExited) + PhysicsEngine->ClearCallbacks(); } // Manager-owned transports aren't UActorComponents, so EndPlay @@ -500,7 +741,21 @@ void AAMjManager::Tick(float DeltaTime) return; } - const TArray Arts = PhysicsEngine->GetAllArticulations(); + ApplyLatestRenderState(); +} + +void AAMjManager::ApplyLatestRenderState() +{ + if (!PhysicsEngine || !PhysicsEngine->IsInitialized()) + { + return; + } + + // Ask the live-mode worker to publish a fresh snapshot; it copies the + // full state only when a consumer (this tick) has requested one. + PhysicsEngine->bSnapshotWanted.store(true, std::memory_order_release); + + const TArray& Arts = PhysicsEngine->GetAllArticulations(); const TArray Quicks = PhysicsEngine->GetAllQuickComponents(); PhysicsEngine->WithRenderState([&](const FMjRenderSnapshot& Snap) { @@ -518,6 +773,10 @@ void AAMjManager::Tick(float DeltaTime) Quick->ApplyRenderState(Snap); } } + // Record which post-step state the actors now reflect so cameras can + // tag their readbacks with it (frame_id association for the bridge). + LastAppliedRenderFrameId.store(Snap.FrameId, std::memory_order_release); + LastAppliedRenderSimTime.store(Snap.SimTime, std::memory_order_release); }); } @@ -566,6 +825,10 @@ bool AAMjManager::CompileModel() m_heightfieldActors = PhysicsEngine->m_heightfieldActors; m_ArticulationMap = PhysicsEngine->m_ArticulationMap; + // Component ids/views were re-bound; rebuild the state-IR caches so the + // collector's weak ptrs and entity table match the fresh model. + RefreshStateCaches(); + return Result; } @@ -574,7 +837,7 @@ AMjArticulation* AAMjManager::GetArticulation(const FString& ActorName) const return PhysicsEngine ? PhysicsEngine->GetArticulation(ActorName) : nullptr; } -TArray AAMjManager::GetAllArticulations() const +const TArray& AAMjManager::GetAllArticulations() const { return PhysicsEngine ? PhysicsEngine->GetAllArticulations() : m_articulations; } diff --git a/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp b/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp index 3b9dc2a3..a5670e3a 100644 --- a/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp +++ b/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp @@ -700,8 +700,8 @@ void AMjArticulation::ApplyControls(bool bSkipController) // thread races against game-thread mutations and corrupts nearby heap. // The bridge can opt this articulation out for the current sub-step // by setting `bSkipController=true` (mirrors a per-step - // `control_mode="raw"` from the wire); the staged `NetworkValue` - // then lands directly on `d->ctrl` without controller transformation. + // `control_mode="raw"` from the wire); d->ctrl is written directly + // by ApplyStepCtrl and the staged NetworkValue path is skipped. if (!bSkipController && CachedController && CachedController->bEnabled && CachedController->IsBound()) { @@ -709,6 +709,9 @@ void AMjArticulation::ApplyControls(bool bSkipController) return; } + if (bSkipController) + return; + // Default path: write control values directly to d->ctrl for (auto& Elem : ActuatorIdMap) { diff --git a/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp b/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp index 34fd9587..f804b277 100644 --- a/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp +++ b/Source/URLab/Private/MuJoCo/Core/MjPhysicsEngine.cpp @@ -22,6 +22,7 @@ #include "MuJoCo/Core/MjPhysicsEngine.h" #include "MuJoCo/Core/MjArticulation.h" +#include "State/MjCanonicalName.h" #include "MuJoCo/Components/QuickConvert/MjQuickConvertComponent.h" #include "MuJoCo/Components/QuickConvert/AMjHeightfieldActor.h" #include "MuJoCo/Core/Spec/MjSpecWrapper.h" @@ -167,7 +168,36 @@ UMjPhysicsEngine::UMjPhysicsEngine() void UMjPhysicsEngine::BeginDestroy() { + // Stop and JOIN the async worker before tearing anything down. The + // worker captures `this` and dereferences m_model / m_data / + // m_articulations every iteration, and it may be parked on + // StepRequestEvent — returning that event to the pool (below) while + // the worker still waits on it is a use-after-free. Wait() outside any + // lock the worker takes so it can reach its bShouldStopTask check. + bShouldStopTask = true; if (StepRequestEvent) + StepRequestEvent->Trigger(); + + // Bounded join. BeginDestroy runs on the GC path, so an unbounded wait on a + // wedged mj_step would hang garbage collection (and with it the editor). If + // the worker does not exit in time we leak its sync event and MuJoCo state + // rather than block forever or free memory the still-running worker reads. + bool bWorkerExited = true; + if (AsyncPhysicsFuture.IsValid()) + { + constexpr double kBeginDestroyWaitSec = 3.0; + bWorkerExited = AsyncPhysicsFuture.WaitFor(FTimespan::FromSeconds(kBeginDestroyWaitSec)); + if (!bWorkerExited) + { + UE_LOG(LogURLab, Warning, + TEXT("Physics async worker still running at BeginDestroy after %.1fs; ") + TEXT("leaking its sync event to avoid a use-after-free in the stuck step."), + kBeginDestroyWaitSec); + } + } + + // Only recycle the event once the worker has provably stopped waiting on it. + if (bWorkerExited && StepRequestEvent) { FPlatformProcess::ReturnSynchEventToPool(StepRequestEvent); StepRequestEvent = nullptr; @@ -175,6 +205,17 @@ void UMjPhysicsEngine::BeginDestroy() Super::BeginDestroy(); } +#if WITH_EDITOR +void UMjPhysicsEngine::PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent) +{ + Super::PostEditChangeProperty(PropertyChangedEvent); + // Keep the worker's lock-free shadows in step with details-panel edits + // of bIsPaused / SimSpeedPercent. + bPausedAtomic.store(bIsPaused, std::memory_order_release); + SimSpeedAtomic.store(SimSpeedPercent, std::memory_order_release); +} +#endif + void UMjPhysicsEngine::PreCompile() { m_spec = mj_makeSpec(); @@ -204,7 +245,7 @@ void UMjPhysicsEngine::PreCompile() if (AMjArticulation* Articulation = Cast(actor)) { Articulation->Setup(m_spec, &m_vfs); - m_articulations.Add(Articulation); + RegisterArticulation(Articulation); if (FMujocoSpecWrapper* W = Articulation->GetWrapper()) { for (const FString& Path : W->ActiveAssetPaths) @@ -407,29 +448,81 @@ void UMjPhysicsEngine::RunMujocoAsync() bShouldStopTask = false; + // Seed the worker's lock-free shadow state from the current config so it + // never reads UPROPERTYs or the owning actor from the physics thread. + bPausedAtomic.store(bIsPaused, std::memory_order_release); + SimSpeedAtomic.store(SimSpeedPercent, std::memory_order_release); + + // Seed the resolved step mode from the RPC dispatcher, which is the runtime + // owner of the mode: a client hello promotes Live -> Direct/Puppet after + // startup, so the manager's configured StepMode is only the initial default. + // This path also runs on recompile (CompileModel restarts the worker); reading + // the configured StepMode here would revert a mid-session recompile back to + // Live while the Direct/Puppet handler stays installed, so the worker would + // pace real-time with the wrong controller pass. SetStepMode re-applies the + // engine-side effects of the strategy's OnEnter (pacing + unpausing client + // modes); the installed CustomStepHandler and the publisher-pause flag both + // survive the recompile, so the full invariant is restored. Fall back to the + // configured mode only before a dispatcher exists. + if (AAMjManager* Mgr = Cast(GetOwner())) + { + FURLabRpcDispatcher* Disp = Mgr->GetStepDispatcher(); + SetStepMode(Disp ? Disp->GetActiveStepMode() : Mgr->StepMode); + // A recompile rebuilt m_model/m_data under a live session; re-run the + // active strategy's OnEnter so its step handler is reinstalled onto the + // fresh engine and the pause / pacing invariants are restored. + if (Disp) + Disp->ReapplyActiveStepMode(); + } + AsyncPhysicsFuture = Async(EAsyncExecution::Thread, [this]() { + bWorkerRunning.store(true, std::memory_order_release); FPlatformProcess::Sleep(0.0f); while (true) { const double LoopStartTime = FPlatformTime::Seconds(); - // Re-read per iteration so set_sim_options retunes the pacer live. - const float TargetInterval = m_model ? (float)m_model->opt.timestep : 0.002f; if (bShouldStopTask) break; + // Runtime-resolved step mode, owned by the RPC dispatcher and + // seeded from config. Drives the controller pass and the pacer + // without a per-iteration actor cast. + const EStepMode Mode = ResolvedStepMode.load(std::memory_order_acquire); + + // Real-time pacer interval. Read from the model under CallbackMutex + // below (a concurrent CompileModel frees m_model, so reading it + // outside the lock races the delete); re-read per iteration so + // set_sim_options retunes the pacer live. Defaulted so pacing stays + // sane if the model is momentarily absent. + float TargetInterval = 0.002f; + { FScopeLock Lock(&CallbackMutex); if (!m_model || !m_data || bShouldStopTask) break; + TargetInterval = (float)m_model->opt.timestep; + + // Did mjData actually change this iteration? Only then do we + // publish a render snapshot (which bumps FrameId and drives + // state-change camera capture). Paused iterations and idle + // direct/puppet wakes leave this false. + bool bAdvanced = false; + + // A step handler that self-publishes (direct mode) sets this so + // the tail below doesn't publish a second time and advance the + // id past what the step reported. + bRenderStatePublishedThisStep = false; + if (bPendingReset) { mj_resetData(m_model, m_data); mj_forward(m_model, m_data); bPendingReset = false; + bAdvanced = true; // Zero all actuator control values so stale commands // don't persist after reset. @@ -444,10 +537,22 @@ void UMjPhysicsEngine::RunMujocoAsync() } } - AsyncTask(ENamedThreads::GameThread, [this]() { - for (AMjArticulation* Art : m_articulations) + // Snapshot the registry into weak refs under CallbackMutex. + // The broadcast runs later on the game thread and must not + // capture a raw `this` (the engine may be torn down before it + // runs) nor iterate the worker-owned m_articulations array + // off the worker thread. + TArray> ResetTargets; + ResetTargets.Reserve(m_articulations.Num()); + for (AMjArticulation* Art : m_articulations) + { + if (Art) + ResetTargets.Add(Art); + } + AsyncTask(ENamedThreads::GameThread, [ResetTargets = MoveTemp(ResetTargets)]() { + for (const TWeakObjectPtr& Target : ResetTargets) { - if (Art) + if (AMjArticulation* Art = Target.Get()) Art->OnSimulationReset.Broadcast(); } }); @@ -455,11 +560,24 @@ void UMjPhysicsEngine::RunMujocoAsync() if (bPendingRestore) { - bPendingRestore = false; - if (PendingStateVector.Num() > 0) + TArray RestoreState; + int32 RestoreMask = 0; + { + // Swap the pending vector out under CommandMutex so a + // concurrent RestoreSnapshot can't tear it mid-read. + FScopeLock CmdLock(&CommandMutex); + if (bPendingRestore) + { + RestoreState = MoveTemp(PendingStateVector); + RestoreMask = PendingStateMask; + bPendingRestore = false; + } + } + if (RestoreState.Num() > 0) { - mj_setState(m_model, m_data, PendingStateVector.GetData(), PendingStateMask); + mj_setState(m_model, m_data, RestoreState.GetData(), RestoreMask); mj_forward(m_model, m_data); + bAdvanced = true; } } @@ -471,11 +589,7 @@ void UMjPhysicsEngine::RunMujocoAsync() // Puppet mode: client pushes qpos/qvel/ctrl directly, so // ApplyControls (NetworkValue → d->ctrl) would clobber the // snapshot. Skip the controller pass. - bool bSkipApplyControls = false; - if (AAMjManager* OwnerMgr = Cast(GetOwner())) - { - bSkipApplyControls = (OwnerMgr->EffectiveStepMode.load(std::memory_order_acquire) == EStepMode::Puppet); - } + const bool bSkipApplyControls = (Mode == EStepMode::Puppet); if (!bSkipApplyControls) { for (AMjArticulation* Art : m_articulations) @@ -485,53 +599,87 @@ void UMjPhysicsEngine::RunMujocoAsync() } } - DrainCommands(); + // A mocap/wrench edit mutates m_data even while paused, so it + // counts as an advance (publish it). + bAdvanced |= DrainCommands(); - if (!bIsPaused) + if (!bPausedAtomic.load(std::memory_order_acquire)) { if (CustomStepHandler) - CustomStepHandler(m_model, m_data); + { + // Direct/puppet/replay handler. It owns its own + // OnPostStep notification and returns true iff it + // dequeued work and stepped this call. + bAdvanced |= CustomStepHandler(m_model, m_data); + } else + { mj_step(m_model, m_data); + // Live/streaming path has no custom handler, so the loop + // owns the single post-step notification here. Handlers + // call OnPostStep themselves, so the loop must not — that + // would double-fire recorders in direct mode. + if (OnPostStep) + OnPostStep(m_model, m_data); + bAdvanced = true; + } } + // Streaming publishers / debug capture, left unconditional: + // they broadcast on their own channels and the puppet inline + // push path (RPC thread) doesn't route through this loop, so + // gating them on bAdvanced here would change puppet-mode + // streaming cadence. for (const FPhysicsCallback& Cb : PostStepCallbacks) { Cb(m_model, m_data); } - if (OnPostStep) + // Publish a coherent render snapshot for game-thread consumers, + // inside the same CallbackMutex scope so it reflects the m_data + // just stepped. Gated on bAdvanced: bumping FrameId on an + // unchanged frame re-triggers state-change camera capture on + // identical pixels and inflates FrameId at the idle wake rate. + // In live mode the game thread consumes at frame rate, so + // publish only when it asked (bSnapshotWanted) instead of + // copying the full snapshot every physics step; direct/puppet + // publish every step because the client associates frames by id. + if (bAdvanced && !bRenderStatePublishedThisStep) { - OnPostStep(m_model, m_data); + const bool bWantPublish = (Mode != EStepMode::Live) + || bSnapshotWanted.exchange(false, std::memory_order_acq_rel); + if (bWantPublish) + { + PushRenderState(); + } } - - // Publish a coherent render snapshot for game-thread - // consumers. Inside the same CallbackMutex scope so the - // snapshot reflects the m_data that was just stepped. - PushRenderState(); } // FScopeLock released here // End-of-iteration pacing. // - // Live mode: UE owns the clock. Spin-wait to TargetInterval so - // the loop runs at real-time physics rate. + // Live mode: UE owns the clock. Pace to TargetInterval so the loop + // runs at real-time physics rate. Sleep off the bulk of the wait + // (relies on UE's ~1ms process timer resolution) and spin only the + // final sub-millisecond for accuracy, rather than spinning the + // whole interval and pinning a CPU core. // Direct / Puppet: the client owns the clock. Block on // StepRequestEvent (signalled by the dispatcher on enqueue) // so we drain commands at the rate Python sends them rather // than capping at 1 / timestep Hz. Short timeout keeps the // bShouldStopTask check responsive on shutdown. - bool bUseRealTimePacing = true; - if (AAMjManager* OwnerMgr = Cast(GetOwner())) - { - // Pace off the resolved mode, not the configured StepMode (which - // defaults to Auto). Auto resolves to Live, so a freshly-started - // live session runs real-time instead of blocking at ~10 Hz. - bUseRealTimePacing = (OwnerMgr->EffectiveStepMode.load(std::memory_order_acquire) == EStepMode::Live); - } + // Pace off the resolved mode, not the configured StepMode (which + // defaults to Auto). Auto resolves to Live, so a freshly-started + // live session runs real-time instead of blocking at ~10 Hz. + const bool bUseRealTimePacing = (Mode == EStepMode::Live); if (bUseRealTimePacing) { - const float SpeedFactor = FMath::Clamp(SimSpeedPercent, 5.0f, 100.0f) / 100.0f; + const float SpeedFactor = FMath::Clamp(SimSpeedAtomic.load(std::memory_order_acquire), 5.0f, 100.0f) / 100.0f; const double TargetTime = LoopStartTime + (TargetInterval / SpeedFactor); + const double Remaining = TargetTime - FPlatformTime::Seconds(); + if (Remaining > 0.0015) + { + FPlatformProcess::SleepNoStats((float)(Remaining - 0.0005)); + } while (FPlatformTime::Seconds() < TargetTime) { FPlatformProcess::YieldThread(); @@ -543,6 +691,8 @@ void UMjPhysicsEngine::RunMujocoAsync() StepRequestEvent->Wait(100); } } + + bWorkerRunning.store(false, std::memory_order_release); }); } @@ -559,6 +709,26 @@ EControlSource UMjPhysicsEngine::GetControlSource() const void UMjPhysicsEngine::SetPaused(bool bPaused) { bIsPaused = bPaused; + bPausedAtomic.store(bPaused, std::memory_order_release); +} + +void UMjPhysicsEngine::SetSimSpeed(float Percent) +{ + SimSpeedPercent = Percent; + SimSpeedAtomic.store(Percent, std::memory_order_release); +} + +void UMjPhysicsEngine::SetStepMode(EStepMode Mode) +{ + const EStepMode Resolved = (Mode == EStepMode::Auto) ? EStepMode::Live : Mode; + ResolvedStepMode.store(Resolved, std::memory_order_release); + // Client-driven modes need the worker unpaused so the async loop calls the + // step handler and drains the request queue; the engine otherwise defaults + // to paused until the editor UI unpauses. + if (Resolved != EStepMode::Live && bIsPaused) + { + SetPaused(false); + } } bool UMjPhysicsEngine::IsRunning() const @@ -581,8 +751,8 @@ void UMjPhysicsEngine::StepSync(int32 NumSteps) if (!IsInitialized()) return; - bool bWasPaused = bIsPaused; - bIsPaused = true; + const bool bWasPaused = bIsPaused; + SetPaused(true); FScopeLock Lock(&CallbackMutex); @@ -597,7 +767,7 @@ void UMjPhysicsEngine::StepSync(int32 NumSteps) // scrub, custom step handlers). PushRenderState(); - bIsPaused = bWasPaused; + SetPaused(bWasPaused); } bool UMjPhysicsEngine::CompileModel() @@ -607,6 +777,16 @@ bool UMjPhysicsEngine::CompileModel() // observes bShouldStopTask without waiting out the Wait timeout. if (StepRequestEvent) StepRequestEvent->Trigger(); + + // JOIN the old worker before teardown. Without this, the worker can be + // mid-iteration (between its flag check and its CallbackMutex acquire) + // while we delete m_data/m_model and Empty() the arrays it iterates; + // worse, RunMujocoAsync() below resets bShouldStopTask=false, so a + // stalled old worker could resume against the NEW model. Wait() here, + // outside CallbackMutex, so the worker can reach its stop check. + if (AsyncPhysicsFuture.IsValid()) + AsyncPhysicsFuture.Wait(); + { FScopeLock Lock(&CallbackMutex); if (m_data) @@ -646,15 +826,30 @@ AMjArticulation* UMjPhysicsEngine::GetArticulation(const FString& ActorName) con { if (const AMjArticulation* const* Found = m_ArticulationMap.Find(ActorName)) return const_cast(*Found); + // Resolve by UE object name, the user-supplied ActorId, or the canonical public + // segment (ArtSegment) so a caller can address an art by its ROS/topic name + // ("franka") as well as its raw UE name. for (AMjArticulation* Art : m_articulations) { - if (Art && Art->GetName() == ActorName) + if (!Art) + continue; + if (Art->GetName() == ActorName || Art->ActorId == ActorName + || FMjCanonicalName::ArtSegment(Art).ToString() == ActorName) return Art; } return nullptr; } -TArray UMjPhysicsEngine::GetAllArticulations() const +void UMjPhysicsEngine::RegisterArticulation(AMjArticulation* Articulation) +{ + if (!Articulation) + return; + FScopeLock Lock(&CallbackMutex); + m_articulations.AddUnique(Articulation); + m_ArticulationMap.Add(Articulation->GetName(), Articulation); +} + +const TArray& UMjPhysicsEngine::GetAllArticulations() const { return m_articulations; } @@ -701,19 +896,22 @@ void UMjPhysicsEngine::ClearCustomStepHandler() UMjSimulationState* UMjPhysicsEngine::CaptureSnapshot() { + check(IsInGameThread()); // NewObject must run on the game thread if (!m_model || !m_data) return nullptr; UMjSimulationState* NewSnapshot = NewObject(GetOwner()); - uint32 Mask = mjSTATE_INTEGRATION; - - int nState = mj_stateSize(m_model, Mask); + const uint32 Mask = mjSTATE_INTEGRATION; + const int nState = mj_stateSize(m_model, Mask); NewSnapshot->StateVector.SetNum(nState); NewSnapshot->StateMask = (int32)Mask; - NewSnapshot->SimTime = (float)m_data->time; { + // Read the live state under the step lock so the capture can't tear + // against the physics worker mid-step. + FScopeLock Lock(&CallbackMutex); + NewSnapshot->SimTime = (float)m_data->time; mj_getState(m_model, m_data, NewSnapshot->StateVector.GetData(), Mask); } @@ -726,9 +924,14 @@ void UMjPhysicsEngine::RestoreSnapshot(UMjSimulationState* Snapshot) if (!Snapshot) return; - PendingStateVector = Snapshot->StateVector; - PendingStateMask = Snapshot->StateMask; - bPendingRestore = true; + { + // Match the worker's guarded swap so two restores (or a restore vs the + // worker's read) can't tear the vector. + FScopeLock Lock(&CommandMutex); + PendingStateVector = Snapshot->StateVector; + PendingStateMask = Snapshot->StateMask; + bPendingRestore = true; + } UE_LOG(LogURLab, Log, TEXT("MuJoCo PhysicsEngine: Restore requested for snapshot t=%f"), Snapshot->SimTime); } @@ -856,6 +1059,12 @@ void UMjPhysicsEngine::WithRenderState( Visitor(RenderSnapshot); } +uint64 UMjPhysicsEngine::GetRenderFrameId() +{ + FScopeLock Lock(&RenderStateMutex); + return RenderSnapshot.FrameId; +} + // ============================================================================= // Command channel (UE -> MuJoCo) // @@ -922,16 +1131,16 @@ void UMjPhysicsEngine::ApplySleepBody(int32 BodyId) } } -void UMjPhysicsEngine::DrainCommands() +bool UMjPhysicsEngine::DrainCommands() { if (!m_model || !m_data) - return; + return false; FCommandQueue Local; { FScopeLock Lock(&CommandMutex); if (PendingCommands.IsEmpty()) - return; + return false; Local = MoveTemp(PendingCommands); PendingCommands = FCommandQueue(); } @@ -963,4 +1172,6 @@ void UMjPhysicsEngine::DrainCommands() continue; FMemory::Memzero(m_data->xfrc_applied + 6 * BodyId, sizeof(double) * 6); } + + return true; } diff --git a/Source/URLab/Private/Replay/MjReplayManager.cpp b/Source/URLab/Private/Replay/MjReplayManager.cpp index 59b2f2de..956834ec 100644 --- a/Source/URLab/Private/Replay/MjReplayManager.cpp +++ b/Source/URLab/Private/Replay/MjReplayManager.cpp @@ -382,8 +382,11 @@ void AMjReplayManager::StartReplay() Manager->PhysicsEngine->SetPaused(false); } - Manager->PhysicsEngine->SetCustomStepHandler([this](mjModel* m, mjData* d) { + Manager->PhysicsEngine->SetCustomStepHandler([this](mjModel* m, mjData* d) -> bool { this->OnReplayStep(m, d); + // Replay applies a frame each tick and does not self-publish, so the + // worker loop owns the render-snapshot push — report an advance. + return true; }); UE_LOG(LogURLabReplay, Log, TEXT("ReplayManager: Started Replay of '%s' (%d frames)"), diff --git a/Source/URLab/Public/MuJoCo/Core/AMjManager.h b/Source/URLab/Public/MuJoCo/Core/AMjManager.h index 8368df52..1b96c2ad 100644 --- a/Source/URLab/Public/MuJoCo/Core/AMjManager.h +++ b/Source/URLab/Public/MuJoCo/Core/AMjManager.h @@ -30,6 +30,9 @@ #include "Bridge/RpcDispatcher.h" #include "Bridge/BridgeServer.h" #include "Transport/SnapshotPublisher.h" +#include "State/MjStateCollector.h" +#include "State/MjStateProducer.h" +#include "State/MjStateConsumer.h" #include #include "AMjManager.generated.h" @@ -42,6 +45,23 @@ class UMjInputHandler; class UMjPerturbation; class UMjSimulationState; class UMjBody; +class UMjUserChannelComponent; +struct FMjUserChannel; +enum class EMjUserChannelKind : uint8; + +/** + * @struct FMjUserInputChannelInfo + * @brief One declared user-input channel and its scope, enumerated for the + * transports that create per-channel input subscriptions (ROS) or route + * writes to it (the set_user_channels RPC op). ArtSegment is the canonical + * art segment for art scope, or empty for scene scope. + */ +struct FMjUserInputChannelInfo +{ + FString ArtSegment; + FName Channel; + EMjUserChannelKind Kind; +}; /** * @struct FMjEntityRecord @@ -124,7 +144,7 @@ class URLAB_API AAMjManager : public AActor AMjArticulation* GetArticulation(const FString& ActorName) const; UFUNCTION(BlueprintCallable, BlueprintPure, Category = "MuJoCo|Global") - TArray GetAllArticulations() const; + const TArray& GetAllArticulations() const; UFUNCTION(BlueprintCallable, BlueprintPure, Category = "MuJoCo|Global") TArray GetAllQuickComponents() const; @@ -138,6 +158,26 @@ class URLAB_API AAMjManager : public AActor /** Refresh the entity cache; called from PostCompile. */ void BuildEntityCache(); + /** Rebuild the caches the state IR reads (entity table + producer cache) and + * (re)bind the collector to this manager. Run after every compile / recompile + * on the game thread. */ + void RefreshStateCaches(); + + /** The per-step state-IR collector. Owned by the manager; used by the + * post-step snapshot fan-out and by the RPC step/reset/forward replies. */ + FMjStateCollector& GetStateCollector() { return StateCollector; } + + /** Export a URDF + binary STL meshes for every articulation from the compiled + * mjModel, dumping each to /URLab/UrdfExport// and caching + * the URDF text for the //robot_description publisher. Runs on the + * game thread; auto-invoked from RefreshStateCaches (every compile) and + * callable directly as the manual re-export trigger. No-op without a model. */ + void ExportRobotDescriptions(); + + /** Cached URDF documents keyed by canonical art segment, filled by + * ExportRobotDescriptions and read by the state publish transport. */ + const TMap& GetRobotDescriptions() const { return RobotDescriptions; } + UFUNCTION(BlueprintPure, Category = "MuJoCo|Status") float GetSimTime() const; @@ -176,18 +216,11 @@ class URLAB_API AAMjManager : public AActor */ std::atomic bPublishersPaused{false}; - /** - * @brief The resolved, authoritative step mode the physics loop runs under. - * - * `StepMode` above is the *configured* value and may be `Auto`, which the - * dispatcher resolves to a concrete mode (Auto starts Live). The physics - * loop must pace off the resolved mode, not the configured one — reading - * `StepMode == Live` directly leaves `Auto` (the default) pacing as if it - * were Direct, blocking on the step-request timeout at ~10 Hz. The - * dispatcher mirrors its `ActiveStepMode` here whenever it changes; defaults - * to Live so a bridge-less PIE session runs real-time. - */ - std::atomic EffectiveStepMode{EStepMode::Live}; + /** Post-step render-snapshot id / sim time last applied to the actors. + * Written on the game thread in ApplyLatestRenderState; read (atomically) + * by cameras when stamping readbacks. */ + std::atomic LastAppliedRenderFrameId{0}; + std::atomic LastAppliedRenderSimTime{0.0}; /** Owns the FURLabRpcDispatcher + transports. Created in BeginPlay, destroyed in EndPlay. */ UPROPERTY() @@ -206,6 +239,44 @@ class URLAB_API AAMjManager : public AActor class UObject* OwnerObj); void UnregisterSnapshotPublisher(IMjSnapshotPublisher* Publisher); + /** Register a typed consumer of the per-step state IR. FanOutStateSnapshot + * calls ConsumeState on every registered consumer once per step, in all step + * modes (unlike the byte fan-out, which the Direct/Puppet pause suppresses). + * OwnerObj keeps the registration alive only while the owner is valid. This + * is the transport-agnostic seam an out-of-core encoder registers against so + * the manager never names a concrete consumer type. */ + void RegisterStateConsumer(IMjStateConsumer* Consumer, class UObject* OwnerObj); + void UnregisterStateConsumer(IMjStateConsumer* Consumer); + + /** Register an IMjStateProducer the collector cannot discover by walking + * articulations (scene-level actors, user channel components). Marks the + * producer cache dirty so scope is re-resolved. Game thread. */ + void RegisterStateProducer(TScriptInterface Producer); + void UnregisterStateProducer(TScriptInterface Producer); + + /** Copy the registered state producers out under the registry lock. Called by + * the collector's game-thread cache rebuild. */ + void GetStateProducers(TArray>& Out) const; + + /** Route an inbound user-channel value to the declaring component. ArtOrNone is + * the canonical art segment for art scope, or None/empty for scene scope. The + * transport (the set_user_channels RPC op, or a ROS subscription) builds the + * value; the component validates it against the declared kind and stores it. + * Returns true when a declaring component accepted the write. Thread-safe; + * callable from any transport thread. This is the input mirror of the state + * consumer seam. */ + bool ApplyUserChannelInput(FName ArtOrNone, FName Channel, const FMjUserChannel& Value); + + /** Enumerate every declared user-input channel across registered components, + * with its scope. Used by ROS to create one subscription per input channel and + * rebuild the set on a StructureVersion change. Thread-safe. */ + void GetUserInputChannels(TArray& Out) const; + + /** Build the per-step IR, encode the canonical `state_full` msgpack, and fan + * the bytes to every registered snapshot publisher. Bound to the physics + * post-step callback; gated by bPublishersPaused (byte fan-out only). */ + void FanOutStateSnapshot(struct mjModel_* m, struct mjData_* d); + /** Bound to Tab key. */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|UI") void ToggleSimulateWidget(); @@ -219,6 +290,12 @@ class URLAB_API AAMjManager : public AActor TArray EntityCache; + /** Builds the per-step state IR consumed by the msgpack encoder. */ + FMjStateCollector StateCollector; + + /** Per-art URDF documents, keyed by canonical art segment. */ + TMap RobotDescriptions; + public: /** Manager-owned UObject publish transports * (UURLabShmPublishTransport + UURLabZmqPublishTransport). @@ -247,12 +324,49 @@ class URLAB_API AAMjManager : public AActor TArray SnapshotPublishers; mutable FCriticalSection SnapshotPublishersMutex; + struct FRegisteredStateConsumer + { + TWeakObjectPtr Owner; + IMjStateConsumer* Consumer = nullptr; + }; + /** Typed state consumers registered by their owning transports. Read on the + * physics async thread (fan-out), mutated on the game thread; guarded by + * StateConsumersMutex. */ + TArray StateConsumers; + mutable FCriticalSection StateConsumersMutex; + + /** IMjStateProducers registered by owners the collector cannot walk to. + * Read on the game thread (collector rebuild), mutated on the game thread + * (BeginPlay / EndPlay); guarded by StateProducersMutex for safety. */ + TArray> StateProducers; + mutable FCriticalSection StateProducersMutex; + virtual void BeginPlay() override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; public: virtual void Tick(float DeltaTime) override; + /** Pull the latest physics render snapshot and push it onto the UE + * actor/component transforms. Normally driven once per frame from Tick. + * Records the applied snapshot's FrameId / SimTime so cameras can tag + * their readbacks with the post-step state they show. Game thread only. */ + void ApplyLatestRenderState(); + + /** Render-snapshot id last applied to the actors (post-step state id that + * the currently-rendered scene reflects). Cameras stamp readbacks with + * this; the bridge associates an image with a step by frame_id. */ + uint64 GetLastAppliedFrameId() const + { + return LastAppliedRenderFrameId.load(std::memory_order_acquire); + } + + /** MuJoCo sim time of the snapshot last applied to the actors. */ + double GetLastAppliedSimTime() const + { + return LastAppliedRenderSimTime.load(std::memory_order_acquire); + } + UPROPERTY(EditAnywhere, BlueprintReadOnly, Category = "Mujoco Physics|Objects") TArray m_MujocoComponents; diff --git a/Source/URLab/Public/MuJoCo/Core/MjPhysicsEngine.h b/Source/URLab/Public/MuJoCo/Core/MjPhysicsEngine.h index 9120f5ee..261de4e2 100644 --- a/Source/URLab/Public/MuJoCo/Core/MjPhysicsEngine.h +++ b/Source/URLab/Public/MuJoCo/Core/MjPhysicsEngine.h @@ -84,6 +84,9 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent public: UMjPhysicsEngine(); virtual void BeginDestroy() override; +#if WITH_EDITOR + virtual void PostEditChangeProperty(FPropertyChangedEvent& PropertyChangedEvent) override; +#endif // --- MuJoCo Core Pointers --- @@ -102,6 +105,13 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent std::atomic bShouldStopTask{false}; TFuture AsyncPhysicsFuture; + /** True for exactly the lifetime of the async worker lambda. The direct-mode + * step body reads this to decide whether to submit to the worker or run the + * handler inline: unlike AsyncPhysicsFuture.IsValid() it is cleared the + * instant the worker returns, so a joined-but-not-yet-reset future can't be + * mistaken for a live worker. */ + std::atomic bWorkerRunning{false}; + /** Wakes the async physics worker when a step request lands in * direct/puppet mode. Dispatcher Triggers on enqueue; worker * Waits on this in lieu of the real-time spin pacer when the @@ -109,10 +119,38 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent * returned to the pool on shutdown. */ FEvent* StepRequestEvent = nullptr; + // --- Worker shadow state (lock-free reads on the physics thread) --- + // + // The physics worker must not read UPROPERTYs (torn cross-thread) or + // reach into the owning actor for the step mode. These mirror the + // authoritative values: SetPaused / SetSimSpeed / SetStepMode + // (plus PostEditChangeProperty for details-panel edits) keep them in + // sync, and RunMujocoAsync seeds them when the worker starts. + std::atomic bPausedAtomic{true}; + std::atomic SimSpeedAtomic{100.0f}; + std::atomic ResolvedStepMode{EStepMode::Live}; + + /** Set by the game thread each time it consumes the render snapshot; the + * live-mode worker publishes a new snapshot only when it is set, so the + * full-state copy runs at the consumer's frame rate rather than the + * physics rate. Direct/puppet publish every step (frame association). */ + std::atomic bSnapshotWanted{true}; + + /** Worker-thread only. Reset at the top of each worker iteration; a step + * handler that publishes the render snapshot itself (direct mode captures + * its exact frame id for the reply) sets this so the loop tail does not + * publish again and bump the id past what the step reported. */ + bool bRenderStatePublishedThisStep = false; + // --- Step Callbacks --- - /** If bound, replaces mj_step (used by replay). */ - using FMujocoStepCallback = std::function; + /** If bound, replaces mj_step (direct/puppet stepping and replay). + * Returns true iff it advanced sim state this call (dequeued work and + * stepped); false on an idle wake with nothing to do, so the worker + * loop can skip the render-snapshot publish instead of inflating + * FrameId on unchanged state. A handler that steps owns its own + * OnPostStep notification (per sub-step for direct mode). */ + using FMujocoStepCallback = std::function; FMujocoStepCallback CustomStepHandler; /** Called after mj_step (or custom step); used for recording. */ @@ -206,6 +244,25 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent void RunMujocoAsync(); void SetPaused(bool bPaused); + + /** Set the real-time speed target (percent). Writes the UPROPERTY (for + * the details panel) and the worker's lock-free shadow. */ + void SetSimSpeed(float Percent); + + /** Single entry point for the runtime step mode. Stores the resolved mode + * the worker honours (pacing + whether it runs the UE controller pass; + * Auto resolves to Live) and unpauses the worker for client-driven modes + * (direct/puppet) so the async loop calls the step handler and drains the + * request queue. The RPC dispatcher is the runtime owner; call on every + * mode change. */ + void SetStepMode(EStepMode Mode); + + /** The resolved step mode the physics worker is currently pacing off + * (Auto already collapsed to Live). This is the authoritative value the + * loop reads, so it is what regression coverage for the live 10 Hz lock + * should assert. */ + EStepMode GetStepMode() const { return ResolvedStepMode.load(std::memory_order_acquire); } + bool IsRunning() const; bool IsInitialized() const; float GetSimTime() const; @@ -219,7 +276,21 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent void SetControlSource(EControlSource NewSource); EControlSource GetControlSource() const; AMjArticulation* GetArticulation(const FString& ActorName) const; - TArray GetAllArticulations() const; + /** The live articulation registry. Registration happens in bulk at compile + * time (PreCompile) while the worker thread is stopped and joined, so the + * array is immutable for the duration of a play session. The returned + * reference is therefore stable to read on the game thread, but it is NOT a + * synchronised snapshot: it must not be retained across a recompile, and + * callers on other threads that need a stable copy must take one themselves. + * The physics worker iterates the underlying array directly, not through + * this accessor. */ + const TArray& GetAllArticulations() const; + + /** Register an articulation into the registry the physics worker iterates + * (ApplyControls). Takes CallbackMutex so bulk registration can't tear the + * array or the name map out from under a step; in practice registration + * runs at compile time with the worker joined. */ + void RegisterArticulation(AMjArticulation* Articulation); TArray GetAllQuickComponents() const; TArray GetAllHeightfields() const; FString GetLastCompileError() const; @@ -256,6 +327,11 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent */ void WithRenderState(TFunctionRef Visitor); + /** Current render-snapshot frame id (monotonic, bumped each PushRenderState + * i.e. each step's post-step state). Returned in step replies so a client + * can fetch the matching camera frame by id. Thread-safe. */ + uint64 GetRenderFrameId(); + // --- Command channel (UE -> MuJoCo) -------------------------------- // // Game-thread writers (mocap, wrench, sleep) enqueue under @@ -331,6 +407,8 @@ class URLAB_API UMjPhysicsEngine : public UActorComponent FCommandQueue PendingCommands; /** Drains PendingCommands into m_data. Must be called by the - * stepping thread while it already holds CallbackMutex. */ - void DrainCommands(); + * stepping thread while it already holds CallbackMutex. Returns true + * iff it applied at least one command, so the caller can treat a mocap + * / wrench edit as a state advance (publish it) even while paused. */ + bool DrainCommands(); }; From 2a2fc2ec9f94a9db50f24b735254f8c38e917352 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:38 +0100 Subject: [PATCH 02/32] Decouple camera capture from the step loop Capture became the bottleneck: a synchronous readback stalled the stream. It is now an async pipelined GPU readback with a one-deep queue and drain-to-latest, a frame history addressable by frame id, and a render-on-demand fast path for callers that need the frame the step produced. Camera identity is canonical and filename-safe, and fovy converts from MuJoCo's vertical to Unreal's horizontal. --- .../Components/Sensors/CameraShmWriter.cpp | 15 +- .../MuJoCo/Components/Sensors/MjCamera.cpp | 1145 +++++++++++++---- .../Components/Sensors/MjCameraFrameBus.cpp | 29 + .../Components/Sensors/MjCameraSubsystem.cpp | 80 ++ .../MuJoCo/Components/Sensors/MjSensor.cpp | 370 +----- Source/URLab/Private/UI/MjCameraFeedEntry.cpp | 18 +- .../Components/Sensors/CameraShmWriter.h | 25 +- .../MuJoCo/Components/Sensors/MjCamera.h | 492 +++++-- .../Components/Sensors/MjCameraFrameBus.h | 80 ++ .../Components/Sensors/MjCameraSubsystem.h | 41 + .../MuJoCo/Components/Sensors/MjCameraTypes.h | 49 + .../MuJoCo/Components/Sensors/MjSensor.h | 3 +- 12 files changed, 1694 insertions(+), 653 deletions(-) create mode 100644 Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraFrameBus.cpp create mode 100644 Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraSubsystem.cpp create mode 100644 Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraFrameBus.h create mode 100644 Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraSubsystem.h diff --git a/Source/URLab/Private/MuJoCo/Components/Sensors/CameraShmWriter.cpp b/Source/URLab/Private/MuJoCo/Components/Sensors/CameraShmWriter.cpp index d5a78270..a60ed253 100644 --- a/Source/URLab/Private/MuJoCo/Components/Sensors/CameraShmWriter.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Sensors/CameraShmWriter.cpp @@ -26,8 +26,9 @@ bool FCameraShmWriter::Open(const FString& Path, FIntPoint Resolution) return false; } - // 4 bytes per pixel (FColor) + 4-byte size prefix per slot. - const uint32 Stride = static_cast(ExpectedPixels) * 4u + sizeof(uint32); + // 4 bytes per pixel (FColor) + 4-byte size prefix + frame meta per slot. + const uint32 Stride = static_cast(ExpectedPixels) * 4u + + sizeof(uint32) + sizeof(FMjCameraFrameMeta); return Region.Open(Path, Stride, /*NBuffers=*/2); } @@ -37,7 +38,7 @@ void FCameraShmWriter::Close(bool bDeleteFile) ExpectedPixels = 0; } -void FCameraShmWriter::PushFrame(const void* Data, uint32 ByteCount) +void FCameraShmWriter::PushFrame(const void* Data, uint32 ByteCount, const FMjCameraFrameMeta& Meta) { if (!Region.IsOpen() || !Data) return; @@ -55,8 +56,12 @@ void FCameraShmWriter::PushFrame(const void* Data, uint32 ByteCount) if (!Slot) return; - FMemory::Memcpy(Slot, &ByteCount, sizeof(uint32)); - FMemory::Memcpy(Slot + sizeof(uint32), Data, ByteCount); + // Slot layout: [u32 payload_size][FMjCameraFrameMeta][pixels]. payload_size + // covers meta + pixels so the consumer reads one length then splits. + const uint32 PayloadSize = sizeof(FMjCameraFrameMeta) + ByteCount; + FMemory::Memcpy(Slot, &PayloadSize, sizeof(uint32)); + FMemory::Memcpy(Slot + sizeof(uint32), &Meta, sizeof(FMjCameraFrameMeta)); + FMemory::Memcpy(Slot + sizeof(uint32) + sizeof(FMjCameraFrameMeta), Data, ByteCount); Hdr->LatestIdx.store(Target, std::memory_order_release); Hdr->Sequence.fetch_add(1, std::memory_order_release); diff --git a/Source/URLab/Private/MuJoCo/Components/Sensors/MjCamera.cpp b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCamera.cpp index 6bb96633..ffaa4b51 100644 --- a/Source/URLab/Private/MuJoCo/Components/Sensors/MjCamera.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCamera.cpp @@ -21,9 +21,13 @@ // CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. #include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Sensors/MjCameraSubsystem.h" +#include "MuJoCo/Components/Sensors/MjCameraFrameBus.h" #include "MuJoCo/Components/Sensors/CameraShmWriter.h" #include "MuJoCo/Core/AMjManager.h" #include "MuJoCo/Core/MjDebugVisualizer.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/BridgeServerConfigUtils.h" #include "Transport/NetworkManager.h" #include "Transport/ShmPublishTransport.h" // ResolveSessionDir #include "Misc/Paths.h" @@ -37,21 +41,81 @@ #include "ContentStreaming.h" #include "XmlNode.h" #include "RHICommandList.h" +#include "RenderingThread.h" +#include "HAL/ThreadSafeBool.h" +#include "HAL/RunnableThread.h" #include "Utils/URLabLogging.h" #include "MuJoCo/Core/MjArticulation.h" #include "MuJoCo/Utils/MjOrientationUtils.h" +#include "State/MjCanonicalName.h" #include "zmq.h" +namespace +{ +// Resolve a camera's canonical / segments through the single naming +// owner. The art segment is the owning articulation's name, or the owning +// actor's name for a manager-level global camera; the part is the MJCF camera +// name (or UE component name if unset) with the art prefix stripped. +void ResolveCameraCanonical(const UMjCamera& Cam, FName& OutArt, FName& OutPart) +{ + const AActor* Owner = Cam.GetOwner(); + const AMjArticulation* Art = Cast(Owner); + OutArt = Art ? FMjCanonicalName::ArtSegment(Art) + : FName(*FMjCanonicalName::Sanitize(Owner ? Owner->GetName() : TEXT("unknown"))); + FString Source = Cam.GetMjName(); + if (Source.IsEmpty()) + Source = Cam.GetName(); + OutPart = FMjCanonicalName::PartSegment(Art, Source); +} +} // namespace + // --------------------------------------------------------------------------- // FCameraZmqWorker // --------------------------------------------------------------------------- std::atomic FCameraZmqWorker::bPublishersPaused{false}; +// All transport internals live here so the header carries no raw libzmq handles. +struct FCameraZmqWorker::FState +{ + FString RequestedEndpoint; + FString BoundEndpoint; + FString Topic; + FIntPoint Resolution = FIntPoint::ZeroValue; + + void* ZmqContext = nullptr; + void* ZmqPublisher = nullptr; + + FThreadSafeBool bStopThread{false}; + + // Each queued frame carries its metadata header so the Run() loop can prepend + // it to the published bytes (the client associates the streamed frame with the + // step that produced it via Meta.FrameId). + struct FQueuedColorFrame + { + FMjCameraFrameMeta Meta; + TArray Pixels; + }; + struct FQueuedFloatFrame + { + FMjCameraFrameMeta Meta; + TArray Pixels; + }; + + // Two queues, one per pixel format. Real / seg cameras drive the FColor queue, + // depth cameras drive the float queue. Per-camera CaptureMode never changes + // after streaming starts, so only one queue is ever active per worker. + TQueue FrameQueue; + TQueue FloatFrameQueue; +}; + FCameraZmqWorker::FCameraZmqWorker(const FString& InEndpoint, const FString& InTopic, FIntPoint InRes) - : RequestedEndpoint(InEndpoint), Topic(InTopic), resolution(InRes), bStopThread(false) { - BoundEndpoint = RequestedEndpoint; + State = MakePimpl(); + State->RequestedEndpoint = InEndpoint; + State->BoundEndpoint = InEndpoint; + State->Topic = InTopic; + State->Resolution = InRes; } FCameraZmqWorker::~FCameraZmqWorker() @@ -61,23 +125,39 @@ FCameraZmqWorker::~FCameraZmqWorker() bool FCameraZmqWorker::Init() { - ZmqContext = zmq_ctx_new(); - ZmqPublisher = zmq_socket(ZmqContext, ZMQ_PUB); - - // Optimize for High Bandwidth (large HWM) - int hwm = 10; - zmq_setsockopt(ZmqPublisher, ZMQ_SNDHWM, &hwm, sizeof(hwm)); + State->ZmqContext = zmq_ctx_new(); + if (!State->ZmqContext) + { + UE_LOG(LogURLabNet, Error, TEXT("CameraZmqWorker: zmq_ctx_new failed")); + return false; + } + State->ZmqPublisher = zmq_socket(State->ZmqContext, ZMQ_PUB); + if (!State->ZmqPublisher) + { + UE_LOG(LogURLabNet, Error, TEXT("CameraZmqWorker: zmq_socket failed")); + zmq_ctx_term(State->ZmqContext); + State->ZmqContext = nullptr; + return false; + } - // Simple port increment logic if port is busy - FString TryEndpoint = RequestedEndpoint; - int32 Port = 5558; + // A live camera feed only cares about the FRESHEST frame, so keep the PUB send + // queue shallow. With HWM=1 the PUB holds at most one frame in flight, so a + // slow consumer always gets a near-latest frame instead of draining a backlog. + int hwm = 1; + zmq_setsockopt(State->ZmqPublisher, ZMQ_SNDHWM, &hwm, sizeof(hwm)); + // LINGER=0 so a connected-but-not-reading subscriber can never block + // zmq_ctx_term at shutdown (libzmq default is infinite). + int linger = 0; + zmq_setsockopt(State->ZmqPublisher, ZMQ_LINGER, &linger, sizeof(linger)); + + // Auto-increment the port on bind conflict so co-located cameras (and + // co-located editor processes) don't fight over a single port. FString BaseAddr = TEXT("tcp://0.0.0.0:"); - - // Extract port from requested if it's not the default format - if (RequestedEndpoint.Contains(TEXT(":"))) + int32 Port = 5558; + if (State->RequestedEndpoint.Contains(TEXT(":"))) { FString Left, Right; - RequestedEndpoint.Split(TEXT(":"), &Left, &Right, ESearchCase::IgnoreCase, ESearchDir::FromEnd); + State->RequestedEndpoint.Split(TEXT(":"), &Left, &Right, ESearchCase::IgnoreCase, ESearchDir::FromEnd); if (Right.IsNumeric()) { Port = FCString::Atoi(*Right); @@ -88,57 +168,92 @@ bool FCameraZmqWorker::Init() int rc = -1; for (int i = 0; i < 10; ++i) { - TryEndpoint = FString::Printf(TEXT("%s%d"), *BaseAddr, Port + i); - rc = zmq_bind(ZmqPublisher, TCHAR_TO_UTF8(*TryEndpoint)); + const FString TryEndpoint = FString::Printf(TEXT("%s%d"), *BaseAddr, Port + i); + rc = zmq_bind(State->ZmqPublisher, TCHAR_TO_UTF8(*TryEndpoint)); if (rc == 0) { - BoundEndpoint = TryEndpoint; + State->BoundEndpoint = TryEndpoint; break; } } if (rc != 0) { - UE_LOG(LogURLabNet, Error, TEXT("CameraZmqWorker Failed to bind ZMQ after 10 retries. Starting at %s"), *RequestedEndpoint); + UE_LOG(LogURLabNet, Error, + TEXT("CameraZmqWorker failed to bind ZMQ after 10 retries, starting at %s"), + *State->RequestedEndpoint); + // Release the half-open socket + context so a failed Init leaks nothing. + zmq_close(State->ZmqPublisher); + State->ZmqPublisher = nullptr; + zmq_ctx_term(State->ZmqContext); + State->ZmqContext = nullptr; return false; } - UE_LOG(LogURLabNet, Log, TEXT("CameraZmqWorker Bound at %s [Topic: %s]"), *BoundEndpoint, *Topic); + UE_LOG(LogURLabNet, Log, TEXT("CameraZmqWorker bound at %s [Topic: %s]"), + *State->BoundEndpoint, *State->Topic); return true; } uint32 FCameraZmqWorker::Run() { - auto SendBinary = [this](const void* Data, size_t Size) { + // Publish one message as [topic][meta + pixels]. The metadata header is + // prepended to the pixel bytes in a single payload frame so the ZMQ and SHM + // consumers parse an identical (meta, pixels) layout. + auto SendFrame = [this](const FMjCameraFrameMeta& Meta, const void* Pixels, size_t PixelBytes) { if (bPublishersPaused.load(std::memory_order_acquire)) return; - const FString TopicSpace = Topic + TEXT(" "); + TArray Payload; + Payload.SetNumUninitialized(sizeof(FMjCameraFrameMeta) + static_cast(PixelBytes)); + FMemory::Memcpy(Payload.GetData(), &Meta, sizeof(FMjCameraFrameMeta)); + FMemory::Memcpy(Payload.GetData() + sizeof(FMjCameraFrameMeta), Pixels, PixelBytes); + + const FString TopicSpace = State->Topic + TEXT(" "); const FTCHARToUTF8 TopicUtf8(*TopicSpace); - zmq_send(ZmqPublisher, TopicUtf8.Get(), TopicUtf8.Length(), ZMQ_SNDMORE); - zmq_send(ZmqPublisher, Data, Size, 0); + if (zmq_send(State->ZmqPublisher, TopicUtf8.Get(), TopicUtf8.Length(), ZMQ_SNDMORE) < 0) + return; // topic frame dropped (e.g. HWM); skip the body so we don't desync the multipart message + if (zmq_send(State->ZmqPublisher, Payload.GetData(), Payload.Num(), 0) < 0) + { + // Best-effort feed: a dropped body under HWM=1 just means this frame is + // skipped; the next push carries a fresher one. + } }; - while (!bStopThread) + const int32 ExpectedPixels = State->Resolution.X * State->Resolution.Y; + while (!State->bStopThread) { - const int32 ExpectedPixels = resolution.X * resolution.Y; bool bSent = false; - TArray ColorFrame; - if (FrameQueue.Dequeue(ColorFrame)) + // Drain to the FRESHEST frame: a live feed only wants the latest, so if the + // producer outran us, skip the backlog rather than send stale frames FIFO. + FState::FQueuedColorFrame ColorFrame; + bool bHaveColor = false; + while (State->FrameQueue.Dequeue(ColorFrame)) { - if (ColorFrame.Num() == ExpectedPixels) + bHaveColor = true; + } + if (bHaveColor) + { + if (ColorFrame.Pixels.Num() == ExpectedPixels) { - SendBinary(ColorFrame.GetData(), ColorFrame.Num() * sizeof(FColor)); + SendFrame(ColorFrame.Meta, ColorFrame.Pixels.GetData(), + ColorFrame.Pixels.Num() * sizeof(FColor)); } bSent = true; } - TArray FloatFrame; - if (FloatFrameQueue.Dequeue(FloatFrame)) + FState::FQueuedFloatFrame FloatFrame; + bool bHaveFloat = false; + while (State->FloatFrameQueue.Dequeue(FloatFrame)) { - if (FloatFrame.Num() == ExpectedPixels) + bHaveFloat = true; + } + if (bHaveFloat) + { + if (FloatFrame.Pixels.Num() == ExpectedPixels) { - SendBinary(FloatFrame.GetData(), FloatFrame.Num() * sizeof(float)); + SendFrame(FloatFrame.Meta, FloatFrame.Pixels.GetData(), + FloatFrame.Pixels.Num() * sizeof(float)); } bSent = true; } @@ -153,34 +268,44 @@ uint32 FCameraZmqWorker::Run() void FCameraZmqWorker::Stop() { - bStopThread = true; + if (State) + { + State->bStopThread = true; + } } void FCameraZmqWorker::Exit() { - if (ZmqPublisher) + if (!State) + return; + if (State->ZmqPublisher) { - zmq_close(ZmqPublisher); - ZmqPublisher = nullptr; + zmq_close(State->ZmqPublisher); + State->ZmqPublisher = nullptr; } - if (ZmqContext) + if (State->ZmqContext) { - zmq_ctx_term(ZmqContext); - ZmqContext = nullptr; + zmq_ctx_term(State->ZmqContext); + State->ZmqContext = nullptr; } } -void FCameraZmqWorker::PushFrame(const TArray& FrameData) +void FCameraZmqWorker::PushFrame(const TArray& FrameData, const FMjCameraFrameMeta& Meta) { - // ZMQ HWM on the socket handles network-side backpressure if the - // queue grows. TQueue lacks a cheap Count(), so don't try to drop - // here. - FrameQueue.Enqueue(FrameData); + // Enqueue unconditionally; the worker drains this queue to the latest frame + // before sending (and the PUB socket runs HWM=1), so a backlog here is + // collapsed to the freshest frame rather than streamed FIFO. + State->FrameQueue.Enqueue(FState::FQueuedColorFrame{Meta, FrameData}); } -void FCameraZmqWorker::PushFrame(const TArray& FrameData) +void FCameraZmqWorker::PushFrame(const TArray& FrameData, const FMjCameraFrameMeta& Meta) { - FloatFrameQueue.Enqueue(FrameData); + State->FloatFrameQueue.Enqueue(FState::FQueuedFloatFrame{Meta, FrameData}); +} + +FString FCameraZmqWorker::GetBoundEndpoint() const +{ + return State ? State->BoundEndpoint : FString(); } // --------------------------------------------------------------------------- @@ -195,6 +320,28 @@ bool IsSegMode(EMjCameraMode mode) || mode == EMjCameraMode::InstanceSegmentation; } +// MuJoCo `fovy` is the VERTICAL field of view; UE SceneCaptureComponent2D +// `FOVAngle` is the HORIZONTAL FOV. Copying fovy->FOVAngle verbatim over-narrows +// the view on non-square render targets (looks "zoomed in"). Convert via the RT +// aspect so UE's derived vertical FOV matches MuJoCo fovy exactly. Identity when +// the RT is square. +float HorizontalFOVFromFovy(float Fovy, FIntPoint Resolution) +{ + // A camera with no fovy set (0) produces a degenerate zero-FOV frustum that + // renders pure black. Fall back to MuJoCo's default vertical fov (45 deg). + if (Fovy <= 0.0f) + { + Fovy = 45.0f; + } + if (Resolution.X <= 0 || Resolution.Y <= 0) + { + return Fovy; + } + const float Aspect = static_cast(Resolution.X) / static_cast(Resolution.Y); + const float FovyRad = FMath::DegreesToRadians(Fovy); + return FMath::RadiansToDegrees(2.0f * FMath::Atan(FMath::Tan(FovyRad * 0.5f) * Aspect)); +} + UMjDebugVisualizer* FindDebugVisualizer(UWorld* FallbackWorld = nullptr) { if (AAMjManager* Manager = AAMjManager::GetManager()) @@ -218,12 +365,16 @@ UMjDebugVisualizer* FindDebugVisualizer(UWorld* FallbackWorld = nullptr) UMjCamera::UMjCamera() { - PrimaryComponentTick.bCanEverTick = true; - PrimaryComponentTick.bStartWithTickEnabled = true; + // The capture pipeline is driven once per frame by UMjCameraSubsystem, not a + // per-component tick. + PrimaryComponentTick.bCanEverTick = false; + PrimaryComponentTick.bStartWithTickEnabled = false; // Default resolution: codegen emits resolution as TArray{} (empty); seed [w,h]. resolution = {640, 480}; + ResultsQueue = MakeShared(); + // Create the scene capture sub-component CaptureComponent = CreateDefaultSubobject(TEXT("SceneCapture")); if (CaptureComponent) @@ -234,21 +385,21 @@ UMjCamera::UMjCamera() // MuJoCo cameras look down -Z (forward), +Y (up). // Unreal cameras look down +X (forward), +Z (up). // MjToUERotation negates X/Z quat components (handedness flip), - // which mirrors the Y axis — so "up" becomes -Y after conversion. + // which mirrors the Y axis, so "up" becomes -Y after conversion. const FVector MjForward = FVector(0.0f, 0.0f, -1.0f); const FVector MjUp = FVector(0.0f, -1.0f, 0.0f); const FRotator CorrectionRot = FRotationMatrix::MakeFromXZ(MjForward, MjUp).Rotator(); CaptureComponent->SetRelativeRotation(CorrectionRot); - // Start dormant — no capture cost until explicitly enabled + // Start dormant: no capture cost until explicitly enabled. CaptureComponent->bCaptureEveryFrame = false; CaptureComponent->bCaptureOnMovement = false; CaptureComponent->bAlwaysPersistRenderingState = true; CaptureComponent->MaxViewDistanceOverride = -1.0f; - // SceneCaptureComponent2D does NOT automatically respect scene Post Process Volumes. - // We set PostProcessBlendWeight=1 so the component's own PostProcessSettings are used. - // At BeginPlay, we copy settings from the scene's PPV to match the viewport look. + // SceneCaptureComponent2D does NOT automatically respect scene Post Process + // Volumes. Set PostProcessBlendWeight=1 so the component's own settings are + // used; at BeginPlay we copy from the scene PPV to match the viewport look. CaptureComponent->PostProcessBlendWeight = 1.0f; CaptureComponent->bUseRayTracingIfEnabled = false; @@ -261,7 +412,7 @@ void UMjCamera::OnRegister() Super::OnRegister(); if (CaptureComponent) { - CaptureComponent->FOVAngle = fovy; + CaptureComponent->FOVAngle = HorizontalFOVFromFovy(fovy, GetResolution()); } } @@ -275,8 +426,8 @@ void UMjCamera::BeginPlay() Manager->NetworkManager->RegisterCamera(this); } - // Copy post-process settings from the scene's Post Process Volume(s) - // so the capture component matches the viewport look. + // Copy post-process settings from the scene's Post Process Volume(s) so the + // capture component matches the viewport look. if (CaptureComponent && GetWorld()) { for (TActorIterator It(GetWorld()); It; ++It) @@ -296,10 +447,27 @@ void UMjCamera::BeginPlay() { SetStreamingEnabled(true); } + + // Register with the world subsystem that drives the per-frame capture pass. + if (UWorld* World = GetWorld()) + { + if (UMjCameraSubsystem* Sub = World->GetSubsystem()) + { + Sub->RegisterCamera(this); + } + } } void UMjCamera::EndPlay(const EEndPlayReason::Type EndPlayReason) { + if (UWorld* World = GetWorld()) + { + if (UMjCameraSubsystem* Sub = World->GetSubsystem()) + { + Sub->UnregisterCamera(this); + } + } + if (AAMjManager* Manager = AAMjManager::GetManager()) { if (Manager->NetworkManager) @@ -318,7 +486,7 @@ void UMjCamera::EndPlay(const EEndPlayReason::Type EndPlayReason) ZmqWorker = nullptr; } - // Make sure we stop rendering when the actor is torn down + // Make sure we stop rendering when the actor is torn down. if (bStreamingEnabled) { SetStreamingEnabled(false); @@ -326,71 +494,322 @@ void UMjCamera::EndPlay(const EEndPlayReason::Type EndPlayReason) Super::EndPlay(EndPlayReason); } -void UMjCamera::TickComponent(float DeltaTime, ELevelTick TickType, - FActorComponentTickFunction* ThisTickFunction) +FIntPoint UMjCamera::GetResolution() const +{ + const int32 W = (resolution.Num() > 0 && resolution[0] > 0) ? resolution[0] : 640; + const int32 H = (resolution.Num() > 1 && resolution[1] > 0) ? resolution[1] : 480; + return FIntPoint(W, H); +} + +void UMjCamera::NormalizeResolution() +{ + const FIntPoint R = GetResolution(); + resolution.SetNum(2); + resolution[0] = R.X; + resolution[1] = R.Y; +} + +void UMjCamera::UpdateCapturePipeline() { - Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + const bool bActive = IsCaptureActive(); + + // Per-camera capture gating: lazily start capturing when a camera becomes + // active (e.g. first include_cameras request on a non-broadcast camera), and + // pause the GPU scene capture when it goes dormant so a scene with many + // cameras only pays for the ones actually consumed. + if (bActive && !bStreamingEnabled) + { + SetStreamingEnabled(true); + } if (bStreamingEnabled && CaptureComponent && CaptureComponent->TextureTarget) { - // Register this viewpoint with the streaming manager every tick - // (IStreamingManager uses timeout-based decay). - RegisterWithStreamingManager(); + // Normal cameras are captured manually in MaybeCapture right before the + // readback copy, so the automatic per-frame capture stays OFF for them (it + // would render on the main cadence and race the copy, yielding stale or + // black frames). Only main-renderer cameras, which must render on the main + // cadence, keep it on while active. + const bool bWantEveryFrame = bActive && bRenderInMainRenderer; + if (CaptureComponent->bCaptureEveryFrame != bWantEveryFrame) + { + CaptureComponent->bCaptureEveryFrame = bWantEveryFrame; + } + if (bActive) + { + // Register this viewpoint with the streaming manager every tick + // (IStreamingManager uses timeout-based decay). + RegisterWithStreamingManager(); + + // Non-seg cameras re-sync HiddenComponents each tick so siblings spawned + // by a late-starting seg camera don't leak into this capture. + if (!IsSegMode(CaptureMode)) + { + RefreshHiddenComponentsFromSegPools(); + } + } + } + + HarvestCompletedReadbacks(); + + // Drive capture + readback and any delayed publish while active. + if (bStreamingEnabled && bActive) + { + AAMjManager* Mgr = AAMjManager::GetManager(); + MaybeCapture(Mgr); + PublishDueDelayedFrames(Mgr); + } +} - // Non-seg cameras re-sync their HiddenComponents each tick so siblings - // spawned by a late-starting seg camera don't leak into this capture. - if (!IsSegMode(CaptureMode)) +void UMjCamera::HarvestCompletedReadbacks() +{ + // Two async stages, both driven from the game thread with no render-thread + // flush: promote GPU-ready readbacks to a render-thread map/copy, then drain + // whatever the render thread has finished into the history ring. + DispatchReadyReadbacks(); + DrainCompletedFrames(); +} + +void UMjCamera::DispatchReadyReadbacks() +{ + // FIFO: a readback completes in submission order, so stop at the first entry + // whose GPU copy isn't ready yet rather than reordering later frames ahead. + while (InFlightReadbacks.Num() > 0) + { + FInFlightReadback& Head = InFlightReadbacks[0]; + if (!Head.Gpu.IsValid()) { - RefreshHiddenComponentsFromSegPools(); + InFlightReadbacks.RemoveAt(0); + continue; + } + if (!Head.Gpu->IsReady()) + { + break; } - // bCaptureEveryFrame (set by SetStreamingEnabled) drives the per-tick - // capture. Calling CaptureScene() again here is redundant — UE warns - // "Scene capture with bCaptureEveryFrame enabled was told to update — - // major inefficiency", and under sustained render pressure the doubled - // command submission can leave RHI frame breadcrumbs unbalanced. + FInFlightReadback Front = MoveTemp(InFlightReadbacks[0]); + InFlightReadbacks.RemoveAt(0); + + TSharedPtr Frame = MakeShared(); + Frame->FrameId = Front.FrameId; + Frame->SimTime = Front.SimTime; + Frame->Width = Front.Width; + Frame->Height = Front.Height; + Frame->CaptureUnixTime = Front.CaptureUnixSeconds; + + const int32 W = Front.Width; + const int32 H = Front.Height; + const EMjCameraMode Mode = CaptureMode; + TSharedPtr Gpu = Front.Gpu; + + ++PendingMapCommands; + + // Map + copy the staging buffer on the render thread. FRHIGPUTextureReadback::Lock + // asserts IsInRenderingThread; the readback is already Ready(), so this is a + // CPU memcpy from mapped staging memory with no GPU wait. The command captures + // the shared results queue (not `this`), so a teardown race that destroys the + // component before the command runs pushes into a still-live queue rather than + // a freed object. RowPitch is in PIXELS and >= width (rows are padded), so we + // copy row-by-row into a tightly-packed array. + ENQUEUE_RENDER_COMMAND(MjCameraMapReadback) + ([Frame, Gpu, W, H, Mode, Results = ResultsQueue](FRHICommandListImmediate&) { + bool bCopied = false; + int32 RowPitchPixels = 0; + void* Data = Gpu->Lock(RowPitchPixels); + if (Data && W > 0 && H > 0) + { + if (Mode == EMjCameraMode::Depth) + { + Frame->Depth.SetNumUninitialized(W * H); + const float* Src = static_cast(Data); + for (int32 y = 0; y < H; ++y) + FMemory::Memcpy(&Frame->Depth[y * W], &Src[y * RowPitchPixels], W * sizeof(float)); + } + else + { + Frame->Color.SetNumUninitialized(W * H); + const FColor* Src = static_cast(Data); + for (int32 y = 0; y < H; ++y) + FMemory::Memcpy(&Frame->Color[y * W], &Src[y * RowPitchPixels], W * sizeof(FColor)); + } + bCopied = true; + } + Gpu->Unlock(); + Results->Enqueue(FCompletedReadback{Frame, Gpu, bCopied}); + }); + } +} + +void UMjCamera::DrainCompletedFrames() +{ + // Single-consumer drain. The guard makes the invariant explicit: even if a + // render flush elsewhere ever pumped the game thread mid-drain, the nested + // call bails instead of racing the SPSC queue's read cursor. + if (bDrainingResults) + { + return; } + TGuardValue DrainGuard(bDrainingResults, true); - // Check if an in-flight readback has completed. After the fence, - // copy the buffer to the always-on workers, then move it into - // Ready* for the bridge consumer. Pending* is left empty so the - // next RequestReadback can Emplace fresh without disturbing data - // the bridge is about to MoveTemp. - if (bReadbackPending && ReadbackFence.IsFenceComplete()) + FCompletedReadback Completed; + while (ResultsQueue->Dequeue(Completed)) { - bReadbackPending = false; - if (PendingPixels.IsSet()) + --PendingMapCommands; + + if (Completed.bCopied && Completed.Frame.IsValid()) { - if (bEnableZmqBroadcast && ZmqWorker) - ZmqWorker->PushFrame(PendingPixels.GetValue()); - if (bEnableShmBroadcast && ShmWriter) - ShmWriter->PushFrame(PendingPixels.GetValue()); + FMjCameraFrame& Frame = *Completed.Frame; + Frame.Seq = ++HarvestSeq; + // Reveal time = capture clock + sampled latency. Stamped here so the + // delayed publish can select by it; unused when no delay is configured. + Frame.RevealValue = FrameClock(Frame) + SampleDelaySeconds(); + + // No-delay path publishes the instant the frame is harvested. With + // latency emulation on, harvested frames go to History only and are + // published later by the reveal-time selection, so the stream lags by + // the configured delay. + if (!IsDelayActive()) { - FScopeLock Lock(&FrameLock); - ReadyPixels.Emplace(MoveTemp(PendingPixels.GetValue())); + PublishFrameToWorkers(Frame); } - PendingPixels.Reset(); + PushFrameToHistoryShared(Completed.Frame); + } + else + { + // The map failed (RT torn down mid-flight, or a zero-size target). Drop + // the frame but still recycle the readback below; warn sparingly. + UE_LOG(LogURLabNet, Verbose, + TEXT("[MjCamera] '%s' dropped a readback whose staging map failed"), *MjName); + } + + if (Completed.Gpu.IsValid()) + { + FreeReadbacks.Add(Completed.Gpu); // recycle (staging texture reused in place) + } + } +} + +void UMjCamera::WaitAndHarvestReadbacks(double TimeoutSeconds) +{ + // Poll the pipeline instead of flushing: the render thread runs the map/copy + // commands independently while we yield, and pushes finished frames onto the + // results queue for us to drain. Bounded by the deadline. + const double Deadline = FPlatformTime::Seconds() + TimeoutSeconds; + for (;;) + { + HarvestCompletedReadbacks(); + if (InFlightReadbacks.Num() == 0 && PendingMapCommands == 0) + { + break; + } + if (FPlatformTime::Seconds() >= Deadline) + { + break; + } + FPlatformProcess::SleepNoStats(0.0005f); + } +} + +void UMjCamera::IssueSyncCapture() +{ + // Force a render + readback for the current applied state. TouchRequested keeps + // per-camera gating live so the per-frame capture pass (MaybeCapture) keeps + // producing afterwards; EnqueueReadback no-ops while the RT is still cold, and + // MaybeCapture retries until a real frame lands. Force the RHI submit so this + // on-demand copy's fence signals promptly (lower render:sync latency). + TouchRequested(); + if (CaptureComponent && CaptureComponent->TextureTarget) + { + CaptureComponent->CaptureScene(); + } + uint64 ShowId = 0; + double ShowTime = 0.0; + if (AAMjManager* Mgr = AAMjManager::GetManager()) + { + ShowId = Mgr->GetLastAppliedFrameId(); + ShowTime = Mgr->GetLastAppliedSimTime(); + } + EnqueueReadback(ShowId, ShowTime, /*bForceSubmit=*/true); +} + +void UMjCamera::MaybeCapture(AAMjManager* Mgr) +{ + // Resource savers (see header): + // - bCaptureOnStateChange: only render + read back when the applied physics + // state advanced (the rendered scene actually changed). No-op in live. + // - CaptureMaxFps: an optional wall-clock cap on top. + const uint64 AppliedId = Mgr ? Mgr->GetLastAppliedFrameId() : 0; + const double AppliedTime = Mgr ? Mgr->GetLastAppliedSimTime() : 0.0; + const double NowWall = FPlatformTime::Seconds(); + const bool bFpsOk = (CaptureMaxFps <= 0.0f) + || (NowWall - LastCaptureWallSeconds) >= (1.0 / static_cast(CaptureMaxFps)); + // The state-change gate is only "consumed" once a readback actually goes out + // (see bIssued below), so a freshly-enabled (cold) camera whose RT has no RHI + // texture yet keeps this true and retries across ticks until a real frame can + // be captured, instead of latching a single missed capture. + const bool bStateAdvanced = !bCaptureOnStateChange || (AppliedId != LastCapturedFrameId); + + if (bFpsOk && bStateAdvanced) + { + bool bIssued = false; + if (bRenderInMainRenderer) + { + // This camera renders as a nested pass of the main renderer on the main + // cadence (bCaptureEveryFrame stays on), so the RT is updated during the + // main render AFTER this readback copy is submitted. Stamp the id whose + // render the RT currently holds so pixels and frame_id agree. + bIssued = EnqueueReadback(LastRenderedAppliedId, LastRenderedAppliedTime); } - if (PendingFloatPixels.IsSet()) + else { - if (bEnableZmqBroadcast && ZmqWorker) - ZmqWorker->PushFrame(PendingFloatPixels.GetValue()); - if (bEnableShmBroadcast && ShmWriter) - ShmWriter->PushFrame(PendingFloatPixels.GetValue()); + // Render the scene into the RT immediately before enqueuing the readback + // copy, so the copy captures a freshly-rendered frame for the current + // applied state (identical to the render:sync path). Relying on the + // component's automatic every-frame capture would enqueue the copy before + // that frame's capture renders, producing a stale or black frame. + if (CaptureComponent) { - FScopeLock Lock(&FrameLock); - ReadyFloatPixels.Emplace(MoveTemp(PendingFloatPixels.GetValue())); + CaptureComponent->CaptureScene(); } - PendingFloatPixels.Reset(); + bIssued = EnqueueReadback(AppliedId, AppliedTime); + } + + // Only consume the state-change gate once a readback actually went out; a + // cold RT that could not enqueue is retried next tick. + if (bIssued) + { + LastCapturedFrameId = AppliedId; + LastCaptureWallSeconds = NowWall; } - bReadbackComplete = true; } - // Always refresh PendingPixels while streaming; include_cameras consumes it - // without enabling ZMQ/SHM broadcast (those flags only gate the workers below). - if (bStreamingEnabled && !bReadbackPending) + // Track the applied state whose render a main-renderer RT will show next tick. + LastRenderedAppliedId = AppliedId; + LastRenderedAppliedTime = AppliedTime; +} + +void UMjCamera::PublishDueDelayedFrames(AAMjManager* Mgr) +{ + // Publish the delayed selection (newest frame whose reveal time <= now), each + // Seq exactly once, so the stream lags by the configured delay. The no-delay + // path already published inline at harvest. + if (!IsDelayActive()) + return; + + // On the sim clock this is deterministic by design: while the sim is paused + // GetLastAppliedSimTime() is frozen, so a frame captured during the pause never + // reaches its reveal time and delayed publishing stalls until stepping resumes. + // That is the intended behaviour for a sim-time delay (frozen time = frozen + // latency); callers wanting reveals to advance in real time while paused set + // bDelayUseWallClock. Any pile-up of unrevealed frames is bounded by the history + // ring's MaxHistoryCapacity ceiling. + const double NowVal = bDelayUseWallClock + ? (FDateTime::UtcNow() - FDateTime(1970, 1, 1)).GetTotalSeconds() + : (Mgr ? Mgr->GetLastAppliedSimTime() : 0.0); + TSharedPtr Selected = SelectDelayedFrameShared(NowVal, LastPublishedSeq); + if (Selected.IsValid()) { - RequestReadback(); + PublishFrameToWorkers(*Selected); + LastPublishedSeq = Selected->Seq; } } @@ -400,18 +819,19 @@ void UMjCamera::TickComponent(float DeltaTime, ELevelTick TickType, void UMjCamera::SetupRenderTarget() { + const FIntPoint Res = GetResolution(); UTextureRenderTarget2D* RT = NewObject(this); const bool bDepthMode = (CaptureMode == EMjCameraMode::Depth); if (bDepthMode) { RT->RenderTargetFormat = ETextureRenderTargetFormat::RTF_R32f; - RT->InitCustomFormat(resolution[0], resolution[1], PF_R32_FLOAT, /*bForceLinearGamma=*/true); + RT->InitCustomFormat(Res.X, Res.Y, PF_R32_FLOAT, /*bForceLinearGamma=*/true); } else { RT->RenderTargetFormat = ETextureRenderTargetFormat::RTF_RGBA8; - RT->InitCustomFormat(resolution[0], resolution[1], PF_B8G8R8A8, /*bForceLinearGamma=*/true); + RT->InitCustomFormat(Res.X, Res.Y, PF_B8G8R8A8, /*bForceLinearGamma=*/true); } RT->bGPUSharedFlag = true; @@ -422,10 +842,11 @@ void UMjCamera::SetupRenderTarget() CaptureComponent->TextureTarget = RT; CaptureComponent->bAlwaysPersistRenderingState = true; + CaptureComponent->bRenderInMainRenderer = bRenderInMainRenderer; CaptureComponent->MaxViewDistanceOverride = -1.0f; - // 1mm near clip on every capture mode — without this, robot-internal - // geometry can intrude on the frustum and produce black-on-black frames - // when the camera is mounted inside a body shell. + // 1mm near clip on every capture mode: without this, robot-internal geometry + // can intrude on the frustum and produce black-on-black frames when the camera + // is mounted inside a body shell. CaptureComponent->bOverride_CustomNearClippingPlane = true; CaptureComponent->CustomNearClippingPlane = 0.1f; @@ -439,14 +860,18 @@ void UMjCamera::SetupRenderTarget() case EMjCameraMode::InstanceSegmentation: // FinalToneCurveHDR (not SCS_BaseColor): BasicShapeMaterial's `Color` param // isn't wired to the BaseColor G-buffer, so SCS_BaseColor renders empty. - // Tints end up lit (not pure flat masks) until a dedicated unlit material lands. CaptureComponent->CaptureSource = ESceneCaptureSource::SCS_FinalToneCurveHDR; CaptureComponent->PrimitiveRenderMode = ESceneCapturePrimitiveRenderMode::PRM_UseShowOnlyList; break; case EMjCameraMode::Real: default: - CaptureComponent->CaptureSource = ESceneCaptureSource::SCS_FinalToneCurveHDR; + // LDR final color: already tone-mapped and gamma-encoded to the same + // sRGB the viewport shows, so the BGRA8 readback matches the editor + // image (the readback skips TargetGamma, so the HDR-then-encode source + // delivered a darker frame). It also saves one conversion/copy versus + // SCS_FinalToneCurveHDR. Seg/Depth keep their own sources below/above. + CaptureComponent->CaptureSource = ESceneCaptureSource::SCS_FinalColorLDR; CaptureComponent->PrimitiveRenderMode = ESceneCapturePrimitiveRenderMode::PRM_LegacySceneCapture; break; } @@ -456,7 +881,7 @@ void UMjCamera::SetupRenderTarget() TEXT("[MjCamera] '%s' RT created mode=%s (%dx%d)"), *MjName, *UEnum::GetValueAsString(CaptureMode), - resolution[0], resolution[1]); + Res.X, Res.Y); } void UMjCamera::RefreshHiddenComponentsFromSegPools() @@ -485,14 +910,15 @@ void UMjCamera::RegisterWithStreamingManager() { // Register as an active viewpoint so textures stream for the camera's frustum // even when the player pawn is far away. + const FIntPoint Res = GetResolution(); const float HFov = CaptureComponent ? CaptureComponent->FOVAngle : fovy; const float Distance = (HFov > 0.0f) - ? resolution[0] / FMath::Tan(FMath::DegreesToRadians(HFov * 0.5f)) + ? Res.X / FMath::Tan(FMath::DegreesToRadians(HFov * 0.5f)) : 1000.0f; IStreamingManager::Get().AddViewInformation( GetComponentLocation(), - resolution[0], + Res.X, Distance, StreamingBoost, /*bOverrideLocation=*/false, @@ -504,6 +930,11 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) { if (bEnable) { + // Every pixel-sizing path downstream reads GetResolution(), but normalise + // the backing array too so an editor-edited malformed value is corrected + // once here rather than defended at each read. + NormalizeResolution(); + if (!RenderTarget) { SetupRenderTarget(); @@ -531,7 +962,7 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) else { UE_LOG(LogURLabImport, Warning, - TEXT("[MjCamera] '%s' seg mode requested but no DebugVisualizer found — seg cam will show nothing."), + TEXT("[MjCamera] '%s' seg mode requested but no DebugVisualizer found; seg cam will show nothing."), *MjName); } } @@ -546,34 +977,40 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) if (bEnableZmqBroadcast && !ZmqWorker) { - AMjArticulation* Articulation = Cast(GetOwner()); - FString Prefix = Articulation ? Articulation->GetName() : (GetOwner() ? GetOwner()->GetName() : TEXT("unknown")); - FString Topic = FString::Printf(TEXT("%s/camera/%s"), *Prefix, *GetName()); + const FString Topic = GetCanonicalName(); - const FIntPoint Res(resolution.Num() > 0 ? resolution[0] : 0, - resolution.Num() > 1 ? resolution[1] : 0); - ZmqWorker = new FCameraZmqWorker(ZmqEndpoint, Topic, Res); + // Bind in this instance's camera port block (CamBasePort + index) so + // multiple editors acting as render servers never fight over one port. + ZmqEndpoint = ResolveStreamEndpoint(); + ZmqWorker = new FCameraZmqWorker(ZmqEndpoint, Topic, GetResolution()); WorkerThread = FRunnableThread::Create(ZmqWorker, TEXT("CameraZmqWorkerThread"), 0, TPri_BelowNormal); + if (!WorkerThread) + { + // Thread creation failed: drop the worker so we don't hold a runnable + // that is never driven (and never publishes). + UE_LOG(LogURLabNet, Error, + TEXT("[MjCamera] '%s' failed to create ZMQ worker thread; disabling ZMQ broadcast"), + *MjName); + delete ZmqWorker; + ZmqWorker = nullptr; + } } - // SHM publisher: opens an mmap'd file under the live URLab session - // dir. Slot stride is `pixels * 4 bytes` -- works for BGRA8 (Real / - // seg modes) and float32 single-channel (Depth) alike. + // SHM publisher: opens an mmap'd file under the live URLab session dir. if (bEnableShmBroadcast && !ShmWriter) { - AMjArticulation* Articulation = Cast(GetOwner()); - const FString Prefix = Articulation ? Articulation->GetName() - : (GetOwner() ? GetOwner()->GetName() : TEXT("unknown")); + // The "live" session segment is process-global on one host: parameterise + // it per instance before running several editors as render servers. const FString Dir = UURLabShmPublishTransport::ResolveSessionDir(TEXT("live")); IFileManager::Get().MakeDirectory(*Dir, /*Tree=*/true); + FName CanonArt, CanonPart; + ResolveCameraCanonical(*this, CanonArt, CanonPart); const FString FileName = FString::Printf( - TEXT("cam_%s_%s.shm"), *Prefix, *GetName()); + TEXT("cam_%s_%s.shm"), *CanonArt.ToString(), *CanonPart.ToString()); const FString FullPath = FPaths::Combine(Dir, FileName); - const FIntPoint ShmRes(resolution.Num() > 0 ? resolution[0] : 0, - resolution.Num() > 1 ? resolution[1] : 0); ShmWriter = new FCameraShmWriter(); - if (!ShmWriter->Open(FullPath, ShmRes)) + if (!ShmWriter->Open(FullPath, GetResolution())) { delete ShmWriter; ShmWriter = nullptr; @@ -585,24 +1022,27 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) *MjName, *FullPath); } } + if (CaptureComponent) { - CaptureComponent->FOVAngle = fovy; + CaptureComponent->FOVAngle = HorizontalFOVFromFovy(fovy, GetResolution()); - // CRITICAL: SetVisibility(true) must be called to allow the component - // to dispatch scene capture updates. bHiddenInGame alone is not sufficient — + // CRITICAL: SetVisibility(true) must be called to allow the component to + // dispatch scene capture updates. bHiddenInGame alone is not sufficient: // the capture system checks IsVisible() each frame. CaptureComponent->SetVisibility(true); CaptureComponent->SetActive(true); CaptureComponent->bHiddenInGame = false; - CaptureComponent->bCaptureEveryFrame = true; - CaptureComponent->bCaptureOnMovement = false; // We drive capture manually each tick + // Normal cameras are driven manually in MaybeCapture right before each + // readback; only main-renderer cameras render on the automatic cadence. + CaptureComponent->bCaptureEveryFrame = bRenderInMainRenderer; + CaptureComponent->bCaptureOnMovement = false; } bStreamingEnabled = true; RegisterWithStreamingManager(); - // force an immediate capture so the UI doesn't wait a full tick. - // CaptureScene() bypasses the visibility check — it always fires. + // Force an immediate capture so the UI doesn't wait a full tick. + // CaptureScene() bypasses the visibility check: it always fires. if (CaptureComponent) { CaptureComponent->CaptureScene(); @@ -635,6 +1075,24 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) CaptureComponent->TextureTarget = nullptr; } + // Teardown is the one place a render-thread flush is correct: the in-flight + // EnqueueCopy commands and any already-dispatched map/copy commands hold + // shared readback objects, so drain the render thread once so nothing runs + // against freed memory, then release. We do NOT dispatch new map commands + // here; whatever finished lands in the results queue and is discarded with + // the rest of the pipeline. + if (InFlightReadbacks.Num() > 0 || PendingMapCommands > 0) + { + FlushRenderingCommands(); + } + InFlightReadbacks.Empty(); + FCompletedReadback Discard; + while (ResultsQueue->Dequeue(Discard)) + { + } + PendingMapCommands = 0; + FreeReadbacks.Empty(); + // Drop the RT so the next enable rebuilds it in the current CaptureMode. RenderTarget = nullptr; @@ -657,125 +1115,368 @@ void UMjCamera::SetStreamingEnabled(bool bEnable) ShmWriter = nullptr; } + // Tell any out-of-core image sink to release per-camera resources. + FMjCameraFrameBus::Get().OnStreamStopped.Broadcast(GetCanonicalName()); + UE_LOG(LogURLabImport, Log, TEXT("[MjCamera] '%s' streaming DISABLED."), *MjName); } } // --------------------------------------------------------------------------- -// On-demand readback +// Async readback // --------------------------------------------------------------------------- void UMjCamera::RequestReadback() { - if (bReadbackPending || !RenderTarget) + if (AAMjManager* Mgr = AAMjManager::GetManager()) { - return; + EnqueueReadback(Mgr->GetLastAppliedFrameId(), Mgr->GetLastAppliedSimTime()); + } + else + { + EnqueueReadback(0, 0.0); + } +} + +bool UMjCamera::EnqueueReadback(uint64 ShowFrameId, double ShowSimTime, bool bForceSubmit) +{ + // Pipeline cap: keep at most MaxInFlightReadbacks async copies outstanding. + if (!RenderTarget || InFlightReadbacks.Num() >= MaxInFlightReadbacks) + { + return false; } FTextureRenderTargetResource* Resource = RenderTarget->GameThread_GetRenderTargetResource(); if (!Resource) { - return; + return false; } - const FIntRect Rect(0, 0, Resource->GetSizeXY().X, Resource->GetSizeXY().Y); - if (Rect.Width() <= 0 || Rect.Height() <= 0) + const FIntPoint Size = Resource->GetSizeXY(); + if (Size.X <= 0 || Size.Y <= 0) { // RT not yet sized (allocation in flight on the render thread). - // Skip this tick; auto-readback will retry next frame. - return; + return false; } - // No lock needed -- Pending* is touched only by the game thread - // (this function and TickComponent's fence-complete handler), and - // the bReadbackPending guard at the top of this function blocks - // concurrent RequestReadback re-entry while a render command is in - // flight. Ready* is what the bridge consumer touches; it's separate - // and managed under FrameLock in TickComponent / ConsumePixels. - TArray* PixelsPtr = nullptr; - TArray* FloatPtr = nullptr; - bReadbackPending = true; - if (CaptureMode == EMjCameraMode::Depth) + // A freshly-enabled render target reports its size on the game thread before + // its RHI texture is created on the render thread. Copying from a null texture + // would stage nothing and yield a black frame that then latches in history, so + // wait until the texture exists and let the caller retry next tick. + FTextureRHIRef SourceTexture = Resource->GetRenderTargetTexture(); + if (!SourceTexture.IsValid()) { - PendingFloatPixels.Emplace(); - FloatPtr = &PendingFloatPixels.GetValue(); - FloatPtr->SetNumUninitialized(Rect.Width() * Rect.Height()); + return false; + } + + FInFlightReadback Entry; + if (FreeReadbacks.Num() > 0) + { + Entry.Gpu = FreeReadbacks.Pop(EAllowShrinking::No); // recycle } else { - PendingPixels.Emplace(); - PixelsPtr = &PendingPixels.GetValue(); - PixelsPtr->SetNumUninitialized(Rect.Width() * Rect.Height()); + Entry.Gpu = MakeShared(TEXT("MjCameraReadback")); + } + Entry.Width = Size.X; + Entry.Height = Size.Y; + // Unix-epoch capture time for the wire meta (matches state wall_time / py time.time()). + Entry.CaptureUnixSeconds = (FDateTime::UtcNow() - FDateTime(1970, 1, 1)).GetTotalSeconds(); + // The post-step state the harvested pixels show: for a manual capture this is + // the current applied id; in every-frame mode the caller passes the id whose + // render the RT currently holds. + Entry.FrameId = ShowFrameId; + Entry.SimTime = ShowSimTime; + + // Schedule the GPU->staging copy without stalling the render thread. SourceTexture + // is captured as a TRefCountPtr (FTextureRHIRef) so an RT reallocation between + // enqueue and execution cannot copy against a released texture: the reference + // keeps the underlying RHI texture alive until the command runs. + FRHIGPUTextureReadback* Readback = Entry.Gpu.Get(); + ENQUEUE_RENDER_COMMAND(MjCameraEnqueueReadback) + ([Readback, SourceTexture, bForceSubmit](FRHICommandListImmediate& RHICmdList) { + Readback->EnqueueCopy(RHICmdList, SourceTexture.GetReference()); + if (bForceSubmit) + { + // Dispatch the copy to the RHI thread now so the readback fence can + // signal mid-frame rather than quantizing to the next frame boundary. + // This is a render-thread-side submit, not a game-thread flush, so it + // neither stalls the game thread nor re-enters the harvest. + RHICmdList.ImmediateFlush(EImmediateFlushType::DispatchToRHIThread); + } + }); + + InFlightReadbacks.Add(MoveTemp(Entry)); + return true; +} + +void UMjCamera::PushFrameToHistory(FMjCameraFrame&& Frame) +{ + TSharedPtr Shared = MakeShared(MoveTemp(Frame)); + PushFrameToHistoryShared(Shared); +} + +void UMjCamera::PushFrameToHistoryShared(const TSharedPtr& Frame) +{ + if (!Frame.IsValid()) + return; + + FScopeLock Lock(&HistoryLock); + History.Add(Frame); + + // Hard frame ceiling regardless of mode: a single frame can be MBs and many + // cameras share the budget, so bound worst-case retention. Sourced from the + // same MaxHistoryCapacity that backs HistoryCapacity's ClampMax so the two + // limits are one source of truth and can never disagree. + constexpr int32 HardCap = MaxHistoryCapacity; + + if (!IsDelayActive()) + { + // Keep the last HistoryCapacity frames for by-id retrieval. + const int32 Cap = FMath::Clamp(HistoryCapacity, 1, HardCap); + while (History.Num() > Cap) + { + History.RemoveAt(0); + } + return; + } + + // Latency emulation: retain enough history to cover the delay window so the + // reveal-time selection always has the frame it needs. Evict a front frame only + // once it is older than the window behind the newest (self-sizing and + // fps-independent), still capped by the memory ceiling. + const double RetainWindow = static_cast(DelaySeconds + DelayJitterSeconds) + 0.10; + const double NewestClock = FrameClock(*History.Last()); + while (History.Num() > 1) + { + const bool bExpired = (NewestClock - FrameClock(*History[0])) > RetainWindow; + if (History.Num() > HardCap || bExpired) + { + History.RemoveAt(0); + } + else + { + break; + } + } +} + +TSharedPtr UMjCamera::GetFrameShared(uint64 MinFrameId) const +{ + FScopeLock Lock(&HistoryLock); + if (History.Num() == 0) + { + return nullptr; + } + if (MinFrameId == 0) + { + return History.Last(); // latest available frame } + // Oldest retained frame at/after the requested step (history is oldest-first), + // i.e. the frame that shows state >= MinFrameId. + for (const TSharedPtr& F : History) + { + if (F->FrameId >= MinFrameId) + { + return F; + } + } + return nullptr; +} + +bool UMjCamera::GetFrame(uint64 MinFrameId, FMjCameraFrame& Out) const +{ + TSharedPtr Frame = GetFrameShared(MinFrameId); + if (!Frame.IsValid()) + { + return false; + } + Out = *Frame; + return true; +} + +TSharedPtr UMjCamera::GetFrameForRequest(uint64 MinFrameId, bool bIgnoreDelay) const +{ + // With latency emulation active, an RPC read must see what the stream is + // currently publishing (the delayed past), not the undelayed newest frame, so + // RPC and stream agree. bIgnoreDelay opts out for a caller that explicitly + // wants the freshest rendered (ground-truth) frame. + if (!bIgnoreDelay && IsDelayActive()) + { + return SelectDelayedFrameShared(NowClockValue(), 0); + } + return GetFrameShared(MinFrameId); +} + +uint64 UMjCamera::GetLatestFrameId() const +{ + FScopeLock Lock(&HistoryLock); + return History.Num() > 0 ? History.Last()->FrameId : 0; +} + +// --------------------------------------------------------------------------- +// Camera latency emulation + capture-rate control +// --------------------------------------------------------------------------- + +double UMjCamera::FrameClock(const FMjCameraFrame& Frame) const +{ + return bDelayUseWallClock ? Frame.CaptureUnixTime : Frame.SimTime; +} + +double UMjCamera::NowClockValue() const +{ + if (bDelayUseWallClock) + { + return (FDateTime::UtcNow() - FDateTime(1970, 1, 1)).GetTotalSeconds(); + } + const AAMjManager* Mgr = AAMjManager::GetManager(); + return Mgr ? Mgr->GetLastAppliedSimTime() : 0.0; +} + +double UMjCamera::SampleDelaySeconds() +{ + double D = static_cast(DelaySeconds); + if (DelayJitterSeconds > 0.0f) + { + // Only draw from the RNG when jitter is configured, so a fixed delay stays + // deterministic and doesn't advance the stream. + const double J = static_cast(DelayJitterSeconds); + D += DelayRng.FRandRange(-J, J); + } + return FMath::Max(0.0, D); +} + +void UMjCamera::PublishFrameToWorkers(const FMjCameraFrame& Frame) +{ + FMjCameraFrameMeta Meta; + Meta.FrameId = Frame.FrameId; + Meta.SimTime = Frame.SimTime; + Meta.Width = static_cast(Frame.Width); + Meta.Height = static_cast(Frame.Height); + // Original capture time, so a delayed frame reports the moment it was taken and + // the client's content-age reflects the injected latency. + Meta.CaptureUnixTime = Frame.CaptureUnixTime; + + // Broadcast the frame to any out-of-core image sink. The payload points at the + // frame's pixel buffer and is valid only for the duration of the broadcast. + FMjCameraFramePayload Payload; + Payload.CanonicalName = GetCanonicalName(); + Payload.Width = Frame.Width; + Payload.Height = Frame.Height; + Payload.bDepth = (CaptureMode == EMjCameraMode::Depth); + Payload.SimTime = Frame.SimTime; + Payload.FrameId = Frame.FrameId; if (CaptureMode == EMjCameraMode::Depth) { - // PF_R32_FLOAT render target: ReadSurfaceFData lands a 4-channel - // FLinearColor per pixel; we keep just the R channel post-fence. - // Slightly wasteful at readback (4x temp memory) but uses the - // stock UE async API. - ENQUEUE_RENDER_COMMAND(MjCameraReadbackDepth)( - [Resource, FloatPtr, Rect](FRHICommandListImmediate& RHICmdList) { - TArray Scratch; - RHICmdList.ReadSurfaceData( - Resource->GetRenderTargetTexture(), - Rect, - Scratch, - FReadSurfaceDataFlags(RCM_MinMax, CubeFace_MAX)); - if (Scratch.Num() == FloatPtr->Num()) - { - for (int32 i = 0; i < Scratch.Num(); ++i) - { - (*FloatPtr)[i] = Scratch[i].R; - } - } - }); + if (Frame.Depth.Num() == 0) + return; + if (bEnableZmqBroadcast && ZmqWorker) + ZmqWorker->PushFrame(Frame.Depth, Meta); + if (bEnableShmBroadcast && ShmWriter) + ShmWriter->PushFrame(Frame.Depth, Meta); + + Payload.Data = reinterpret_cast(Frame.Depth.GetData()); + Payload.DataNumBytes = Frame.Depth.Num() * static_cast(sizeof(float)); + Payload.RowStrideBytes = Frame.Width * static_cast(sizeof(float)); } else { - ENQUEUE_RENDER_COMMAND(MjCameraReadback)( - [Resource, PixelsPtr, Rect](FRHICommandListImmediate& RHICmdList) { - RHICmdList.ReadSurfaceData( - Resource->GetRenderTargetTexture(), - Rect, - *PixelsPtr, - FReadSurfaceDataFlags(RCM_UNorm, CubeFace_MAX)); - }); + if (Frame.Color.Num() == 0) + return; + if (bEnableZmqBroadcast && ZmqWorker) + ZmqWorker->PushFrame(Frame.Color, Meta); + if (bEnableShmBroadcast && ShmWriter) + ShmWriter->PushFrame(Frame.Color, Meta); + + Payload.Data = reinterpret_cast(Frame.Color.GetData()); + Payload.DataNumBytes = Frame.Color.Num() * static_cast(sizeof(FColor)); + Payload.RowStrideBytes = Frame.Width * static_cast(sizeof(FColor)); } - ReadbackFence.BeginFence(); + FMjCameraFrameBus::Get().OnFrameReady.Broadcast(Payload); } -bool UMjCamera::IsReadbackReady() const +TSharedPtr UMjCamera::SelectDelayedFrameShared(double NowValue, uint64 AfterSeq) const { - return bReadbackComplete; + FScopeLock Lock(&HistoryLock); + for (int32 i = History.Num() - 1; i >= 0; --i) + { + if (History[i]->RevealValue <= NowValue) + { + // Newest eligible frame. Deliver it only if it is newer than the last one + // published (monotonic, so the stream never repeats or rewinds). + if (History[i]->Seq > AfterSeq) + { + return History[i]; + } + return nullptr; + } + } + return nullptr; } -TArray UMjCamera::ConsumePixels() +bool UMjCamera::SelectDelayedFrame(double NowValue, uint64 AfterSeq, FMjCameraFrame& Out) const { - FScopeLock Lock(&FrameLock); - if (ReadyPixels.IsSet()) + TSharedPtr Frame = SelectDelayedFrameShared(NowValue, AfterSeq); + if (!Frame.IsValid()) { - TArray Result = MoveTemp(ReadyPixels.GetValue()); - ReadyPixels.Reset(); - bReadbackComplete = ReadyFloatPixels.IsSet(); - return Result; + return false; } - return TArray(); + Out = *Frame; + return true; } -TArray UMjCamera::ConsumeFloatPixels() +void UMjCamera::SetCameraDelay(float InDelaySeconds, float InJitterSeconds, bool bInUseWallClock, int32 InSeed) { - FScopeLock Lock(&FrameLock); - if (ReadyFloatPixels.IsSet()) + DelaySeconds = FMath::Max(0.0f, InDelaySeconds); + DelayJitterSeconds = FMath::Max(0.0f, InJitterSeconds); + bDelayUseWallClock = bInUseWallClock; + const int32 Seed = (InSeed != 0) ? InSeed : static_cast(GetTypeHash(GetCanonicalName())); + DelayRng.Initialize(Seed); + // Re-arm the publish dedup so the new policy re-selects cleanly. + LastPublishedSeq = 0; +} + +void UMjCamera::SetCaptureRate(bool bInOnStateChange, float InMaxFps) +{ + bCaptureOnStateChange = bInOnStateChange; + CaptureMaxFps = FMath::Max(0.0f, InMaxFps); +} + +void UMjCamera::TouchRequested() +{ + LastRequestedSeconds.store(FPlatformTime::Seconds(), std::memory_order_release); +} + +bool UMjCamera::IsCaptureActive() const +{ + // Broadcast cameras stream continuously; others capture only while recently + // requested (within the active TTL). + if (bEnableZmqBroadcast || bEnableShmBroadcast) { - TArray Result = MoveTemp(ReadyFloatPixels.GetValue()); - ReadyFloatPixels.Reset(); - bReadbackComplete = ReadyPixels.IsSet(); - return Result; + return true; } - return TArray(); + const double Last = LastRequestedSeconds.load(std::memory_order_acquire); + if (Last <= 0.0) + { + return false; + } + return (FPlatformTime::Seconds() - Last) <= static_cast(RequestActiveTtlSeconds); +} + +FString UMjCamera::ResolveStreamEndpoint() const +{ + if (const AAMjManager* Mgr = AAMjManager::GetManager()) + { + if (Mgr->BridgeServer) + { + return URLabBridgeServerConfigUtils::BuildCameraEndpoint( + Mgr->BridgeServer->GetInstanceConfig(), StreamPortIndex); + } + } + return ZmqEndpoint; } FString UMjCamera::GetActualZmqEndpoint() const @@ -784,7 +1485,17 @@ FString UMjCamera::GetActualZmqEndpoint() const { return ZmqWorker->GetBoundEndpoint(); } - return ZmqEndpoint; + // Not yet streaming (e.g. a dormant camera advertised in the hello handshake): + // report the endpoint this camera WILL bind from its instance's port block, so + // discovery never advertises the stale default port. + return ResolveStreamEndpoint(); +} + +FString UMjCamera::GetCanonicalName() const +{ + FName Art, Part; + ResolveCameraCanonical(*this, Art, Part); + return FMjCanonicalName::Full(Art, Part); } // --------------------------------------------------------------------------- @@ -978,15 +1689,14 @@ void UMjCamera::ImportFromXml(const FXmlNode* Node, const FMjCompilerSettings& C SetRelativeRotation(Quat); // --- CODEGEN_IMPORT_END --- - // Name fallback (codegen above reads MjName from the "name" attribute; - // here we provide a sensible default if the user omitted it). + // Name fallback (codegen above reads MjName from the "name" attribute; here we + // provide a sensible default if the user omitted it). if (MjName.IsEmpty()) MjName = TEXT("Camera"); - // fovy — direct attribute wins; otherwise derive from MJCF intrinsics. - // MuJoCo's compiler computes fovy from focal_pixel / focal_length when - // present, so we mirror that here so the imported UE FOV matches what - // mujoco itself would report. + // fovy: direct attribute wins; otherwise derive from MJCF intrinsics. MuJoCo's + // compiler computes fovy from focal_pixel / focal_length when present, so we + // mirror that here so the imported UE FOV matches what mujoco would report. FString FovyStr = Node->GetAttribute(TEXT("fovy")); if (!FovyStr.IsEmpty()) { @@ -1005,8 +1715,13 @@ void UMjCamera::ImportFromXml(const FXmlNode* Node, const FMjCompilerSettings& C fovy = 45.0f; } + // Collapse the imported resolution to a validated {width, height} pair so every + // pixel-sizing site can index it safely (MJCF `resolution="640"` or an empty + // array would otherwise be a single element or none). + NormalizeResolution(); + if (CaptureComponent) { - CaptureComponent->FOVAngle = fovy; + CaptureComponent->FOVAngle = HorizontalFOVFromFovy(fovy, GetResolution()); } } diff --git a/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraFrameBus.cpp b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraFrameBus.cpp new file mode 100644 index 00000000..05573126 --- /dev/null +++ b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraFrameBus.cpp @@ -0,0 +1,29 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "MuJoCo/Components/Sensors/MjCameraFrameBus.h" + +FMjCameraFrameBus& FMjCameraFrameBus::Get() +{ + static FMjCameraFrameBus Instance; + return Instance; +} diff --git a/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraSubsystem.cpp b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraSubsystem.cpp new file mode 100644 index 00000000..9fcbacb1 --- /dev/null +++ b/Source/URLab/Private/MuJoCo/Components/Sensors/MjCameraSubsystem.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. + +#include "MuJoCo/Components/Sensors/MjCameraSubsystem.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Core/AMjManager.h" + +void UMjCameraSubsystem::RegisterCamera(UMjCamera* Camera) +{ + if (Camera) + { + Cameras.AddUnique(Camera); + } +} + +void UMjCameraSubsystem::UnregisterCamera(UMjCamera* Camera) +{ + Cameras.RemoveAll([Camera](const TWeakObjectPtr& C) { + return !C.IsValid() || C.Get() == Camera; + }); +} + +void UMjCameraSubsystem::Tick(float DeltaTime) +{ + Super::Tick(DeltaTime); + + // Prune dead cameras and note whether any is actively capturing this tick. + bool bAnyActive = false; + for (int32 i = Cameras.Num() - 1; i >= 0; --i) + { + UMjCamera* Cam = Cameras[i].Get(); + if (!Cam) + { + Cameras.RemoveAtSwap(i, EAllowShrinking::No); + continue; + } + if (Cam->IsCaptureActive()) + { + bAnyActive = true; + } + } + + // Apply the latest physics snapshot once before any camera captures this tick, + // so a tick-driven capture (MaybeCapture) renders the same up-to-date, + // correctly-lit scene the synchronous render path does. Bridge-driven direct / + // puppet sessions do not otherwise apply the snapshot on the game thread every + // frame, which left the streamed capture rendering an unlit (dark) scene. + // Gated on an active camera so idle worlds pay nothing. + if (bAnyActive) + { + if (AAMjManager* Mgr = AAMjManager::GetManager()) + { + Mgr->ApplyLatestRenderState(); + } + } + + for (const TWeakObjectPtr& CamPtr : Cameras) + { + if (UMjCamera* Cam = CamPtr.Get()) + { + Cam->UpdateCapturePipeline(); + } + } +} + +TStatId UMjCameraSubsystem::GetStatId() const +{ + RETURN_QUICK_DECLARE_CYCLE_STAT(UMjCameraSubsystem, STATGROUP_Tickables); +} + +bool UMjCameraSubsystem::DoesSupportWorldType(const EWorldType::Type WorldType) const +{ + // Cameras run in PIE, cooked game, and the editor sim (non-PIE), so tick in + // all of them. + return WorldType == EWorldType::Game + || WorldType == EWorldType::PIE + || WorldType == EWorldType::Editor; +} diff --git a/Source/URLab/Private/MuJoCo/Components/Sensors/MjSensor.cpp b/Source/URLab/Private/MuJoCo/Components/Sensors/MjSensor.cpp index b1fa3685..bf5348c5 100644 --- a/Source/URLab/Private/MuJoCo/Components/Sensors/MjSensor.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Sensors/MjSensor.cpp @@ -34,6 +34,10 @@ #include "MuJoCo/Components/Tendons/MjTendon.h" #include "MuJoCo/Components/Actuators/MjActuator.h" #include "MuJoCo/Utils/MjOrientationUtils.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" +#include "MuJoCo/Generated/MjSensorTypeInfo.h" UMjSensor::UMjSensor() { @@ -178,220 +182,40 @@ void UMjSensor::ExportTo(mjsSensor* Element, mjsDefault* Default) for (int i = 0; i < IntParams.Num() && i < 3; i++) Element->intprm[i] = IntParams[i]; - switch (Type) + // Sensor type, objtype, and reftype all come from the codegen-emitted + // FMjSensorTypeInfo descriptor (MuJoCo/Generated/MjSensorTypeInfo.h). + // The descriptor says whether each of objtype/reftype is a fixed mjOBJ_* + // literal, read from the UE ObjType/RefType property, computed from the + // attachment (rangefinder), or left unset. + const FMjSensorTypeInfo& Info = MjSensorTypeInfoFor(Type); + Element->type = (mjtSensor)Info.MjType; + + switch (Info.ObjSource) { - // --- CODEGEN_SENSOR_TYPE_SWITCH_START --- - case EMjSensorType::Touch: - Element->type = mjSENS_TOUCH; - Element->objtype = mjOBJ_SITE; - break; - case EMjSensorType::Accelerometer: - Element->type = mjSENS_ACCELEROMETER; - Element->objtype = mjOBJ_SITE; - break; - case EMjSensorType::Velocimeter: - Element->type = mjSENS_VELOCIMETER; - Element->objtype = mjOBJ_SITE; - break; - case EMjSensorType::Gyro: - Element->type = mjSENS_GYRO; - Element->objtype = mjOBJ_SITE; + case EMjSensorObjSource::Static: + Element->objtype = (mjtObj)Info.ObjType; break; - case EMjSensorType::Force: - Element->type = mjSENS_FORCE; - Element->objtype = mjOBJ_SITE; + case EMjSensorObjSource::FromXml: + Element->objtype = (mjtObj)EnumToMjObj(ObjType); break; - case EMjSensorType::Torque: - Element->type = mjSENS_TORQUE; - Element->objtype = mjOBJ_SITE; - break; - case EMjSensorType::Magnetometer: - Element->type = mjSENS_MAGNETOMETER; - Element->objtype = mjOBJ_SITE; - break; - case EMjSensorType::CamProjection: - Element->type = mjSENS_CAMPROJECTION; - Element->objtype = mjOBJ_SITE; - Element->reftype = mjOBJ_CAMERA; - break; - case EMjSensorType::RangeFinder: - Element->type = mjSENS_RANGEFINDER; + case EMjSensorObjSource::Computed: Element->objtype = (ObjType == EMjObjType::Camera) ? mjOBJ_CAMERA : mjOBJ_SITE; break; - case EMjSensorType::JointPos: - Element->type = mjSENS_JOINTPOS; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::JointVel: - Element->type = mjSENS_JOINTVEL; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::BallQuat: - Element->type = mjSENS_BALLQUAT; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::BallAngVel: - Element->type = mjSENS_BALLANGVEL; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::JointLimitPos: - Element->type = mjSENS_JOINTLIMITPOS; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::JointLimitVel: - Element->type = mjSENS_JOINTLIMITVEL; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::JointLimitFrc: - Element->type = mjSENS_JOINTLIMITFRC; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::TendonPos: - Element->type = mjSENS_TENDONPOS; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::TendonVel: - Element->type = mjSENS_TENDONVEL; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::TendonLimitPos: - Element->type = mjSENS_TENDONLIMITPOS; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::TendonLimitVel: - Element->type = mjSENS_TENDONLIMITVEL; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::TendonLimitFrc: - Element->type = mjSENS_TENDONLIMITFRC; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::ActuatorPos: - Element->type = mjSENS_ACTUATORPOS; - Element->objtype = mjOBJ_ACTUATOR; - break; - case EMjSensorType::ActuatorVel: - Element->type = mjSENS_ACTUATORVEL; - Element->objtype = mjOBJ_ACTUATOR; - break; - case EMjSensorType::ActuatorFrc: - Element->type = mjSENS_ACTUATORFRC; - Element->objtype = mjOBJ_ACTUATOR; - break; - case EMjSensorType::JointActFrc: - Element->type = mjSENS_JOINTACTFRC; - Element->objtype = mjOBJ_JOINT; - break; - case EMjSensorType::TendonActFrc: - Element->type = mjSENS_TENDONACTFRC; - Element->objtype = mjOBJ_TENDON; - break; - case EMjSensorType::FramePos: - Element->type = mjSENS_FRAMEPOS; - break; - case EMjSensorType::FrameQuat: - Element->type = mjSENS_FRAMEQUAT; - break; - case EMjSensorType::FrameXAxis: - Element->type = mjSENS_FRAMEXAXIS; - break; - case EMjSensorType::FrameYAxis: - Element->type = mjSENS_FRAMEYAXIS; - break; - case EMjSensorType::FrameZAxis: - Element->type = mjSENS_FRAMEZAXIS; - break; - case EMjSensorType::FrameLinVel: - Element->type = mjSENS_FRAMELINVEL; - break; - case EMjSensorType::FrameAngVel: - Element->type = mjSENS_FRAMEANGVEL; - break; - case EMjSensorType::FrameLinAcc: - Element->type = mjSENS_FRAMELINACC; - break; - case EMjSensorType::FrameAngAcc: - Element->type = mjSENS_FRAMEANGACC; - break; - case EMjSensorType::SubtreeCom: - Element->type = mjSENS_SUBTREECOM; - Element->objtype = mjOBJ_BODY; - break; - case EMjSensorType::SubtreeLinVel: - Element->type = mjSENS_SUBTREELINVEL; - Element->objtype = mjOBJ_BODY; - break; - case EMjSensorType::SubtreeAngMom: - Element->type = mjSENS_SUBTREEANGMOM; - Element->objtype = mjOBJ_BODY; + case EMjSensorObjSource::None: break; - case EMjSensorType::InsideSite: - Element->type = mjSENS_INSIDESITE; - Element->reftype = mjOBJ_SITE; - break; - case EMjSensorType::GeomDist: - Element->type = mjSENS_GEOMDIST; - break; - case EMjSensorType::GeomNormal: - Element->type = mjSENS_GEOMNORMAL; - break; - case EMjSensorType::GeomFromTo: - Element->type = mjSENS_GEOMFROMTO; - break; - case EMjSensorType::Contact: - Element->type = mjSENS_CONTACT; - break; - case EMjSensorType::EPotential: - Element->type = mjSENS_E_POTENTIAL; - Element->objtype = mjOBJ_UNKNOWN; - break; - case EMjSensorType::EKinetic: - Element->type = mjSENS_E_KINETIC; - Element->objtype = mjOBJ_UNKNOWN; - break; - case EMjSensorType::Clock: - Element->type = mjSENS_CLOCK; - Element->objtype = mjOBJ_UNKNOWN; - break; - case EMjSensorType::Tactile: - Element->type = mjSENS_TACTILE; - Element->objtype = mjOBJ_MESH; - Element->reftype = mjOBJ_GEOM; - break; - case EMjSensorType::User: - Element->type = mjSENS_USER; - break; - case EMjSensorType::Plugin: - Element->type = mjSENS_PLUGIN; - break; - default: - Element->type = mjSENS_ACCELEROMETER; - Element->objtype = mjOBJ_SITE; - break; - // --- CODEGEN_SENSOR_TYPE_SWITCH_END --- } - // Variable objtype / reftype handling. The codegen switch above sets - // STATIC objtype/reftype literals when sensor_per_type carries a - // mjOBJ_X value. For sensors whose objtype/reftype is "from_xml" or - // "computed" in MuJoCo, the UE side reads ObjType / RefType properties - // and translates them here AFTER the switch fires. - if (Element->type >= mjSENS_FRAMEPOS && Element->type <= mjSENS_FRAMEANGACC) + switch (Info.RefSource) { - Element->objtype = (mjtObj)EnumToMjObj(ObjType); - if (RefType != EMjObjType::Unknown) + case EMjSensorObjSource::Static: + Element->reftype = (mjtObj)Info.RefType; + break; + case EMjSensorObjSource::FromXml: Element->reftype = (mjtObj)EnumToMjObj(RefType); - } - else if (Type == EMjSensorType::GeomDist || Type == EMjSensorType::GeomNormal || Type == EMjSensorType::GeomFromTo || Type == EMjSensorType::Contact || Type == EMjSensorType::Plugin) - { - Element->objtype = (mjtObj)EnumToMjObj(ObjType); - Element->reftype = (mjtObj)EnumToMjObj(RefType); - } - else if (Type == EMjSensorType::User || Type == EMjSensorType::InsideSite) - { - // InsideSite's reftype = mjOBJ_SITE is already set by the codegen - // switch above; here we add the UE-driven objtype. - Element->objtype = (mjtObj)EnumToMjObj(ObjType); + break; + case EMjSensorObjSource::Computed: + case EMjSensorObjSource::None: + break; } // --- CODEGEN_EXPORT_START --- @@ -550,64 +374,10 @@ void UMjSensor::ImportFromXml(const FXmlNode* Node, const FMjCompilerSettings& C ReferenceName = Node->GetAttribute(TEXT("refname")); // --- CODEGEN_IMPORT_END --- - // Determine sensor type from the XML tag name - static const TMap TagToType = { - // --- CODEGEN_SENSOR_TAG_TO_TYPE_START --- - { TEXT("touch"), EMjSensorType::Touch}, - { TEXT("accelerometer"), EMjSensorType::Accelerometer}, - { TEXT("velocimeter"), EMjSensorType::Velocimeter}, - { TEXT("gyro"), EMjSensorType::Gyro}, - { TEXT("force"), EMjSensorType::Force}, - { TEXT("torque"), EMjSensorType::Torque}, - { TEXT("magnetometer"), EMjSensorType::Magnetometer}, - { TEXT("camprojection"), EMjSensorType::CamProjection}, - { TEXT("rangefinder"), EMjSensorType::RangeFinder}, - { TEXT("jointpos"), EMjSensorType::JointPos}, - { TEXT("jointvel"), EMjSensorType::JointVel}, - { TEXT("ballquat"), EMjSensorType::BallQuat}, - { TEXT("ballangvel"), EMjSensorType::BallAngVel}, - { TEXT("jointlimitpos"), EMjSensorType::JointLimitPos}, - { TEXT("jointlimitvel"), EMjSensorType::JointLimitVel}, - { TEXT("jointlimitfrc"), EMjSensorType::JointLimitFrc}, - { TEXT("tendonpos"), EMjSensorType::TendonPos}, - { TEXT("tendonvel"), EMjSensorType::TendonVel}, - { TEXT("tendonlimitpos"), EMjSensorType::TendonLimitPos}, - { TEXT("tendonlimitvel"), EMjSensorType::TendonLimitVel}, - { TEXT("tendonlimitfrc"), EMjSensorType::TendonLimitFrc}, - { TEXT("actuatorpos"), EMjSensorType::ActuatorPos}, - { TEXT("actuatorvel"), EMjSensorType::ActuatorVel}, - { TEXT("actuatorfrc"), EMjSensorType::ActuatorFrc}, - { TEXT("jointactuatorfrc"), EMjSensorType::JointActFrc}, - {TEXT("tendonactuatorfrc"), EMjSensorType::TendonActFrc}, - { TEXT("framepos"), EMjSensorType::FramePos}, - { TEXT("framequat"), EMjSensorType::FrameQuat}, - { TEXT("framexaxis"), EMjSensorType::FrameXAxis}, - { TEXT("frameyaxis"), EMjSensorType::FrameYAxis}, - { TEXT("framezaxis"), EMjSensorType::FrameZAxis}, - { TEXT("framelinvel"), EMjSensorType::FrameLinVel}, - { TEXT("frameangvel"), EMjSensorType::FrameAngVel}, - { TEXT("framelinacc"), EMjSensorType::FrameLinAcc}, - { TEXT("frameangacc"), EMjSensorType::FrameAngAcc}, - { TEXT("subtreecom"), EMjSensorType::SubtreeCom}, - { TEXT("subtreelinvel"), EMjSensorType::SubtreeLinVel}, - { TEXT("subtreeangmom"), EMjSensorType::SubtreeAngMom}, - { TEXT("insidesite"), EMjSensorType::InsideSite}, - { TEXT("distance"), EMjSensorType::GeomDist}, - { TEXT("normal"), EMjSensorType::GeomNormal}, - { TEXT("fromto"), EMjSensorType::GeomFromTo}, - { TEXT("contact"), EMjSensorType::Contact}, - { TEXT("e_potential"), EMjSensorType::EPotential}, - { TEXT("e_kinetic"), EMjSensorType::EKinetic}, - { TEXT("clock"), EMjSensorType::Clock}, - { TEXT("tactile"), EMjSensorType::Tactile}, - { TEXT("user"), EMjSensorType::User}, - { TEXT("plugin"), EMjSensorType::Plugin}, - // --- CODEGEN_SENSOR_TAG_TO_TYPE_END --- - }; + // Determine sensor type from the XML tag name via the descriptor table. const FString Tag = Node->GetTag().ToLower(); - const EMjSensorType* Found = TagToType.Find(Tag); - if (Found) - Type = *Found; + if (const FMjSensorTypeInfo* Info = MjSensorTypeInfoForTag(Tag)) + Type = Info->Type; // TargetName / ReferenceName (target_collations), ObjType / RefType // (xml_enum_attrs), MjClassName (common_imports) are all codegen-emitted @@ -639,22 +409,22 @@ void UMjSensor::Bind(mjModel* Model, mjData* Data, const FString& Prefix) } // Apply the MuJoCo → UE coordinate transform appropriate for each sensor type. -// Rules: +// The coordinate/unit family (EMjSensorValueKind) comes from the descriptor +// table; the rules are: // Position outputs (meters): scale ×100, negate Y // Direction vector outputs: negate Y only // 3-D vector quantities (vel/acc/force/torque/angular): negate Y only // Quaternion outputs (w,x,y,z): reorder to UE (x,y,z,w) with handedness fix +// GeomFromTo (two positions): scale ×100, negate Y on each // Scalar outputs: no transform static void TransformSensorReading(TArray& R, EMjSensorType Type) { if (R.Num() == 0) return; - switch (Type) + switch (MjSensorTypeInfoFor(Type).ValueKind) { - // --- Position outputs (scale ×100 cm, negate Y) --- - case EMjSensorType::FramePos: - case EMjSensorType::SubtreeCom: + case EMjSensorValueKind::Position: if (R.Num() >= 3) { R[0] *= 100.0f; @@ -663,36 +433,14 @@ static void TransformSensorReading(TArray& R, EMjSensorType Type) } break; - // --- Direction vectors (negate Y only) --- - case EMjSensorType::FrameXAxis: - case EMjSensorType::FrameYAxis: - case EMjSensorType::FrameZAxis: - case EMjSensorType::GeomNormal: + case EMjSensorValueKind::Direction: + case EMjSensorValueKind::Vector3: if (R.Num() >= 3) R[1] = -R[1]; break; - // --- 3-D vector quantities: velocity, acceleration, force, torque, angular (negate Y only) --- - case EMjSensorType::Accelerometer: - case EMjSensorType::Velocimeter: - case EMjSensorType::Gyro: - case EMjSensorType::Force: - case EMjSensorType::Torque: - case EMjSensorType::Magnetometer: - case EMjSensorType::BallAngVel: - case EMjSensorType::FrameLinVel: - case EMjSensorType::FrameAngVel: - case EMjSensorType::FrameLinAcc: - case EMjSensorType::FrameAngAcc: - case EMjSensorType::SubtreeLinVel: - case EMjSensorType::SubtreeAngMom: - if (R.Num() >= 3) - R[1] = -R[1]; - break; - - // --- Quaternion (w,x,y,z) → UE (X,Y,Z,W) with handedness fix: X=-mjX, Y=mjY, Z=-mjZ, W=mjW --- - case EMjSensorType::BallQuat: - case EMjSensorType::FrameQuat: + // Quaternion (w,x,y,z) → UE (X,Y,Z,W): X=-mjX, Y=mjY, Z=-mjZ, W=mjW. + case EMjSensorValueKind::Quaternion: if (R.Num() >= 4) { const float mj_w = R[0], mj_x = R[1], mj_y = R[2], mj_z = R[3]; @@ -703,8 +451,7 @@ static void TransformSensorReading(TArray& R, EMjSensorType Type) } break; - // --- GeomFromTo: two 3D positions concatenated (scale ×100, negate Y each) --- - case EMjSensorType::GeomFromTo: + case EMjSensorValueKind::GeomFromTo: if (R.Num() >= 6) { R[0] *= 100.0f; @@ -716,8 +463,7 @@ static void TransformSensorReading(TArray& R, EMjSensorType Type) } break; - // --- Scalars and types with no coordinate meaning: no transform --- - default: + case EMjSensorValueKind::Scalar: break; } } @@ -768,27 +514,21 @@ void UMjSensor::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) ExportTo(sensor, effectiveDefault); } -void UMjSensor::BuildBinaryPayload(FBufferArchive& OutBuffer) const +void UMjSensor::DescribeState(FMjArticulationState& Out) const { - int32 SensorID = m_ID; - OutBuffer << SensorID; - - int32 NumElements = GetDimension(); - OutBuffer << NumElements; - - if (m_SensorView.id != -1 && m_SensorView.sensordata && NumElements > 0) - { - for (int i = 0; i < NumElements; ++i) - { - float Val = (float)m_SensorView.sensordata[i]; - OutBuffer << Val; - } - } -} + const SensorView& V = m_SensorView; + if (V.id < 0 || !V.sensordata || V.sensor_dim <= 0) + return; -FString UMjSensor::GetTelemetryTopicName() const -{ - return FString::Printf(TEXT("sensor/%s"), *GetName()); + // The IR carries raw MuJoCo SI values (MuJoCo frame, double precision), like + // joints and bodies do. The MuJoCo -> UE coordinate/unit fixup lives on the + // display-facing accessor GetReading(), not on the serialization path. + FMjSensorState& S = Out.Sensors.AddDefaulted_GetRef(); + S.Name = FMjCanonicalName::PartSegment(Cast(GetOwner()), GetMjName()); + S.Semantic = MjSensorTypeInfoFor(Type).Semantic; + S.Values.SetNumUninitialized(V.sensor_dim); + for (int32 i = 0; i < V.sensor_dim; ++i) + S.Values[i] = V.sensordata[i]; } #if WITH_EDITOR diff --git a/Source/URLab/Private/UI/MjCameraFeedEntry.cpp b/Source/URLab/Private/UI/MjCameraFeedEntry.cpp index 0b1e3696..c8a88b73 100644 --- a/Source/URLab/Private/UI/MjCameraFeedEntry.cpp +++ b/Source/URLab/Private/UI/MjCameraFeedEntry.cpp @@ -61,9 +61,12 @@ void UMjCameraFeedEntry::RefreshBrush() if (!RT) return; + // Read the resolution through the camera's validated accessor so a malformed + // `resolution` array can never index out of bounds here. + const FIntPoint Res = BoundCamera->GetResolution(); const float W = 320.f; - const float H = (BoundCamera->resolution[0] > 0) - ? W * static_cast(BoundCamera->resolution[1]) / static_cast(BoundCamera->resolution[0]) + const float H = (Res.X > 0) + ? W * static_cast(Res.Y) / static_cast(Res.X) : W * 0.75f; // Depth RT is R32f — slate can't display it directly. Build (or reuse) @@ -73,11 +76,11 @@ void UMjCameraFeedEntry::RefreshBrush() if (BoundCamera->CaptureMode == EMjCameraMode::Depth) { if (!DepthPreviewTexture - || DepthPreviewTexture->GetSizeX() != BoundCamera->resolution[0] - || DepthPreviewTexture->GetSizeY() != BoundCamera->resolution[1]) + || DepthPreviewTexture->GetSizeX() != Res.X + || DepthPreviewTexture->GetSizeY() != Res.Y) { DepthPreviewTexture = UTexture2D::CreateTransient( - BoundCamera->resolution[0], BoundCamera->resolution[1], PF_B8G8R8A8, + Res.X, Res.Y, PF_B8G8R8A8, TEXT("URLabDepthPreview")); DepthPreviewTexture->CompressionSettings = TC_VectorDisplacementmap; DepthPreviewTexture->SRGB = false; @@ -168,7 +171,10 @@ void UMjCameraFeedEntry::UpdateFeed() if (!BoundCamera || !FeedImage || !BoundCamera->RenderTarget) return; - if (!FeedImage->GetBrush().GetResourceObject()) + // Rebind whenever the camera's RenderTarget changes identity, not just when + // the brush is empty -- set_camera_streaming can rebuild the RenderTarget, + // and a stale brush would otherwise freeze on the last frame. + if (FeedImage->GetBrush().GetResourceObject() != BoundCamera->RenderTarget) { RefreshBrush(); } diff --git a/Source/URLab/Public/MuJoCo/Components/Sensors/CameraShmWriter.h b/Source/URLab/Public/MuJoCo/Components/Sensors/CameraShmWriter.h index 92b83f0d..afce5934 100644 --- a/Source/URLab/Public/MuJoCo/Components/Sensors/CameraShmWriter.h +++ b/Source/URLab/Public/MuJoCo/Components/Sensors/CameraShmWriter.h @@ -16,6 +16,7 @@ #include "CoreMinimal.h" #include "Transport/ShmRegion.h" +#include "MuJoCo/Components/Sensors/MjCameraTypes.h" /** * @class FCameraShmWriter @@ -27,8 +28,12 @@ * background thread -- the camera readback callback feeds this directly * on the game / render-thread sidecar. * - * Wire layout per slot: `[u32 size][bytes...]`. The byte format depends - * on the camera mode and is documented out-of-band via the handshake: + * Wire layout per slot: `[u32 size][FMjCameraFrameMeta][pixels...]`, where + * `size` is the byte count of `meta + pixels` (i.e. it includes the 40-byte + * header). The consumer reads `size`, parses the leading 40 bytes as the + * frame metadata (frame_id / sim_time / w / h), and treats the remainder as + * pixels. The pixel byte format depends on the camera mode and is documented + * out-of-band via the handshake: * - Real: width * height * 4 bytes BGRA8 (FColor). * - Depth: width * height * 4 bytes float32 (single channel). * - Semantic / Instance: width * height * 4 bytes BGRA8 with a @@ -52,21 +57,21 @@ class URLAB_API FCameraShmWriter bool IsOpen() const { return Region.IsOpen(); } - /** Push one frame's raw bytes. Silently drops if `ByteCount` doesn't - * match the configured pixel count * 4 -- a partial frame can't be - * decoded by the consumer anyway. */ - void PushFrame(const void* Data, uint32 ByteCount); + /** Push one frame's raw bytes plus its metadata header. Silently drops if + * `ByteCount` doesn't match the configured pixel count * 4 -- a partial + * frame can't be decoded by the consumer anyway. */ + void PushFrame(const void* Data, uint32 ByteCount, const FMjCameraFrameMeta& Meta); /** Convenience overload for color frames. */ - void PushFrame(const TArray& Pixels) + void PushFrame(const TArray& Pixels, const FMjCameraFrameMeta& Meta) { - PushFrame(Pixels.GetData(), static_cast(Pixels.Num()) * sizeof(FColor)); + PushFrame(Pixels.GetData(), static_cast(Pixels.Num()) * sizeof(FColor), Meta); } /** Convenience overload for single-channel float frames (depth). */ - void PushFrame(const TArray& Pixels) + void PushFrame(const TArray& Pixels, const FMjCameraFrameMeta& Meta) { - PushFrame(Pixels.GetData(), static_cast(Pixels.Num()) * sizeof(float)); + PushFrame(Pixels.GetData(), static_cast(Pixels.Num()) * sizeof(float), Meta); } FString GetPath() const { return Region.GetPath(); } diff --git a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCamera.h b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCamera.h index 62caefa1..1f2c358a 100644 --- a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCamera.h +++ b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCamera.h @@ -27,16 +27,21 @@ #include "Engine/TextureRenderTarget2D.h" #include "MuJoCo/Components/MjComponent.h" #include "HAL/Runnable.h" -#include "HAL/ThreadSafeBool.h" #include "Containers/Queue.h" +#include "Templates/PimplPtr.h" #include "MuJoCo/Components/Sensors/MjCameraTypes.h" #include "MuJoCo/Utils/MjOrientationUtils.h" +#include "RHIGPUReadback.h" +#include "Math/RandomStream.h" #include #include "MjCamera.generated.h" /** * @class FCameraZmqWorker - * @brief Background thread for publishing high-bandwidth camera frames via ZeroMQ. + * @brief Background thread that publishes high-bandwidth camera frames over + * ZeroMQ. All transport internals (the libzmq context/socket handles, the + * per-format frame queues, the bind bookkeeping) live in an opaque state + * object defined in the .cpp, so this header stays free of raw handles. */ class FCameraZmqWorker : public FRunnable { @@ -49,31 +54,49 @@ class FCameraZmqWorker : public FRunnable virtual void Stop() override; virtual void Exit() override; - void PushFrame(const TArray& FrameData); - void PushFrame(const TArray& FrameData); - FString GetBoundEndpoint() const { return BoundEndpoint; } + void PushFrame(const TArray& FrameData, const FMjCameraFrameMeta& Meta); + void PushFrame(const TArray& FrameData, const FMjCameraFrameMeta& Meta); + FString GetBoundEndpoint() const; - /** Process-wide pause gate. Workers drain without sending while set, - * to bound RT memory in Direct/Puppet mode. */ + /** Process-wide pause gate. Workers drain without sending while set, to bound + * render-thread memory in Direct/Puppet mode. */ static URLAB_API std::atomic bPublishersPaused; private: - FString RequestedEndpoint; - FString BoundEndpoint; - FString Topic; - FIntPoint resolution; - - void* ZmqContext = nullptr; - void* ZmqPublisher = nullptr; - - FThreadSafeBool bStopThread; - // Two queues -- one per pixel format. Real / seg cameras drive the - // FColor queue, depth cameras drive the float queue. The Run() loop - // drains both and ships whatever it finds. Per-camera CaptureMode - // never changes after streaming starts, so only one queue is ever - // active per worker instance. - TQueue, EQueueMode::Spsc> FrameQueue; - TQueue, EQueueMode::Spsc> FloatFrameQueue; + struct FState; + TPimplPtr State; +}; + +/** + * @struct FMjCameraFrame + * @brief One retained camera frame, tagged with the post-step physics state it + * shows. CaptureMode decides which pixel buffer is populated. + * + * Frames are retained in the history ring as `TSharedPtr`, + * so history retention, the RPC fetch, and the streaming publish all share one + * allocation by refcount rather than deep-copying the multi-MB pixel array. + */ +struct FMjCameraFrame +{ + uint64 FrameId = 0; // post-step render-snapshot id this frame shows + double SimTime = 0.0; + int32 Width = 0; + int32 Height = 0; + TArray Color; // Real / seg modes (BGRA8) + TArray Depth; // Depth mode (float32) + // Unix-epoch capture time (FDateTime::UtcNow at readback request). Carried + // here so a delayed re-publish can rebuild the v2 wire meta with the + // ORIGINAL capture time, so the client's content-age math reflects the + // injected latency rather than the moment we re-sent the bytes. + double CaptureUnixTime = 0.0; + // Camera-latency emulation bookkeeping (see UMjCamera delay API). + // - RevealValue: the clock value (SimTime or CaptureUnixTime, per + // bDelayUseWallClock) at which this frame becomes eligible to publish, + // i.e. capture_clock + sampled_delay. + // - Seq: monotonic per-harvest counter so each delayed frame publishes + // exactly once (FrameId can repeat across intra-step captures). + double RevealValue = 0.0; + uint64 Seq = 0; }; /** @@ -85,11 +108,16 @@ class FCameraZmqWorker : public FRunnable * SetStreamingEnabled(true) is called. * * Key design points: - * - No ExportTo / RegisterToSpec — camera is UE-side only, not fed back to MuJoCo. - * - SetStreamingEnabled() allocates the RT and calls IStreamingManager::AddViewInformation - * so textures load correctly even when the player pawn is far away. - * - RequestReadback() enqueues a non-blocking GPU→CPU copy; check IsReadbackReady() - * on a subsequent tick, then consume with ConsumePixels(). + * - No ExportTo / RegisterToSpec: camera is UE-side only, not fed back to MuJoCo. + * - SetStreamingEnabled() allocates the RT and registers the viewpoint with the + * streaming manager so textures load correctly even when the pawn is far away. + * - The GPU readback is fully asynchronous and decoupled from stepping: the + * render thread maps + copies each finished readback and pushes the pixels + * onto a results queue; the game thread pops them next tick into a history + * ring tagged with the post-step FrameId/SimTime they show. No render-thread + * flush is taken on the steady-state or synchronous paths. + * - Per-camera capture gating: a camera only captures while broadcast-enabled + * or recently requested (TouchRequested), so idle cameras cost no GPU. */ UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) class URLAB_API UMjCamera : public UMjComponent @@ -194,9 +222,9 @@ class URLAB_API UMjCamera : public UMjComponent meta = (EditCondition = "bOverride_Projection")) EMjCameraProjection Projection = EMjCameraProjection::Perspective; - UMjCamera(); + // ---- Capture configuration ---- - /** What this camera captures. Read at SetStreamingEnabled(true) time — + /** What this camera captures. Read at SetStreamingEnabled(true) time: * toggle streaming off/on after changing. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera") EMjCameraMode CaptureMode = EMjCameraMode::Real; @@ -211,14 +239,19 @@ class URLAB_API UMjCamera : public UMjComponent meta = (EditCondition = "CaptureMode == EMjCameraMode::Depth", ClampMin = "1.0")) float DepthFarCm = 10000.0f; - // ---- Streaming ---- - /** @brief Boost factor passed to IStreamingManager::AddViewInformation. * Increase to force higher-quality texture mips near this camera. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Streaming") float StreamingBoost = 1.0f; - // ---- Capture Components ---- + /** Render this capture as nested passes of the main renderer (UE 5.3+), + * sharing visibility/GPU-scene setup, instead of a standalone scene render + * (cheaper with many live cameras). Off by default: it renders on the main + * render cadence, so it is NOT compatible with the render:sync fast path + * (which captures on demand). Enable only for pure live-streaming cameras + * that are never requested with render:"sync". */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Streaming") + bool bRenderInMainRenderer = false; /** @brief The underlying SceneCaptureComponent2D. Capture is disabled by default. */ UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "MuJoCo|Camera") @@ -228,25 +261,85 @@ class URLAB_API UMjCamera : public UMjComponent UPROPERTY(VisibleAnywhere, BlueprintReadOnly, Category = "MuJoCo|Camera") UTextureRenderTarget2D* RenderTarget = nullptr; - // ---- ZeroMQ Streaming ---- - - /** @brief If true, the camera will automatically broadcast its frames over ZeroMQ when streaming is enabled. */ + /** @brief If true, the camera automatically broadcasts its frames over ZeroMQ when streaming is enabled. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Network") bool bEnableZmqBroadcast = false; - /** @brief The ZMQ Endpoint for this specific camera (e.g., tcp://0.0.0.0:5558). Must be unique per camera. */ + /** @brief The ZMQ endpoint this camera's PUB socket binds. Resolved when + * streaming is enabled from the instance's camera port block + * (BindAddress + CamBasePort + StreamPortIndex), so N farm instances on + * distinct CamBasePort blocks never collide. This authored value is only a + * fallback used when no instance config is reachable (e.g. an isolated + * component test). The port is still auto-incremented on bind conflict. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Network") FString ZmqEndpoint = TEXT("tcp://0.0.0.0:5558"); - // ---- Shared-memory streaming ---- - - /** @brief If true, the camera also writes each frame into a per-camera - * SHM region (`/URLabShm//cam__.shm`). The - * ZMQ broadcast is unaffected -- both transports can run in parallel. */ + /** @brief If true, the camera also writes each frame into a per-camera SHM + * region (`/URLabShm//cam__.shm`). The ZMQ + * broadcast is unaffected: both transports can run in parallel. The session + * segment is currently the literal "live", so multiple editor processes on + * one host collide on the same file; parameterise the session per instance + * before running a multi-process render farm. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Network") bool bEnableShmBroadcast = false; - // ---- Public API ---- + // ---- Camera latency emulation + capture-rate control ---- + + /** Simulated camera latency (seconds). The streamed / served frame is the + * newest whose reveal time <= now, where reveal = capture_clock + + * sampled_delay. 0 = no delay (frames published as soon as harvested). + * Measured in SimTime by default, or wall-clock when bDelayUseWallClock. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Delay", meta = (ClampMin = "0.0")) + float DelaySeconds = 0.0f; + + /** Symmetric uniform jitter half-range (seconds): per-frame effective delay + * ~ U(DelaySeconds - this, DelaySeconds + this), clamped >= 0. 0 = a fixed + * delay. Sampled from a seeded per-camera RNG so jittered latency is + * reproducible across runs. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Delay", meta = (ClampMin = "0.0")) + float DelayJitterSeconds = 0.0f; + + /** If true, delay / jitter / reveal selection use wall-clock (frame capture + * unix time) instead of SimTime. Wall-clock suits real-latency emulation in + * live mode; SimTime is deterministic for stepped runs. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Delay") + bool bDelayUseWallClock = false; + + /** Capture + read back only when the applied physics state advances (the + * manager's FrameId changes). Between steps the world is unchanged, so a + * re-render + GPU readback is wasted work. No-op in live (state advances + * every frame); a large GPU saving while stepping, and zero capture cost + * while paused. Disable to force a capture every engine frame. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Capture") + bool bCaptureOnStateChange = true; + + /** Optional hard cap on capture rate (frames/sec, wall-clock). 0 = uncapped. + * Applied on top of bCaptureOnStateChange to further throttle a high-rate + * feed when the consumer needs fewer frames than the sim emits. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera|Capture", meta = (ClampMin = "0.0")) + float CaptureMaxFps = 0.0f; + + /** How many recent frames to retain for by-id retrieval. ClampMax mirrors + * MaxHistoryCapacity, the absolute ceiling the eviction paths enforce; keep + * the two in sync. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera", meta = (ClampMin = "1", ClampMax = "64")) + int32 HistoryCapacity = 8; + + /** How long after a request a camera keeps capturing before going dormant. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Camera", meta = (ClampMin = "0.0")) + float RequestActiveTtlSeconds = 2.0f; + + UMjCamera(); + + // ---- Runtime configuration ---- + + /** Configure latency emulation at runtime (RPC-driven; applied on the game + * thread alongside the per-tick capture, so no extra locking is needed). + * Seed 0 derives a stable seed from the canonical name. */ + void SetCameraDelay(float InDelaySeconds, float InJitterSeconds, bool bInUseWallClock, int32 InSeed); + + /** Configure capture-rate control at runtime. */ + void SetCaptureRate(bool bInOnStateChange, float InMaxFps); /** * @brief Allocates the render target and begins streaming / scene capture. @@ -255,37 +348,104 @@ class URLAB_API UMjCamera : public UMjComponent UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") void SetStreamingEnabled(bool bEnable); - /** @brief Returns true once SetStreamingEnabled(true) has set up the - * RT and the capture component. Cheap, lock-free read intended for - * best-effort gating from the bridge worker thread (a stale read is - * benign — at worst we marshal an extra idempotent - * SetStreamingEnabled to the game thread). */ + /** @brief Returns true once SetStreamingEnabled(true) has set up the RT and + * the capture component. Cheap, lock-free read intended for best-effort + * gating from the bridge worker thread (a stale read is benign). */ bool IsStreamingActive() const { return bStreamingEnabled; } + /** Resolution as a validated {width, height} pair, substituting the 640x480 + * default for any missing or non-positive element. Every pixel-sizing site + * reads through this so a malformed `resolution` array can never index out + * of bounds. */ + FIntPoint GetResolution() const; + /** - * @brief Enqueues a non-blocking asynchronous GPU→CPU pixel readback. - * No-op if streaming is not enabled or a readback is already in flight. + * @brief Enqueues a non-blocking asynchronous GPU->CPU pixel readback for the + * current applied state. No-op if streaming is not enabled or the + * in-flight cap is reached. Completed readbacks land in the history + * ring (see GetFrame), tagged with the post-step state id they show. */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") void RequestReadback(); - /** - * @brief Returns true when the GPU→CPU copy started by RequestReadback() is complete. - */ - UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") - bool IsReadbackReady() const; + /** One per-frame capture pass: capture gating (lazy streaming enable / + * every-frame vs state-change), advance the async readback pipeline, and + * (while active) issue a capture and publish any due delayed frames. Driven + * by UMjCameraSubsystem once per frame rather than a per-component tick. */ + void UpdateCapturePipeline(); + + /** Render-on-demand: capture the scene and enqueue a readback right now for + * the current applied state. Requires streaming already enabled. Pair with + * HarvestCompletedReadbacks (called each tick and by the sync render path) + * to pull the finished frame into history. */ + void IssueSyncCapture(); + + /** Advance the async readback pipeline: dispatch every GPU-ready readback to + * the render thread for map/copy, and drain the render thread's finished + * frames into the history ring (publishing inline when no delay is + * configured). FIFO; takes no render-thread flush. Called each tick and by + * the synchronous render path. */ + void HarvestCompletedReadbacks(); + + /** Poll the async readback pipeline (dispatch + harvest) up to TimeoutSeconds, + * yielding between passes so the render thread can finish its map/copy. Used + * by the synchronous render path to obtain a fresh frame without flushing the + * render thread. */ + void WaitAndHarvestReadbacks(double TimeoutSeconds); + + /** Count of async readbacks awaiting GPU completion (diagnostics). Excludes + * frames already dispatched to the render thread's map/copy. */ + int32 NumInFlightReadbacks() const { return InFlightReadbacks.Num(); } + + /** True while any readback is still awaiting GPU completion or its finished + * frame is still queued for harvest. Lets the synchronous render path avoid + * re-issuing a capture while one is already in flight. */ + bool HasPendingReadbacks() const { return InFlightReadbacks.Num() > 0 || PendingMapCommands > 0; } /** - * @brief Consumes and returns the pixel array from the last completed readback. - * Returns an empty array if not ready. Clears the pending state. + * @brief Fetch a frame from this camera's history ring (thread-safe). + * + * MinFrameId == 0 returns the most recent retained frame. Otherwise returns + * the oldest retained frame whose FrameId >= MinFrameId, i.e. the frame + * showing the state at/after that step. Returns false if no matching frame is + * retained yet. Fills Out.Color for Real/seg modes or Out.Depth for Depth. + * Deep-copies the pixels into Out; production code should prefer GetFrameShared + * / GetFrameForRequest, which hand back the shared retained frame by refcount. */ - UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") - TArray ConsumePixels(); + bool GetFrame(uint64 MinFrameId, FMjCameraFrame& Out) const; + + /** Shared-refcount variant of GetFrame: returns the retained frame without + * copying its pixels. Null if no matching frame is retained. */ + TSharedPtr GetFrameShared(uint64 MinFrameId) const; + + /** Resolve the frame to return for an RPC request. When latency emulation is + * active and bIgnoreDelay is false, returns the frame currently revealed by + * the delay policy (so an RPC read agrees with what the stream is showing); + * otherwise returns the frame at/after MinFrameId. Null if none available. */ + TSharedPtr GetFrameForRequest(uint64 MinFrameId, bool bIgnoreDelay) const; + + /** Most recent frame id retained in history (0 if none). */ + uint64 GetLatestFrameId() const; + + /** Select the newest history frame eligible at NowValue (clock chosen per + * bDelayUseWallClock) whose Seq > AfterSeq. Returns false if none. Thread- + * safe; public so automation tests can drive the selection directly. */ + bool SelectDelayedFrame(double NowValue, uint64 AfterSeq, FMjCameraFrame& Out) const; + + /** Push a completed frame into the history ring, evicting the oldest beyond + * the retention window. Exposed (not a UFUNCTION) so automation tests can + * drive the ring with synthetic frames without a live GPU. */ + void PushFrameToHistory(FMjCameraFrame&& Frame); + + /** Mark this camera as actively consumed right now (called when a client + * requests it via include_cameras / get_frame). Per-camera capture gating + * keeps a camera capturing only while broadcast-enabled or touched within + * RequestActiveTtlSeconds, so idle cameras cost no GPU. Thread-safe. */ + void TouchRequested(); - /** Depth-mode counterpart to ConsumePixels. Returns single-channel - * float32 pixels in row-major (h, w) order. Empty if Depth readback - * isn't ready or the camera isn't in Depth mode. */ - TArray ConsumeFloatPixels(); + /** True if this camera should be capturing right now: it has a streaming + * broadcast enabled, or was requested within the active TTL. */ + bool IsCaptureActive() const; /** * @brief Returns the ZMQ endpoint actually bound (may differ from ZmqEndpoint if auto-incremented). @@ -293,11 +453,25 @@ class URLAB_API UMjCamera : public UMjComponent UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") FString GetActualZmqEndpoint() const; + /** Ordinal of this camera within its instance's camera port block. Assigned + * by UMjNetworkManager at registration so each camera seeds a distinct port + * (CamBasePort + index) upward from the instance's CamBasePort. */ + void SetStreamPortIndex(int32 InIndex) { StreamPortIndex = InIndex; } + /** - * @brief Returns the bound camera component pointer (for UI wiring). + * @brief Canonical transport identity for this camera: "/". + * + * Routed through FMjCanonicalName (the single naming owner): the art segment + * is the owning articulation's name (or the owning actor's name for a + * manager-level global camera) and the part is the MJCF name (or UE component + * name if unset) with the art prefix stripped, both sanitized to + * [A-Za-z0-9_]. This one string is the ZMQ topic, the hello handshake key + + * zmq_topic, the set_camera_streaming key, and the include_cameras lookup; + * the SHM filename is "cam__.shm". UE writer and bridge reader + * apply the same scheme so both ends rendezvous on the same name. */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|Camera") - UMjCamera* GetSelf() { return this; } + FString GetCanonicalName() const; /** * @brief Exports camera properties to a MuJoCo spec camera structure. @@ -315,56 +489,174 @@ class URLAB_API UMjCamera : public UMjComponent protected: virtual void BeginPlay() override; - virtual void TickComponent(float DeltaTime, ELevelTick TickType, - FActorComponentTickFunction* ThisTickFunction) override; virtual void OnRegister() override; virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; private: - // ---- Internal helpers ---- + // ---- Streaming setup helpers ---- void SetupRenderTarget(); void RegisterWithStreamingManager(); - /** Refresh HiddenComponents from live seg pools so a late-starting seg - * camera doesn't contaminate an already-streaming RGB/Depth capture. */ + /** Resolve the ZMQ endpoint this camera's PUB socket should bind from the + * live instance config (BindAddress + CamBasePort + StreamPortIndex), so + * cameras land in their instance's own port block. Falls back to the + * authored ZmqEndpoint when no manager / bridge config is reachable. */ + FString ResolveStreamEndpoint() const; + + /** Ordinal within the instance's camera port block (see SetStreamPortIndex). */ + int32 StreamPortIndex = 0; + + /** Ensure `resolution` holds exactly two positive elements (defaults applied). */ + void NormalizeResolution(); + + /** Issue a scene capture + readback for the current applied state when the + * fps cap and state-change gate allow it. */ + void MaybeCapture(class AAMjManager* Mgr); + + /** With latency emulation on, publish the newest frame whose reveal time has + * passed (each Seq once). No-op when no delay is configured. */ + void PublishDueDelayedFrames(class AAMjManager* Mgr); + + /** Refresh HiddenComponents from live seg pools so a late-starting seg camera + * doesn't contaminate an already-streaming RGB/Depth capture. */ void RefreshHiddenComponentsFromSegPools(); - // ---- Readback state ---- - // CaptureMode picks which Pending/Ready pair is used: BGRA8 (Real / - // SemSeg / InstanceSeg) drives PendingPixels/ReadyPixels, Depth - // drives PendingFloatPixels/ReadyFloatPixels. Only one mode is in - // flight at a time. - // - // Pending* is the render-thread destination. RequestReadback - // Emplaces a fresh TArray, SetNumUninitialized's it, and enqueues a - // render command capturing a pointer into it. The render thread - // writes to that pointer. Only the game thread touches Pending*, - // and only RequestReadback (gated by !bReadbackPending) and - // TickComponent's fence-complete handler do so, so the captured - // pointer stays valid until the fence completes. - // - // Ready* is the consumer-facing buffer. TickComponent moves - // Pending* into Ready* once the fence completes (workers PushFrame - // a copy first). ConsumePixels / ConsumeFloatPixels move out of - // Ready*. FrameLock serialises Ready* access between the game - // thread (move-in) and the bridge worker thread (move-out). - FCriticalSection FrameLock; - TOptional> PendingPixels; - TOptional> PendingFloatPixels; - TOptional> ReadyPixels; - TOptional> ReadyFloatPixels; - FRenderCommandFence ReadbackFence; - bool bReadbackPending = false; - bool bReadbackComplete = false; + /** Enqueue one async GPU->staging copy, stamping the frame with the id/time of + * the applied state the pixels will show. Returns false without enqueuing when + * the render target is not yet renderable (no RHI texture, or the in-flight cap + * is reached), so a caller can keep retrying a cold camera. When bForceSubmit + * is set (the on-demand sync path) the copy is dispatched to the RHI thread + * immediately so its fence can signal mid-frame instead of at the next frame + * boundary; the steady streaming path leaves it false to avoid the per-capture + * submit cost. */ + bool EnqueueReadback(uint64 ShowFrameId, double ShowSimTime, bool bForceSubmit = false); + + /** Dispatch every GPU-ready readback to the render thread for map/copy (FIFO). */ + void DispatchReadyReadbacks(); + + /** Drain the render thread's finished frames into history + recycle readbacks. */ + void DrainCompletedFrames(); + + /** Newest revealed frame under the delay policy at NowValue, refcount-shared. + * Null when nothing is eligible or (with AfterSeq) nothing new. */ + TSharedPtr SelectDelayedFrameShared(double NowValue, uint64 AfterSeq) const; + + /** Store a completed frame into the ring; shared so retention is a refcount + * bump rather than a pixel deep-copy. */ + void PushFrameToHistoryShared(const TSharedPtr& Frame); + + /** Current delay-policy clock value (wall-clock or applied SimTime). */ + double NowClockValue() const; + + /** Sample an effective delay (seconds) from DelaySeconds +/- jitter via the + * seeded RNG, clamped >= 0. */ + double SampleDelaySeconds(); + + /** Clock value (SimTime or CaptureUnixTime) used for delay maths on Frame, + * per bDelayUseWallClock. */ + double FrameClock(const FMjCameraFrame& Frame) const; + + /** Push one frame onto the streaming transports (ZMQ + SHM) and broadcast it + * on FMjCameraFrameBus for any out-of-core image sink. Reconstructs the v2 + * wire meta. Shared by the no-delay (inline) and delayed publish. */ + void PublishFrameToWorkers(const FMjCameraFrame& Frame); + + /** True when latency emulation is configured (delay or jitter > 0). */ + bool IsDelayActive() const { return DelaySeconds > 0.0f || DelayJitterSeconds > 0.0f; } + + // ---- Async readback pipeline ---- + // GPU->CPU pixel readback uses FRHIGPUTextureReadback (async, non-stalling) + // rather than a synchronous RHICmdList.ReadSurfaceData, so the readback rate + // tracks the render rate instead of collapsing to 1/(GPU render time). The + // game thread enqueues the copy and later, once IsReady(), dispatches a + // render-thread command that maps + copies the staging buffer and pushes the + // finished frame onto ResultsQueue. The game thread pops that queue next tick. + // No render-thread flush is taken on either path, which structurally removes + // the re-entrancy the earlier per-readback flush created. + struct FInFlightReadback + { + TSharedPtr Gpu; + uint64 FrameId = 0; + double SimTime = 0.0; + int32 Width = 0; + int32 Height = 0; + // Unix-epoch seconds (FDateTime::UtcNow) at request time, stamped into the + // streamed frame's meta so clients can measure content latency. + double CaptureUnixSeconds = 0.0; + }; + TArray InFlightReadbacks; + static constexpr int32 MaxInFlightReadbacks = 3; + + // A frame whose GPU->staging map+copy has completed on the render thread, + // carrying the readback object back for recycling on the game thread. + struct FCompletedReadback + { + TSharedPtr Frame; + TSharedPtr Gpu; + bool bCopied = false; + }; + using FCompletedReadbackQueue = TQueue; + // Render thread produces, game thread consumes. Held by shared pointer so a + // render command outliving the component (teardown race) references the queue, + // not a destroyed UObject. + TSharedPtr ResultsQueue; + // Map/copy commands dispatched to the render thread but not yet drained. Only + // the game thread touches it (dispatch increments, drain decrements). + int32 PendingMapCommands = 0; + // Guards against a nested drain if a render flush ever pumps the game thread + // while a drain is in progress (the async design avoids such flushes, but the + // guard keeps the single-consumer queue contract structurally safe). + bool bDrainingResults = false; + + // Recycle pool for the readback objects. FRHIGPUTextureReadback owns a staging + // texture, so reuse them (EnqueueCopy re-arms in place) rather than allocating + // one per request. + TArray> FreeReadbacks; + + // ---- Frame history ring ---- + // Retains recent frames so a client can fetch the frame for a specific + // post-step state (by FrameId) or the latest. HistoryLock serialises the game + // thread (push) against the bridge worker thread (GetFrame). Oldest-first; + // newest is Last(). Frames are shared + const so a fetch is a refcount bump. + mutable FCriticalSection HistoryLock; + TArray> History; + // Absolute ceiling on retained frames, shared by the no-delay (fixed-capacity) + // and delay (time-windowed) eviction paths so neither can grow past the + // configured maximum. Must match the HistoryCapacity ClampMax meta above. + static constexpr int32 MaxHistoryCapacity = 64; + + // ---- Camera latency emulation state ---- + // DelayRng seeds the per-frame jitter sample; HarvestSeq tags each harvested + // frame so the streaming publish emits each delayed frame exactly once + // (LastPublishedSeq is the last Seq sent). Touched only on the game thread. + FRandomStream DelayRng; + uint64 HarvestSeq = 0; + uint64 LastPublishedSeq = 0; + + // ---- Capture-rate gating state ---- + // LastCapturedFrameId is the applied FrameId at the last capture; a capture + // fires only when it changes (bCaptureOnStateChange). LastCaptureWallSeconds + // backs the optional CaptureMaxFps cap. LastRenderedAppliedId is the applied + // id whose render the every-frame RT currently shows, used to stamp a readback + // in every-frame mode with the state its pixels actually contain (the RT lags + // the game tick by one automatic capture). + uint64 LastCapturedFrameId = 0; + double LastCaptureWallSeconds = 0.0; + uint64 LastRenderedAppliedId = 0; + double LastRenderedAppliedTime = 0.0; // ---- Streaming state ---- bool bStreamingEnabled = false; - // ---- ZMQ Worker ---- + // A camera captures only while "active": a streaming broadcast is enabled, or + // it was requested within RequestActiveTtlSeconds. LastRequestedSeconds is an + // FPlatformTime::Seconds() stamp, stored atomically so TouchRequested is + // callable from the bridge worker thread. + std::atomic LastRequestedSeconds{0.0}; + + // ---- Transports ---- FCameraZmqWorker* ZmqWorker = nullptr; FRunnableThread* WorkerThread = nullptr; - - // ---- SHM Writer ---- // Forward-declared to keep the header light; full type pulled in by the cpp. class FCameraShmWriter* ShmWriter = nullptr; }; diff --git a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraFrameBus.h b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraFrameBus.h new file mode 100644 index 00000000..7c4d429d --- /dev/null +++ b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraFrameBus.h @@ -0,0 +1,80 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +/** + * @struct FMjCameraFramePayload + * @brief One camera frame handed to out-of-core image sinks, transport-neutral. + * + * The camera owns no image-transport handle of its own beyond the ZMQ / SHM + * workers; any additional sink (e.g. an optional message-bus image publisher in + * a separate module) subscribes to FMjCameraFrameBus and receives this payload + * per published frame. Data points at the camera's frame buffer and is valid + * only for the duration of the broadcast, so a sink must copy or publish before + * returning; it must not retain the pointer. + */ +struct FMjCameraFramePayload +{ + /** Canonical "/" identity, the same string used as the ZMQ topic + * and SHM filename stem; a sink derives its topic from it. */ + FString CanonicalName; + int32 Width = 0; + int32 Height = 0; + /** true: single-channel float32 depth. false: BGRA8 colour (FColor order). */ + bool bDepth = false; + /** Tightly-packed pixel bytes (row stride == Width * bytes-per-pixel). */ + const uint8* Data = nullptr; + int32 DataNumBytes = 0; + /** Bytes per row, i.e. Width * sizeof(pixel). */ + int32 RowStrideBytes = 0; + double SimTime = 0.0; + uint64 FrameId = 0; +}; + +/** + * @class FMjCameraFrameBus + * @brief Process-wide broadcast point decoupling UMjCamera from any additional + * image sink. + * + * The camera broadcasts every published frame on OnFrameReady and announces when + * a camera stops streaming on OnStreamStopped (so a sink can release per-camera + * resources). Baseline ZMQ / SHM streaming does not use the bus; it exists so an + * optional out-of-core module can consume frames without the camera referencing + * that module's types. Both delegates fire on the game thread. + */ +class URLAB_API FMjCameraFrameBus +{ +public: + static FMjCameraFrameBus& Get(); + + DECLARE_MULTICAST_DELEGATE_OneParam(FOnFrameReady, const FMjCameraFramePayload&); + DECLARE_MULTICAST_DELEGATE_OneParam(FOnStreamStopped, const FString& /*CanonicalName*/); + + FOnFrameReady OnFrameReady; + FOnStreamStopped OnStreamStopped; + +private: + FMjCameraFrameBus() = default; +}; diff --git a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraSubsystem.h b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraSubsystem.h new file mode 100644 index 00000000..e50d37ed --- /dev/null +++ b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraSubsystem.h @@ -0,0 +1,41 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. + +#pragma once + +#include "CoreMinimal.h" +#include "Subsystems/WorldSubsystem.h" +#include "MjCameraSubsystem.generated.h" + +class UMjCamera; + +/** + * @class UMjCameraSubsystem + * @brief World subsystem that owns the active MjCameras and drives their + * per-frame capture pipeline (gating, harvest, capture, publish) in one + * pass, instead of every camera ticking itself. + * + * Cameras register on BeginPlay and unregister on EndPlay. The subsystem's Tick + * runs each registered camera's UpdateCapturePipeline once per frame. + */ +UCLASS() +class URLAB_API UMjCameraSubsystem : public UTickableWorldSubsystem +{ + GENERATED_BODY() + +public: + void RegisterCamera(UMjCamera* Camera); + void UnregisterCamera(UMjCamera* Camera); + + // UTickableWorldSubsystem + virtual void Tick(float DeltaTime) override; + virtual TStatId GetStatId() const override; + virtual bool DoesSupportWorldType(const EWorldType::Type WorldType) const override; + +private: + // Weak so a camera destroyed without a clean EndPlay can't dangle; nulls are + // pruned on the next Tick. + TArray> Cameras; +}; diff --git a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraTypes.h b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraTypes.h index 26944130..cc928473 100644 --- a/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraTypes.h +++ b/Source/URLab/Public/MuJoCo/Components/Sensors/MjCameraTypes.h @@ -42,3 +42,52 @@ enum class EMjCameraMode : uint8 // EMjCameraTrackingMode, EMjCameraProjection moved to // MuJoCo/Generated/MjArticulationEnums.h. + +/** Magic identifying a camera frame-metadata header. Stored as a uint32 that + * reads as the ASCII bytes "UCM1" in a little-endian hexdump, so the C++ and + * Python sides agree by inspection. */ +inline constexpr uint32 URLAB_CAMERA_META_MAGIC = 0x314D4355u; // 'UCM1' +// v2 appends CaptureUnixTime (Unix-epoch seconds when the frame's readback was +// requested) so a client can measure true content latency. v1 consumers that +// stop reading after Height still parse correctly (the field is appended). +inline constexpr uint32 URLAB_CAMERA_META_VERSION = 2u; + +/** + * @struct FMjCameraFrameMeta + * @brief Per-frame metadata prepended to streamed camera pixels on BOTH the + * ZMQ and SHM transports. + * + * The streaming channels (PUB socket / SHM ring) are how the client gets + * camera frames in every step mode — frames are no longer bundled into the + * step RPC reply. This header is what lets the client associate a streamed + * frame with the step that produced it (FrameId) so a "fresh" query can wait + * for FrameId >= the step's post-state id. + * + * Fixed 40-byte POD, little-endian, no padding (verified by static_assert). + * The Python consumer parses the identical layout: " GetReading() const; - virtual void BuildBinaryPayload(FBufferArchive& OutBuffer) const override; - virtual FString GetTelemetryTopicName() const override; + virtual void DescribeState(FMjArticulationState& Out) const override; /** @brief Gets the first scalar reading (index 0). */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|Runtime") From 36d9c9634e58b565029df70974354e56546280e4 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:38 +0100 Subject: [PATCH 03/32] Split the RPC dispatcher and give the bridge ownership and jobs Per-domain handler files instead of one dispatcher, long editor operations report through a job poll rather than blocking, and control is claimed and released per articulation so two clients cannot fight over one robot. --- Source/URLab/Private/Bridge/AssetCache.cpp | 302 ++ Source/URLab/Private/Bridge/BridgeServer.cpp | 311 +- .../Bridge/BridgeServerConfigUtils.cpp | 105 + .../URLab/Private/Bridge/ControlOwnership.cpp | 138 + .../URLab/Private/Bridge/InstanceRegistry.cpp | 107 + .../URLab/Private/Bridge/MsgpackHelpers.cpp | 93 +- Source/URLab/Private/Bridge/RpcDispatcher.cpp | 2527 ++--------------- .../URLab/Private/Bridge/RpcHandlerCommon.h | 22 + .../Private/Bridge/RpcHandlers_Camera.cpp | 588 ++++ .../Private/Bridge/RpcHandlers_Control.cpp | 245 ++ .../Private/Bridge/RpcHandlers_Lease.cpp | 91 + .../Bridge/RpcHandlers_ModelUpload.cpp | 636 +++++ .../Private/Bridge/RpcHandlers_Scene.cpp | 668 +++++ .../Private/Bridge/RpcHandlers_SimOptions.cpp | 586 ++++ .../URLab/Private/Bridge/RpcHandlers_Step.cpp | 1086 +++++++ Source/URLab/Public/Bridge/AssetCache.h | 86 + Source/URLab/Public/Bridge/BridgeServer.h | 88 + .../URLab/Public/Bridge/BridgeServerConfig.h | 32 + .../Public/Bridge/BridgeServerConfigUtils.h | 27 + Source/URLab/Public/Bridge/ControlOwnership.h | 98 + Source/URLab/Public/Bridge/InstanceRegistry.h | 46 + Source/URLab/Public/Bridge/MsgpackHelpers.h | 15 +- Source/URLab/Public/Bridge/RpcDispatcher.h | 217 +- Source/URLab/Public/Bridge/RpcErrorCodes.h | 55 + Source/URLab/Public/Bridge/StepCommands.h | 103 + 25 files changed, 5873 insertions(+), 2399 deletions(-) create mode 100644 Source/URLab/Private/Bridge/AssetCache.cpp create mode 100644 Source/URLab/Private/Bridge/ControlOwnership.cpp create mode 100644 Source/URLab/Private/Bridge/InstanceRegistry.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlerCommon.h create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_Camera.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_Control.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_Lease.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_ModelUpload.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_Scene.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_SimOptions.cpp create mode 100644 Source/URLab/Private/Bridge/RpcHandlers_Step.cpp create mode 100644 Source/URLab/Public/Bridge/AssetCache.h create mode 100644 Source/URLab/Public/Bridge/ControlOwnership.h create mode 100644 Source/URLab/Public/Bridge/InstanceRegistry.h create mode 100644 Source/URLab/Public/Bridge/RpcErrorCodes.h create mode 100644 Source/URLab/Public/Bridge/StepCommands.h diff --git a/Source/URLab/Private/Bridge/AssetCache.cpp b/Source/URLab/Private/Bridge/AssetCache.cpp new file mode 100644 index 00000000..5541286e --- /dev/null +++ b/Source/URLab/Private/Bridge/AssetCache.cpp @@ -0,0 +1,302 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/AssetCache.h" + +#include "HAL/FileManager.h" +#include "HAL/PlatformMisc.h" +#include "HAL/PlatformProcess.h" +#include "Misc/DateTime.h" +#include "Misc/FileHelper.h" +#include "Misc/Guid.h" +#include "Misc/Paths.h" + +namespace +{ +// --- SHA-256 (FIPS 180-4), self-contained ----------------------------------- +// UE's Core exposes SHA-1 (FSHA1) but no public SHA-256; the upload protocol is +// content-addressed on SHA-256, so a small standard implementation lives here. +// It hashes raw bytes only (no secrets), so this is a plain digest, not a MAC. +struct FSha256 +{ + uint32 State[8]; + uint64 BitLen = 0; + uint8 Block[64]; + int32 BlockLen = 0; + + FSha256() { Reset(); } + + void Reset() + { + State[0] = 0x6a09e667u; + State[1] = 0xbb67ae85u; + State[2] = 0x3c6ef372u; + State[3] = 0xa54ff53au; + State[4] = 0x510e527fu; + State[5] = 0x9b05688cu; + State[6] = 0x1f83d9abu; + State[7] = 0x5be0cd19u; + BitLen = 0; + BlockLen = 0; + } + + static uint32 Rotr(uint32 X, uint32 N) { return (X >> N) | (X << (32 - N)); } + + void Transform() + { + static const uint32 K[64] = { + 0x428a2f98u, 0x71374491u, 0xb5c0fbcfu, 0xe9b5dba5u, 0x3956c25bu, 0x59f111f1u, + 0x923f82a4u, 0xab1c5ed5u, 0xd807aa98u, 0x12835b01u, 0x243185beu, 0x550c7dc3u, + 0x72be5d74u, 0x80deb1feu, 0x9bdc06a7u, 0xc19bf174u, 0xe49b69c1u, 0xefbe4786u, + 0x0fc19dc6u, 0x240ca1ccu, 0x2de92c6fu, 0x4a7484aau, 0x5cb0a9dcu, 0x76f988dau, + 0x983e5152u, 0xa831c66du, 0xb00327c8u, 0xbf597fc7u, 0xc6e00bf3u, 0xd5a79147u, + 0x06ca6351u, 0x14292967u, 0x27b70a85u, 0x2e1b2138u, 0x4d2c6dfcu, 0x53380d13u, + 0x650a7354u, 0x766a0abbu, 0x81c2c92eu, 0x92722c85u, 0xa2bfe8a1u, 0xa81a664bu, + 0xc24b8b70u, 0xc76c51a3u, 0xd192e819u, 0xd6990624u, 0xf40e3585u, 0x106aa070u, + 0x19a4c116u, 0x1e376c08u, 0x2748774cu, 0x34b0bcb5u, 0x391c0cb3u, 0x4ed8aa4au, + 0x5b9cca4fu, 0x682e6ff3u, 0x748f82eeu, 0x78a5636fu, 0x84c87814u, 0x8cc70208u, + 0x90befffau, 0xa4506cebu, 0xbef9a3f7u, 0xc67178f2u}; + + uint32 W[64]; + for (int32 i = 0; i < 16; ++i) + { + W[i] = (uint32(Block[i * 4]) << 24) | (uint32(Block[i * 4 + 1]) << 16) + | (uint32(Block[i * 4 + 2]) << 8) | uint32(Block[i * 4 + 3]); + } + for (int32 i = 16; i < 64; ++i) + { + const uint32 S0 = Rotr(W[i - 15], 7) ^ Rotr(W[i - 15], 18) ^ (W[i - 15] >> 3); + const uint32 S1 = Rotr(W[i - 2], 17) ^ Rotr(W[i - 2], 19) ^ (W[i - 2] >> 10); + W[i] = W[i - 16] + S0 + W[i - 7] + S1; + } + + uint32 A = State[0], B = State[1], C = State[2], D = State[3]; + uint32 E = State[4], F = State[5], G = State[6], H = State[7]; + for (int32 i = 0; i < 64; ++i) + { + const uint32 S1 = Rotr(E, 6) ^ Rotr(E, 11) ^ Rotr(E, 25); + const uint32 Ch = (E & F) ^ (~E & G); + const uint32 T1 = H + S1 + Ch + K[i] + W[i]; + const uint32 S0 = Rotr(A, 2) ^ Rotr(A, 13) ^ Rotr(A, 22); + const uint32 Maj = (A & B) ^ (A & C) ^ (B & C); + const uint32 T2 = S0 + Maj; + H = G; G = F; F = E; E = D + T1; + D = C; C = B; B = A; A = T1 + T2; + } + State[0] += A; State[1] += B; State[2] += C; State[3] += D; + State[4] += E; State[5] += F; State[6] += G; State[7] += H; + } + + void Update(const uint8* Data, int32 Size) + { + for (int32 i = 0; i < Size; ++i) + { + Block[BlockLen++] = Data[i]; + if (BlockLen == 64) + { + Transform(); + BitLen += 512; + BlockLen = 0; + } + } + } + + FString Finalize() + { + const uint64 TotalBits = BitLen + uint64(BlockLen) * 8; + Block[BlockLen++] = 0x80; + if (BlockLen > 56) + { + while (BlockLen < 64) + Block[BlockLen++] = 0; + Transform(); + BlockLen = 0; + } + while (BlockLen < 56) + Block[BlockLen++] = 0; + for (int32 i = 7; i >= 0; --i) + Block[BlockLen++] = uint8((TotalBits >> (i * 8)) & 0xff); + Transform(); + + FString Hex; + Hex.Reserve(64); + const TCHAR* Digits = TEXT("0123456789abcdef"); + for (int32 i = 0; i < 8; ++i) + { + for (int32 Shift = 28; Shift >= 0; Shift -= 4) + Hex.AppendChar(Digits[(State[i] >> Shift) & 0xf]); + } + return Hex; + } +}; +} // namespace + +FURLabAssetCache::FURLabAssetCache(const FString& InRoot) + : Root(InRoot) +{ +} + +FURLabAssetCache& FURLabAssetCache::Get() +{ + static FURLabAssetCache Instance(ResolveCacheRoot()); + return Instance; +} + +FString FURLabAssetCache::ResolveCacheRoot() +{ + const FString Override = FPlatformMisc::GetEnvironmentVariable(TEXT("URLAB_ASSET_CACHE")); + if (!Override.IsEmpty()) + return Override; + +#if PLATFORM_WINDOWS + FString Base = FPlatformMisc::GetEnvironmentVariable(TEXT("LOCALAPPDATA")); + if (Base.IsEmpty()) + Base = FPlatformProcess::UserSettingsDir(); +#else + FString Base = FPlatformMisc::GetEnvironmentVariable(TEXT("XDG_CACHE_HOME")); + if (Base.IsEmpty()) + Base = FPaths::Combine(FPlatformMisc::GetEnvironmentVariable(TEXT("HOME")), TEXT(".cache")); +#endif + return FPaths::Combine(Base, TEXT("URLab"), TEXT("cache")); +} + +FString FURLabAssetCache::Sha256Hex(const uint8* Data, int32 Size) +{ + FSha256 Ctx; + if (Data && Size > 0) + Ctx.Update(Data, Size); + return Ctx.Finalize(); +} + +bool FURLabAssetCache::IsValidSha256Hex(const FString& Candidate) +{ + if (Candidate.Len() != 64) + return false; + for (const TCHAR C : Candidate) + { + const bool bHex = (C >= '0' && C <= '9') || (C >= 'a' && C <= 'f'); + if (!bHex) + return false; + } + return true; +} + +FString FURLabAssetCache::BlobPath(const FString& Sha256Hex) const +{ + return FPaths::Combine(Root, TEXT("blobs"), Sha256Hex.Left(2), Sha256Hex); +} + +bool FURLabAssetCache::Has(const FString& Sha256Hex) const +{ + if (!IsValidSha256Hex(Sha256Hex)) + return false; + return IFileManager::Get().FileExists(*BlobPath(Sha256Hex)); +} + +bool FURLabAssetCache::GetPath(const FString& Sha256Hex, FString& OutPath) const +{ + if (!Has(Sha256Hex)) + return false; + OutPath = BlobPath(Sha256Hex); + return true; +} + +bool FURLabAssetCache::Put(const FString& Sha256Hex, const uint8* Data, int32 Size) +{ + if (!IsValidSha256Hex(Sha256Hex)) + return false; + + const FString Final = BlobPath(Sha256Hex); + if (IFileManager::Get().FileExists(*Final)) + return true; // immutable content already present + + const FString Dir = FPaths::GetPath(Final); + IFileManager::Get().MakeDirectory(*Dir, /*Tree=*/true); + + // Temp file in the SAME directory so the rename is an atomic same-volume + // move. A distinct GUID per writer keeps concurrent writers of the same + // hash from clobbering each other's temp file. + const FString Temp = Final + TEXT(".") + FGuid::NewGuid().ToString(EGuidFormats::Digits) + TEXT(".tmp"); + + TArray Payload; + if (Data && Size > 0) + Payload.Append(Data, Size); + if (!FFileHelper::SaveArrayToFile(Payload, *Temp)) + return false; + + // bReplace=true: content is identical across writers, so replacing a blob a + // racing process just created is harmless and keeps this call idempotent. + if (!IFileManager::Get().Move(*Final, *Temp, /*bReplace=*/true)) + { + // A concurrent writer may have won the rename; treat an existing final + // blob as success and drop our temp copy. + IFileManager::Get().Delete(*Temp, /*RequireExists=*/false, /*EvenReadOnly=*/true); + return IFileManager::Get().FileExists(*Final); + } + return true; +} + +int64 FURLabAssetCache::EvictToBudget(int64 MaxBytes) +{ + if (MaxBytes <= 0) + { + const FString Env = FPlatformMisc::GetEnvironmentVariable(TEXT("URLAB_ASSET_CACHE_MAX_BYTES")); + MaxBytes = Env.IsEmpty() ? 0 : FCString::Atoi64(*Env); + } + if (MaxBytes <= 0) + return 0; // eviction disabled + + const FString BlobsDir = FPaths::Combine(Root, TEXT("blobs")); + + struct FBlob + { + FString Path; + int64 Size = 0; + FDateTime Modified; + }; + TArray Blobs; + int64 Total = 0; + IFileManager::Get().IterateDirectoryStatRecursively(*BlobsDir, + [&Blobs, &Total](const TCHAR* Path, const FFileStatData& Stat) { + if (!Stat.bIsDirectory && Stat.FileSize > 0) + { + Blobs.Add({Path, Stat.FileSize, Stat.ModificationTime}); + Total += Stat.FileSize; + } + return true; + }); + + if (Total <= MaxBytes) + return 0; + + Blobs.Sort([](const FBlob& A, const FBlob& B) { return A.Modified < B.Modified; }); + + int64 Freed = 0; + for (const FBlob& B : Blobs) + { + if (Total - Freed <= MaxBytes) + break; + if (IFileManager::Get().Delete(*B.Path, /*RequireExists=*/false, /*EvenReadOnly=*/true)) + Freed += B.Size; + } + return Freed; +} diff --git a/Source/URLab/Private/Bridge/BridgeServer.cpp b/Source/URLab/Private/Bridge/BridgeServer.cpp index 0ce9ab5c..54335eca 100644 --- a/Source/URLab/Private/Bridge/BridgeServer.cpp +++ b/Source/URLab/Private/Bridge/BridgeServer.cpp @@ -7,11 +7,127 @@ #include "Bridge/RpcDispatcher.h" #include "Transport/ZmqRpcTransport.h" #include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Transport/PublishTransport.h" +#include "Transport/MjExternalTransportProvider.h" #include "MuJoCo/Core/AMjManager.h" #include "Utils/URLabLogging.h" +#include "HAL/IConsoleManager.h" +#include "HAL/PlatformMisc.h" +#include "Engine/Engine.h" +#include "Misc/CommandLine.h" +#include "Misc/Parse.h" +#include "Misc/Guid.h" +#include "HAL/PlatformTime.h" + +namespace +{ +/** Parse the trailing port from a "tcp://host:port" endpoint. Returns 0 when + * no numeric port is present. */ +int32 ParseEndpointPort(const FString& Endpoint) +{ + int32 ColonIdx = INDEX_NONE; + if (Endpoint.FindLastChar(TEXT(':'), ColonIdx)) + { + const FString PortStr = Endpoint.Mid(ColonIdx + 1); + if (!PortStr.IsEmpty() && PortStr.IsNumeric()) + return FCString::Atoi(*PortStr); + } + return 0; +} + +/** Resolve the step-RPC endpoint, letting an operator override the port per + * editor instance without editing the project INI. Precedence: command-line + * `-URLabStepPort=N`, then the `URLAB_STEP_PORT` environment variable, then + * the requested endpoint unchanged. This is what lets many render-server + * editors run side by side on one host, each on its own port. */ +FString ResolveStepEndpoint(const FString& Requested) +{ + int32 OverridePort = 0; + if (!FParse::Value(FCommandLine::Get(), TEXT("URLabStepPort="), OverridePort)) + { + const FString Env = FPlatformMisc::GetEnvironmentVariable(TEXT("URLAB_STEP_PORT")); + if (!Env.IsEmpty() && Env.IsNumeric()) + OverridePort = FCString::Atoi(*Env); + } + if (OverridePort <= 0) + return Requested; + + int32 ColonIdx = INDEX_NONE; + if (Requested.FindLastChar(TEXT(':'), ColonIdx)) + return FString::Printf(TEXT("%s:%d"), *Requested.Left(ColonIdx), OverridePort); + return FString::Printf(TEXT("tcp://0.0.0.0:%d"), OverridePort); +} +} // namespace UURLabBridgeServer::UURLabBridgeServer() = default; +void UURLabBridgeServer::EnsureDispatcher() +{ + if (!Dispatcher.IsValid()) + { + Dispatcher = MakeUnique(); + Dispatcher->SetOwningBridge(this); + } +} + +void UURLabBridgeServer::ApplyPerformanceOverrides() +{ + if (bPacingOverridden || !GEngine) + return; + + auto Override = [](const TCHAR* Name, const TCHAR* Command, float& OutSaved, bool& OutHad) { + if (IConsoleVariable* CVar = IConsoleManager::Get().FindConsoleVariable(Name)) + { + OutHad = true; + OutSaved = CVar->GetFloat(); + } + GEngine->Exec(nullptr, Command); + }; + + Override(TEXT("r.VSync"), TEXT("r.VSync 0"), SavedVSync, bHadVSync); +#if WITH_EDITOR + Override(TEXT("r.VSyncEditor"), TEXT("r.VSyncEditor 0"), SavedVSyncEditor, bHadVSyncEditor); +#endif + Override(TEXT("t.MaxFPS"), TEXT("t.MaxFPS 240"), SavedMaxFPS, bHadMaxFPS); + + // Keep rendering when the editor is not the foreground window. Without this + // Slate throttles the whole app to a few FPS in the background, which starves + // camera capture/readback -- a headless RPC client then sees near-zero frame + // throughput even though the bridge is serving. + Override(TEXT("Slate.bAllowThrottling"), TEXT("Slate.bAllowThrottling 0"), + SavedSlateThrottle, bHadSlateThrottle); + Override(TEXT("t.IdleWhenNotForeground"), TEXT("t.IdleWhenNotForeground 0"), + SavedIdleWhenNotForeground, bHadIdleWhenNotForeground); + + bPacingOverridden = true; + UE_LOG(LogURLabNet, Log, + TEXT("UURLabBridgeServer: disabled editor frame pacing + background throttling " + "while serving (VSync off, MaxFPS 240, Slate throttling off)")); +} + +void UURLabBridgeServer::RestorePerformanceOverrides() +{ + if (!bPacingOverridden || !GEngine) + return; + + auto Restore = [](const TCHAR* Name, float Value, bool bHad) { + if (bHad) + GEngine->Exec(nullptr, *FString::Printf(TEXT("%s %g"), Name, Value)); + }; + + Restore(TEXT("r.VSync"), SavedVSync, bHadVSync); +#if WITH_EDITOR + Restore(TEXT("r.VSyncEditor"), SavedVSyncEditor, bHadVSyncEditor); +#endif + Restore(TEXT("t.MaxFPS"), SavedMaxFPS, bHadMaxFPS); + Restore(TEXT("Slate.bAllowThrottling"), SavedSlateThrottle, bHadSlateThrottle); + Restore(TEXT("t.IdleWhenNotForeground"), SavedIdleWhenNotForeground, bHadIdleWhenNotForeground); + + bPacingOverridden = false; + UE_LOG(LogURLabNet, Log, TEXT("UURLabBridgeServer: restored editor frame pacing")); +} + void UURLabBridgeServer::BeginDestroy() { Stop(); @@ -20,16 +136,13 @@ void UURLabBridgeServer::BeginDestroy() void UURLabBridgeServer::Start(const FString& StepEndpoint) { - if (!Dispatcher.IsValid()) - { - Dispatcher = MakeUnique(); - } + EnsureDispatcher(); // Empty endpoint: dispatcher only, no transports (test path). if (StepEndpoint.IsEmpty()) return; - EnsureZmqBound(StepEndpoint); + EnsureZmqBound(ResolveStepEndpoint(StepEndpoint)); } bool UURLabBridgeServer::EnsureZmqBound(const FString& Endpoint) @@ -37,10 +150,7 @@ bool UURLabBridgeServer::EnsureZmqBound(const FString& Endpoint) if (Endpoint.IsEmpty()) return false; - if (!Dispatcher.IsValid()) - { - Dispatcher = MakeUnique(); - } + EnsureDispatcher(); for (const TObjectPtr& T : RpcTransports) { @@ -49,7 +159,9 @@ bool UURLabBridgeServer::EnsureZmqBound(const FString& Endpoint) return true; } - UURLabZmqRpcTransport* Zmq = NewObject(this, TEXT("BridgeZmqTransport")); + // NAME_None: let UE pick a fresh unique name. A fixed name collides on + // rebind while the previous worker is still tearing down. + UURLabZmqRpcTransport* Zmq = NewObject(this, NAME_None); Zmq->StepEndpoint = Endpoint; Zmq->SetOwningBridge(this); if (!Zmq->TransportInit()) @@ -59,6 +171,7 @@ bool UURLabBridgeServer::EnsureZmqBound(const FString& Endpoint) return false; } RpcTransports.Add(Zmq); + ApplyPerformanceOverrides(); UE_LOG(LogURLabNet, Log, TEXT("UURLabBridgeServer: ZMQ REP bound at %s"), *Endpoint); return true; @@ -66,10 +179,7 @@ bool UURLabBridgeServer::EnsureZmqBound(const FString& Endpoint) bool UURLabBridgeServer::EnsureShmBound(const FString& SessionId) { - if (!Dispatcher.IsValid()) - { - Dispatcher = MakeUnique(); - } + EnsureDispatcher(); const FString Sid = SessionId.IsEmpty() ? FString(TEXT("live")) : SessionId; @@ -86,8 +196,24 @@ bool UURLabBridgeServer::EnsureShmBound(const FString& SessionId) } } - UURLabShmRpcTransport* Shm = NewObject(this, TEXT("BridgeShmTransport")); + // SHM session naming includes the instance's step port for traceability. + // SHM currently depends on ZMQ being bound first: the step port is read from + // the ZMQ transport's bound endpoint. If ZMQ is not running (e.g. a + // same-host-only deployment that skips TCP), InstancePort stays 0 and the + // session name falls back to the bare session id. + int32 StepPort = 0; + for (const TObjectPtr& T : RpcTransports) + { + if (UURLabZmqRpcTransport* Zmq = Cast(T)) + { + StepPort = ParseEndpointPort(Zmq->StepEndpoint); + break; + } + } + + UURLabShmRpcTransport* Shm = NewObject(this, NAME_None); Shm->SessionId = SessionId; // empty -> defaults to "live" inside Init + Shm->InstancePort = StepPort; Shm->SetOwningBridge(this); if (!Shm->TransportInit()) { @@ -96,11 +222,91 @@ bool UURLabBridgeServer::EnsureShmBound(const FString& SessionId) return false; } RpcTransports.Add(Shm); + ApplyPerformanceOverrides(); UE_LOG(LogURLabNet, Log, TEXT("UURLabBridgeServer: SHM RPC bound (session=%s)"), *Sid); return true; } +bool UURLabBridgeServer::EnsureExternalTransportsBound() +{ + // The concrete transports live in a separate, optional module that installs + // factory hooks at startup. Absent that module the hooks are unbound, so there + // is nothing to bind and the core names no external transport type. + if (!FMjExternalTransportProvider::HasControlRpcTransport()) + { + UE_LOG(LogURLabNet, Log, + TEXT("UURLabBridgeServer: external control transport unavailable; " + "EnsureExternalTransportsBound is a no-op.")); + return false; + } + + EnsureDispatcher(); + + // RPC / control leg: one external executor transport, persisting across PIE + // like the other RPC transports. Its TransportInit brings up the process-wide + // context; a false return means the runtime is unavailable, so there is + // nothing to bind. Identified by its transport name so the core needs no cast. + bool bHaveRpc = false; + for (const TObjectPtr& T : RpcTransports) + { + if (T && T->GetTransportName() == TEXT("ros2-rpc")) + { + bHaveRpc = true; + break; + } + } + if (!bHaveRpc) + { + UURLabRpcTransport* External = FMjExternalTransportProvider::MakeControlRpcTransport.Execute(this); + if (!External || !External->TransportInit()) + { + UE_LOG(LogURLabNet, Log, + TEXT("UURLabBridgeServer: external control runtime unavailable; " + "EnsureExternalTransportsBound is a no-op.")); + return false; + } + RpcTransports.Add(External); + ApplyPerformanceOverrides(); + UE_LOG(LogURLabNet, Log, TEXT("UURLabBridgeServer: control RPC transport bound")); + } + + // Publish / fan-out leg: registered with the live manager, which owns per-PIE + // publish transports and tears them down in EndPlay. When no manager is live + // yet the publish leg is deferred to the next call with one present. + if (AAMjManager* Manager = GetActiveManager()) + { + bool bHavePub = false; + for (const TObjectPtr& T : Manager->ManagerOwnedPublishTransports) + { + if (T && T->GetTransportName() == TEXT("ros2-pub")) + { + bHavePub = true; + break; + } + } + if (!bHavePub && FMjExternalTransportProvider::MakeStatePublishTransport.IsBound()) + { + UURLabPublishTransport* Pub = + FMjExternalTransportProvider::MakeStatePublishTransport.Execute(Manager); + if (Pub && Pub->TransportInit()) + { + Manager->ManagerOwnedPublishTransports.Add(Pub); + UE_LOG(LogURLabNet, Log, + TEXT("UURLabBridgeServer: state publish transport registered with manager")); + } + } + } + else + { + UE_LOG(LogURLabNet, Warning, + TEXT("UURLabBridgeServer: EnsureExternalTransportsBound with no active manager; " + "publish leg deferred until a manager is live.")); + } + + return true; +} + void UURLabBridgeServer::Stop() { // Drain before tearing transports down so blocking handlers see the @@ -117,6 +323,7 @@ void UURLabBridgeServer::Stop() T->TransportShutdown(); } RpcTransports.Reset(); + RestorePerformanceOverrides(); if (!Dispatcher.IsValid()) return; @@ -148,3 +355,77 @@ void UURLabBridgeServer::UnregisterManager(AAMjManager* InManager) } ActiveManager.Reset(); } + +double UURLabBridgeServer::LeaseNow() const +{ + return LeaseClockOverrideForTest >= 0.0 ? LeaseClockOverrideForTest : FPlatformTime::Seconds(); +} + +bool UURLabBridgeServer::IsLeaseHeldInternal(double NowSeconds) +{ + if (bLeaseHeld && (NowSeconds - LeaseLastActivitySeconds) > LeaseTtlSeconds) + { + // Idle past its TTL: auto-release so the next acquire succeeds. + bLeaseHeld = false; + LeaseId.Empty(); + LeaseOwner.Empty(); + } + return bLeaseHeld; +} + +bool UURLabBridgeServer::TryAcquireLease(const FString& Owner, double TtlSeconds, + FString& OutLeaseId, FString& OutExistingLeaseId) +{ + FScopeLock Lock(&LeaseMutex); + const double Now = LeaseNow(); + if (IsLeaseHeldInternal(Now)) + { + OutExistingLeaseId = LeaseId; + return false; + } + + bLeaseHeld = true; + LeaseId = FGuid::NewGuid().ToString(EGuidFormats::DigitsWithHyphens); + LeaseOwner = Owner; + LeaseTtlSeconds = TtlSeconds; + LeaseLastActivitySeconds = Now; + OutLeaseId = LeaseId; + return true; +} + +bool UURLabBridgeServer::ReleaseLease(const FString& InLeaseId) +{ + FScopeLock Lock(&LeaseMutex); + if (!bLeaseHeld || !LeaseId.Equals(InLeaseId)) + return false; + + bLeaseHeld = false; + LeaseId.Empty(); + LeaseOwner.Empty(); + return true; +} + +void UURLabBridgeServer::TouchLease() +{ + FScopeLock Lock(&LeaseMutex); + if (bLeaseHeld) + LeaseLastActivitySeconds = LeaseNow(); +} + +bool UURLabBridgeServer::IsLeaseHeld() +{ + FScopeLock Lock(&LeaseMutex); + return IsLeaseHeldInternal(LeaseNow()); +} + +FString UURLabBridgeServer::GetLeaseId() const +{ + FScopeLock Lock(&LeaseMutex); + return LeaseId; +} + +void UURLabBridgeServer::SetLeaseClockForTest(double NowSeconds) +{ + FScopeLock Lock(&LeaseMutex); + LeaseClockOverrideForTest = NowSeconds; +} diff --git a/Source/URLab/Private/Bridge/BridgeServerConfigUtils.cpp b/Source/URLab/Private/Bridge/BridgeServerConfigUtils.cpp index dacea2dd..b3734e3d 100644 --- a/Source/URLab/Private/Bridge/BridgeServerConfigUtils.cpp +++ b/Source/URLab/Private/Bridge/BridgeServerConfigUtils.cpp @@ -8,7 +8,10 @@ #include "Interfaces/IPluginManager.h" #include "Misc/ConfigCacheIni.h" #include "Misc/Paths.h" +#include "Misc/CommandLine.h" +#include "Misc/Parse.h" #include "HAL/FileManager.h" +#include "HAL/PlatformMisc.h" namespace URLabBridgeServerConfigUtils { @@ -47,6 +50,21 @@ void LoadFromIni(FURLabBridgeServerConfig& Out) Out.StepPort = TmpInt; if (File.GetInt(SectionName, TEXT("StatePort"), TmpInt)) Out.StatePort = TmpInt; + + FString TmpStr; + if (File.GetString(SectionName, TEXT("InstanceId"), TmpStr)) + Out.InstanceId = TmpStr; + if (File.GetInt(SectionName, TEXT("InstanceIndex"), TmpInt)) + Out.InstanceIndex = TmpInt; + if (File.GetInt(SectionName, TEXT("PortBase"), TmpInt)) + Out.PortBase = TmpInt; + if (File.GetInt(SectionName, TEXT("PortStride"), TmpInt)) + Out.PortStride = TmpInt; + if (File.GetInt(SectionName, TEXT("CamBasePort"), TmpInt)) + Out.CamBasePort = TmpInt; + if (File.GetString(SectionName, TEXT("BindAddress"), TmpStr)) + Out.BindAddress = TmpStr; + if (File.GetBool(SectionName, TEXT("StopOnPIEEnd"), TmpBool)) Out.bStopOnPIEEnd = TmpBool; } @@ -62,9 +80,96 @@ void SaveToIni(const FURLabBridgeServerConfig& In) File.SetString(SectionName, TEXT("AutoStart"), In.bAutoStart ? TEXT("True") : TEXT("False")); File.SetInt64(SectionName, TEXT("StepPort"), In.StepPort); File.SetInt64(SectionName, TEXT("StatePort"), In.StatePort); + File.SetString(SectionName, TEXT("InstanceId"), *In.InstanceId); + File.SetInt64(SectionName, TEXT("InstanceIndex"), In.InstanceIndex); + File.SetInt64(SectionName, TEXT("PortBase"), In.PortBase); + File.SetInt64(SectionName, TEXT("PortStride"), In.PortStride); + File.SetInt64(SectionName, TEXT("CamBasePort"), In.CamBasePort); + File.SetString(SectionName, TEXT("BindAddress"), *In.BindAddress); File.SetString(SectionName, TEXT("StopOnPIEEnd"), In.bStopOnPIEEnd ? TEXT("True") : TEXT("False")); File.Dirty = true; File.Write(Path); } + +void DerivePorts(FURLabBridgeServerConfig& Cfg, + bool bStepExplicit, bool bStateExplicit, bool bCamExplicit) +{ + if (Cfg.InstanceIndex >= 0) + { + const int32 Slot = Cfg.PortBase + Cfg.InstanceIndex * Cfg.PortStride; + if (!bStepExplicit) + Cfg.StepPort = Slot + 0; + if (!bStateExplicit) + Cfg.StatePort = Slot + 1; + if (!bCamExplicit) + Cfg.CamBasePort = Slot + 2; + } + + if (Cfg.CamBasePort == 0) + Cfg.CamBasePort = Cfg.StepPort + 2; + + if (Cfg.InstanceId.IsEmpty() && Cfg.InstanceIndex >= 0) + Cfg.InstanceId = FString::Printf(TEXT("instance_%d"), Cfg.InstanceIndex); +} + +FString BuildCameraEndpoint(const FURLabBridgeServerConfig& Cfg, int32 CameraIndex) +{ + const int32 Index = FMath::Max(0, CameraIndex); + return FString::Printf(TEXT("tcp://%s:%d"), *Cfg.BindAddress, Cfg.CamBasePort + Index); +} + +void ApplyEnvAndCommandLineOverrides(FURLabBridgeServerConfig& Cfg) +{ + // Command line beats environment; a value from either replaces the INI + // one. Returns true when a value was found so callers can flag explicit + // port overrides (which then survive derivation). + auto ResolveString = [](const TCHAR* CmdKey, const TCHAR* EnvKey, FString& Out) -> bool + { + if (FParse::Value(FCommandLine::Get(), CmdKey, Out)) + return true; + const FString Env = FPlatformMisc::GetEnvironmentVariable(EnvKey); + if (!Env.IsEmpty()) + { + Out = Env; + return true; + } + return false; + }; + auto ResolveInt = [&ResolveString](const TCHAR* CmdKey, const TCHAR* EnvKey, int32& Out) -> bool + { + FString Str; + if (!ResolveString(CmdKey, EnvKey, Str) || !Str.IsNumeric()) + return false; + Out = FCString::Atoi(*Str); + return true; + }; + + FString StrVal; + int32 IntVal = 0; + + if (ResolveString(TEXT("URLabInstanceId="), TEXT("URLAB_INSTANCE_ID"), StrVal)) + Cfg.InstanceId = StrVal; + if (ResolveInt(TEXT("URLabInstanceIndex="), TEXT("URLAB_INSTANCE_INDEX"), IntVal)) + Cfg.InstanceIndex = IntVal; + if (ResolveInt(TEXT("URLabPortBase="), TEXT("URLAB_PORT_BASE"), IntVal)) + Cfg.PortBase = IntVal; + if (ResolveInt(TEXT("URLabPortStride="), TEXT("URLAB_PORT_STRIDE"), IntVal)) + Cfg.PortStride = IntVal; + + const bool bStepExplicit = ResolveInt(TEXT("URLabStepPort="), TEXT("URLAB_STEP_PORT"), IntVal); + if (bStepExplicit) + Cfg.StepPort = IntVal; + const bool bStateExplicit = ResolveInt(TEXT("URLabStatePort="), TEXT("URLAB_STATE_PORT"), IntVal); + if (bStateExplicit) + Cfg.StatePort = IntVal; + const bool bCamExplicit = ResolveInt(TEXT("URLabCamBasePort="), TEXT("URLAB_CAM_BASE_PORT"), IntVal); + if (bCamExplicit) + Cfg.CamBasePort = IntVal; + + if (ResolveString(TEXT("URLabBindAddress="), TEXT("URLAB_BIND_ADDRESS"), StrVal)) + Cfg.BindAddress = StrVal; + + DerivePorts(Cfg, bStepExplicit, bStateExplicit, bCamExplicit); +} } // namespace URLabBridgeServerConfigUtils diff --git a/Source/URLab/Private/Bridge/ControlOwnership.cpp b/Source/URLab/Private/Bridge/ControlOwnership.cpp new file mode 100644 index 00000000..42ae50df --- /dev/null +++ b/Source/URLab/Private/Bridge/ControlOwnership.cpp @@ -0,0 +1,138 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/ControlOwnership.h" +#include "HAL/PlatformTime.h" + +double FMjControlOwnership::Now() const +{ + return ClockOverrideForTest >= 0.0 ? ClockOverrideForTest : FPlatformTime::Seconds(); +} + +bool FMjControlOwnership::IsExpired(const FMjControlClaim& Claim, double NowSeconds) +{ + return Claim.TtlSeconds > 0.0 && (NowSeconds - Claim.LastActivitySeconds) > Claim.TtlSeconds; +} + +FMjControlOwnership::EClaimResult FMjControlOwnership::Claim(FName Art, const FString& Source, + double TtlSeconds, bool bForce, FString& OutCurrentOwner) +{ + FScopeLock Lock(&Mutex); + const double N = Now(); + + if (FMjControlClaim* Existing = Claims.Find(Art)) + { + // An expired claim is free for the taking; a live one held by someone + // else blocks unless the caller forces the steal. + if (!IsExpired(*Existing, N) && !bForce && !Existing->Owner.Equals(Source)) + { + OutCurrentOwner = Existing->Owner; + return EClaimResult::AlreadyOwned; + } + } + + FMjControlClaim& Held = Claims.FindOrAdd(Art); + Held.Owner = Source; + Held.TtlSeconds = TtlSeconds; + Held.LastActivitySeconds = N; + OutCurrentOwner = Source; + return EClaimResult::Ok; +} + +bool FMjControlOwnership::Release(FName Art, const FString& Source) +{ + FScopeLock Lock(&Mutex); + const double N = Now(); + + FMjControlClaim* Existing = Claims.Find(Art); + if (!Existing) + return false; + if (IsExpired(*Existing, N)) + { + Claims.Remove(Art); + return false; + } + if (!Existing->Owner.Equals(Source)) + return false; + + Claims.Remove(Art); + return true; +} + +FMjControlOwnership::EWriteCheck FMjControlOwnership::CheckWrite(FName Art, const FString& Source, + FString& OutCurrentOwner) +{ + FScopeLock Lock(&Mutex); + const double N = Now(); + + FMjControlClaim* Existing = Claims.Find(Art); + if (Existing && IsExpired(*Existing, N)) + { + Claims.Remove(Art); + Existing = nullptr; + } + + if (!Existing) + { + OutCurrentOwner = TEXT("(unclaimed)"); + return EWriteCheck::NotOwner; + } + if (!Existing->Owner.Equals(Source)) + { + OutCurrentOwner = Existing->Owner; + return EWriteCheck::NotOwner; + } + + Existing->LastActivitySeconds = N; + OutCurrentOwner = Existing->Owner; + return EWriteCheck::Ok; +} + +TMap FMjControlOwnership::GetActiveOwners() +{ + FScopeLock Lock(&Mutex); + const double N = Now(); + + TMap Owners; + for (auto It = Claims.CreateIterator(); It; ++It) + { + if (IsExpired(It->Value, N)) + { + It.RemoveCurrent(); + continue; + } + Owners.Add(It->Key, It->Value.Owner); + } + return Owners; +} + +void FMjControlOwnership::Reset() +{ + FScopeLock Lock(&Mutex); + Claims.Empty(); +} + +void FMjControlOwnership::SetClockOverrideForTest(double NowSeconds) +{ + FScopeLock Lock(&Mutex); + ClockOverrideForTest = NowSeconds; +} diff --git a/Source/URLab/Private/Bridge/InstanceRegistry.cpp b/Source/URLab/Private/Bridge/InstanceRegistry.cpp new file mode 100644 index 00000000..1fab5c3a --- /dev/null +++ b/Source/URLab/Private/Bridge/InstanceRegistry.cpp @@ -0,0 +1,107 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. + +#include "Bridge/InstanceRegistry.h" +#include "Bridge/BridgeServerConfig.h" + +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" +#include "Serialization/JsonSerializer.h" +#include "Serialization/JsonWriter.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.h" +#include "Misc/DateTime.h" +#include "HAL/FileManager.h" +#include "HAL/PlatformProcess.h" +#include "HAL/PlatformMisc.h" + +namespace +{ +/** Instance id used for the file name and the JSON, falling back to the SHM + * default so a single-editor entry stays readable. */ +FString EffectiveInstanceId(const FURLabBridgeServerConfig& Cfg) +{ + return Cfg.InstanceId.IsEmpty() ? FString(TEXT("live")) : Cfg.InstanceId; +} +} // namespace + +const TArray& FURLabInstanceRegistry::Capabilities() +{ + static const TArray Caps = { + TEXT("render_sync"), + TEXT("render_async"), + TEXT("shm_rpc"), + TEXT("model_upload"), + TEXT("content_cache"), + }; + return Caps; +} + +FString FURLabInstanceRegistry::ResolveRegistryDir() +{ + const FString Override = FPlatformMisc::GetEnvironmentVariable(TEXT("URLAB_REGISTRY_DIR")); + if (!Override.IsEmpty()) + return Override; + +#if PLATFORM_WINDOWS + FString Base = FPlatformMisc::GetEnvironmentVariable(TEXT("LOCALAPPDATA")); + if (Base.IsEmpty()) + Base = FPlatformProcess::UserSettingsDir(); +#else + FString Base = FPlatformMisc::GetEnvironmentVariable(TEXT("XDG_CACHE_HOME")); + if (Base.IsEmpty()) + Base = FPaths::Combine(FPlatformMisc::GetEnvironmentVariable(TEXT("HOME")), TEXT(".cache")); +#endif + return FPaths::Combine(Base, TEXT("URLab"), TEXT("registry")); +} + +FString FURLabInstanceRegistry::ResolveEntryPath(const FURLabBridgeServerConfig& Cfg) +{ + const FString FileName = FString::Printf(TEXT("%s_%u.json"), + *EffectiveInstanceId(Cfg), FPlatformProcess::GetCurrentProcessId()); + return FPaths::Combine(ResolveRegistryDir(), FileName); +} + +void FURLabInstanceRegistry::WriteEntry(const FURLabBridgeServerConfig& Cfg, + const FString& UrlabVersion, bool bManagerPresent, bool bBusy) +{ + TSharedPtr Entry = MakeShared(); + Entry->SetStringField(TEXT("instance_id"), EffectiveInstanceId(Cfg)); + Entry->SetNumberField(TEXT("index"), Cfg.InstanceIndex); + Entry->SetNumberField(TEXT("pid"), static_cast(FPlatformProcess::GetCurrentProcessId())); + Entry->SetStringField(TEXT("host"), FPlatformProcess::ComputerName()); + Entry->SetNumberField(TEXT("step_port"), Cfg.StepPort); + Entry->SetNumberField(TEXT("state_port"), Cfg.StatePort); + Entry->SetNumberField(TEXT("cam_base_port"), Cfg.CamBasePort); + Entry->SetBoolField(TEXT("manager_present"), bManagerPresent); + Entry->SetBoolField(TEXT("busy"), bBusy); + Entry->SetStringField(TEXT("urlab_version"), UrlabVersion); + + TArray> Caps; + for (const FString& Cap : Capabilities()) + Caps.Add(MakeShared(Cap)); + Entry->SetArrayField(TEXT("capabilities"), Caps); + + Entry->SetStringField(TEXT("registry_written_at"), FDateTime::UtcNow().ToIso8601()); + + FString Serialized; + TSharedRef> Writer = TJsonWriterFactory<>::Create(&Serialized); + FJsonSerializer::Serialize(Entry.ToSharedRef(), Writer); + + const FString Path = ResolveEntryPath(Cfg); + IFileManager::Get().MakeDirectory(*FPaths::GetPath(Path), /*Tree=*/true); + FFileHelper::SaveStringToFile(Serialized, *Path); +} + +void FURLabInstanceRegistry::RefreshEntry(const FURLabBridgeServerConfig& Cfg, + const FString& UrlabVersion, bool bManagerPresent, bool bBusy) +{ + WriteEntry(Cfg, UrlabVersion, bManagerPresent, bBusy); +} + +void FURLabInstanceRegistry::RemoveEntry(const FURLabBridgeServerConfig& Cfg) +{ + IFileManager::Get().Delete(*ResolveEntryPath(Cfg), /*RequireExists=*/false, /*EvenReadOnly=*/true); +} diff --git a/Source/URLab/Private/Bridge/MsgpackHelpers.cpp b/Source/URLab/Private/Bridge/MsgpackHelpers.cpp index 117dbd55..ac7fda35 100644 --- a/Source/URLab/Private/Bridge/MsgpackHelpers.cpp +++ b/Source/URLab/Private/Bridge/MsgpackHelpers.cpp @@ -25,6 +25,7 @@ #include "Dom/JsonObject.h" #include "Dom/JsonValue.h" #include "Misc/Base64.h" +#include "Utils/URLabLogging.h" // rpclib's msgpack-cxx headers use member functions and templates named // `check`, which collides with UE's `check(cond)` assertion macro. Save @@ -50,6 +51,37 @@ namespace constexpr const TCHAR* kBinSuffix = TEXT("__b64__"); constexpr int32 kBinSuffixLen = 7; +// FJsonValue carrying raw bytes to pack straight through as msgpack `bin`, with +// no base64 encode/decode round trip and no intermediate FString. It holds a +// Keeper that owns the underlying buffer so the bytes stay valid until the reply +// is packed. It is always stored under a kBinSuffix key, and its Type is set to +// EJson::Null so PackJsonObjectInner can tell it apart from the legacy base64 +// string form (a msgpack->JSON round trip yields EJson::String there, and no +// code ever stores a plain null under a __b64__ key). EJson::Null also serialises +// safely if the reply ever goes out as JSON: the __b64__ field is the +// msgpack-canonical form and JSON clients read the parallel *_base64 field. +class FURLabJsonValueBinary : public FJsonValue +{ +public: + FURLabJsonValueBinary(const uint8* InData, int32 InSize, + TSharedPtr InKeeper) + : Data(InData), Size(InSize), Keeper(MoveTemp(InKeeper)) + { + Type = EJson::Null; + } + + const uint8* GetBinaryData() const { return Data; } + int32 GetBinarySize() const { return Size; } + +protected: + virtual FString GetType() const override { return TEXT("URLabBinary"); } + +private: + const uint8* Data = nullptr; + int32 Size = 0; + TSharedPtr Keeper; +}; + template void PackString(clmdep_msgpack::packer& Packer, const FString& S) { @@ -74,24 +106,39 @@ void PackJsonObjectInner(clmdep_msgpack::packer& Packer, for (const auto& Kv : Obj->Values) { const FString& Key = Kv.Key; - // Special-case keys with kBinSuffix: emit real msgpack bin rather - // than the base64-as-string form the JSON tree carries. + // Keys with kBinSuffix emit real msgpack bin (the suffix is stripped from + // the wire field name). The value is either our raw-binary carrier + // (EJson::Null, bytes packed directly) or a legacy base64 string (from a + // msgpack->JSON round trip, decoded back to bytes). if (Key.EndsWith(kBinSuffix, ESearchCase::CaseSensitive)) { - FString StrippedKey = Key.LeftChop(kBinSuffixLen); - PackString(Packer, StrippedKey); - FString Encoded; - if (Kv.Value.IsValid() && Kv.Value->TryGetString(Encoded)) + PackString(Packer, Key.LeftChop(kBinSuffixLen)); + + const FJsonValue* Val = Kv.Value.Get(); + if (Val && Val->Type == EJson::Null) { - TArray Decoded; - FBase64::Decode(Encoded, Decoded); - Packer.pack_bin(Decoded.Num()); - if (Decoded.Num() > 0) - Packer.pack_bin_body(reinterpret_cast(Decoded.GetData()), Decoded.Num()); + // Raw bytes already in hand: pack directly, no base64 round trip. + const FURLabJsonValueBinary* Bin = static_cast(Val); + const int32 N = Bin->GetBinarySize(); + Packer.pack_bin(N); + if (N > 0 && Bin->GetBinaryData()) + Packer.pack_bin_body(reinterpret_cast(Bin->GetBinaryData()), N); } else { - Packer.pack_bin(0); + FString Encoded; + if (Val && Kv.Value->TryGetString(Encoded)) + { + TArray Decoded; + FBase64::Decode(Encoded, Decoded); + Packer.pack_bin(Decoded.Num()); + if (Decoded.Num() > 0) + Packer.pack_bin_body(reinterpret_cast(Decoded.GetData()), Decoded.Num()); + } + else + { + Packer.pack_bin(0); + } } continue; } @@ -164,11 +211,24 @@ void FURLabMsgpackUtil::SetBinaryField(TSharedPtr& Obj, const FStri { if (!Obj.IsValid()) return; - FString Encoded; + // Own a single copy of the bytes so the caller's buffer need not outlive the + // reply, then pack it straight through as msgpack bin (no base64). The __b64__ + // key suffix tells PackJsonObjectInner to strip the suffix and emit bin. + TSharedRef, ESPMode::ThreadSafe> Owned = + MakeShared, ESPMode::ThreadSafe>(); if (Size > 0 && Data) - Encoded = FBase64::Encode(Data, Size); - // The packer recognises the __b64__ suffix and converts back to msgpack bin. - Obj->SetStringField(Field + kBinSuffix, Encoded); + Owned->Append(Data, Size); + Obj->SetField(Field + kBinSuffix, + MakeShared(Owned->GetData(), Owned->Num(), Owned)); +} + +void FURLabMsgpackUtil::SetBinaryFieldShared(TSharedPtr& Obj, const FString& Field, + const uint8* Data, int32 Size, TSharedPtr Keeper) +{ + if (!Obj.IsValid()) + return; + Obj->SetField(Field + kBinSuffix, + MakeShared(Data, Size, MoveTemp(Keeper))); } // ============================================================================= @@ -274,6 +334,7 @@ bool FURLabMsgpackUtil::UnpackToJsonObject(const uint8* Data, int32 Size, } catch (...) { + UE_LOG(LogURLab, Warning, TEXT("MsgpackHelpers: parse failure in UnpackToJsonObject")); return false; } } diff --git a/Source/URLab/Private/Bridge/RpcDispatcher.cpp b/Source/URLab/Private/Bridge/RpcDispatcher.cpp index 9758d6f8..c81ae2cf 100644 --- a/Source/URLab/Private/Bridge/RpcDispatcher.cpp +++ b/Source/URLab/Private/Bridge/RpcDispatcher.cpp @@ -21,11 +21,13 @@ // CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. #include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" #include "Bridge/OpRegistry.h" -#include "Transport/ZmqRpcTransport.h" +#include "Bridge/StepCommands.h" #include "Bridge/MsgpackHelpers.h" #include "MuJoCo/Core/AMjManager.h" #include "MuJoCo/Core/MjArticulation.h" +#include "State/MjCanonicalName.h" #include "MuJoCo/Components/Actuators/MjActuator.h" #include "MuJoCo/Components/Sensors/MjSensor.h" #include "MuJoCo/Components/Sensors/MjCamera.h" @@ -36,6 +38,13 @@ #include "MuJoCo/Input/MjTwistController.h" #include "Transport/NetworkManager.h" #include "Transport/ShmPublishTransport.h" +#include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/BridgeServerConfig.h" +#include "Bridge/InstanceRegistry.h" +#include "HAL/PlatformProcess.h" +#include "Transport/ShmRegion.h" // FMjShmHeader (header_size in shm_rpc block) #include "Replay/MjReplayManager.h" #include "Kismet/GameplayStatics.h" #include "Misc/Base64.h" @@ -50,48 +59,6 @@ namespace { -/** Map enum to wire-format string, matching the Python StepMode enum values. */ -FString StepModeToString(EStepMode Mode) -{ - switch (Mode) - { - case EStepMode::Live: - return TEXT("live"); - case EStepMode::Direct: - return TEXT("direct"); - case EStepMode::Puppet: - return TEXT("puppet"); - case EStepMode::Auto: - return TEXT("auto"); - } - return TEXT("live"); -} - -bool StepModeFromString(const FString& Str, EStepMode& OutMode) -{ - if (Str.Equals(TEXT("live"), ESearchCase::IgnoreCase) || Str.Equals(TEXT("streaming"), ESearchCase::IgnoreCase)) - { - OutMode = EStepMode::Live; - return true; - } - if (Str.Equals(TEXT("direct"), ESearchCase::IgnoreCase)) - { - OutMode = EStepMode::Direct; - return true; - } - if (Str.Equals(TEXT("puppet"), ESearchCase::IgnoreCase)) - { - OutMode = EStepMode::Puppet; - return true; - } - if (Str.Equals(TEXT("auto"), ESearchCase::IgnoreCase)) - { - OutMode = EStepMode::Auto; - return true; - } - return false; -} - /** Map URLab actuator enum -> wire-format string. */ FString ActuatorTypeToString(EMjActuatorType T) { @@ -119,25 +86,23 @@ FString ActuatorTypeToString(EMjActuatorType T) return TEXT("motor"); } -/** Resolve a save / load path. Bare filename -> /Saved/URLab/Replays/. - * Must match AMjReplayManager::SaveRecordingToFile so the - * recording_save_ok absolute_path actually resolves on the bridge. */ -FString ResolveReplayPath(const FString& UserPath, const FString& DefaultBaseName = TEXT("")) -{ - FString BaseDir = FPaths::ProjectSavedDir() / TEXT("URLab") / TEXT("Replays"); - IFileManager::Get().MakeDirectory(*BaseDir, true); - - FString Path = UserPath; - if (Path.IsEmpty()) - { - Path = DefaultBaseName.IsEmpty() ? TEXT("recording.json") : DefaultBaseName; - } - - if (FPaths::IsRelative(Path)) - { - Path = FPaths::Combine(BaseDir, Path); - } - return FPaths::ConvertRelativePathToFull(Path); +/** Ops that do NOT count as lease-owner activity: discovery, bootstrap, and + * lifecycle/status polls that a non-owner (e.g. a pool client probing for a + * free instance) issues to inspect an instance. Refreshing the lease on these + * would let steady discovery polling keep a held lease alive forever and + * defeat TTL auto-release. Real owner activity (step / reset / forward / set_* + * and every other state-advancing op) still refreshes. */ +bool OpRefreshesLease(const FString& Op) +{ + static const TSet NonActivityOps = { + TEXT("hello"), + TEXT("meta"), + TEXT("pie_status"), + TEXT("op_status"), + TEXT("acquire_lease"), + TEXT("release_lease"), + }; + return !NonActivityOps.Contains(Op); } } // namespace @@ -192,6 +157,14 @@ void FURLabRpcDispatcher::RegisterDispatcherOps() [this](auto& R) { return HandleSetPaused(R); }, /*Reply=*/{TEXT("op:string"), TEXT("paused:bool")}, /*Required=*/{TEXT("paused")}); + Reg(TEXT("set_camera_streaming"), EOpCategory::ManagerRequired, TEXT("runtime"), + [this](auto& R) { return HandleSetCameraStreaming(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("cameras:object")}, + /*Required=*/{TEXT("cameras")}); + Reg(TEXT("set_camera_delay"), EOpCategory::ManagerRequired, TEXT("runtime"), + [this](auto& R) { return HandleSetCameraDelay(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("cameras:object")}, + /*Required=*/{TEXT("cameras")}); Reg(TEXT("configure_controller"), EOpCategory::ManagerRequired, TEXT("runtime"), [this](auto& R) { return HandleConfigureController(R); }, /*Reply=*/{TEXT("op:string"), TEXT("articulation:string"), TEXT("params:object")}, @@ -206,9 +179,20 @@ void FURLabRpcDispatcher::RegisterDispatcherOps() Reg(TEXT("set_control_source"), EOpCategory::ManagerRequired, TEXT("runtime"), [this](auto& R) { return HandleSetControlSource(R); }, {TEXT("op:string")}); + Reg(TEXT("claim_control"), EOpCategory::ManagerRequired, TEXT("runtime"), + [this](auto& R) { return HandleClaimControl(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("articulation:string"), TEXT("owner:string"), TEXT("ttl_s:float")}, + /*Required=*/{TEXT("articulation")}); + Reg(TEXT("release_control"), EOpCategory::ManagerRequired, TEXT("runtime"), + [this](auto& R) { return HandleReleaseControl(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("articulation:string")}, + /*Required=*/{TEXT("articulation")}); Reg(TEXT("set_twist"), EOpCategory::ManagerRequired, TEXT("runtime"), [this](auto& R) { return HandleSetTwist(R); }, {TEXT("op:string")}); + Reg(TEXT("set_user_channels"), EOpCategory::ManagerRequired, TEXT("runtime"), + [this](auto& R) { return HandleSetUserChannels(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("applied:int"), TEXT("rejected:object")}); Reg(TEXT("set_qpos"), EOpCategory::ManagerRequired, TEXT("runtime"), [this](auto& R) { return HandleSetQpos(R); }, /*Reply=*/{TEXT("op:string"), TEXT("target:string"), TEXT("actor_id:string?"), TEXT("actor_name:string?"), TEXT("qpos:array"), TEXT("free_base_shortcut:bool")}, @@ -228,6 +212,36 @@ void FURLabRpcDispatcher::RegisterDispatcherOps() [this](auto& R) { return HandleListKeyframes(R); }, {TEXT("op:string"), TEXT("keyframes:array")}); + // Cooperative render-farm lease. No-manager: a pool client claims the + // process itself, whether or not a scene is loaded. + Reg(TEXT("acquire_lease"), EOpCategory::NoManager, TEXT("farm"), + [this](auto& R) { return HandleAcquireLease(R); }, + {TEXT("op:string"), TEXT("lease_id:string"), TEXT("ttl_s:float")}); + // lease_id is validated in the handler (not declaratively) so a missing + // value returns `bad_request` per the schema, not the generic + // `missing_field`. + Reg(TEXT("release_lease"), EOpCategory::NoManager, TEXT("farm"), + [this](auto& R) { return HandleReleaseLease(R); }, + /*Reply=*/{TEXT("op:string")}); + + // Network model upload (RpcHandlers_ModelUpload.cpp). Manifest + chunk are + // pure data staging (no manager, no editor). Commit drives the existing + // import_xml editor job on a materialised temp dir; it self-checks for the + // editor import handler, so it stays NoManager and returns `import_failed` + // when the editor module isn't loaded. + Reg(TEXT("upload_model_manifest"), EOpCategory::NoManager, TEXT("scene"), + [this](auto& R) { return HandleUploadModelManifest(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("upload_id:string"), TEXT("need_xml:bool"), TEXT("need_assets:array"), TEXT("max_asset_bytes:int"), TEXT("max_total_bytes:int")}, + /*Required=*/{TEXT("xml_sha256")}); + Reg(TEXT("upload_model_chunk"), EOpCategory::NoManager, TEXT("scene"), + [this](auto& R) { return HandleUploadModelChunk(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("name:string"), TEXT("received:int"), TEXT("complete:bool")}, + /*Required=*/{TEXT("upload_id"), TEXT("kind")}); + Reg(TEXT("upload_model_commit"), EOpCategory::NoManager, TEXT("scene"), + [this](auto& R) { return HandleUploadModelCommit(R); }, + /*Reply=*/{TEXT("op:string"), TEXT("imported:bool"), TEXT("nq:int?"), TEXT("nv:int?"), TEXT("nu:int?"), TEXT("nbody:int?"), TEXT("ngeom:int?"), TEXT("mjb:object?"), TEXT("warnings:array")}, + /*Required=*/{TEXT("upload_id")}); + auto RecBody = [this](auto& R) { FString OpName; R->TryGetStringField(TEXT("op"), OpName); @@ -272,18 +286,17 @@ void FURLabRpcDispatcher::Init(AAMjManager* InManager) const EStepMode InitMode = (OwnerMgr->StepMode == EStepMode::Auto) ? EStepMode::Live : OwnerMgr->StepMode; - ActiveStepMode.store(InitMode, std::memory_order_release); - // Mirror onto the manager so the physics loop paces off the resolved mode - // (Auto -> Live), not the configured StepMode which would stay Auto. - OwnerMgr->EffectiveStepMode.store(InitMode, std::memory_order_release); - const bool bPaused = (InitMode != EStepMode::Live); - OwnerMgr->bPublishersPaused.store(bPaused, std::memory_order_release); - FCameraZmqWorker::bPublishersPaused.store(bPaused, std::memory_order_release); - - if (InitMode == EStepMode::Puppet) - InstallPuppetHandler(); - else if (InitMode == EStepMode::Direct) - InstallDirectHandler(); + { + // Serialise strategy construction + OnEnter against a concurrent + // set_mode / OnManagerGone (PIE-end) touching the same members. + FScopeLock Lock(&DispatchMutex); + ActiveStepMode.store(InitMode, std::memory_order_release); + // Camera publishers stream in every mode; the strategy's OnEnter handles + // the state/ctrl publishers, the engine step mode, and the handler install. + FCameraZmqWorker::bPublishersPaused.store(false, std::memory_order_release); + CurrentStepStrategy = MakeStepStrategy(InitMode); + CurrentStepStrategy->OnEnter(*this, *OwnerMgr); + } // Cached on the game thread; worker threads later use Get() (TActorIterator // asserts IsInGameThread). @@ -297,13 +310,23 @@ void FURLabRpcDispatcher::Init(AAMjManager* InManager) void FURLabRpcDispatcher::OnManagerGone() { - UninstallPuppetHandler(); + // Serialise handler teardown + strategy reset against a concurrent set_mode + // racing PIE-end (would otherwise be a UAF on CurrentStepStrategy / the + // handler flags). + FScopeLock Lock(&DispatchMutex); + UninstallDirectHandler(); - DrainQueuesForTest(); + CurrentStepStrategy.Reset(); + DrainQueues(); + + // Claims are per-PIE: the articulations die with the world. + ControlOwnership.Reset(); if (OwnerMgr.IsValid()) { OwnerMgr->bPublishersPaused.store(false, std::memory_order_release); + if (OwnerMgr->PhysicsEngine) + OwnerMgr->PhysicsEngine->SetStepMode(EStepMode::Live); } FCameraZmqWorker::bPublishersPaused.store(false, std::memory_order_release); OwnerMgr.Reset(); @@ -340,6 +363,13 @@ void FURLabRpcDispatcher::SetCachedReplayManager(AMjReplayManager* RM) CachedReplayManager = RM; } +// Out-of-line so the TWeakObjectPtr assignment sees the full UURLabBridgeServer +// type (the header only forward-declares it to avoid an include cycle). +void FURLabRpcDispatcher::SetOwningBridge(UURLabBridgeServer* InBridge) +{ + OwningBridge = InBridge; +} + void FURLabRpcDispatcher::EnqueueStepRequestForTest(FMjStepRequest&& Req) { TSharedPtr Cmd = MakeShared(); @@ -354,20 +384,18 @@ void FURLabRpcDispatcher::EnqueueStepRequestForTest(FMjStepRequest&& Req) } } -void FURLabRpcDispatcher::EnqueuePushStateRequestForTest(FMjPushStateRequest&& Req) -{ - PushStateQueue.Enqueue(MoveTemp(Req)); -} - -void FURLabRpcDispatcher::DrainQueuesForTest() +void FURLabRpcDispatcher::DrainQueues() { TSharedPtr Cmd; while (StepQueue.Dequeue(Cmd)) { - } // shared_ptr deallocates on scope exit - FMjPushStateRequest P; - while (PushStateQueue.Dequeue(P)) - { + if (!Cmd.IsValid()) + continue; + // Abandon + wake any RPC thread blocked on this command so a mode switch + // / PIE-end returns immediately instead of eating the 5s step deadline. + Cmd->bAbandoned.store(true, std::memory_order_release); + if (Cmd->Completion) + Cmd->Completion->Trigger(); } } @@ -410,16 +438,28 @@ TSharedPtr FURLabRpcDispatcher::DispatchInternal(const TSharedPtrTryGetStringField(TEXT("op"), Op)) { - return MakeError(TEXT("missing_op"), TEXT("Request missing 'op' field")); + return MakeError(URLabError::MissingOp, TEXT("Request missing 'op' field")); + } + + // Only real owner activity refreshes the lease. Discovery / bootstrap / + // status-poll ops (see OpRefreshesLease) must NOT, or a pool client's + // steady `hello`/status polling would keep a held lease alive forever and + // defeat TTL auto-release. No-op when no lease is held. + if (OpRefreshesLease(Op)) + { + if (UURLabBridgeServer* Bridge = OwningBridge.Get()) + Bridge->TouchLease(); } - // hello / meta are pre-session bootstrap endpoints. + // hello / meta are pre-session ops that run before a client has a + // session_id, so they bypass the registry. Both validate fields inline + // (all hello fields are optional with sensible defaults; meta takes none). if (Op.Equals(TEXT("hello"))) return HandleHello(Req); if (Op.Equals(TEXT("meta"))) @@ -431,7 +471,7 @@ TSharedPtr FURLabRpcDispatcher::DispatchInternal(const TSharedPtrTryGetStringField(TEXT("session_id"), SessionId); if (!ValidateSession(SessionId)) { - return MakeError(TEXT("session_expired"), + return MakeError(URLabError::SessionExpired, FString::Printf(TEXT("Session id '%s' does not match active session"), *SessionId)); } } @@ -441,10 +481,10 @@ TSharedPtr FURLabRpcDispatcher::DispatchInternal(const TSharedPtr FURLabRpcDispatcher::DispatchInternal(const TSharedPtrHasField(Field)) { - return MakeError(TEXT("missing_field"), + return MakeError(URLabError::MissingField, FString::Printf(TEXT("op '%s' missing required field '%s'"), *Op, *Field)); } @@ -462,7 +502,7 @@ TSharedPtr FURLabRpcDispatcher::DispatchInternal(const TSharedPtrCategory == URLabOpRegistry::EOpCategory::ManagerRequired && !OwnerMgr.IsValid()) { - return MakeError(TEXT("no_active_manager"), + return MakeError(URLabError::NoActiveManager, FString::Printf( TEXT("op '%s' requires an active AAMjManager (PIE not running?)"), *Op)); @@ -520,26 +560,6 @@ TSharedPtr FURLabRpcDispatcher::HandleMeta(const TSharedPtr FURLabRpcDispatcher::HandleSetPaused(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - - bool bPause = false; - if (!Req->TryGetBoolField(TEXT("paused"), bPause)) - return MakeError(TEXT("missing_field"), TEXT("set_paused requires 'paused' bool")); - - Mgr->PhysicsEngine->SetPaused(bPause); - UE_LOG(LogURLabNet, Log, TEXT("FURLabRpcDispatcher: set_paused -> %s"), - bPause ? TEXT("true") : TEXT("false")); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_paused_ok")); - Reply->SetBoolField(TEXT("paused"), Mgr->PhysicsEngine->bIsPaused); - return Reply; -} - TSharedPtr FURLabRpcDispatcher::MakeError(const FString& Code, const FString& Message) { TSharedPtr Err = MakeShared(); @@ -596,6 +616,12 @@ TSharedPtr FURLabRpcDispatcher::HandleHello(const TSharedPtrTryGetStringField(TEXT("client_version"), ClientVersion); + UE_LOG(LogURLabNet, Log, TEXT("URLab client connected: %s (server %s)"), + *ClientVersion, *URLabVersion); + // Reset to msgpack default on each handshake; per-session opt-in via encoding=json. bUseJsonEncoding.store(false, std::memory_order_release); FString Encoding; @@ -641,6 +667,34 @@ TSharedPtr FURLabRpcDispatcher::BuildHandshakePayload(AAMjManager* const bool bManagerPresent = (Manager && Manager->PhysicsEngine); Reply->SetBoolField(TEXT("manager_present"), bManagerPresent); + // Per-instance identity for render-farm discovery. Values come from the + // bridge server's resolved config (env / command line / INI), so a client + // learns exactly which instance answered and on which ports. + if (Manager && Manager->BridgeServer) + { + const FURLabBridgeServerConfig& Cfg = Manager->BridgeServer->GetInstanceConfig(); + TSharedPtr Instance = MakeShared(); + Instance->SetStringField(TEXT("instance_id"), + Cfg.InstanceId.IsEmpty() ? FString(TEXT("live")) : Cfg.InstanceId); + Instance->SetNumberField(TEXT("index"), Cfg.InstanceIndex); + Instance->SetNumberField(TEXT("pid"), + static_cast(FPlatformProcess::GetCurrentProcessId())); + Instance->SetStringField(TEXT("host"), FPlatformProcess::ComputerName()); + Instance->SetNumberField(TEXT("step_port"), Cfg.StepPort); + Instance->SetNumberField(TEXT("state_port"), Cfg.StatePort); + Instance->SetNumberField(TEXT("cam_base_port"), Cfg.CamBasePort); + Instance->SetBoolField(TEXT("manager_present"), bManagerPresent); + Instance->SetBoolField(TEXT("busy"), Manager->BridgeServer->IsLeaseHeld()); + Instance->SetStringField(TEXT("urlab_version"), URLabVer); + + TArray> Capabilities; + for (const FString& Cap : FURLabInstanceRegistry::Capabilities()) + Capabilities.Add(MakeShared(Cap)); + Instance->SetArrayField(TEXT("capabilities"), Capabilities); + + Reply->SetObjectField(TEXT("instance"), Instance); + } + if (!bManagerPresent) { // Editor-time / pre-PIE handshake. Only editor-only ops can run @@ -654,35 +708,32 @@ TSharedPtr FURLabRpcDispatcher::BuildHandshakePayload(AAMjManager* mjModel* m = Manager->PhysicsEngine->GetModel(); - // SHM session directory. Ship an absolute path so the bridge can - // open the SHM regions regardless of its own working directory -- - // UE's path APIs return strings relative to the engine binary, which - // would mmap from the bridge's CWD otherwise. + // Let each bound transport append its own block to the handshake. + // SHM publish transport sets shm_session_dir; SHM RPC transport sets + // shm_rpc (paths/events/strides). Adding a third transport type + // requires no changes here — just override AppendHandshakeBlock. + for (const TObjectPtr& T : Manager->ManagerOwnedPublishTransports) { - FString ShmDir; - // Snapshot publisher is a UObject in - // ManagerOwnedPublishTransports. Walk the manager's transport - // array to find it. - for (const TObjectPtr& T : Manager->ManagerOwnedPublishTransports) - { - if (UURLabShmPublishTransport* ShmPub = Cast(T.Get())) - { - const FString StatePath = ShmPub->GetStatePath(); - if (!StatePath.IsEmpty()) - { - ShmDir = FPaths::GetPath(StatePath); - } - break; - } - } - if (ShmDir.IsEmpty()) - { - ShmDir = UURLabShmPublishTransport::ResolveSessionDir(SessionId); - } + T->AppendHandshakeBlock(Reply); + } + + // Fall back to the static helper when no publish transport set + // shm_session_dir (e.g. no SHM transport bound). + if (!Reply->HasField(TEXT("shm_session_dir"))) + { + const FString ShmDir = UURLabShmPublishTransport::ResolveSessionDir(SessionId); Reply->SetStringField(TEXT("shm_session_dir"), FPaths::ConvertRelativePathToFull(ShmDir)); } + if (Manager->BridgeServer) + { + for (const TObjectPtr& T : Manager->BridgeServer->GetRpcTransports()) + { + T->AppendHandshakeBlock(Reply); + } + } + // MJB bytes: real msgpack bin under "mjb" for msgpack clients; // legacy "mjb_base64" / "mjb_size" stays available for JSON clients. if (m) @@ -787,8 +838,11 @@ TSharedPtr FURLabRpcDispatcher::BuildHandshakePayload(AAMjManager* continue; TSharedPtr ArtObj = MakeShared(); - ArtObj->SetStringField(TEXT("prefix"), Art->GetName()); + // The public segment (ActorId-based) is the art's topic / observation + // namespace, matching the IR Art.Name the state stream keys arts under. + ArtObj->SetStringField(TEXT("prefix"), FMjCanonicalName::ArtSegment(Art).ToString()); ArtObj->SetStringField(TEXT("actor_id"), Art->ActorId); + ArtObj->SetStringField(TEXT("actor_name"), Art->GetName()); // Default control mode follows whether a controller is attached. UMjArticulationController* Ctrl = Art->FindComponentByClass(); @@ -910,9 +964,9 @@ TSharedPtr FURLabRpcDispatcher::BuildHandshakePayload(AAMjManager* FString Endpoint = Cam->GetActualZmqEndpoint(); Endpoint.ReplaceInline(TEXT("*"), TEXT("127.0.0.1")); CamObj->SetStringField(TEXT("zmq_endpoint"), Endpoint); - CamObj->SetStringField(TEXT("zmq_topic"), - FString::Printf(TEXT("%s/camera/%s"), *Art->GetName(), *Cam->GetName())); - CamMap->SetObjectField(Cam->GetName(), CamObj); + const FString CamCanon = Cam->GetCanonicalName(); + CamObj->SetStringField(TEXT("zmq_topic"), CamCanon); + CamMap->SetObjectField(CamCanon, CamObj); } ArtObj->SetObjectField(TEXT("camera_topics"), CamMap); @@ -961,2192 +1015,3 @@ TSharedPtr FURLabRpcDispatcher::BuildHandshakePayload(AAMjManager* return Reply; } - -// ============================================================================= -// step -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleStep(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) - { - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - } - - if (ActiveStepMode == EStepMode::Live) - { - // Live mode: UE drives its own physics. step() applies ctrl and - // returns current state. n_steps is ignored (UE steps at its own - // rate). Same per_articulation shape as direct mode. - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - - FMjStepRequest TmpReq; - const TSharedPtr* PerArt = nullptr; - if (Req->TryGetObjectField(TEXT("per_articulation"), PerArt) && PerArt && PerArt->IsValid()) - { - for (auto& Pair : (*PerArt)->Values) - { - const TSharedPtr* ArtObj = nullptr; - if (!Pair.Value->TryGetObject(ArtObj) || !ArtObj || !ArtObj->IsValid()) - continue; - - FString CtlMode; - if ((*ArtObj)->TryGetStringField(TEXT("control_mode"), CtlMode)) - TmpReq.PerArticulationControlMode.Add(Pair.Key, CtlMode); - - const TArray>* CtrlList = nullptr; - if ((*ArtObj)->TryGetArrayField(TEXT("ctrl"), CtrlList) && CtrlList) - { - if (AMjArticulation* Art = Cast(Mgr->GetArticulation(Pair.Key))) - { - TArray Acts = Art->GetActuators(); - for (int32 i = 0; i < CtrlList->Num() && i < Acts.Num(); ++i) - { - UMjActuator* A = Acts[i]; - if (!A) - continue; - FString Local = A->GetMjName(); - FString Prefix = Art->GetName() + TEXT("_"); - if (Local.StartsWith(Prefix)) - Local = Local.Mid(Prefix.Len()); - TmpReq.PerArticulationCtrl.FindOrAdd(Pair.Key).Add( - {Local, (float)(*CtrlList)[i]->AsNumber()}); - } - } - } - } - } - - TSharedPtr Reply = MakeShared(); - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - ApplyStepCtrl(Mgr, TmpReq, m, d); - Reply->SetStringField(TEXT("op"), TEXT("step_ok")); - Reply->SetNumberField(TEXT("time"), d->time); - Reply->SetNumberField(TEXT("step"), StepCounter.load(std::memory_order_relaxed)); - AppendClockFields(Reply, d->time); - TSharedPtr Obs = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); - if (Obs.IsValid()) - Reply->SetObjectField(TEXT("per_articulation"), Obs); - TSharedPtr Scene = BuildEntitiesBlock(Mgr, m, d); - if (Scene.IsValid()) - Reply->SetObjectField(TEXT("entities"), Scene); - } - return Reply; - } - - // Per-step observations override (does not change the session default). - FString StepObs; - if (Req->TryGetStringField(TEXT("observations"), StepObs)) - { - if (StepObs.Equals(TEXT("minimal"), ESearchCase::IgnoreCase)) - ActiveObservationLevel = EObservationLevel::Minimal; - else if (StepObs.Equals(TEXT("full"), ESearchCase::IgnoreCase)) - ActiveObservationLevel = EObservationLevel::Full; - else if (StepObs.Equals(TEXT("standard"), ESearchCase::IgnoreCase)) - ActiveObservationLevel = EObservationLevel::Standard; - } - - // Parse include_cameras: accepts either - // - object: { "": "sync"|"latest" } -- per-camera mode - // - bool true: all registered cameras at "latest" mode (legacy form) - // - false/absent: no cameras - TMap CameraSpec; - { - const TSharedPtr* CamObj = nullptr; - if (Req->TryGetObjectField(TEXT("include_cameras"), CamObj) && CamObj && CamObj->IsValid()) - { - for (const auto& Kv : (*CamObj)->Values) - { - FString Mode; - if (Kv.Value.IsValid() && Kv.Value->TryGetString(Mode)) - { - ECameraInclude E = ECameraInclude::Latest; - if (Mode.Equals(TEXT("sync"), ESearchCase::IgnoreCase)) - E = ECameraInclude::Sync; - CameraSpec.Add(Kv.Key, E); - } - } - } - else - { - bool bAll = false; - if (Req->TryGetBoolField(TEXT("include_cameras"), bAll) && bAll) - { - for (AMjArticulation* Art : Mgr->GetAllArticulations()) - { - if (!Art) - continue; - TArray Cams; - Art->GetComponents(Cams); - for (UMjCamera* C : Cams) - { - if (!C || C->bIsDefault) - continue; - CameraSpec.Add(C->GetName(), ECameraInclude::Latest); - } - } - } - } - } - - if (ActiveStepMode == EStepMode::Puppet) - { - FMjPushStateRequest Push; - const TArray>* QPosArr = nullptr; - const TArray>* QVelArr = nullptr; - const TArray>* CtrlArr = nullptr; - if (Req->TryGetArrayField(TEXT("qpos"), QPosArr)) - { - Push.QPos.Reserve(QPosArr->Num()); - for (auto& V : *QPosArr) - Push.QPos.Add(V->AsNumber()); - } - if (Req->TryGetArrayField(TEXT("qvel"), QVelArr)) - { - Push.QVel.Reserve(QVelArr->Num()); - for (auto& V : *QVelArr) - Push.QVel.Add(V->AsNumber()); - } - if (Req->TryGetArrayField(TEXT("ctrl"), CtrlArr)) - { - Push.bIncludeCtrl = true; - Push.Ctrl.Reserve(CtrlArr->Num()); - for (auto& V : *CtrlArr) - Push.Ctrl.Add(V->AsNumber()); - } - double TimeVal = 0.0; - Req->TryGetNumberField(TEXT("time"), TimeVal); - Push.Time = TimeVal; - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - if (Push.QPos.Num() == m->nq) - FMemory::Memcpy(d->qpos, Push.QPos.GetData(), m->nq * sizeof(mjtNum)); - if (Push.QVel.Num() == m->nv) - FMemory::Memcpy(d->qvel, Push.QVel.GetData(), m->nv * sizeof(mjtNum)); - if (Push.bIncludeCtrl && Push.Ctrl.Num() == m->nu) - FMemory::Memcpy(d->ctrl, Push.Ctrl.GetData(), m->nu * sizeof(mjtNum)); - d->time = Push.Time; - mj_forward(m, d); - - if (Mgr->PhysicsEngine->OnPostStep) - Mgr->PhysicsEngine->OnPostStep(m, d); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("step_ok")); - Reply->SetNumberField(TEXT("time"), d->time); - Reply->SetNumberField(TEXT("step"), StepCounter.fetch_add(1, std::memory_order_relaxed) + 1); - AppendClockFields(Reply, d->time); - TSharedPtr Obs = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); - if (Obs.IsValid()) - Reply->SetObjectField(TEXT("per_articulation"), Obs); - TSharedPtr Scene = BuildEntitiesBlock(Mgr, m, d); - if (Scene.IsValid()) - Reply->SetObjectField(TEXT("entities"), Scene); - if (CameraSpec.Num() > 0) - { - TSharedPtr Cams = BuildCamerasBlock(Mgr, CameraSpec); - if (Cams.IsValid() && Cams->Values.Num() > 0) - Reply->SetObjectField(TEXT("cameras"), Cams); - } - // Puppet-mode perturbation: include the latest sample so the client - // can apply the editor click-drag widget's force to its own MjData. - if (Mgr->Perturbation) - { - FMjPerturbationSample Sample = Mgr->Perturbation->GetLatestPerturbationSample(); - if (Sample.BodyId > 0) - { - TSharedPtr Pert = MakeShared(); - Pert->SetNumberField(TEXT("body_id"), Sample.BodyId); - Pert->SetNumberField(TEXT("version"), Sample.Version); - TArray> Six; - for (int i = 0; i < 6; ++i) - Six.Add(MakeShared(Sample.Xfrc[i])); - Pert->SetArrayField(TEXT("xfrc"), Six); - Reply->SetObjectField(TEXT("perturbation"), Pert); - } - } - return Reply; - } - - // Direct mode. - TSharedPtr Cmd = MakeShared(); - int32 NSteps = 1; - Req->TryGetNumberField(TEXT("n_steps"), NSteps); - Cmd->Request.NSteps = NSteps > 0 ? NSteps : 1; - - const TSharedPtr* PerArt = nullptr; - if (Req->TryGetObjectField(TEXT("per_articulation"), PerArt) && PerArt && PerArt->IsValid()) - { - for (auto& Pair : (*PerArt)->Values) - { - const TSharedPtr* ArtObj = nullptr; - if (!Pair.Value->TryGetObject(ArtObj) || !ArtObj || !ArtObj->IsValid()) - continue; - - // control_mode override - FString CtlMode; - if ((*ArtObj)->TryGetStringField(TEXT("control_mode"), CtlMode)) - { - Cmd->Request.PerArticulationControlMode.Add(Pair.Key, CtlMode); - } - - const TArray>* CtrlList = nullptr; - if ((*ArtObj)->TryGetArrayField(TEXT("ctrl"), CtrlList) && CtrlList) - { - // Positional ctrl array: indexed in articulation actuator order. - if (AMjArticulation* Art = Cast(Mgr->GetArticulation(Pair.Key))) - { - TArray Acts = Art->GetActuators(); - for (int32 i = 0; i < CtrlList->Num() && i < Acts.Num(); ++i) - { - UMjActuator* A = Acts[i]; - if (!A) - continue; - FString LocalName = A->GetMjName(); - FString Prefix = Art->GetName() + TEXT("_"); - if (LocalName.StartsWith(Prefix)) - LocalName = LocalName.Mid(Prefix.Len()); - Cmd->Request.PerArticulationCtrl.FindOrAdd(Pair.Key).Add( - {LocalName, (float)(*CtrlList)[i]->AsNumber()}); - } - } - } - - // Named ctrl map alternative. - const TSharedPtr* CtrlMap = nullptr; - if ((*ArtObj)->TryGetObjectField(TEXT("ctrl_map"), CtrlMap) && CtrlMap && CtrlMap->IsValid()) - { - for (auto& KV : (*CtrlMap)->Values) - { - Cmd->Request.PerArticulationCtrl.FindOrAdd(Pair.Key).Add( - {KV.Key, (float)KV.Value->AsNumber()}); - } - } - - // xfrc_applied: { body_name: [fx,fy,fz,tx,ty,tz] }. Cleared after step. - const TSharedPtr* XfrcMap = nullptr; - if ((*ArtObj)->TryGetObjectField(TEXT("xfrc_applied"), XfrcMap) && XfrcMap && XfrcMap->IsValid()) - { - TMap>& BodyMap = Cmd->Request.PerArticulationXfrc.FindOrAdd(Pair.Key); - for (auto& KV : (*XfrcMap)->Values) - { - const TArray>* Arr = nullptr; - if (KV.Value->TryGetArray(Arr) && Arr && Arr->Num() == 6) - { - TArray& Six = BodyMap.FindOrAdd(KV.Key); - Six.SetNum(6); - for (int i = 0; i < 6; ++i) - Six[i] = (*Arr)[i]->AsNumber(); - } - } - } - } - } - - // Submit to physics-thread custom handler (no race) then wait. If the - // engine isn't running its async loop (test path), fall back to inline. - Cmd->Completion = FPlatformProcess::GetSynchEventFromPool(true); - - const bool bAsyncRunning = Mgr->PhysicsEngine->AsyncPhysicsFuture.IsValid(); - StepQueue.Enqueue(Cmd); - if (Mgr->PhysicsEngine->StepRequestEvent) - { - Mgr->PhysicsEngine->StepRequestEvent->Trigger(); - } - - if (!bAsyncRunning) - { - // Test / editor path: pump the handler synchronously so we don't - // block forever waiting for an engine that isn't ticking. - if (Mgr->PhysicsEngine->CustomStepHandler) - { - Mgr->PhysicsEngine->CustomStepHandler( - Mgr->PhysicsEngine->GetModel(), - Mgr->PhysicsEngine->GetData()); - } - } - - // 5-second hard cap so a wedged engine returns an error rather than - // wedging the RPC thread. Polled in 50ms slices so the bDraining - // flag (set when the bridge is being stopped) can short-circuit the - // wait without forcing the user to sit through the full 5s while - // the editor closes. - bool bSignaled = false; - { - const double Deadline = FPlatformTime::Seconds() + 5.0; - while (FPlatformTime::Seconds() < Deadline) - { - if (bDraining.load(std::memory_order_acquire)) - { - break; - } - if (Cmd->Completion->Wait(FTimespan::FromMilliseconds(50))) - { - bSignaled = true; - break; - } - } - } - - TSharedPtr Reply = MakeShared(); - if (bSignaled && Cmd->bDone) - { - Reply->SetStringField(TEXT("op"), TEXT("step_ok")); - Reply->SetNumberField(TEXT("time"), Cmd->ResultTime); - Reply->SetNumberField(TEXT("step"), Cmd->ResultStep); - AppendClockFields(Reply, Cmd->ResultTime); - if (Cmd->Observations.IsValid()) - Reply->SetObjectField(TEXT("per_articulation"), Cmd->Observations); - if (Cmd->Entities.IsValid()) - Reply->SetObjectField(TEXT("entities"), Cmd->Entities); - - if (CameraSpec.Num() > 0) - { - TSharedPtr Cams = BuildCamerasBlock(Mgr, CameraSpec); - if (Cams.IsValid() && Cams->Values.Num() > 0) - Reply->SetObjectField(TEXT("cameras"), Cams); - } - } - else - { - if (bDraining.load(std::memory_order_acquire)) - { - Reply = MakeError(TEXT("shutting_down"), - TEXT("Bridge stopping; Direct-mode step abandoned")); - } - else - { - Reply = MakeError(TEXT("step_timeout"), - TEXT("Direct-mode step did not complete within 5s")); - } - } - - return Reply; -} - -void FURLabRpcDispatcher::ApplyStepCtrl(AAMjManager* Manager, const FMjStepRequest& Req, - mjModel* m, mjData* d) -{ - if (!Manager) - return; - for (auto& Pair : Req.PerArticulationCtrl) - { - AMjArticulation* Art = Manager->GetArticulation(Pair.Key); - if (!Art) - continue; - - const FString Prefix = Art->GetName() + TEXT("_"); - TMap ByName; - ByName.Reserve(Art->GetActuators().Num() * 2); - for (UMjActuator* A : Art->GetActuators()) - { - if (!A) - continue; - FString FullName = A->GetMjName(); - FString Local = FullName.StartsWith(Prefix) ? FullName.Mid(Prefix.Len()) : FullName; - ByName.Add(Local, A); - ByName.Add(FullName, A); - } - - // Stage to actuator NetworkValue; AMjArticulation::ApplyControls - // copies it into d->ctrl every sub-step. Writing d->ctrl directly - // would be overwritten on the next sub-step. - for (const TPair& KV : Pair.Value) - { - UMjActuator** Found = ByName.Find(KV.Key); - if (!Found || !*Found) - continue; - (*Found)->SetNetworkControl(KV.Value); - } - } - - // xfrc_applied writes: per_articulation -> body_name -> 6-vec. - // MuJoCo clears d->xfrc_applied on every mj_step, so this is a one-shot - // impulse for the next mj_step n_steps loop. Body name lookup tries both - // the local (no-prefix) form and the prefixed full name. - if (m && d) - { - for (auto& APair : Req.PerArticulationXfrc) - { - FString ArtPrefix = APair.Key + TEXT("_"); - for (auto& BPair : APair.Value) - { - if (BPair.Value.Num() != 6) - continue; - FString FullName = ArtPrefix + BPair.Key; - int Bid = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*FullName)); - if (Bid < 0) - Bid = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*BPair.Key)); - if (Bid < 0 || Bid >= m->nbody) - continue; - for (int i = 0; i < 6; ++i) - d->xfrc_applied[6 * Bid + i] = (mjtNum)BPair.Value[i]; - } - } - } -} - -TSharedPtr FURLabRpcDispatcher::BuildStepObservations(AAMjManager* Manager, mjModel* m, mjData* d, - EObservationLevel Level) -{ - TSharedPtr PerArt = MakeShared(); - if (!Manager || !m || !d) - return PerArt; - - const bool bWantStandard = (Level == EObservationLevel::Standard) || (Level == EObservationLevel::Full); - const bool bWantFull = (Level == EObservationLevel::Full); - - for (AMjArticulation* Art : Manager->GetAllArticulations()) - { - if (!Art) - continue; - TSharedPtr ArtObj = MakeShared(); - - // qpos / qvel — present at every level. Joints are emitted in the - // discovery order that GetJoints() returns, which matches the MjModel - // jnt_id order at compile time. Per-joint qpos/qvel slot widths follow - // jnt_type. Wire-side consumers should always rebuild full m.nq / m.nv - // arrays from per-articulation slices in this same order. - TArray Joints = Art->GetJoints(); - TArray> QPos; - TArray> QVel; - for (UMjJoint* J : Joints) - { - if (!J) - continue; - int32 Id = J->GetMjID(); - if (Id < 0 || Id >= m->njnt) - continue; - int QAddr = m->jnt_qposadr[Id]; - int VAddr = m->jnt_dofadr[Id]; - int QSize = 1, VSize = 1; - switch (m->jnt_type[Id]) - { - case mjJNT_FREE: - QSize = 7; - VSize = 6; - break; - case mjJNT_BALL: - QSize = 4; - VSize = 3; - break; - case mjJNT_SLIDE: - case mjJNT_HINGE: - QSize = 1; - VSize = 1; - break; - } - for (int i = 0; i < QSize; ++i) - QPos.Add(MakeShared(d->qpos[QAddr + i])); - for (int i = 0; i < VSize; ++i) - QVel.Add(MakeShared(d->qvel[VAddr + i])); - } - ArtObj->SetArrayField(TEXT("qpos"), QPos); - ArtObj->SetArrayField(TEXT("qvel"), QVel); - - if (bWantStandard) - { - // ctrl positional array, same order as GetActuators(). - TArray> Ctrl; - TArray> Act; - for (UMjActuator* A : Art->GetActuators()) - { - if (!A) - continue; - int32 Id = A->GetMjID(); - if (Id < 0 || Id >= m->nu) - continue; - Ctrl.Add(MakeShared(d->ctrl[Id])); - // Each actuator's "act" slot, if it has one (intvelocity, muscle, ...) - int ActAddr = m->actuator_actadr ? m->actuator_actadr[Id] : -1; - if (ActAddr >= 0 && ActAddr < m->na) - Act.Add(MakeShared(d->act[ActAddr])); - else - Act.Add(MakeShared(0.0)); - } - ArtObj->SetArrayField(TEXT("ctrl"), Ctrl); - ArtObj->SetArrayField(TEXT("act"), Act); - - // sensors by name — use sensor MjID + dim. - TSharedPtr Sensors = MakeShared(); - TArray SensorComponents; - Art->GetComponents(SensorComponents); - FString Prefix = Art->GetName() + TEXT("_"); - for (UMjSensor* S : SensorComponents) - { - if (!S) - continue; - int32 Sid = S->GetMjID(); - if (Sid < 0 || Sid >= m->nsensor) - continue; - int Adr = m->sensor_adr[Sid]; - int Dim = m->sensor_dim[Sid]; - if (Adr < 0 || Dim <= 0 || (Adr + Dim) > m->nsensordata) - continue; - TArray> Vals; - for (int i = 0; i < Dim; ++i) - Vals.Add(MakeShared(d->sensordata[Adr + i])); - FString LocalName = S->GetMjName(); - if (LocalName.StartsWith(Prefix)) - LocalName = LocalName.Mid(Prefix.Len()); - Sensors->SetArrayField(LocalName, Vals); - } - ArtObj->SetObjectField(TEXT("sensors"), Sensors); - } - - if (bWantFull) - { - // body xpos/xquat — discovered through articulation's MjBody components. - TSharedPtr Bodies = MakeShared(); - TArray BodyComponents; - Art->GetComponents(BodyComponents); - FString Prefix = Art->GetName() + TEXT("_"); - for (UMjBody* B : BodyComponents) - { - if (!B || B->bIsDefault) - continue; - int32 Bid = B->GetMjID(); - if (Bid < 0 || Bid >= m->nbody) - continue; - TSharedPtr Bo = MakeShared(); - TArray> XPos, XQuat; - for (int i = 0; i < 3; ++i) - XPos.Add(MakeShared(d->xpos[Bid * 3 + i])); - for (int i = 0; i < 4; ++i) - XQuat.Add(MakeShared(d->xquat[Bid * 4 + i])); - Bo->SetArrayField(TEXT("xpos"), XPos); - Bo->SetArrayField(TEXT("xquat"), XQuat); - FString LocalName = B->GetMjName(); - if (LocalName.StartsWith(Prefix)) - LocalName = LocalName.Mid(Prefix.Len()); - Bodies->SetObjectField(LocalName, Bo); - } - ArtObj->SetObjectField(TEXT("bodies"), Bodies); - - // actuator_force per actuator (positional, same order as ctrl) - TArray> AForce; - for (UMjActuator* A : Art->GetActuators()) - { - if (!A) - continue; - int32 Id = A->GetMjID(); - if (Id < 0 || Id >= m->nu) - continue; - AForce.Add(MakeShared(d->actuator_force[Id])); - } - ArtObj->SetArrayField(TEXT("actuator_force"), AForce); - } - - // geometry_msgs/Twist-aligned: (linear.x, linear.y, angular.z) - // filled; rest stays zero. Only when a TwistController is attached. - if (UMjTwistController* TwistCtrl = Art->FindComponentByClass()) - { - const FVector Twist = TwistCtrl->GetTwist(); // (Vx, Vy, YawRate) - - TArray> Linear; - Linear.Add(MakeShared(Twist.X)); - Linear.Add(MakeShared(Twist.Y)); - Linear.Add(MakeShared(0.0)); - - TArray> Angular; - Angular.Add(MakeShared(0.0)); - Angular.Add(MakeShared(0.0)); - Angular.Add(MakeShared(Twist.Z)); - - TSharedPtr TwistObj = MakeShared(); - TwistObj->SetArrayField(TEXT("linear"), Linear); - TwistObj->SetArrayField(TEXT("angular"), Angular); - ArtObj->SetObjectField(TEXT("twist"), TwistObj); - - ArtObj->SetNumberField(TEXT("actions"), - static_cast(TwistCtrl->GetActiveActions())); - } - - PerArt->SetObjectField(Art->GetName(), ArtObj); - } - return PerArt; -} - -TSharedPtr FURLabRpcDispatcher::BuildCamerasBlock(AAMjManager* Manager, - const TMap& CameraSpec, int32 TimeoutMs) -{ - TSharedPtr Cams = MakeShared(); - if (!Manager || CameraSpec.Num() == 0) - return Cams; - - UWorld* World = Manager->GetWorld(); - if (!World) - return Cams; - - // Build a name -> camera lookup using the canonical "/" - // form the handshake exposes plus the bare camera name as a fallback. - TMap ByName; - for (AMjArticulation* Art : Manager->GetAllArticulations()) - { - if (!Art) - continue; - TArray Cameras; - Art->GetComponents(Cameras); - for (UMjCamera* C : Cameras) - { - if (!C || C->bIsDefault) - continue; - FString Qualified = Art->GetName() + TEXT("/") + C->GetName(); - ByName.Add(Qualified, C); - ByName.Add(C->GetName(), C); - } - } - - for (const TPair& Spec : CameraSpec) - { - UMjCamera** Found = ByName.Find(Spec.Key); - if (!Found || !*Found) - { - UE_LOG(LogURLabNet, Verbose, - TEXT("[BuildCamerasBlock] camera '%s' not found"), *Spec.Key); - continue; - } - UMjCamera* Cam = *Found; - - // Streaming auto-enables in UMjCamera::BeginPlay via - // UMjNetworkManager::RegisterCamera (bEnableAllCameras=true by - // default), so by the time we hit this path the camera is - // already streaming. The earlier worker-thread - // SetStreamingEnabled call here was dead code; left only the - // game-thread RequestReadback marshalling below, which is - // the actual fix — RenderTarget->GameThread_GetRenderTargetResource - // returns null on non-game threads and silently bails the - // readback, which is why include_cameras saw empty frames. - - // For "sync" we kick a readback and poll. RequestReadback - // must run on the game thread (it touches - // RenderTarget->GameThread_GetRenderTargetResource and enqueues - // a render command); calling it from the bridge worker thread - // makes Resource null and the readback never lands. We marshal - // here and wait briefly; the per-tick auto-readback in - // UMjCamera::TickComponent keeps PendingPixels fresh for the - // "latest" path so consumers without an explicit sync call - // still get frames. - if (Spec.Value == ECameraInclude::Sync) - { - FEvent* ReqDone = FPlatformProcess::GetSynchEventFromPool(/*bIsManualReset=*/false); - TWeakObjectPtr WeakCam(Cam); - AsyncTask(ENamedThreads::GameThread, [WeakCam, ReqDone]() { - if (UMjCamera* C = WeakCam.Get()) - { - C->RequestReadback(); - } - ReqDone->Trigger(); - }); - ReqDone->Wait(2000); - FPlatformProcess::ReturnSynchEventToPool(ReqDone); - - const double DeadlineSec = FPlatformTime::Seconds() + (TimeoutMs / 1000.0); - while (!Cam->IsReadbackReady() && FPlatformTime::Seconds() < DeadlineSec) - { - FPlatformProcess::Sleep(0.001f); - } - } - - TSharedPtr CamObj = MakeShared(); - CamObj->SetNumberField(TEXT("width"), Cam->resolution.Num() > 0 ? Cam->resolution[0] : 0); - CamObj->SetNumberField(TEXT("height"), Cam->resolution.Num() > 1 ? Cam->resolution[1] : 0); - - if (Cam->CaptureMode == EMjCameraMode::Depth) - { - TArray Pixels = Cam->ConsumeFloatPixels(); - if (Pixels.Num() == 0) - continue; - CamObj->SetStringField(TEXT("dtype"), TEXT("float32")); - FURLabMsgpackUtil::SetBinaryField(CamObj, TEXT("data"), - reinterpret_cast(Pixels.GetData()), - Pixels.Num() * sizeof(float)); - } - else - { - TArray Pixels = Cam->ConsumePixels(); - if (Pixels.Num() == 0) - continue; - // Real / SemSeg / InstanceSeg all ship 4-byte BGRA. Bridge - // discriminates the seg modes by the camera_topics handshake. - CamObj->SetStringField(TEXT("dtype"), TEXT("bgra8")); - FURLabMsgpackUtil::SetBinaryField(CamObj, TEXT("data"), - reinterpret_cast(Pixels.GetData()), - Pixels.Num() * sizeof(FColor)); - } - Cams->SetObjectField(Spec.Key, CamObj); - } - return Cams; -} - -TSharedPtr FURLabRpcDispatcher::BuildEntitiesBlock(AAMjManager* Manager, mjModel* m, mjData* d) -{ - TSharedPtr Scene = MakeShared(); - if (!Manager || !m || !d) - return Scene; - - // Prefer the cached scene-body record table when populated. Avoids a - // per-call TActorIterator walk on the physics thread. - auto BuildFromBody = [&](int32 Id, const FString& Name) { - if (Id < 0 || Id >= m->nbody) - return; - TSharedPtr Obj = MakeShared(); - TArray> XPos, XQuat; - for (int i = 0; i < 3; ++i) - XPos.Add(MakeShared(d->xpos[Id * 3 + i])); - for (int i = 0; i < 4; ++i) - XQuat.Add(MakeShared(d->xquat[Id * 4 + i])); - Obj->SetArrayField(TEXT("xpos"), XPos); - Obj->SetArrayField(TEXT("xquat"), XQuat); - - // Free-joint detection: a body with a single jntnum=1 of mjJNT_FREE - // owns a 7-vec qpos and 6-vec qvel. Stream both. Other joint types - // get xpos/xquat only — a kinematic-driven heightfield base, etc. - if (Id < m->nbody && m->body_jntnum && m->body_jntadr) - { - int FirstJnt = m->body_jntadr[Id]; - int NumJnt = m->body_jntnum[Id]; - if (FirstJnt >= 0 && NumJnt > 0 && FirstJnt < m->njnt && m->jnt_type[FirstJnt] == mjJNT_FREE) - { - int QAddr = m->jnt_qposadr[FirstJnt]; - int VAddr = m->jnt_dofadr[FirstJnt]; - TArray> QPos, QVel; - for (int i = 0; i < 7; ++i) - QPos.Add(MakeShared(d->qpos[QAddr + i])); - for (int i = 0; i < 6; ++i) - QVel.Add(MakeShared(d->qvel[VAddr + i])); - Obj->SetArrayField(TEXT("qpos"), QPos); - Obj->SetArrayField(TEXT("qvel"), QVel); - } - } - Scene->SetObjectField(Name, Obj); - }; - - // Cache fast path. - const TArray& Cache = Manager->GetEntities(); - if (Cache.Num() > 0) - { - for (const FMjEntityRecord& R : Cache) - BuildFromBody(R.MjId, R.Name); - return Scene; - } - - // Fallback: walk the world via TActorIterator. ONLY safe from the game - // thread -- the iterator asserts IsInGameThread(). DirectStepHandler / - // PuppetStepHandler run on the physics async thread, so when called from - // there with an empty cache (no scene bodies were registered), return an - // empty block rather than crashing. Tests / pre-cache callers on the - // game thread still use the fallback path. - if (!IsInGameThread()) - return Scene; - - UWorld* World = Manager->GetWorld(); - if (!World) - return Scene; - - TSet ArticSet; - for (AMjArticulation* A : Manager->GetAllArticulations()) - ArticSet.Add(A); - - for (TActorIterator It(World); It; ++It) - { - AActor* Actor = *It; - if (!Actor) - continue; - if (AMjArticulation* AsArt = Cast(Actor)) - { - if (ArticSet.Contains(AsArt)) - continue; - } - TArray Bodies; - Actor->GetComponents(Bodies); - for (UMjBody* B : Bodies) - { - if (!B || B->bIsDefault) - continue; - BuildFromBody(B->GetMjID(), B->GetMjName()); - } - } - return Scene; -} - -// ============================================================================= -// reset / set_mode -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleReset(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) - { - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - } - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - - int32 SeedVal = 0; - if (Req->TryGetNumberField(TEXT("seed"), SeedVal)) - { - Mgr->Seed = SeedVal; - // Modern mjOption has no "seed" field; mj_step is deterministic and - // doesn't depend on a stored seed (random elements come from - // user-set noise inputs, not an integrator-internal RNG). The seed - // is recorded on the manager so any RNG used by client code or by - // the recording layer can mirror it for reproducibility. UE itself - // does not reseed the integrator here. - } - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - - FString KfName; - if (Req->TryGetStringField(TEXT("keyframe_name"), KfName) && !KfName.IsEmpty()) - { - int Kid = mj_name2id(m, mjOBJ_KEY, TCHAR_TO_UTF8(*KfName)); - if (Kid < 0) - return MakeError(TEXT("unknown_keyframe"), KfName); - mj_resetDataKeyframe(m, d, Kid); - } - else - { - mj_resetData(m, d); - } - - // Per-articulation qpos overrides (joint-name -> value). - const TSharedPtr* PerArt = nullptr; - if (Req->TryGetObjectField(TEXT("per_articulation_qpos"), PerArt) && PerArt && PerArt->IsValid()) - { - for (auto& APair : (*PerArt)->Values) - { - AMjArticulation* Art = Mgr->GetArticulation(APair.Key); - if (!Art) - continue; - const TSharedPtr* QObj = nullptr; - if (!APair.Value->TryGetObject(QObj) || !QObj || !QObj->IsValid()) - continue; - - FString Prefix = Art->GetName() + TEXT("_"); - for (auto& JPair : (*QObj)->Values) - { - FString FullName = Prefix + JPair.Key; - int Jid = mj_name2id(m, mjOBJ_JOINT, TCHAR_TO_UTF8(*FullName)); - if (Jid < 0) - Jid = mj_name2id(m, mjOBJ_JOINT, TCHAR_TO_UTF8(*JPair.Key)); - if (Jid < 0) - continue; - int QAddr = m->jnt_qposadr[Jid]; - d->qpos[QAddr] = (mjtNum)JPair.Value->AsNumber(); - } - } - } - mj_forward(m, d); - } - - StepCounter.store(0, std::memory_order_relaxed); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("reset_ok")); - Reply->SetNumberField(TEXT("time"), d->time); - Reply->SetNumberField(TEXT("step"), 0); - AppendClockFields(Reply, d->time); - TSharedPtr Obs = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); - if (Obs.IsValid()) - Reply->SetObjectField(TEXT("per_articulation"), Obs); - return Reply; -} - -// Run mj_forward (kinematics + dynamics, no integration) and return -// observations. Lets a client write qpos / qvel then read consistent -// derived state (xpos, sensors, contacts, ...) without advancing time. -TSharedPtr FURLabRpcDispatcher::HandleForward(const TSharedPtr& /*Req*/) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) - { - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - } - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - mj_forward(m, d); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("forward_ok")); - Reply->SetNumberField(TEXT("time"), d->time); - Reply->SetNumberField(TEXT("step"), StepCounter.load(std::memory_order_relaxed)); - AppendClockFields(Reply, d->time); - TSharedPtr Obs = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); - if (Obs.IsValid()) - Reply->SetObjectField(TEXT("per_articulation"), Obs); - return Reply; -} - -TSharedPtr FURLabRpcDispatcher::HandleSetMode(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return MakeError(TEXT("not_ready"), TEXT("Manager missing")); - - if (Mgr->StepMode != EStepMode::Auto) - { - return MakeError(TEXT("mode_locked_by_server"), - FString::Printf(TEXT("Project pinned StepMode to %s"), *StepModeToString(Mgr->StepMode))); - } - - FString ModeStr; - if (!Req->TryGetStringField(TEXT("mode"), ModeStr)) - return MakeError(TEXT("missing_field"), TEXT("set_mode requires 'mode'")); - - EStepMode NewMode; - if (!StepModeFromString(ModeStr, NewMode)) - return MakeError(TEXT("bad_mode"), FString::Printf(TEXT("Unknown mode '%s'"), *ModeStr)); - - EStepMode Prev = ActiveStepMode; - SetActiveStepMode(NewMode); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_mode_ok")); - Reply->SetStringField(TEXT("previous_mode"), StepModeToString(Prev)); - Reply->SetStringField(TEXT("current_mode"), StepModeToString(ActiveStepMode)); - return Reply; -} - -void FURLabRpcDispatcher::SetActiveStepMode(EStepMode NewMode) -{ - // Serialises install/uninstall side effects against concurrent - // set_mode calls; Dispatch releases DispatchMutex before handlers. - FScopeLock Lock(&DispatchMutex); - - const EStepMode CurMode = ActiveStepMode.load(std::memory_order_acquire); - if (NewMode == CurMode) - return; - - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return; - - if (CurMode == EStepMode::Puppet) - UninstallPuppetHandler(); - if (CurMode == EStepMode::Direct) - UninstallDirectHandler(); - DrainQueuesForTest(); - - ActiveStepMode.store(NewMode, std::memory_order_release); - Mgr->EffectiveStepMode.store(NewMode, std::memory_order_release); - const bool bPaused = (NewMode != EStepMode::Live); - Mgr->bPublishersPaused.store(bPaused, std::memory_order_release); - FCameraZmqWorker::bPublishersPaused.store(bPaused, std::memory_order_release); - - if (NewMode == EStepMode::Puppet) - InstallPuppetHandler(); - else if (NewMode == EStepMode::Direct) - InstallDirectHandler(); - - // Engine defaults bIsPaused=true and is normally unpaused via the editor - // UI / hotkey. A remote client has no UI handle, so entering Direct or - // Direct/Puppet imply "client drives physics" — force unpause so the - // async loop calls CustomStepHandler and the request queue drains. - if (Mgr->PhysicsEngine && NewMode != EStepMode::Live) - { - if (Mgr->PhysicsEngine->bIsPaused) - { - Mgr->PhysicsEngine->SetPaused(false); - UE_LOG(LogURLabNet, Log, - TEXT("FURLabRpcDispatcher: unpaused PhysicsEngine for %s mode"), - *StepModeToString(NewMode)); - } - } - - UE_LOG(LogURLabNet, Log, TEXT("FURLabRpcDispatcher: step mode -> %s (publishers_paused=%s)"), - *StepModeToString(NewMode), bPaused ? TEXT("true") : TEXT("false")); -} - -void FURLabRpcDispatcher::InstallPuppetHandler() -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return; - if (bPuppetHandlerInstalled) - return; - - UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; - PuppetStepHandler = [this, Engine](mjModel* m, mjData* d) { - FMjPushStateRequest Req; - if (!PushStateQueue.Dequeue(Req)) - return; - if (Req.QPos.Num() == m->nq) - FMemory::Memcpy(d->qpos, Req.QPos.GetData(), m->nq * sizeof(mjtNum)); - if (Req.QVel.Num() == m->nv) - FMemory::Memcpy(d->qvel, Req.QVel.GetData(), m->nv * sizeof(mjtNum)); - if (Req.bIncludeCtrl && Req.Ctrl.Num() == m->nu) - FMemory::Memcpy(d->ctrl, Req.Ctrl.GetData(), m->nu * sizeof(mjtNum)); - d->time = Req.Time; - mj_forward(m, d); - if (Engine->OnPostStep) - Engine->OnPostStep(m, d); - }; - Engine->SetCustomStepHandler(PuppetStepHandler); - bPuppetHandlerInstalled = true; -} - -void FURLabRpcDispatcher::UninstallPuppetHandler() -{ - if (!bPuppetHandlerInstalled) - return; - if (AAMjManager* Mgr = OwnerMgr.Get()) - { - if (Mgr->PhysicsEngine) - Mgr->PhysicsEngine->ClearCustomStepHandler(); - } - bPuppetHandlerInstalled = false; - PuppetStepHandler = nullptr; -} - -void FURLabRpcDispatcher::InstallDirectHandler() -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return; - if (bDirectHandlerInstalled) - return; - - UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; - DirectStepHandler = [this, Engine, Mgr](mjModel* m, mjData* d) { - // Only the physics-engine async worker thread runs this handler, - // so d is exclusively owned for its duration. - TSharedPtr Cmd; - if (!StepQueue.Dequeue(Cmd) || !Cmd.IsValid()) - return; - - ApplyStepCtrl(Mgr, Cmd->Request, m, d); - - // control_mode="raw" per articulation bypasses the UE controller - // (NetworkValue treated as direct ctrl setpoint). Name-keyed so - // adding/removing articulations doesn't shift the mapping. - TMap SkipController; - for (AMjArticulation* Art : Mgr->GetAllArticulations()) - { - if (!Art) - continue; - const FString* Mode = Cmd->Request.PerArticulationControlMode.Find(Art->GetName()); - const bool bRaw = Mode && Mode->Equals(TEXT("raw"), ESearchCase::IgnoreCase); - SkipController.Add(Art, bRaw); - } - - for (int32 i = 0; i < Cmd->Request.NSteps; ++i) - { - for (AMjArticulation* Art : Mgr->GetAllArticulations()) - { - if (!Art) - continue; - const bool* bSkip = SkipController.Find(Art); - Art->ApplyControls(bSkip != nullptr && *bSkip); - } - mj_step(m, d); - if (Engine->OnPostStep) - Engine->OnPostStep(m, d); - } - Cmd->ResultTime = d->time; - Cmd->ResultStep = StepCounter.fetch_add(Cmd->Request.NSteps, std::memory_order_relaxed) - + Cmd->Request.NSteps; - Cmd->Observations = BuildStepObservations(Mgr, m, d, ActiveObservationLevel); - Cmd->Entities = BuildEntitiesBlock(Mgr, m, d); - Cmd->bDone = true; - if (Cmd->Completion) - Cmd->Completion->Trigger(); - }; - Engine->SetCustomStepHandler(DirectStepHandler); - bDirectHandlerInstalled = true; -} - -void FURLabRpcDispatcher::UninstallDirectHandler() -{ - if (!bDirectHandlerInstalled) - return; - if (AAMjManager* Mgr = OwnerMgr.Get()) - { - if (Mgr->PhysicsEngine) - Mgr->PhysicsEngine->ClearCustomStepHandler(); - } - bDirectHandlerInstalled = false; - DirectStepHandler = nullptr; -} - -// ============================================================================= -// configure_controller -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleConfigureController(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return MakeError(TEXT("not_ready"), TEXT("Manager missing")); - - FString ArtName; - if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) - return MakeError(TEXT("missing_field"), TEXT("configure_controller requires 'articulation'")); - - AMjArticulation* Art = Mgr->GetArticulation(ArtName); - if (!Art) - return MakeError(TEXT("unknown_articulation"), ArtName); - - UMjArticulationController* Ctrl = Art->FindComponentByClass(); - if (!Ctrl) - return MakeError(TEXT("no_controller"), FString::Printf(TEXT("Articulation '%s' has no controller"), *ArtName)); - - const TSharedPtr* Params = nullptr; - if (Req->TryGetObjectField(TEXT("params"), Params) && Params && Params->IsValid()) - { - Ctrl->ApplyConfig(*Params); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("configure_controller_ok")); - Reply->SetStringField(TEXT("articulation"), ArtName); - - TSharedPtr Out = MakeShared(); - Ctrl->GetCurrentConfig(Out); - Reply->SetObjectField(TEXT("params"), Out); - return Reply; -} - -// ============================================================================= -// set_sim_options -// ============================================================================= - -namespace -{ -bool ParseIntegrator(const FString& S, EMjIntegrator& Out) -{ - if (S.Equals(TEXT("euler"), ESearchCase::IgnoreCase)) - { - Out = EMjIntegrator::Euler; - return true; - } - if (S.Equals(TEXT("rk4"), ESearchCase::IgnoreCase)) - { - Out = EMjIntegrator::RK4; - return true; - } - if (S.Equals(TEXT("implicit"), ESearchCase::IgnoreCase)) - { - Out = EMjIntegrator::Implicit; - return true; - } - if (S.Equals(TEXT("implicitfast"), ESearchCase::IgnoreCase)) - { - Out = EMjIntegrator::ImplicitFast; - return true; - } - return false; -} -FString IntegratorToString(EMjIntegrator I) -{ - switch (I) - { - case EMjIntegrator::Euler: - return TEXT("euler"); - case EMjIntegrator::RK4: - return TEXT("rk4"); - case EMjIntegrator::Implicit: - return TEXT("implicit"); - case EMjIntegrator::ImplicitFast: - return TEXT("implicitfast"); - } - return TEXT("euler"); -} -bool ParseCone(const FString& S, EMjCone& Out) -{ - if (S.Equals(TEXT("pyramidal"), ESearchCase::IgnoreCase)) - { - Out = EMjCone::Pyramidal; - return true; - } - if (S.Equals(TEXT("elliptic"), ESearchCase::IgnoreCase)) - { - Out = EMjCone::Elliptic; - return true; - } - return false; -} -FString ConeToString(EMjCone C) -{ - return C == EMjCone::Elliptic ? TEXT("elliptic") : TEXT("pyramidal"); -} -bool ParseSolver(const FString& S, EMjSolver& Out) -{ - if (S.Equals(TEXT("pgs"), ESearchCase::IgnoreCase)) - { - Out = EMjSolver::PGS; - return true; - } - if (S.Equals(TEXT("cg"), ESearchCase::IgnoreCase)) - { - Out = EMjSolver::CG; - return true; - } - if (S.Equals(TEXT("newton"), ESearchCase::IgnoreCase)) - { - Out = EMjSolver::Newton; - return true; - } - return false; -} -FString SolverToString(EMjSolver S) -{ - switch (S) - { - case EMjSolver::PGS: - return TEXT("pgs"); - case EMjSolver::CG: - return TEXT("cg"); - case EMjSolver::Newton: - return TEXT("newton"); - } - return TEXT("newton"); -} - -bool TryReadVec3(const TSharedPtr& Obj, const TCHAR* Key, double Out[3]) -{ - const TArray>* Arr = nullptr; - if (!Obj->TryGetArrayField(Key, Arr) || !Arr || Arr->Num() != 3) - return false; - Out[0] = (*Arr)[0]->AsNumber(); - Out[1] = (*Arr)[1]->AsNumber(); - Out[2] = (*Arr)[2]->AsNumber(); - return true; -} -} // namespace - -TSharedPtr FURLabRpcDispatcher::HandleSetSimOptions(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - if (!m) - return MakeError(TEXT("not_ready"), TEXT("mjModel not compiled")); - - const TSharedPtr* OptsPtr = nullptr; - if (!Req->TryGetObjectField(TEXT("options"), OptsPtr) || !OptsPtr || !(*OptsPtr).IsValid()) - return MakeError(TEXT("missing_field"), TEXT("set_sim_options requires 'options' object")); - const TSharedPtr& Opts = *OptsPtr; - - FMjOptionGenerated& O = Mgr->PhysicsEngine->Options; - - double DNum = 0.0; - if (Opts->TryGetNumberField(TEXT("timestep"), DNum)) - { - O.Timestep = (float)DNum; - O.bOverride_Timestep = true; - } - - // Wire is MJ-native SI; FMjOptionGenerated stores UE cm/s² with Y-flip and - // ApplyOverridesToModel reverses that, so pre-bake the inverse here. - double V3[3]; - if (TryReadVec3(Opts, TEXT("gravity"), V3)) - { - O.Gravity = FVector((float)(V3[0] * 100.0), (float)(-V3[1] * 100.0), (float)(V3[2] * 100.0)); - O.bOverride_Gravity = true; - } - if (TryReadVec3(Opts, TEXT("wind"), V3)) - { - O.Wind = FVector((float)(V3[0] * 100.0), (float)(-V3[1] * 100.0), (float)(V3[2] * 100.0)); - O.bOverride_Wind = true; - } - if (TryReadVec3(Opts, TEXT("magnetic"), V3)) - { - O.Magnetic = FVector((float)V3[0], (float)-V3[1], (float)V3[2]); - O.bOverride_Magnetic = true; - } - - if (Opts->TryGetNumberField(TEXT("density"), DNum)) - { - O.Density = (float)DNum; - O.bOverride_Density = true; - } - if (Opts->TryGetNumberField(TEXT("viscosity"), DNum)) - { - O.Viscosity = (float)DNum; - O.bOverride_Viscosity = true; - } - if (Opts->TryGetNumberField(TEXT("impratio"), DNum)) - { - O.Impratio = (float)DNum; - O.bOverride_Impratio = true; - } - if (Opts->TryGetNumberField(TEXT("tolerance"), DNum)) - { - O.Tolerance = (float)DNum; - O.bOverride_Tolerance = true; - } - - int32 INum = 0; - if (Opts->TryGetNumberField(TEXT("iterations"), INum)) - { - O.Iterations = INum; - O.bOverride_Iterations = true; - } - if (Opts->TryGetNumberField(TEXT("ls_iterations"), INum)) - { - O.LsIterations = INum; - O.bOverride_LsIterations = true; - } - - FString SNum; - if (Opts->TryGetStringField(TEXT("integrator"), SNum)) - { - EMjIntegrator E; - if (!ParseIntegrator(SNum, E)) - return MakeError(TEXT("bad_value"), FString::Printf(TEXT("unknown integrator '%s'"), *SNum)); - O.Integrator = E; - O.bOverride_Integrator = true; - } - if (Opts->TryGetStringField(TEXT("cone"), SNum)) - { - EMjCone E; - if (!ParseCone(SNum, E)) - return MakeError(TEXT("bad_value"), FString::Printf(TEXT("unknown cone '%s'"), *SNum)); - O.Cone = E; - O.bOverride_Cone = true; - } - if (Opts->TryGetStringField(TEXT("solver"), SNum)) - { - EMjSolver E; - if (!ParseSolver(SNum, E)) - return MakeError(TEXT("bad_value"), FString::Printf(TEXT("unknown solver '%s'"), *SNum)); - O.Solver = E; - O.bOverride_Solver = true; - } - - if (Opts->TryGetNumberField(TEXT("noslip_iterations"), INum)) - { - O.NoslipIterations = INum; - O.bOverride_NoslipIterations = true; - } - if (Opts->TryGetNumberField(TEXT("noslip_tolerance"), DNum)) - { - O.NoslipTolerance = (float)DNum; - O.bOverride_NoslipTolerance = true; - } - if (Opts->TryGetNumberField(TEXT("ccd_iterations"), INum)) - { - O.CCD_Iterations = INum; - O.bOverride_CCD_Iterations = true; - } - if (Opts->TryGetNumberField(TEXT("ccd_tolerance"), DNum)) - { - O.CCD_Tolerance = (float)DNum; - O.bOverride_CCD_Tolerance = true; - } - - bool BNum = false; - if (Opts->TryGetBoolField(TEXT("enable_multiccd"), BNum)) - { - O.bEnableMultiCCD = BNum; - } - if (Opts->TryGetBoolField(TEXT("enable_sleep"), BNum)) - { - O.bEnableSleep = BNum; - } - if (Opts->TryGetNumberField(TEXT("sleep_tolerance"), DNum)) - { - O.SleepTolerance = (float)DNum; - } - - // Raw disable / enable bit masks. Values are bitwise-ORs of - // mujoco/mjmodel.h mjtDisableBit / mjtEnableBit constants. - // Applied BEFORE FMjOptionGenerated::ApplyOverridesToModel so any named - // bits the caller also set (enable_sleep / enable_multiccd) win on - // top of the raw mask. Treat the raw masks as a coarse baseline. - int32 DisableMask = 0; - if (Opts->TryGetNumberField(TEXT("disableflags"), DisableMask)) - { - m->opt.disableflags = DisableMask; - } - int32 EnableMask = 0; - if (Opts->TryGetNumberField(TEXT("enableflags"), EnableMask)) - { - m->opt.enableflags = EnableMask; - } - - O.ApplyOverridesToModel(m); - - // Worker thread pool (mju_threadpool). Not a MuJoCo option-struct field — - // it's a URLab engine setting applied to the live mjData. Clamped to the - // detected CPU core count; ApplyThreadPool is idempotent. - int32 NumThreads = 0; - if (Opts->TryGetNumberField(TEXT("num_worker_threads"), NumThreads)) - { - Mgr->PhysicsEngine->NumWorkerThreads = - FMath::Clamp(NumThreads, 0, UMjPhysicsEngine::MaxWorkerThreads()); - Mgr->PhysicsEngine->ApplyThreadPool(); - } - - UE_LOG(LogURLabNet, Log, - TEXT("FURLabRpcDispatcher: set_sim_options applied (timestep=%.5fs, gravity=[%.3f %.3f %.3f] m/s²)"), - m->opt.timestep, m->opt.gravity[0], m->opt.gravity[1], m->opt.gravity[2]); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_sim_options_ok")); - - TSharedPtr Out = MakeShared(); - Out->SetNumberField(TEXT("timestep"), m->opt.timestep); - Out->SetNumberField(TEXT("num_worker_threads"), Mgr->PhysicsEngine->NumWorkerThreads); - Out->SetNumberField(TEXT("max_worker_threads"), UMjPhysicsEngine::MaxWorkerThreads()); - { - TArray> G; - G.Add(MakeShared(m->opt.gravity[0])); - G.Add(MakeShared(m->opt.gravity[1])); - G.Add(MakeShared(m->opt.gravity[2])); - Out->SetArrayField(TEXT("gravity"), G); - TArray> W; - W.Add(MakeShared(m->opt.wind[0])); - W.Add(MakeShared(m->opt.wind[1])); - W.Add(MakeShared(m->opt.wind[2])); - Out->SetArrayField(TEXT("wind"), W); - TArray> Mg; - Mg.Add(MakeShared(m->opt.magnetic[0])); - Mg.Add(MakeShared(m->opt.magnetic[1])); - Mg.Add(MakeShared(m->opt.magnetic[2])); - Out->SetArrayField(TEXT("magnetic"), Mg); - } - Out->SetNumberField(TEXT("density"), m->opt.density); - Out->SetNumberField(TEXT("viscosity"), m->opt.viscosity); - Out->SetNumberField(TEXT("impratio"), m->opt.impratio); - Out->SetNumberField(TEXT("tolerance"), m->opt.tolerance); - Out->SetNumberField(TEXT("iterations"), m->opt.iterations); - Out->SetNumberField(TEXT("ls_iterations"), m->opt.ls_iterations); - Out->SetStringField(TEXT("integrator"), IntegratorToString((EMjIntegrator)m->opt.integrator)); - Out->SetStringField(TEXT("cone"), ConeToString((EMjCone)m->opt.cone)); - Out->SetStringField(TEXT("solver"), SolverToString((EMjSolver)m->opt.solver)); - Out->SetNumberField(TEXT("noslip_iterations"), m->opt.noslip_iterations); - Out->SetNumberField(TEXT("noslip_tolerance"), m->opt.noslip_tolerance); - Out->SetNumberField(TEXT("ccd_iterations"), m->opt.ccd_iterations); - Out->SetNumberField(TEXT("ccd_tolerance"), m->opt.ccd_tolerance); - - constexpr int MJ_ENBL_MULTICCD = 1 << 4; - constexpr int MJ_ENBL_SLEEP = 1 << 5; - Out->SetBoolField(TEXT("enable_multiccd"), (m->opt.enableflags & MJ_ENBL_MULTICCD) != 0); - Out->SetBoolField(TEXT("enable_sleep"), (m->opt.enableflags & MJ_ENBL_SLEEP) != 0); - Out->SetNumberField(TEXT("sleep_tolerance"), m->opt.sleep_tolerance); - - // Echo the raw bit masks so callers using disableflags / enableflags - // can verify the final composed state (named overrides + raw mask). - Out->SetNumberField(TEXT("disableflags"), (int32)m->opt.disableflags); - Out->SetNumberField(TEXT("enableflags"), (int32)m->opt.enableflags); - - Reply->SetObjectField(TEXT("options"), Out); - return Reply; -} - -// ============================================================================= -// set_sim_speed -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleSetSimSpeed(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - - double Pct = 0.0; - if (!Req->TryGetNumberField(TEXT("percent"), Pct)) - return MakeError(TEXT("missing_field"), TEXT("set_sim_speed requires 'percent'")); - - // Engine clamps internally (5..100); echo back so the caller sees what stuck. - Mgr->PhysicsEngine->SimSpeedPercent = (float)Pct; - const float Effective = FMath::Clamp((float)Pct, 5.0f, 100.0f); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_sim_speed_ok")); - Reply->SetNumberField(TEXT("percent"), Effective); - return Reply; -} - -// ============================================================================= -// set_control_source -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleSetControlSource(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine) - return MakeError(TEXT("not_ready"), TEXT("PhysicsEngine not initialised")); - - FString SourceStr; - if (!Req->TryGetStringField(TEXT("source"), SourceStr)) - return MakeError(TEXT("missing_field"), TEXT("set_control_source requires 'source' (\"zmq\" | \"ui\")")); - - EControlSource NewSource; - if (SourceStr.Equals(TEXT("zmq"), ESearchCase::IgnoreCase)) - NewSource = EControlSource::ZMQ; - else if (SourceStr.Equals(TEXT("ui"), ESearchCase::IgnoreCase)) - NewSource = EControlSource::UI; - else - return MakeError(TEXT("bad_value"), FString::Printf(TEXT("unknown source '%s'"), *SourceStr)); - - FString ArtName; - Req->TryGetStringField(TEXT("articulation"), ArtName); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_control_source_ok")); - Reply->SetStringField(TEXT("source"), SourceStr.ToLower()); - - if (ArtName.IsEmpty()) - { - // Global: update engine + every articulation so the per-actor field - // doesn't keep stale state after a global flip. - Mgr->PhysicsEngine->SetControlSource(NewSource); - for (AMjArticulation* Art : Mgr->GetAllArticulations()) - { - if (Art) - Art->ControlSource = (uint8)NewSource; - } - Reply->SetStringField(TEXT("scope"), TEXT("global")); - } - else - { - AMjArticulation* Art = Mgr->GetArticulation(ArtName); - if (!Art) - return MakeError(TEXT("unknown_articulation"), ArtName); - Art->ControlSource = (uint8)NewSource; - Reply->SetStringField(TEXT("scope"), TEXT("articulation")); - Reply->SetStringField(TEXT("articulation"), ArtName); - } - return Reply; -} - -// ============================================================================= -// set_twist -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleSetTwist(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return MakeError(TEXT("not_ready"), TEXT("Manager missing")); - - FString ArtName; - if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) - return MakeError(TEXT("missing_field"), TEXT("set_twist requires 'articulation'")); - - AMjArticulation* Art = Mgr->GetArticulation(ArtName); - if (!Art) - return MakeError(TEXT("unknown_articulation"), ArtName); - - UMjTwistController* TC = Art->FindComponentByClass(); - if (!TC) - return MakeError(TEXT("no_twist_controller"), - FString::Printf(TEXT("Articulation '%s' has no UMjTwistController"), *ArtName)); - - // Wire format mirrors how the bridge already reads twist: linear is - // (vx, vy, _) m/s, angular is (_, _, yaw_rate) rad/s. Tuple slots - // beyond the ones used are accepted but ignored. - auto ReadAxis = [](const TArray>* Arr, int32 Idx, float& Out) { - if (Arr && Arr->IsValidIndex(Idx)) - Out = (float)(*Arr)[Idx]->AsNumber(); - }; - - float Vx = 0.f, Vy = 0.f, YawRate = 0.f; - const TArray>* LinArr = nullptr; - const TArray>* AngArr = nullptr; - Req->TryGetArrayField(TEXT("linear"), LinArr); - Req->TryGetArrayField(TEXT("angular"), AngArr); - ReadAxis(LinArr, 0, Vx); - ReadAxis(LinArr, 1, Vy); - ReadAxis(AngArr, 2, YawRate); - - TC->SetTwist(Vx, Vy, YawRate); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_twist_ok")); - Reply->SetStringField(TEXT("articulation"), ArtName); - { - TArray> L; - L.Add(MakeShared(Vx)); - L.Add(MakeShared(Vy)); - L.Add(MakeShared(0.0)); - Reply->SetArrayField(TEXT("linear"), L); - TArray> A; - A.Add(MakeShared(0.0)); - A.Add(MakeShared(0.0)); - A.Add(MakeShared(YawRate)); - Reply->SetArrayField(TEXT("angular"), A); - } - return Reply; -} - -// ============================================================================= -// set_qpos — manager-required runtime write to a single articulation's qpos. -// -// Two write modes: -// - Free-base 7-vec shortcut: len=7 and the first joint is mjJNT_FREE, -// writes only the 7 free-joint slots (xyz + quat). Skips dof joints. -// - Full per-articulation qpos: len matches the articulation's total qpos -// dim (sum of per-joint slot widths in GetJoints() order). Writes the -// whole slice. -// Always calls mj_forward after the write, mirroring the puppet push-state -// path so derived quantities (xpos, sensors) reflect the new state. -// ============================================================================= -TSharedPtr FURLabRpcDispatcher::HandleSetQpos(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) - return MakeError(TEXT("not_ready"), TEXT("Manager not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - if (!m || !d) - return MakeError(TEXT("not_ready"), TEXT("MjModel/MjData missing")); - - // target/target_by wire shape. target_by="actor_name" looks up via - // the manager's GetArticulation (UE name match); default - // "actor_id" walks ActorId. - FString Target, By; - Req->TryGetStringField(TEXT("target"), Target); - Req->TryGetStringField(TEXT("target_by"), By); - if (Target.IsEmpty()) - { - return MakeError(TEXT("missing_field"), - TEXT("set_qpos: missing 'target' field")); - } - const bool bByName = By.Equals(TEXT("actor_name"), ESearchCase::IgnoreCase); - - AMjArticulation* Art = nullptr; - if (bByName) - { - Art = Mgr->GetArticulation(Target); - } - else - { - for (AMjArticulation* A : Mgr->GetAllArticulations()) - { - if (A && A->ActorId.Equals(Target)) - { - Art = A; - break; - } - } - } - if (!Art) - { - return MakeError(TEXT("unknown_articulation"), Target); - } - - const TArray>* QPosArr = nullptr; - if (!Req->TryGetArrayField(TEXT("qpos"), QPosArr) || !QPosArr) - return MakeError(TEXT("missing_field"), TEXT("set_qpos requires 'qpos' array")); - - struct FJointSlot - { - int32 Adr; - int32 Size; - int32 Type; - }; - TArray Slots; - int32 ArtQDim = 0; - for (UMjJoint* J : Art->GetJoints()) - { - if (!J) - continue; - int32 Id = J->GetMjID(); - if (Id < 0 || Id >= m->njnt) - continue; - int32 Size = 1; - switch (m->jnt_type[Id]) - { - case mjJNT_FREE: - Size = 7; - break; - case mjJNT_BALL: - Size = 4; - break; - case mjJNT_SLIDE: - case mjJNT_HINGE: - Size = 1; - break; - } - Slots.Add({m->jnt_qposadr[Id], Size, m->jnt_type[Id]}); - ArtQDim += Size; - } - - if (Slots.Num() == 0) - return MakeError(TEXT("no_joints"), - TEXT("Articulation has no joints; nothing to write")); - - const int32 InN = QPosArr->Num(); - bool bFreeBaseShortcut = false; - if (InN == 7 && Slots[0].Type == mjJNT_FREE && ArtQDim != 7) - bFreeBaseShortcut = true; - else if (InN != ArtQDim) - return MakeError(TEXT("dim_mismatch"), - FString::Printf( - TEXT("qpos length %d != articulation qpos dim %d (free-base shortcut requires len=7 with FREE root)"), - InN, ArtQDim)); - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - if (bFreeBaseShortcut) - { - const int32 Adr = Slots[0].Adr; - for (int32 i = 0; i < 7; ++i) - d->qpos[Adr + i] = (mjtNum)(*QPosArr)[i]->AsNumber(); - } - else - { - int32 Cursor = 0; - for (const FJointSlot& S : Slots) - { - for (int32 i = 0; i < S.Size; ++i, ++Cursor) - d->qpos[S.Adr + i] = (mjtNum)(*QPosArr)[Cursor]->AsNumber(); - } - } - mj_forward(m, d); - } - - TArray> Out; - if (bFreeBaseShortcut) - { - const int32 Adr = Slots[0].Adr; - for (int32 i = 0; i < 7; ++i) - Out.Add(MakeShared(d->qpos[Adr + i])); - } - else - { - for (const FJointSlot& S : Slots) - for (int32 i = 0; i < S.Size; ++i) - Out.Add(MakeShared(d->qpos[S.Adr + i])); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_qpos_ok")); - // Echo back the resolved actor identifiers so the caller can - // confirm which articulation actually got the write. `target` - // matches the request's target field; `actor_name` is the UE - // name (always present, even if actor_id was the lookup key). - Reply->SetStringField(TEXT("target"), Target); - Reply->SetStringField(TEXT("actor_name"), Art->GetName()); - if (!Art->ActorId.IsEmpty()) - Reply->SetStringField(TEXT("actor_id"), Art->ActorId); - Reply->SetArrayField(TEXT("qpos"), Out); - Reply->SetBoolField(TEXT("free_base_shortcut"), bFreeBaseShortcut); - return Reply; -} - -// ============================================================================= -// set_mocap_pose / read_mocap_pose / get_contacts — runtime MJ-side reads/writes. -// -// All three operate directly on the live mjModel/mjData under the engine's -// CallbackMutex (same as set_qpos). Body name lookup uses mj_name2id with -// the full compiled MJ name (URLab prefixes are already included). -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleSetMocapPose(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) - return MakeError(TEXT("not_ready"), TEXT("Manager not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - if (!m || !d) - return MakeError(TEXT("not_ready"), TEXT("MjModel/MjData missing")); - - FString Body; - Req->TryGetStringField(TEXT("body"), Body); - if (Body.IsEmpty()) - return MakeError(TEXT("missing_field"), TEXT("set_mocap_pose: missing 'body'")); - - const int32 BodyId = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*Body)); - if (BodyId < 0) - return MakeError(TEXT("unknown_body"), Body); - - const int32 MocapId = m->body_mocapid[BodyId]; - if (MocapId < 0) - return MakeError(TEXT("not_mocap_body"), - FString::Printf(TEXT("Body '%s' is not a mocap body"), *Body)); - - const TArray>* PosArr = nullptr; - const TArray>* QuatArr = nullptr; - const bool bHasPos = Req->TryGetArrayField(TEXT("pos"), PosArr) && PosArr && PosArr->Num() == 3; - const bool bHasQuat = Req->TryGetArrayField(TEXT("quat"), QuatArr) && QuatArr && QuatArr->Num() == 4; - if (!bHasPos && !bHasQuat) - return MakeError(TEXT("missing_field"), - TEXT("set_mocap_pose requires at least one of pos[3] or quat[4]")); - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - if (bHasPos) - { - for (int32 i = 0; i < 3; ++i) - d->mocap_pos[3 * MocapId + i] = (mjtNum)(*PosArr)[i]->AsNumber(); - } - if (bHasQuat) - { - for (int32 i = 0; i < 4; ++i) - d->mocap_quat[4 * MocapId + i] = (mjtNum)(*QuatArr)[i]->AsNumber(); - } - } - - TArray> PosOut, QuatOut; - for (int32 i = 0; i < 3; ++i) - PosOut.Add(MakeShared(d->mocap_pos[3 * MocapId + i])); - for (int32 i = 0; i < 4; ++i) - QuatOut.Add(MakeShared(d->mocap_quat[4 * MocapId + i])); - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("set_mocap_pose_ok")); - Reply->SetStringField(TEXT("body"), Body); - Reply->SetArrayField(TEXT("pos"), PosOut); - Reply->SetArrayField(TEXT("quat"), QuatOut); - return Reply; -} - -TSharedPtr FURLabRpcDispatcher::HandleReadMocapPose(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) - return MakeError(TEXT("not_ready"), TEXT("Manager not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - if (!m || !d) - return MakeError(TEXT("not_ready"), TEXT("MjModel/MjData missing")); - - FString Body; - Req->TryGetStringField(TEXT("body"), Body); - if (Body.IsEmpty()) - return MakeError(TEXT("missing_field"), TEXT("read_mocap_pose: missing 'body'")); - - const int32 BodyId = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*Body)); - if (BodyId < 0) - return MakeError(TEXT("unknown_body"), Body); - - const int32 MocapId = m->body_mocapid[BodyId]; - if (MocapId < 0) - return MakeError(TEXT("not_mocap_body"), - FString::Printf(TEXT("Body '%s' is not a mocap body"), *Body)); - - TArray> PosOut, QuatOut; - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - for (int32 i = 0; i < 3; ++i) - PosOut.Add(MakeShared(d->mocap_pos[3 * MocapId + i])); - for (int32 i = 0; i < 4; ++i) - QuatOut.Add(MakeShared(d->mocap_quat[4 * MocapId + i])); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("read_mocap_pose_ok")); - Reply->SetStringField(TEXT("body"), Body); - Reply->SetArrayField(TEXT("pos"), PosOut); - Reply->SetArrayField(TEXT("quat"), QuatOut); - return Reply; -} - -TSharedPtr FURLabRpcDispatcher::HandleGetContacts(const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) - return MakeError(TEXT("not_ready"), TEXT("Manager not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - mjData* d = Mgr->PhysicsEngine->GetData(); - if (!m || !d) - return MakeError(TEXT("not_ready"), TEXT("MjModel/MjData missing")); - - int32 MaxContacts = 64; - { - int32 Cap = 0; - if (Req->TryGetNumberField(TEXT("max_contacts"), Cap) && Cap > 0) - MaxContacts = Cap; - } - - // Optional filter: {body1?, body2?, geom1?, geom2?}. AND across set fields. - FString FBody1, FBody2, FGeom1, FGeom2; - const TSharedPtr* FilterObj = nullptr; - if (Req->TryGetObjectField(TEXT("filter"), FilterObj) && FilterObj && *FilterObj) - { - (*FilterObj)->TryGetStringField(TEXT("body1"), FBody1); - (*FilterObj)->TryGetStringField(TEXT("body2"), FBody2); - (*FilterObj)->TryGetStringField(TEXT("geom1"), FGeom1); - (*FilterObj)->TryGetStringField(TEXT("geom2"), FGeom2); - } - - auto NameOrEmpty = [&](int Type, int Id) -> FString { - if (Id < 0) - return FString(); - const char* p = mj_id2name(m, Type, Id); - return p ? FString(UTF8_TO_TCHAR(p)) : FString(); - }; - - TArray> Out; - bool bTruncated = false; - int32 Matched = 0; - - { - FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); - const int32 N = d->ncon; - for (int32 i = 0; i < N; ++i) - { - const mjContact& c = d->contact[i]; - const int32 G1 = c.geom[0]; - const int32 G2 = c.geom[1]; - const int32 B1 = (G1 >= 0 && G1 < m->ngeom) ? m->geom_bodyid[G1] : -1; - const int32 B2 = (G2 >= 0 && G2 < m->ngeom) ? m->geom_bodyid[G2] : -1; - const FString G1Name = NameOrEmpty(mjOBJ_GEOM, G1); - const FString G2Name = NameOrEmpty(mjOBJ_GEOM, G2); - const FString B1Name = NameOrEmpty(mjOBJ_BODY, B1); - const FString B2Name = NameOrEmpty(mjOBJ_BODY, B2); - - if (!FGeom1.IsEmpty() && !G1Name.Equals(FGeom1)) - continue; - if (!FGeom2.IsEmpty() && !G2Name.Equals(FGeom2)) - continue; - if (!FBody1.IsEmpty() && !B1Name.Equals(FBody1)) - continue; - if (!FBody2.IsEmpty() && !B2Name.Equals(FBody2)) - continue; - - if (Matched >= MaxContacts) - { - bTruncated = true; - break; - } - - mjtNum Force[6] = {0}; - mj_contactForce(m, d, i, Force); - - TArray> Pos; - for (int32 k = 0; k < 3; ++k) - Pos.Add(MakeShared(c.pos[k])); - // First row of the contact frame is the contact normal. - TArray> Normal; - for (int32 k = 0; k < 3; ++k) - Normal.Add(MakeShared(c.frame[k])); - TArray> ForceArr; - for (int32 k = 0; k < 6; ++k) - ForceArr.Add(MakeShared(Force[k])); - - TSharedPtr CObj = MakeShared(); - CObj->SetStringField(TEXT("geom1"), G1Name); - CObj->SetStringField(TEXT("geom2"), G2Name); - CObj->SetStringField(TEXT("body1"), B1Name); - CObj->SetStringField(TEXT("body2"), B2Name); - CObj->SetArrayField(TEXT("pos"), Pos); - CObj->SetArrayField(TEXT("normal"), Normal); - CObj->SetNumberField(TEXT("dist"), c.dist); - CObj->SetArrayField(TEXT("force"), ForceArr); - Out.Add(MakeShared(CObj)); - ++Matched; - } - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("get_contacts_ok")); - Reply->SetNumberField(TEXT("n_contacts"), Matched); - Reply->SetBoolField(TEXT("truncated"), bTruncated); - Reply->SetArrayField(TEXT("contacts"), Out); - return Reply; -} - -TSharedPtr FURLabRpcDispatcher::HandleListKeyframes(const TSharedPtr& /*Req*/) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) - return MakeError(TEXT("not_ready"), TEXT("Manager not initialised")); - - mjModel* m = Mgr->PhysicsEngine->GetModel(); - if (!m) - return MakeError(TEXT("not_ready"), TEXT("MjModel missing")); - - auto Slice = [](const mjtNum* src, int32 stride, int32 idx, int32 width) { - TArray> Out; - if (!src || width <= 0) - return Out; - for (int32 k = 0; k < width; ++k) - Out.Add(MakeShared(src[idx * stride + k])); - return Out; - }; - - TArray> Keys; - for (int32 i = 0; i < m->nkey; ++i) - { - const char* NameC = mj_id2name(m, mjOBJ_KEY, i); - TSharedPtr K = MakeShared(); - K->SetStringField(TEXT("name"), NameC ? UTF8_TO_TCHAR(NameC) : TEXT("")); - K->SetNumberField(TEXT("time"), m->key_time ? m->key_time[i] : 0.0); - K->SetArrayField(TEXT("qpos"), Slice(m->key_qpos, m->nq, i, m->nq)); - K->SetArrayField(TEXT("qvel"), Slice(m->key_qvel, m->nv, i, m->nv)); - K->SetArrayField(TEXT("ctrl"), Slice(m->key_ctrl, m->nu, i, m->nu)); - K->SetArrayField(TEXT("mocap_pos"), Slice(m->key_mpos, m->nmocap * 3, i, m->nmocap * 3)); - K->SetArrayField(TEXT("mocap_quat"), Slice(m->key_mquat, m->nmocap * 4, i, m->nmocap * 4)); - Keys.Add(MakeShared(K)); - } - - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("list_keyframes_ok")); - Reply->SetArrayField(TEXT("keyframes"), Keys); - return Reply; -} - -// ============================================================================= -// recording_* / replay_* delegate to AMjReplayManager -// ============================================================================= - -TSharedPtr FURLabRpcDispatcher::HandleRecording(const FString& Op, const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return MakeError(TEXT("not_ready"), TEXT("Manager missing")); - // Use the game-thread-cached pointer; TActorIterator from this worker - // thread would assert IsInGameThread() and crash. - AMjReplayManager* RM = CachedReplayManager.Get(); - if (!RM) - return MakeError(TEXT("not_ready"), TEXT("AMjReplayManager not present in scene")); - - if (Op.Equals(TEXT("recording_start"))) - { - if (RM->bIsRecording) - return MakeError(TEXT("recording_already_active"), TEXT("Recording already active")); - double MaxDur = 0.0; - if (Req->TryGetNumberField(TEXT("max_duration_s"), MaxDur)) - RM->MaxRecordDuration = (float)MaxDur; - else - RM->MaxRecordDuration = FLT_MAX; - RM->StartRecording(); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("recording_start_ok")); - Reply->SetStringField(TEXT("name"), AMjReplayManager::LiveSessionName); - Reply->SetNumberField(TEXT("max_duration_s"), RM->MaxRecordDuration); - return Reply; - } - if (Op.Equals(TEXT("recording_stop"))) - { - if (!RM->bIsRecording) - return MakeError(TEXT("recording_not_active"), TEXT("Recording is not active")); - RM->StopRecording(); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("recording_stop_ok")); - Reply->SetStringField(TEXT("name"), AMjReplayManager::LiveSessionName); - // Populate the summary fields the Python client maps to - // RecordingSummary. Recording has been stopped (StopRecording - // above set bIsRecording=false) so the OnPostStep hook isn't - // mutating Frames in parallel. - Reply->SetNumberField(TEXT("frame_count"), static_cast(RM->GetLiveFrameCount())); - Reply->SetNumberField(TEXT("sim_duration_s"), RM->GetLiveSimDurationS()); - return Reply; - } - if (Op.Equals(TEXT("recording_save"))) - { - FString Path; - Req->TryGetStringField(TEXT("path"), Path); - FString FileName = Path.IsEmpty() ? TEXT("recording.json") : FPaths::GetCleanFilename(Path); - if (Path.IsEmpty()) - Path = FileName; - bool bOk = RM->SaveRecordingToFile(FileName); - // ResolveReplayPath now matches the manager's - // ProjectSavedDir/URLab/Replays/ output dir, so the absolute_path - // returned to the client points at the file the manager actually wrote. - FString Abs = ResolveReplayPath(Path); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), bOk ? TEXT("recording_save_ok") : TEXT("error")); - Reply->SetStringField(TEXT("absolute_path"), Abs); - if (!bOk) - Reply->SetStringField(TEXT("code"), TEXT("path_not_writable")); - return Reply; - } - if (Op.Equals(TEXT("recording_clear"))) - { - RM->ClearRecording(); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("recording_clear_ok")); - return Reply; - } - return MakeError(TEXT("unknown_op"), Op); -} - -TSharedPtr FURLabRpcDispatcher::HandleReplay(const FString& Op, const TSharedPtr& Req) -{ - AAMjManager* Mgr = OwnerMgr.Get(); - if (!Mgr) - return MakeError(TEXT("not_ready"), TEXT("Manager missing")); - AMjReplayManager* RM = CachedReplayManager.Get(); - if (!RM) - return MakeError(TEXT("not_ready"), TEXT("AMjReplayManager not present in scene")); - - if (Op.Equals(TEXT("replay_load"))) - { - FString P; - if (!Req->TryGetStringField(TEXT("path"), P)) - return MakeError(TEXT("missing_field"), TEXT("replay_load requires 'path'")); - FString FileName = FPaths::GetCleanFilename(P); - bool bOk = RM->LoadRecordingFromFile(FileName); - if (!bOk) - return MakeError(TEXT("path_not_readable"), P); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("replay_load_ok")); - Reply->SetStringField(TEXT("name"), FPaths::GetBaseFilename(FileName)); - return Reply; - } - if (Op.Equals(TEXT("replay_list_sessions"))) - { - TArray> Names; - for (const FString& N : RM->GetSessionNames()) - Names.Add(MakeShared(N)); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("replay_list_sessions_ok")); - Reply->SetArrayField(TEXT("sessions"), Names); - return Reply; - } - if (Op.Equals(TEXT("replay_set_active"))) - { - FString N; - if (!Req->TryGetStringField(TEXT("name"), N)) - return MakeError(TEXT("missing_field"), TEXT("replay_set_active requires 'name'")); - if (!RM->Sessions.Contains(N)) - return MakeError(TEXT("replay_session_not_found"), N); - RM->SetActiveSession(N); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("replay_set_active_ok")); - return Reply; - } - if (Op.Equals(TEXT("replay_start"))) - { - if (ActiveStepMode == EStepMode::Live) - return MakeError(TEXT("replay_requires_stepped"), - TEXT("Switch to direct or puppet before starting replay")); - RM->StartReplay(); - int32 Total = RM->Sessions.Contains(RM->GetActiveSessionName()) - ? RM->Sessions[RM->GetActiveSessionName()].Frames.Num() - : 0; - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("replay_start_ok")); - Reply->SetStringField(TEXT("active_session"), RM->GetActiveSessionName()); - Reply->SetNumberField(TEXT("total_frames"), Total); - return Reply; - } - if (Op.Equals(TEXT("replay_stop"))) - { - RM->StopReplay(); - TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("replay_stop_ok")); - return Reply; - } - return MakeError(TEXT("unknown_op"), Op); -} diff --git a/Source/URLab/Private/Bridge/RpcHandlerCommon.h b/Source/URLab/Private/Bridge/RpcHandlerCommon.h new file mode 100644 index 00000000..dbe269c2 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlerCommon.h @@ -0,0 +1,22 @@ +// Shared includes for RPC handler translation units. Each RpcHandlers_*.cpp +// previously copy-pasted ~30 includes; this header carries the common set. +// Handler-specific headers (e.g. Async/Async.h for Camera) stay in the +// individual .cpp files. +#pragma once + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/OpRegistry.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/MsgpackHelpers.h" +#include "State/MjStateTypes.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Controllers/MjArticulationController.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Transport/NetworkManager.h" +#include "Utils/URLabLogging.h" diff --git a/Source/URLab/Private/Bridge/RpcHandlers_Camera.cpp b/Source/URLab/Private/Bridge/RpcHandlers_Camera.cpp new file mode 100644 index 00000000..d23b94c2 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_Camera.cpp @@ -0,0 +1,588 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/OpRegistry.h" +#include "Bridge/MsgpackHelpers.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Controllers/MjArticulationController.h" +#include "MuJoCo/Input/MjPerturbation.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Transport/NetworkManager.h" +#include "Transport/ShmPublishTransport.h" +#include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Transport/ShmRegion.h" // FMjShmHeader (header_size in shm_rpc block) +#include "Bridge/BridgeServer.h" +#include "Replay/MjReplayManager.h" +#include "Kismet/GameplayStatics.h" +#include "Misc/Base64.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "Internationalization/Regex.h" +#include "HAL/FileManager.h" +#include "EngineUtils.h" +#include "Engine/World.h" +#include "Misc/Guid.h" +#include "Utils/URLabLogging.h" +#include "Async/Async.h" +#include "RenderingThread.h" + +bool FURLabRpcDispatcher::RenderCamerasSync(AAMjManager* Mgr, + const TMap& CameraSpec, + uint64 MinFrameId, int32 TimeoutMs, + TMap& CameraMinFrameIds, + bool bWait) +{ + if (CameraSpec.Num() == 0 || !Mgr) + return true; + if (bWait && MinFrameId == 0) + return true; + + TArray Keys; + CameraSpec.GetKeys(Keys); + + // Kick the render on the game thread: apply the just-produced physics snapshot, + // ensure each requested camera is streaming, and force an immediate capture. + // For the fresh (bWait) path the task then drives the readback to completion so + // the frame lands in ~render+readback time rather than waiting for the next + // editor tick to harvest it (that tick is background-throttled when the editor + // is unfocused, which added ~a full frame of latency). IssueSyncCapture submits + // the GPU copy to the RHI thread immediately, so the readback fence signals + // without a frame boundary and this pump finishes quickly; it takes no + // render-thread flush and is bounded by the request timeout (the worker wait + // below is the real deadline), so it cannot wedge the game thread the way the + // earlier sole-path, non-submitting poll did. Pipelined mode (!bWait) never + // pumps: it returns at once and serves the most-recently-completed frame. + TWeakObjectPtr WeakMgr(Mgr); + AsyncTask(ENamedThreads::GameThread, [WeakMgr, Keys, MinFrameId, TimeoutMs, bWait]() { + AAMjManager* M = WeakMgr.Get(); + if (!M) + return; + + M->ApplyLatestRenderState(); + + TMap ByName; + BuildCameraNameMap(M, ByName); + TArray Cams; + for (const FString& K : Keys) + { + if (UMjCamera* Cam = ByName.FindRef(K)) + { + if (!Cam->IsStreamingActive()) + Cam->SetStreamingEnabled(true); + Cams.Add(Cam); + } + } + + for (UMjCamera* Cam : Cams) + Cam->IssueSyncCapture(); + + if (!bWait) + return; // pipelined: kick only, no pump + + // Bound the game-thread pump under the request timeout: it needs to cover + // render + readback (and a cold RT's one-time warm-up) for every requested + // camera, and capping it means a genuinely stuck frame frees the game thread + // rather than freezing it for the whole timeout. The budget scales with the + // camera count -- each camera is a separate scene render + readback, so a + // single fixed cap (tuned for one camera) starved multi-camera requests and + // dropped their tail onto the slow off-thread wait, inflating latency under + // load. The worker wait below, off the game thread, remains the real deadline + // for anything the pump does not finish. + const int32 MaxPumpMs = FMath::Max(60, Cams.Num() * 50); + const double Deadline = + FPlatformTime::Seconds() + FMath::Min(FMath::Max(1, TimeoutMs), MaxPumpMs) / 1000.0; + for (;;) + { + bool bAllReady = true; + for (UMjCamera* Cam : Cams) + { + Cam->HarvestCompletedReadbacks(); + if (Cam->GetLatestFrameId() < MinFrameId) + { + bAllReady = false; + // Re-issue only while nothing is outstanding, so a cold RT that + // was not renderable on the first attempt retries without piling + // captures behind an in-flight one. + if (!Cam->HasPendingReadbacks()) + Cam->IssueSyncCapture(); + } + } + if (bAllReady || FPlatformTime::Seconds() >= Deadline) + break; + FPlatformProcess::SleepNoStats(0.0002f); + } + }); + + // Pipelined mode serves the most-recently-completed frame (up to one step + // stale), so it does not wait for the kicked frame. + if (!bWait) + return true; + + // Fresh mode: record the reached cameras so BuildCamerasBlock serves this step's + // frame. WaitForCameraFrames reads the thread-safe history and returns as soon + // as the frame is present, which the game-thread pump above has already made + // true for a warm camera. + return WaitForCameraFrames(Mgr, CameraSpec, MinFrameId, TimeoutMs, CameraMinFrameIds); +} + +bool FURLabRpcDispatcher::WaitForCameraFrames(AAMjManager* Mgr, + const TMap& CameraSpec, + uint64 MinFrameId, int32 TimeoutMs, + TMap& CameraMinFrameIds) +{ + if (MinFrameId == 0 || CameraSpec.Num() == 0) + return true; + + TMap ByName; + BuildCameraNameMap(Mgr, ByName); + + const double Deadline = FPlatformTime::Seconds() + FMath::Max(0, TimeoutMs) / 1000.0; + bool bAllReady = true; + for (const TPair& Spec : CameraSpec) + { + UMjCamera* Cam = ByName.FindRef(Spec.Key); + if (!Cam) + { + bAllReady = false; + continue; + } + // Mark it consumed so per-camera capture gating keeps it live. + Cam->TouchRequested(); + while (Cam->GetLatestFrameId() < MinFrameId + && FPlatformTime::Seconds() < Deadline + && !bDraining.load(std::memory_order_acquire)) + { + FPlatformProcess::SleepNoStats(0.002f); + } + if (Cam->GetLatestFrameId() >= MinFrameId) + CameraMinFrameIds.Add(Spec.Key, MinFrameId); + else + bAllReady = false; + } + return bAllReady; +} + +void FURLabRpcDispatcher::BuildCameraNameMap(AAMjManager* Manager, + TMap& OutByName) +{ + if (!Manager) + return; + // One canonical identity per camera: "/" from FMjCanonicalName, + // matching the zmq_topic the hello handshake advertises and the camera binds. + // First writer wins so a sanitize-collision can't hide an already-registered + // distinct camera. + auto AddCanonical = [&OutByName](UMjCamera* C) { + if (!C || C->bIsDefault) + return; + UMjCamera*& Slot = OutByName.FindOrAdd(C->GetCanonicalName()); + if (Slot == nullptr) + Slot = C; + }; + for (AMjArticulation* Art : Manager->GetAllArticulations()) + { + if (!Art) + continue; + TArray Cameras; + Art->GetComponents(Cameras); + for (UMjCamera* C : Cameras) + AddCanonical(C); + } + // Manager-owned (global) cameras: not attached to any articulation. Their + // canonical name uses the owning actor name as the art segment (see + // UMjCamera::GetCanonicalName). Kept in sync with the include_cameras:true + // walk in ParseStepCommon so a global camera the client asks for resolves + // here instead of being dropped. + TArray GlobalCameras; + Manager->GetComponents(GlobalCameras); + for (UMjCamera* C : GlobalCameras) + AddCanonical(C); +} + +TSharedPtr FURLabRpcDispatcher::ApplyCameraStreamingGameThread( + AAMjManager* Manager, const TMap>& Requests) +{ + // Game thread: render target + ZMQ/SHM workers must be set up here. + check(IsInGameThread()); + TSharedPtr Out = MakeShared(); + if (!Manager) + return Out; + + TMap ByName; + BuildCameraNameMap(Manager, ByName); + + for (const TPair>& Req : Requests) + { + UMjCamera** Found = ByName.Find(Req.Key); + if (!Found || !*Found) + { + UE_LOG(LogURLabNet, Warning, + TEXT("[set_camera_streaming] camera '%s' not found"), *Req.Key); + continue; + } + UMjCamera* Cam = *Found; + const bool bZmq = Req.Value.Key; + const bool bShm = Req.Value.Value; + + const bool bStream = bZmq || bShm; + // Idempotent: only (re)build when the requested flags actually change. + // SetStreamingEnabled(false) destroys the RenderTarget, so toggling on + // every call orphans any consumer bound to it -- the editor Simulate + // camera-feed widget then freezes on its last frame. Clients call this + // on every discover/set_mode, so unchanged requests MUST be no-ops. + const bool bUnchanged = (Cam->bEnableZmqBroadcast == bZmq) + && (Cam->bEnableShmBroadcast == bShm) + && (Cam->IsStreamingActive() == bStream); + if (!bUnchanged) + { + Cam->bEnableZmqBroadcast = bZmq; + Cam->bEnableShmBroadcast = bShm; + Cam->SetStreamingEnabled(false); + if (bStream) + Cam->SetStreamingEnabled(true); + } + + TSharedPtr CamObj = MakeShared(); + CamObj->SetBoolField(TEXT("streaming"), bStream); + CamObj->SetBoolField(TEXT("zmq"), bZmq); + CamObj->SetBoolField(TEXT("shm"), bShm); + if (bZmq) + { + FString Endpoint = Cam->GetActualZmqEndpoint(); + Endpoint.ReplaceInline(TEXT("*"), TEXT("127.0.0.1")); + CamObj->SetStringField(TEXT("zmq_endpoint"), Endpoint); + CamObj->SetStringField(TEXT("zmq_topic"), Cam->GetCanonicalName()); + } + // Key the reply by the canonical name so the bridge always gets a + // stable identity back regardless of which alias it requested. + Out->SetObjectField(Cam->GetCanonicalName(), CamObj); + } + return Out; +} + +TSharedPtr FURLabRpcDispatcher::HandleSetCameraStreaming( + const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + const TSharedPtr* CamObj = nullptr; + if (!Req->TryGetObjectField(TEXT("cameras"), CamObj) || !CamObj || !CamObj->IsValid()) + return MakeError(URLabError::MissingField, + TEXT("set_camera_streaming requires a 'cameras' object")); + + // Parse per-camera requests. Value forms: + // true / false -> both transports on / off + // { "zmq": b, "shm": b } -> per-transport; an omitted sub-field is off + // UNLESS both are omitted, which means "both on" + TMap> Requests; // key -> {zmq, shm} + for (const TPair>& Kv : (*CamObj)->Values) + { + if (!Kv.Value.IsValid()) + continue; + bool bZmq = true; + bool bShm = true; + bool bBool = false; + const TSharedPtr* Sub = nullptr; + if (Kv.Value->TryGetBool(bBool)) + { + bZmq = bBool; + bShm = bBool; + } + else if (Kv.Value->TryGetObject(Sub) && Sub && Sub->IsValid()) + { + bool z = false, s = false; + const bool bHasZ = (*Sub)->TryGetBoolField(TEXT("zmq"), z); + const bool bHasS = (*Sub)->TryGetBoolField(TEXT("shm"), s); + if (bHasZ || bHasS) + { + bZmq = bHasZ ? z : false; + bShm = bHasS ? s : false; + } + // else leave both true (default = enable both) + } + Requests.Add(Kv.Key, TPair(bZmq, bShm)); + } + + if (Requests.Num() == 0) + return MakeError(URLabError::BadRequest, TEXT("'cameras' had no usable entries")); + + // Camera RT / worker setup is game-thread only; marshal and wait. + struct FResult + { + FEvent* Done = nullptr; + TSharedPtr Cameras; + }; + TSharedPtr Res = MakeShared(); + Res->Done = FPlatformProcess::GetSynchEventFromPool(/*bIsManualReset=*/false); + TWeakObjectPtr WeakMgr(Mgr); + AsyncTask(ENamedThreads::GameThread, [Res, WeakMgr, Requests]() { + if (AAMjManager* M = WeakMgr.Get()) + Res->Cameras = ApplyCameraStreamingGameThread(M, Requests); + Res->Done->Trigger(); + }); + + TSharedPtr Reply = MakeShared(); + if (Res->Done->Wait(5000)) + { + FPlatformProcess::ReturnSynchEventToPool(Res->Done); + Res->Done = nullptr; + Reply->SetStringField(TEXT("op"), TEXT("set_camera_streaming_ok")); + Reply->SetObjectField(TEXT("cameras"), + Res->Cameras.IsValid() ? Res->Cameras : MakeShared()); + return Reply; + } + // Timed out — leave the event un-pooled (the task still references it). + return MakeError(URLabError::Timeout, TEXT("set_camera_streaming game-thread apply timed out")); +} + +namespace +{ +// Parsed per-camera latency + capture-rate request. bSetCapture marks whether +// the on_state_change / max_fps capture knobs were present (so we only override +// them when the caller actually sent them). +struct FCameraDelayReq +{ + float DelaySeconds = 0.0f; + float JitterSeconds = 0.0f; + bool bWallClock = false; + int32 Seed = 0; + bool bOnStateChange = true; + float MaxFps = 0.0f; + bool bSetCapture = false; +}; +} // namespace + +TSharedPtr FURLabRpcDispatcher::HandleSetCameraDelay( + const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + const TSharedPtr* CamObj = nullptr; + if (!Req->TryGetObjectField(TEXT("cameras"), CamObj) || !CamObj || !CamObj->IsValid()) + return MakeError(URLabError::MissingField, + TEXT("set_camera_delay requires a 'cameras' object")); + + // Parse per-camera requests. A bare number means delay_s with defaults; an + // object carries delay_s / jitter_s / clock / seed / on_state_change / max_fps. + TMap Requests; + for (const TPair>& Kv : (*CamObj)->Values) + { + if (!Kv.Value.IsValid()) + continue; + FCameraDelayReq R; + double Num = 0.0; + const TSharedPtr* Sub = nullptr; + if (Kv.Value->TryGetNumber(Num)) + { + R.DelaySeconds = static_cast(Num); + } + else if (Kv.Value->TryGetObject(Sub) && Sub && Sub->IsValid()) + { + double D = 0.0; + if ((*Sub)->TryGetNumberField(TEXT("delay_s"), D)) + R.DelaySeconds = static_cast(D); + double J = 0.0; + if ((*Sub)->TryGetNumberField(TEXT("jitter_s"), J)) + R.JitterSeconds = static_cast(J); + FString Clock; + if ((*Sub)->TryGetStringField(TEXT("clock"), Clock)) + R.bWallClock = Clock.Equals(TEXT("wall"), ESearchCase::IgnoreCase); + int32 Seed = 0; + if ((*Sub)->TryGetNumberField(TEXT("seed"), Seed)) + R.Seed = Seed; + bool bOnState = true; + if ((*Sub)->TryGetBoolField(TEXT("on_state_change"), bOnState)) + { + R.bOnStateChange = bOnState; + R.bSetCapture = true; + } + double Fps = 0.0; + if ((*Sub)->TryGetNumberField(TEXT("max_fps"), Fps)) + { + R.MaxFps = static_cast(Fps); + R.bSetCapture = true; + } + } + Requests.Add(Kv.Key, R); + } + + if (Requests.Num() == 0) + return MakeError(URLabError::BadRequest, TEXT("'cameras' had no usable entries")); + + // SetCameraDelay / SetCaptureRate touch state the game thread reads each tick; + // marshal and wait, like set_camera_streaming. + struct FResult + { + FEvent* Done = nullptr; + TSharedPtr Cameras; + }; + TSharedPtr Res = MakeShared(); + Res->Done = FPlatformProcess::GetSynchEventFromPool(/*bIsManualReset=*/false); + TWeakObjectPtr WeakMgr(Mgr); + AsyncTask(ENamedThreads::GameThread, [Res, WeakMgr, Requests]() { + TSharedPtr Out = MakeShared(); + if (AAMjManager* M = WeakMgr.Get()) + { + TMap ByName; + FURLabRpcDispatcher::BuildCameraNameMap(M, ByName); + for (const TPair& Req : Requests) + { + UMjCamera** Found = ByName.Find(Req.Key); + if (!Found || !*Found) + { + UE_LOG(LogURLabNet, Warning, + TEXT("[set_camera_delay] camera '%s' not found"), *Req.Key); + continue; + } + UMjCamera* Cam = *Found; + const FCameraDelayReq& V = Req.Value; + Cam->SetCameraDelay(V.DelaySeconds, V.JitterSeconds, V.bWallClock, V.Seed); + if (V.bSetCapture) + Cam->SetCaptureRate(V.bOnStateChange, V.MaxFps); + + TSharedPtr CamOut = MakeShared(); + CamOut->SetNumberField(TEXT("delay_s"), Cam->DelaySeconds); + CamOut->SetNumberField(TEXT("jitter_s"), Cam->DelayJitterSeconds); + CamOut->SetStringField(TEXT("clock"), + Cam->bDelayUseWallClock ? TEXT("wall") : TEXT("sim")); + CamOut->SetBoolField(TEXT("on_state_change"), Cam->bCaptureOnStateChange); + CamOut->SetNumberField(TEXT("max_fps"), Cam->CaptureMaxFps); + Out->SetObjectField(Cam->GetCanonicalName(), CamOut); + } + } + Res->Cameras = Out; + Res->Done->Trigger(); + }); + + TSharedPtr Reply = MakeShared(); + if (Res->Done->Wait(5000)) + { + FPlatformProcess::ReturnSynchEventToPool(Res->Done); + Res->Done = nullptr; + Reply->SetStringField(TEXT("op"), TEXT("set_camera_delay_ok")); + Reply->SetObjectField(TEXT("cameras"), + Res->Cameras.IsValid() ? Res->Cameras : MakeShared()); + return Reply; + } + return MakeError(URLabError::Timeout, TEXT("set_camera_delay game-thread apply timed out")); +} + +TSharedPtr FURLabRpcDispatcher::BuildCamerasBlock(AAMjManager* Manager, + const TMap& CameraSpec, + const TMap& MinFrameIds, int32 TimeoutMs) +{ + (void)TimeoutMs; // retrieval is non-blocking; param kept for ABI compatibility + TSharedPtr Cams = MakeShared(); + if (!Manager || CameraSpec.Num() == 0) + return Cams; + + UWorld* World = Manager->GetWorld(); + if (!World) + return Cams; + + TMap ByName; + BuildCameraNameMap(Manager, ByName); + + // Non-blocking retrieval from each camera's frame-history ring. No capture, + // no FlushRenderingCommands — the per-tick async readback (decoupled from + // the step, like MuJoCo simulate's render thread) keeps the ring fresh. + // A client gets the frame for a specific step by passing its frame_id + // (MinFrameIds); otherwise it gets the latest available frame. + for (const TPair& Spec : CameraSpec) + { + UMjCamera** Found = ByName.Find(Spec.Key); + if (!Found || !*Found) + { + UE_LOG(LogURLabNet, Warning, + TEXT("[BuildCamerasBlock] camera '%s' not found; dropped from reply"), + *Spec.Key); + continue; + } + UMjCamera* Cam = *Found; + + // Mark consumed so per-camera capture gating keeps this camera live. + Cam->TouchRequested(); + + const uint64* MinIdPtr = MinFrameIds.Find(Spec.Key); + const uint64 MinId = MinIdPtr ? *MinIdPtr : 0; + + // A camera requested in "sync" mode wants the freshest rendered (ground + // truth) frame; a plain "latest" request should instead agree with what + // the stream is publishing, which under latency emulation is the delayed + // past. GetFrameForRequest routes both correctly and hands the retained + // frame back by refcount, so no multi-MB pixel copy happens under the + // history lock. + const bool bIgnoreDelay = (Spec.Value == ECameraInclude::Sync); + TSharedPtr Frame = Cam->GetFrameForRequest(MinId, bIgnoreDelay); + if (!Frame.IsValid()) + { + // Not ready yet (camera just activated, or the requested step's + // frame hasn't been rendered/read back). Omit; the client retries + // on a later step. First request of a dormant camera always misses. + continue; + } + + TSharedPtr CamObj = MakeShared(); + CamObj->SetNumberField(TEXT("width"), Frame->Width); + CamObj->SetNumberField(TEXT("height"), Frame->Height); + CamObj->SetNumberField(TEXT("frame_id"), static_cast(Frame->FrameId)); + CamObj->SetNumberField(TEXT("sim_time"), Frame->SimTime); + + if (Cam->CaptureMode == EMjCameraMode::Depth) + { + if (Frame->Depth.Num() == 0) + continue; + CamObj->SetStringField(TEXT("dtype"), TEXT("float32")); + // Zero-copy: pack the pixel bytes straight from the shared frame (no + // base64, no intermediate FString). The frame is retained as the keeper + // until the reply is packed, so the buffer stays valid. + FURLabMsgpackUtil::SetBinaryFieldShared(CamObj, TEXT("data"), + reinterpret_cast(Frame->Depth.GetData()), + Frame->Depth.Num() * sizeof(float), Frame); + } + else + { + // Real / SemSeg / InstanceSeg all ship 4-byte BGRA. Bridge + // discriminates the seg modes by the camera_topics handshake. + if (Frame->Color.Num() == 0) + continue; + CamObj->SetStringField(TEXT("dtype"), TEXT("bgra8")); + FURLabMsgpackUtil::SetBinaryFieldShared(CamObj, TEXT("data"), + reinterpret_cast(Frame->Color.GetData()), + Frame->Color.Num() * sizeof(FColor), Frame); + } + Cams->SetObjectField(Spec.Key, CamObj); + } + return Cams; +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_Control.cpp b/Source/URLab/Private/Bridge/RpcHandlers_Control.cpp new file mode 100644 index 00000000..6ce97af3 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_Control.cpp @@ -0,0 +1,245 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "State/MjStateTypes.h" + +namespace +{ +// Parse one inbound user-channel value into an IR channel. The inferred kind only +// distinguishes the text family from the numeric family; the declaring component +// stores the value under its own declared kind. Returns false with a reason for a +// shape no input kind accepts. A bare object without pos/quat (a generic struct) +// is rejected: only declared typed channels accept input. +bool JsonValueToUserChannel(const TSharedPtr& Value, FMjUserChannel& Out, + FString& OutReason) +{ + if (!Value.IsValid()) + { + OutReason = TEXT("null_value"); + return false; + } + switch (Value->Type) + { + case EJson::Boolean: + Out.Kind = EMjUserChannelKind::Bool; + Out.Values = {Value->AsBool() ? 1.0 : 0.0}; + return true; + case EJson::Number: + Out.Kind = EMjUserChannelKind::Scalar; + Out.Values = {Value->AsNumber()}; + return true; + case EJson::String: + Out.Kind = EMjUserChannelKind::String; + Out.Text = Value->AsString(); + return true; + case EJson::Array: + { + Out.Kind = EMjUserChannelKind::Array; + for (const TSharedPtr& E : Value->AsArray()) + Out.Values.Add(E.IsValid() ? E->AsNumber() : 0.0); + return true; + } + case EJson::Object: + { + // {pos:[3], quat:[4]} is a transform; anything else is unsupported input. + const TSharedPtr Obj = Value->AsObject(); + const TArray>* Pos = nullptr; + const TArray>* Quat = nullptr; + if (Obj.IsValid() && Obj->TryGetArrayField(TEXT("pos"), Pos) + && Obj->TryGetArrayField(TEXT("quat"), Quat)) + { + Out.Kind = EMjUserChannelKind::Transform; + Out.Values.SetNumZeroed(7); + for (int32 i = 0; i < 3 && i < Pos->Num(); ++i) + Out.Values[i] = (*Pos)[i].IsValid() ? (*Pos)[i]->AsNumber() : 0.0; + for (int32 i = 0; i < 4 && i < Quat->Num(); ++i) + Out.Values[3 + i] = (*Quat)[i].IsValid() ? (*Quat)[i]->AsNumber() : 0.0; + return true; + } + OutReason = TEXT("unsupported_object"); + return false; + } + default: + OutReason = TEXT("unsupported_value"); + return false; + } +} +} // namespace + +FString FURLabRpcDispatcher::ResolveControlSource(const TSharedPtr& Req) const +{ + FString Source; + if (Req->TryGetStringField(TEXT("source"), Source) && !Source.IsEmpty()) + return Source; + Req->TryGetStringField(TEXT("session_id"), Source); + return Source; +} + +TSharedPtr FURLabRpcDispatcher::RejectIfNotControlOwner(FName ArtKey, + const TSharedPtr& Req) +{ + const FString Source = ResolveControlSource(Req); + FString CurrentOwner; + if (ControlOwnership.CheckWrite(ArtKey, Source, CurrentOwner) + == FMjControlOwnership::EWriteCheck::Ok) + { + return nullptr; + } + + TSharedPtr Err = MakeError(TEXT("not_control_owner"), + FString::Printf(TEXT("%s owned by %s"), *ArtKey.ToString(), *CurrentOwner)); + Err->SetStringField(TEXT("owner"), CurrentOwner); + return Err; +} + +TSharedPtr FURLabRpcDispatcher::HandleClaimControl(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + FString ArtName; + if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) + return MakeError(URLabError::MissingField, TEXT("claim_control requires 'articulation'")); + + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + return MakeError(URLabError::UnknownArticulation, ArtName); + + const FName Key(*Art->GetName()); + const FString Source = ResolveControlSource(Req); + + double Ttl = 0.0; + Req->TryGetNumberField(TEXT("ttl_s"), Ttl); + bool bForce = false; + Req->TryGetBoolField(TEXT("force"), bForce); + + FString CurrentOwner; + if (ControlOwnership.Claim(Key, Source, Ttl, bForce, CurrentOwner) + == FMjControlOwnership::EClaimResult::AlreadyOwned) + { + TSharedPtr Err = MakeError(TEXT("control_claimed"), + FString::Printf(TEXT("%s already owned by %s"), *Key.ToString(), *CurrentOwner)); + Err->SetStringField(TEXT("owner"), CurrentOwner); + return Err; + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("claim_control_ok")); + Reply->SetStringField(TEXT("articulation"), Art->GetName()); + Reply->SetStringField(TEXT("owner"), Source); + Reply->SetNumberField(TEXT("ttl_s"), Ttl); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleReleaseControl(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + FString ArtName; + if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) + return MakeError(URLabError::MissingField, TEXT("release_control requires 'articulation'")); + + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + return MakeError(URLabError::UnknownArticulation, ArtName); + + const FName Key(*Art->GetName()); + const FString Source = ResolveControlSource(Req); + + if (!ControlOwnership.Release(Key, Source)) + { + FString CurrentOwner; + ControlOwnership.CheckWrite(Key, Source, CurrentOwner); + TSharedPtr Err = MakeError(TEXT("not_control_owner"), + FString::Printf(TEXT("%s owned by %s"), *Key.ToString(), *CurrentOwner)); + Err->SetStringField(TEXT("owner"), CurrentOwner); + return Err; + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("release_control_ok")); + Reply->SetStringField(TEXT("articulation"), Art->GetName()); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleSetUserChannels(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + int32 Applied = 0; + TSharedPtr Rejected = MakeShared(); + + // One scope's channel map: {channel: value}. ArtOrNone is the canonical art + // segment (None for scene). Undeclared names and unparseable / kind-mismatched + // values land in `rejected` rather than failing the whole batch. + auto ApplyScope = [&](const TSharedPtr& Channels, FName ArtOrNone, + const FString& RejectPrefix) { + if (!Channels.IsValid()) + return; + for (const TPair>& Pair : Channels->Values) + { + const FName Channel(*Pair.Key); + FMjUserChannel Value; + FString Reason; + if (!JsonValueToUserChannel(Pair.Value, Value, Reason)) + { + Rejected->SetStringField(RejectPrefix + Pair.Key, Reason); + continue; + } + if (Mgr->ApplyUserChannelInput(ArtOrNone, Channel, Value)) + ++Applied; + else + Rejected->SetStringField(RejectPrefix + Pair.Key, + TEXT("undeclared_or_kind_mismatch")); + } + }; + + const TSharedPtr* ArtsObj = nullptr; + if (Req->TryGetObjectField(TEXT("arts"), ArtsObj)) + { + for (const TPair>& ArtPair : (*ArtsObj)->Values) + { + const TSharedPtr* ArtChannels = nullptr; + if (ArtPair.Value.IsValid() && ArtPair.Value->TryGetObject(ArtChannels)) + ApplyScope(*ArtChannels, FName(*ArtPair.Key), ArtPair.Key + TEXT("/")); + } + } + + const TSharedPtr* SceneObj = nullptr; + if (Req->TryGetObjectField(TEXT("scene"), SceneObj)) + ApplyScope(*SceneObj, NAME_None, TEXT("scene/")); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_user_channels_ok")); + Reply->SetNumberField(TEXT("applied"), Applied); + Reply->SetObjectField(TEXT("rejected"), Rejected); + return Reply; +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_Lease.cpp b/Source/URLab/Private/Bridge/RpcHandlers_Lease.cpp new file mode 100644 index 00000000..6859cf12 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_Lease.cpp @@ -0,0 +1,91 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/BridgeServer.h" + +// Cooperative render-farm lease ops. A lease is a claim, not a security +// boundary: one client leases an instance so a pool won't hand the same +// editor process to two clients. One lease per process. Auto-release is +// TTL-based (an idle lease past its TTL frees on the next check); socket-level +// disconnect detection is a Phase 5 hardening item, deferred. + +TSharedPtr FURLabRpcDispatcher::HandleAcquireLease(const TSharedPtr& Req) +{ + UURLabBridgeServer* Bridge = OwningBridge.Get(); + if (!Bridge) + { + return MakeError(URLabError::NotReady, + TEXT("lease state is unavailable (no bridge server)")); + } + + FString Owner; + if (Req.IsValid()) + Req->TryGetStringField(TEXT("owner"), Owner); + + double TtlSeconds = 60.0; + if (Req.IsValid()) + Req->TryGetNumberField(TEXT("ttl_s"), TtlSeconds); + + FString AcquiredLeaseId; + FString ExistingLeaseId; + if (!Bridge->TryAcquireLease(Owner, TtlSeconds, AcquiredLeaseId, ExistingLeaseId)) + { + TSharedPtr Err = MakeError(TEXT("busy"), + TEXT("instance is already leased")); + Err->SetStringField(TEXT("lease_id"), ExistingLeaseId); + return Err; + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("acquire_lease_ok")); + Reply->SetStringField(TEXT("lease_id"), AcquiredLeaseId); + Reply->SetNumberField(TEXT("ttl_s"), TtlSeconds); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleReleaseLease(const TSharedPtr& Req) +{ + UURLabBridgeServer* Bridge = OwningBridge.Get(); + if (!Bridge) + { + return MakeError(URLabError::NotReady, + TEXT("lease state is unavailable (no bridge server)")); + } + + FString LeaseId; + if (!Req.IsValid() || !Req->TryGetStringField(TEXT("lease_id"), LeaseId) || LeaseId.IsEmpty()) + { + return MakeError(URLabError::BadRequest, TEXT("release_lease requires a non-empty lease_id")); + } + + if (!Bridge->ReleaseLease(LeaseId)) + { + return MakeError(URLabError::BadRequest, + TEXT("lease_id does not match the held lease (or no lease is held)")); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("release_lease_ok")); + return Reply; +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_ModelUpload.cpp b/Source/URLab/Private/Bridge/RpcHandlers_ModelUpload.cpp new file mode 100644 index 00000000..80eaae1b --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_ModelUpload.cpp @@ -0,0 +1,636 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// Network model upload: a remote client ships raw MJCF XML bytes plus the asset +// bytes the XML references over ZMQ; the server materialises them to a temp dir +// and runs the EXISTING import_xml editor job on the result (no separate compile +// path). Three ops: +// upload_model_manifest content-addressed negotiation (which blobs are missing) +// upload_model_chunk bulk, chunked bytes; each completed blob is verified + cached +// upload_model_commit materialise + invoke import_xml + echo model dims/mjb +// The manifest/chunk ops are pure data staging on the RPC worker thread; only +// commit drives the editor import (via the shared op registry, so no URLabEditor +// link dependency). Assets reuse the vfs_assets msgpack-bin framing and the +// bare-filename flatten convention of the DOWN handshake. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/AssetCache.h" +#include "Bridge/OpRegistry.h" + +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "Bridge/MsgpackHelpers.h" +#include "Utils/URLabLogging.h" + +#include "HAL/FileManager.h" +#include "HAL/PlatformProcess.h" +#include "Misc/Base64.h" +#include "Misc/DateTime.h" +#include "Misc/FileHelper.h" +#include "Misc/Guid.h" +#include "Misc/Paths.h" +#include "Misc/ScopeLock.h" + +namespace +{ +// Security caps (plan 4.6). Advertised in the manifest reply and enforced on +// every chunk. +constexpr int64 kMaxAssetBytes = 64ll * 1024 * 1024; // 64 MiB per blob +constexpr int64 kMaxTotalBytes = 512ll * 1024 * 1024; // 512 MiB per upload +constexpr int32 kMaxAssets = 4096; +constexpr int32 kMaxOpenUploads = 16; +constexpr double kUploadTtlSeconds = 300.0; + +/** One asset's staging slot. The receive buffer only exists while incomplete; + * once verified it is written to the content-addressed cache and dropped, so + * materialisation reads the blob back from the cache by hash. */ +struct FAssetSlot +{ + FString Sha; + int64 DeclaredSize = 0; + TArray Buffer; + int64 Received = 0; + bool bComplete = false; +}; + +struct FStagingEntry +{ + double LastTouch = 0.0; + FString StepMode; + + FString XmlSha; + TArray XmlBuffer; + int64 XmlReceived = 0; + int64 XmlTotal = 0; + bool bXmlComplete = false; + + // keyed by bare filename + TMap Assets; +}; + +/** Process-wide staging store. One editor process is one instance, so uploads + * are per-process; the map is keyed by upload_id (FGuid) and guarded by a + * mutex. Concurrent-upload cap + TTL expiry live here. */ +class FUploadStaging +{ +public: + static FUploadStaging& Get() + { + static FUploadStaging Instance; + return Instance; + } + + FCriticalSection Mutex; + TMap Entries; + + /** Drop entries idle past the TTL. Caller holds Mutex. */ + void SweepExpired() + { + const double Now = FPlatformTime::Seconds(); + for (auto It = Entries.CreateIterator(); It; ++It) + { + if (Now - It->Value.LastTouch > kUploadTtlSeconds) + It.RemoveCurrent(); + } + } +}; + +/** True if Name is a bare filename safe to materialise next to the XML: no + * path separators, no parent refs, no drive letter, no leading separator, + * no NUL. */ +bool IsBareFilename(const FString& Name) +{ + if (Name.IsEmpty() || Name == TEXT(".")) + return false; + if (Name.Contains(TEXT("/")) || Name.Contains(TEXT("\\"))) + return false; + if (Name.Contains(TEXT(".."))) + return false; + // Drive letter (C:) or any embedded colon (alternate data stream / drive). + if (Name.Contains(TEXT(":"))) + return false; + for (const TCHAR C : Name) + { + if (C == TEXT('\0')) + return false; + } + return true; +} + +/** Read a msgpack-bin request field. On the wire a bin map-value arrives with + * a `__b64__`-suffixed key holding base64 (see MsgpackHelpers); accept a plain + * base64 string under the bare name too (JSON clients / tests). */ +bool ReadBinField(const TSharedPtr& Req, const FString& Name, TArray& Out) +{ + FString B64; + if (Req->TryGetStringField(Name + TEXT("__b64__"), B64) || Req->TryGetStringField(Name, B64)) + { + Out.Reset(); + return FBase64::Decode(B64, Out); + } + return false; +} +} // namespace + +TSharedPtr FURLabRpcDispatcher::HandleUploadModelManifest(const TSharedPtr& Req) +{ + FString XmlSha; + if (!Req->TryGetStringField(TEXT("xml_sha256"), XmlSha) + || !FURLabAssetCache::IsValidSha256Hex(XmlSha)) + { + return MakeError(URLabError::BadRequest, + TEXT("upload_model_manifest requires a valid hex 'xml_sha256'")); + } + + const TArray>* AssetsArr = nullptr; + Req->TryGetArrayField(TEXT("assets"), AssetsArr); + const int32 AssetCount = AssetsArr ? AssetsArr->Num() : 0; + if (AssetCount > kMaxAssets) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("manifest lists %d assets; cap is %d"), AssetCount, kMaxAssets)); + } + + double TotalBytes = 0.0; + Req->TryGetNumberField(TEXT("total_bytes"), TotalBytes); + if (static_cast(TotalBytes) > kMaxTotalBytes) + { + return MakeError(TEXT("payload_too_large"), + FString::Printf(TEXT("total_bytes %.0f exceeds max_total_bytes %lld"), + TotalBytes, kMaxTotalBytes)); + } + + FString StepMode; + Req->TryGetStringField(TEXT("step_mode"), StepMode); + + FURLabAssetCache& Cache = FURLabAssetCache::Get(); + + FStagingEntry Entry; + Entry.StepMode = StepMode; + Entry.XmlSha = XmlSha; + Entry.bXmlComplete = Cache.Has(XmlSha); + + TArray> NeedAssets; + for (int32 i = 0; i < AssetCount; ++i) + { + const TSharedPtr* AObj = nullptr; + if (!(*AssetsArr)[i]->TryGetObject(AObj) || !AObj) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("manifest asset[%d] is not an object"), i)); + } + + FString Name, Sha; + (*AObj)->TryGetStringField(TEXT("name"), Name); + (*AObj)->TryGetStringField(TEXT("sha256"), Sha); + double Size = 0.0; + (*AObj)->TryGetNumberField(TEXT("size"), Size); + + if (!IsBareFilename(Name)) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("asset name '%s' is not a bare filename"), *Name)); + } + if (!FURLabAssetCache::IsValidSha256Hex(Sha)) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("asset '%s' has an invalid sha256"), *Name)); + } + if (static_cast(Size) > kMaxAssetBytes) + { + return MakeError(TEXT("payload_too_large"), + FString::Printf(TEXT("asset '%s' size %.0f exceeds max_asset_bytes %lld"), + *Name, Size, kMaxAssetBytes)); + } + + FAssetSlot Slot; + Slot.Sha = Sha; + Slot.DeclaredSize = static_cast(Size); + Slot.bComplete = Cache.Has(Sha); + if (!Slot.bComplete) + NeedAssets.Add(MakeShared(Name)); + Entry.Assets.Add(Name, MoveTemp(Slot)); + } + + const FString UploadId = FGuid::NewGuid().ToString(EGuidFormats::DigitsWithHyphens); + Entry.LastTouch = FPlatformTime::Seconds(); + + { + FUploadStaging& Staging = FUploadStaging::Get(); + FScopeLock Lock(&Staging.Mutex); + Staging.SweepExpired(); + if (Staging.Entries.Num() >= kMaxOpenUploads) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("too many concurrent uploads (cap %d); retry later"), + kMaxOpenUploads)); + } + Staging.Entries.Add(UploadId, MoveTemp(Entry)); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("upload_model_manifest_ok")); + Reply->SetStringField(TEXT("upload_id"), UploadId); + Reply->SetBoolField(TEXT("need_xml"), !Cache.Has(XmlSha)); + Reply->SetArrayField(TEXT("need_assets"), NeedAssets); + Reply->SetNumberField(TEXT("max_asset_bytes"), static_cast(kMaxAssetBytes)); + Reply->SetNumberField(TEXT("max_total_bytes"), static_cast(kMaxTotalBytes)); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleUploadModelChunk(const TSharedPtr& Req) +{ + // The reassembled blob is verified against the manifest-declared hash, not + // the chunk's own sha256 field, so that field is accepted but not consulted. + FString UploadId, Kind, Name; + Req->TryGetStringField(TEXT("upload_id"), UploadId); + Req->TryGetStringField(TEXT("kind"), Kind); + Req->TryGetStringField(TEXT("name"), Name); + + double OffsetD = 0.0, TotalD = 0.0; + Req->TryGetNumberField(TEXT("offset"), OffsetD); + Req->TryGetNumberField(TEXT("total"), TotalD); + const int64 Offset = static_cast(OffsetD); + const int64 Total = static_cast(TotalD); + + if (UploadId.IsEmpty() || (Kind != TEXT("xml") && Kind != TEXT("asset"))) + { + return MakeError(URLabError::BadRequest, + TEXT("upload_model_chunk requires 'upload_id' and kind 'xml'|'asset'")); + } + if (Offset < 0 || Total < 0) + { + return MakeError(URLabError::BadRequest, TEXT("offset/total must be non-negative")); + } + if (Total > kMaxAssetBytes) + { + return MakeError(TEXT("payload_too_large"), + FString::Printf(TEXT("blob total %lld exceeds max_asset_bytes %lld"), + Total, kMaxAssetBytes)); + } + + TArray Data; + if (!ReadBinField(Req, TEXT("data"), Data)) + { + return MakeError(URLabError::BadRequest, TEXT("upload_model_chunk missing binary 'data'")); + } + if (Offset + Data.Num() > Total) + { + return MakeError(URLabError::BadRequest, + TEXT("chunk offset+len exceeds declared total")); + } + if (Offset + Data.Num() > kMaxAssetBytes) + { + return MakeError(TEXT("payload_too_large"), + FString::Printf(TEXT("chunk end %lld exceeds max_asset_bytes %lld"), + Offset + Data.Num(), kMaxAssetBytes)); + } + + FURLabAssetCache& Cache = FURLabAssetCache::Get(); + FUploadStaging& Staging = FUploadStaging::Get(); + FScopeLock Lock(&Staging.Mutex); + + FStagingEntry* Entry = Staging.Entries.Find(UploadId); + if (!Entry || FPlatformTime::Seconds() - Entry->LastTouch > kUploadTtlSeconds) + { + if (Entry) + Staging.Entries.Remove(UploadId); + return MakeError(TEXT("unknown_upload"), + FString::Printf(TEXT("upload_id '%s' is unknown or expired"), *UploadId)); + } + Entry->LastTouch = FPlatformTime::Seconds(); + + // Route to the XML slot or a named asset slot. + TArray* Buffer = nullptr; + int64* Received = nullptr; + bool* bComplete = nullptr; + FString DeclaredSha; + int64* XmlTotalPtr = nullptr; + + if (Kind == TEXT("xml")) + { + Buffer = &Entry->XmlBuffer; + Received = &Entry->XmlReceived; + bComplete = &Entry->bXmlComplete; + DeclaredSha = Entry->XmlSha; + XmlTotalPtr = &Entry->XmlTotal; + } + else + { + FAssetSlot* Slot = Entry->Assets.Find(Name); + if (!Slot) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("asset '%s' was not declared in the manifest"), *Name)); + } + if (Slot->DeclaredSize != Total) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("asset '%s' chunk total %lld != manifest size %lld"), + *Name, Total, Slot->DeclaredSize)); + } + Buffer = &Slot->Buffer; + Received = &Slot->Received; + bComplete = &Slot->bComplete; + DeclaredSha = Slot->Sha; + } + + if (XmlTotalPtr) + *XmlTotalPtr = Total; + + // Already satisfied (e.g. resolved from the cache at manifest time): ack. + if (*bComplete && Buffer->Num() == 0) + { + TSharedPtr Ack = MakeShared(); + Ack->SetStringField(TEXT("op"), TEXT("upload_model_chunk_ok")); + Ack->SetStringField(TEXT("name"), Kind == TEXT("xml") ? TEXT("") : Name); + Ack->SetNumberField(TEXT("received"), static_cast(Total)); + Ack->SetBoolField(TEXT("complete"), true); + return Ack; + } + + const int32 TotalI = static_cast(Total); // <= kMaxAssetBytes, fits int32 + if (Buffer->Num() != TotalI) + { + Buffer->SetNumZeroed(TotalI); + *Received = 0; + } + if (Data.Num() > 0) + FMemory::Memcpy(Buffer->GetData() + Offset, Data.GetData(), Data.Num()); + *Received += Data.Num(); + + bool bJustCompleted = false; + if (*Received >= Total) + { + const FString ActualSha = FURLabAssetCache::Sha256Hex(*Buffer); + if (ActualSha != DeclaredSha) + { + // Reset the slot so the client can retry cleanly. + Buffer->Reset(); + *Received = 0; + return MakeError(TEXT("hash_mismatch"), + FString::Printf(TEXT("%s sha256 %s does not match declared %s"), + *(Kind == TEXT("xml") ? FString(TEXT("xml")) : Name), *ActualSha, *DeclaredSha)); + } + Cache.Put(DeclaredSha, *Buffer); + Buffer->Empty(); // blob now lives in the cache; free staging memory + *bComplete = true; + bJustCompleted = true; + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("upload_model_chunk_ok")); + Reply->SetStringField(TEXT("name"), Kind == TEXT("xml") ? TEXT("") : Name); + Reply->SetNumberField(TEXT("received"), + static_cast(bJustCompleted ? Total : *Received)); + Reply->SetBoolField(TEXT("complete"), *bComplete); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleUploadModelCommit(const TSharedPtr& Req) +{ + FString UploadId; + Req->TryGetStringField(TEXT("upload_id"), UploadId); + if (UploadId.IsEmpty()) + return MakeError(URLabError::BadRequest, TEXT("upload_model_commit requires 'upload_id'")); + + // Snapshot the staging entry (xml sha, asset name->sha, step mode) under the + // lock, verify completeness, then release before the slow materialise+import. + FString XmlSha, StepMode; + TMap AssetShaByName; + { + FUploadStaging& Staging = FUploadStaging::Get(); + FScopeLock Lock(&Staging.Mutex); + FStagingEntry* Entry = Staging.Entries.Find(UploadId); + if (!Entry || FPlatformTime::Seconds() - Entry->LastTouch > kUploadTtlSeconds) + { + if (Entry) + Staging.Entries.Remove(UploadId); + return MakeError(TEXT("unknown_upload"), + FString::Printf(TEXT("upload_id '%s' is unknown or expired"), *UploadId)); + } + if (!Entry->bXmlComplete) + return MakeError(URLabError::BadRequest, TEXT("commit before the xml blob completed")); + for (const TPair& Kv : Entry->Assets) + { + if (!Kv.Value.bComplete) + { + return MakeError(URLabError::BadRequest, + FString::Printf(TEXT("commit before asset '%s' completed"), *Kv.Key)); + } + AssetShaByName.Add(Kv.Key, Kv.Value.Sha); + } + XmlSha = Entry->XmlSha; + StepMode = Entry->StepMode; + Entry->LastTouch = FPlatformTime::Seconds(); + } + + // Materialise to a per-process temp working dir. Keyed by upload_id under the + // instance's own ProjectIntermediateDir so farm instances never collide. + FURLabAssetCache& Cache = FURLabAssetCache::Get(); + const FString WorkDir = FPaths::Combine(FPaths::ProjectIntermediateDir(), + TEXT("URLabUpload"), UploadId); + IFileManager::Get().MakeDirectory(*WorkDir, /*Tree=*/true); + + auto Cleanup = [&WorkDir, &UploadId]() { + IFileManager::Get().DeleteDirectory(*WorkDir, /*RequireExists=*/false, /*Tree=*/true); + FUploadStaging& Staging = FUploadStaging::Get(); + FScopeLock Lock(&Staging.Mutex); + Staging.Entries.Remove(UploadId); + }; + + auto CopyBlob = [&Cache](const FString& Sha, const FString& Dest, FString& OutErr) -> bool { + FString SrcPath; + if (!Cache.GetPath(Sha, SrcPath)) + { + OutErr = FString::Printf(TEXT("blob %s missing from cache"), *Sha); + return false; + } + if (IFileManager::Get().Copy(*Dest, *SrcPath) != COPY_OK) + { + OutErr = FString::Printf(TEXT("failed to materialise %s"), *Dest); + return false; + } + return true; + }; + + const FString XmlPath = FPaths::Combine(WorkDir, TEXT("model.xml")); + FString CopyErr; + if (!CopyBlob(XmlSha, XmlPath, CopyErr)) + { + Cleanup(); + return MakeError(TEXT("import_failed"), CopyErr); + } + for (const TPair& Kv : AssetShaByName) + { + const FString Dest = FPaths::Combine(WorkDir, Kv.Key); + if (!CopyBlob(Kv.Value, Dest, CopyErr)) + { + Cleanup(); + return MakeError(TEXT("import_failed"), CopyErr); + } + } + + // Run the EXISTING import_xml editor job on the materialised temp XML. It is + // registered (by URLabEditor) as an async game-thread job returning + // op_started + job_id; we drive it to completion by polling op_status, the + // same contract a remote client uses. No separate compile path. + URLabOpRegistry::FHandler ImportFn = URLabOpRegistry::GetHandler(TEXT("import_xml")); + URLabOpRegistry::FHandler StatusFn = URLabOpRegistry::GetHandler(TEXT("op_status")); + if (!ImportFn || !StatusFn) + { + Cleanup(); + return MakeError(TEXT("import_failed"), + TEXT("import_xml op is unavailable (editor module not loaded)")); + } + + TSharedPtr ImportReq = MakeShared(); + ImportReq->SetStringField(TEXT("op"), TEXT("import_xml")); + ImportReq->SetStringField(TEXT("path"), FPaths::ConvertRelativePathToFull(XmlPath)); + ImportReq->SetBoolField(TEXT("force_reimport"), true); + + TSharedPtr Started = ImportFn(ImportReq); + FString JobId, StartedOp; + if (Started.IsValid()) + { + Started->TryGetStringField(TEXT("op"), StartedOp); + Started->TryGetStringField(TEXT("job_id"), JobId); + } + + // A synchronous handler (no async job) returns the terminal reply directly. + TSharedPtr ImportResult; + if (StartedOp == TEXT("op_started") && !JobId.IsEmpty()) + { + // Poll op_status until terminal. No hard deadline (imports vary wildly); + // the client's recv timeout is the operational bound, and a draining + // bridge aborts us early. + for (;;) + { + if (IsDraining()) + { + Cleanup(); + return MakeError(URLabError::ShuttingDown, + TEXT("bridge draining; upload_model_commit abandoned")); + } + TSharedPtr StatusReq = MakeShared(); + StatusReq->SetStringField(TEXT("op"), TEXT("op_status")); + StatusReq->SetStringField(TEXT("job_id"), JobId); + TSharedPtr Status = StatusFn(StatusReq); + FString State; + if (Status.IsValid()) + Status->TryGetStringField(TEXT("state"), State); + if (State == TEXT("done") || State == TEXT("failed")) + { + const TSharedPtr* ResObj = nullptr; + if (Status->TryGetObjectField(TEXT("result"), ResObj) && ResObj) + ImportResult = *ResObj; + break; + } + if (State.IsEmpty()) // job vanished (unknown_job) + { + break; + } + FPlatformProcess::Sleep(0.05f); + } + } + else + { + ImportResult = Started; + } + + FString ResultOp, ResultErr; + if (ImportResult.IsValid()) + { + ImportResult->TryGetStringField(TEXT("op"), ResultOp); + ImportResult->TryGetStringField(TEXT("message"), ResultErr); + } + if (ResultOp != TEXT("import_xml_ok")) + { + Cleanup(); + return MakeError(TEXT("import_failed"), + ResultErr.IsEmpty() ? TEXT("import_xml did not complete") : ResultErr); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("upload_model_commit_ok")); + Reply->SetBoolField(TEXT("imported"), true); + { + FString ClassPath, ShortName; + ImportResult->TryGetStringField(TEXT("blueprint_class_path"), ClassPath); + ImportResult->TryGetStringField(TEXT("blueprint_short_name"), ShortName); + if (!ClassPath.IsEmpty()) + Reply->SetStringField(TEXT("blueprint_class_path"), ClassPath); + if (!ShortName.IsEmpty()) + Reply->SetStringField(TEXT("blueprint_short_name"), ShortName); + } + + // Echo model dims + mjb when a live manager already holds a compiled model + // (i.e. a scene is running). A fresh import only produces the Blueprint + // asset; standing up a live model requires PIE + spawn, which the client + // drives after commit. Absent a live model these fields are omitted. + if (!StepMode.IsEmpty()) + { + EStepMode Mode = EStepMode::Auto; + bool bHaveMode = true; + if (StepMode == TEXT("live")) + Mode = EStepMode::Live; + else if (StepMode == TEXT("direct")) + Mode = EStepMode::Direct; + else if (StepMode == TEXT("puppet")) + Mode = EStepMode::Puppet; + else + bHaveMode = false; + if (bHaveMode && OwnerMgr.IsValid()) + SetActiveStepMode(Mode); + } + + if (AAMjManager* Mgr = OwnerMgr.Get()) + { + if (Mgr->PhysicsEngine) + { + if (mjModel* m = Mgr->PhysicsEngine->GetModel()) + { + Reply->SetNumberField(TEXT("nq"), m->nq); + Reply->SetNumberField(TEXT("nv"), m->nv); + Reply->SetNumberField(TEXT("nu"), m->nu); + Reply->SetNumberField(TEXT("nbody"), m->nbody); + Reply->SetNumberField(TEXT("ngeom"), m->ngeom); + const int Sz = mj_sizeModel(m); + TArray Buf; + Buf.SetNum(Sz); + mj_saveModel(m, nullptr, Buf.GetData(), Sz); + FURLabMsgpackUtil::SetBinaryField(Reply, TEXT("mjb"), Buf.GetData(), Sz); + } + } + } + Reply->SetArrayField(TEXT("warnings"), TArray>()); + + UE_LOG(LogURLabNet, Log, TEXT("upload_model_commit: imported %s (%d assets)"), + *UploadId, AssetShaByName.Num()); + + // Opportunistic LRU eviction (no-op unless URLAB_ASSET_CACHE_MAX_BYTES is set). + Cache.EvictToBudget(); + + Cleanup(); + return Reply; +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_Scene.cpp b/Source/URLab/Private/Bridge/RpcHandlers_Scene.cpp new file mode 100644 index 00000000..73c7fb73 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_Scene.cpp @@ -0,0 +1,668 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/OpRegistry.h" +#include "Bridge/MsgpackHelpers.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Controllers/MjArticulationController.h" +#include "MuJoCo/Input/MjPerturbation.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Transport/NetworkManager.h" +#include "Transport/ShmPublishTransport.h" +#include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Transport/ShmRegion.h" // FMjShmHeader (header_size in shm_rpc block) +#include "Bridge/BridgeServer.h" +#include "Replay/MjReplayManager.h" +#include "Kismet/GameplayStatics.h" +#include "Misc/Base64.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "Internationalization/Regex.h" +#include "HAL/FileManager.h" +#include "EngineUtils.h" +#include "Engine/World.h" +#include "Misc/Guid.h" +#include "Utils/URLabLogging.h" + +namespace +{ +/** Resolve a save / load path. Bare filename -> /Saved/URLab/Replays/. + * Must match AMjReplayManager::SaveRecordingToFile so the + * recording_save_ok absolute_path actually resolves on the bridge. */ +FString ResolveReplayPath(const FString& UserPath, const FString& DefaultBaseName = TEXT("")) +{ + FString BaseDir = FPaths::ProjectSavedDir() / TEXT("URLab") / TEXT("Replays"); + IFileManager::Get().MakeDirectory(*BaseDir, true); + + FString Path = UserPath; + if (Path.IsEmpty()) + { + Path = DefaultBaseName.IsEmpty() ? TEXT("recording.json") : DefaultBaseName; + } + + if (FPaths::IsRelative(Path)) + { + Path = FPaths::Combine(BaseDir, Path); + } + return FPaths::ConvertRelativePathToFull(Path); +} +} // namespace + +// ============================================================================= +// set_qpos — manager-required runtime write to a single articulation's qpos. +// +// Two write modes: +// - Free-base 7-vec shortcut: len=7 and the first joint is mjJNT_FREE, +// writes only the 7 free-joint slots (xyz + quat). Skips dof joints. +// - Full per-articulation qpos: len matches the articulation's total qpos +// dim (sum of per-joint slot widths in GetJoints() order). Writes the +// whole slice. +// Always calls mj_forward after the write, mirroring the puppet push-state +// path so derived quantities (xpos, sensors) reflect the new state. +// ============================================================================= +TSharedPtr FURLabRpcDispatcher::HandleSetQpos(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) + return MakeError(URLabError::NotReady, TEXT("Manager not initialised")); + + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("MjModel/MjData missing")); + + // target/target_by wire shape. target_by="actor_name" looks up via + // the manager's GetArticulation (UE name match); default + // "actor_id" walks ActorId. + FString Target, By; + Req->TryGetStringField(TEXT("target"), Target); + Req->TryGetStringField(TEXT("target_by"), By); + if (Target.IsEmpty()) + { + return MakeError(URLabError::MissingField, + TEXT("set_qpos: missing 'target' field")); + } + const bool bByName = By.Equals(TEXT("actor_name"), ESearchCase::IgnoreCase); + + AMjArticulation* Art = nullptr; + if (bByName) + { + Art = Mgr->GetArticulation(Target); + } + else + { + for (AMjArticulation* A : Mgr->GetAllArticulations()) + { + if (A && A->ActorId.Equals(Target)) + { + Art = A; + break; + } + } + } + if (!Art) + { + return MakeError(URLabError::UnknownArticulation, Target); + } + + if (TSharedPtr Denied = RejectIfNotControlOwner(FName(*Art->GetName()), Req)) + return Denied; + + const TArray>* QPosArr = nullptr; + if (!Req->TryGetArrayField(TEXT("qpos"), QPosArr) || !QPosArr) + return MakeError(URLabError::MissingField, TEXT("set_qpos requires 'qpos' array")); + + struct FJointSlot + { + int32 Adr; + int32 Size; + int32 Type; + }; + TArray Slots; + int32 ArtQDim = 0; + for (UMjJoint* J : Art->GetJoints()) + { + if (!J) + continue; + int32 Id = J->GetMjID(); + if (Id < 0 || Id >= m->njnt) + continue; + int32 Size = 1; + switch (m->jnt_type[Id]) + { + case mjJNT_FREE: + Size = 7; + break; + case mjJNT_BALL: + Size = 4; + break; + case mjJNT_SLIDE: + case mjJNT_HINGE: + Size = 1; + break; + } + Slots.Add({m->jnt_qposadr[Id], Size, m->jnt_type[Id]}); + ArtQDim += Size; + } + + if (Slots.Num() == 0) + return MakeError(URLabError::NoJoints, + TEXT("Articulation has no joints; nothing to write")); + + const int32 InN = QPosArr->Num(); + bool bFreeBaseShortcut = false; + if (InN == 7 && Slots[0].Type == mjJNT_FREE && ArtQDim != 7) + bFreeBaseShortcut = true; + else if (InN != ArtQDim) + return MakeError(URLabError::DimMismatch, + FString::Printf( + TEXT("qpos length %d != articulation qpos dim %d (free-base shortcut requires len=7 with FREE root)"), + InN, ArtQDim)); + + // Read-back of the written qpos must stay inside the lock: a concurrent + // worker step (or a recompile) would otherwise tear the echoed values. + TArray> Out; + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + if (bFreeBaseShortcut) + { + const int32 Adr = Slots[0].Adr; + for (int32 i = 0; i < 7; ++i) + d->qpos[Adr + i] = (mjtNum)(*QPosArr)[i]->AsNumber(); + } + else + { + int32 Cursor = 0; + for (const FJointSlot& S : Slots) + { + for (int32 i = 0; i < S.Size; ++i, ++Cursor) + d->qpos[S.Adr + i] = (mjtNum)(*QPosArr)[Cursor]->AsNumber(); + } + } + mj_forward(m, d); + + if (bFreeBaseShortcut) + { + const int32 Adr = Slots[0].Adr; + for (int32 i = 0; i < 7; ++i) + Out.Add(MakeShared(d->qpos[Adr + i])); + } + else + { + for (const FJointSlot& S : Slots) + for (int32 i = 0; i < S.Size; ++i) + Out.Add(MakeShared(d->qpos[S.Adr + i])); + } + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_qpos_ok")); + // Echo back the resolved actor identifiers so the caller can + // confirm which articulation actually got the write. `target` + // matches the request's target field; `actor_name` is the UE + // name (always present, even if actor_id was the lookup key). + Reply->SetStringField(TEXT("target"), Target); + Reply->SetStringField(TEXT("actor_name"), Art->GetName()); + if (!Art->ActorId.IsEmpty()) + Reply->SetStringField(TEXT("actor_id"), Art->ActorId); + Reply->SetArrayField(TEXT("qpos"), Out); + Reply->SetBoolField(TEXT("free_base_shortcut"), bFreeBaseShortcut); + return Reply; +} + +// ============================================================================= +// set_mocap_pose / read_mocap_pose / get_contacts — runtime MJ-side reads/writes. +// +// All three operate directly on the live mjModel/mjData under the engine's +// CallbackMutex (same as set_qpos). Body name lookup uses mj_name2id with +// the full compiled MJ name (URLab prefixes are already included). +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleSetMocapPose(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) + return MakeError(URLabError::NotReady, TEXT("Manager not initialised")); + + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("MjModel/MjData missing")); + + FString Body; + Req->TryGetStringField(TEXT("body"), Body); + if (Body.IsEmpty()) + return MakeError(URLabError::MissingField, TEXT("set_mocap_pose: missing 'body'")); + + const int32 BodyId = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*Body)); + if (BodyId < 0) + return MakeError(URLabError::UnknownBody, Body); + + { + FName ArtKey; + for (AMjArticulation* Art : Mgr->GetAllArticulations()) + { + if (!Art) + continue; + for (UMjBody* B : Art->GetBodies()) + { + if (B && B->GetMjName().Equals(Body)) + { + ArtKey = FName(*Art->GetName()); + break; + } + } + if (!ArtKey.IsNone()) + break; + } + if (!ArtKey.IsNone()) + { + if (TSharedPtr Denied = RejectIfNotControlOwner(ArtKey, Req)) + return Denied; + } + } + + const int32 MocapId = m->body_mocapid[BodyId]; + if (MocapId < 0) + return MakeError(URLabError::NotMocapBody, + FString::Printf(TEXT("Body '%s' is not a mocap body"), *Body)); + + const TArray>* PosArr = nullptr; + const TArray>* QuatArr = nullptr; + const bool bHasPos = Req->TryGetArrayField(TEXT("pos"), PosArr) && PosArr && PosArr->Num() == 3; + const bool bHasQuat = Req->TryGetArrayField(TEXT("quat"), QuatArr) && QuatArr && QuatArr->Num() == 4; + if (!bHasPos && !bHasQuat) + return MakeError(URLabError::MissingField, + TEXT("set_mocap_pose requires at least one of pos[3] or quat[4]")); + + // Read-back of the written mocap pose must stay inside the lock: a concurrent + // worker step (or a recompile) would otherwise tear the echoed values. + TArray> PosOut, QuatOut; + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + if (bHasPos) + { + for (int32 i = 0; i < 3; ++i) + d->mocap_pos[3 * MocapId + i] = (mjtNum)(*PosArr)[i]->AsNumber(); + } + if (bHasQuat) + { + for (int32 i = 0; i < 4; ++i) + d->mocap_quat[4 * MocapId + i] = (mjtNum)(*QuatArr)[i]->AsNumber(); + } + + for (int32 i = 0; i < 3; ++i) + PosOut.Add(MakeShared(d->mocap_pos[3 * MocapId + i])); + for (int32 i = 0; i < 4; ++i) + QuatOut.Add(MakeShared(d->mocap_quat[4 * MocapId + i])); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_mocap_pose_ok")); + Reply->SetStringField(TEXT("body"), Body); + Reply->SetArrayField(TEXT("pos"), PosOut); + Reply->SetArrayField(TEXT("quat"), QuatOut); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleReadMocapPose(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) + return MakeError(URLabError::NotReady, TEXT("Manager not initialised")); + + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("MjModel/MjData missing")); + + FString Body; + Req->TryGetStringField(TEXT("body"), Body); + if (Body.IsEmpty()) + return MakeError(URLabError::MissingField, TEXT("read_mocap_pose: missing 'body'")); + + const int32 BodyId = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*Body)); + if (BodyId < 0) + return MakeError(URLabError::UnknownBody, Body); + + const int32 MocapId = m->body_mocapid[BodyId]; + if (MocapId < 0) + return MakeError(URLabError::NotMocapBody, + FString::Printf(TEXT("Body '%s' is not a mocap body"), *Body)); + + TArray> PosOut, QuatOut; + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + for (int32 i = 0; i < 3; ++i) + PosOut.Add(MakeShared(d->mocap_pos[3 * MocapId + i])); + for (int32 i = 0; i < 4; ++i) + QuatOut.Add(MakeShared(d->mocap_quat[4 * MocapId + i])); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("read_mocap_pose_ok")); + Reply->SetStringField(TEXT("body"), Body); + Reply->SetArrayField(TEXT("pos"), PosOut); + Reply->SetArrayField(TEXT("quat"), QuatOut); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleGetContacts(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) + return MakeError(URLabError::NotReady, TEXT("Manager not initialised")); + + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("MjModel/MjData missing")); + + int32 MaxContacts = 64; + { + int32 Cap = 0; + if (Req->TryGetNumberField(TEXT("max_contacts"), Cap) && Cap > 0) + MaxContacts = Cap; + } + + // Optional filter: {body1?, body2?, geom1?, geom2?}. AND across set fields. + FString FBody1, FBody2, FGeom1, FGeom2; + const TSharedPtr* FilterObj = nullptr; + if (Req->TryGetObjectField(TEXT("filter"), FilterObj) && FilterObj && *FilterObj) + { + (*FilterObj)->TryGetStringField(TEXT("body1"), FBody1); + (*FilterObj)->TryGetStringField(TEXT("body2"), FBody2); + (*FilterObj)->TryGetStringField(TEXT("geom1"), FGeom1); + (*FilterObj)->TryGetStringField(TEXT("geom2"), FGeom2); + } + + auto NameOrEmpty = [&](int Type, int Id) -> FString { + if (Id < 0) + return FString(); + const char* p = mj_id2name(m, Type, Id); + return p ? FString(UTF8_TO_TCHAR(p)) : FString(); + }; + + TArray> Out; + bool bTruncated = false; + int32 Matched = 0; + + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + const int32 N = d->ncon; + for (int32 i = 0; i < N; ++i) + { + const mjContact& c = d->contact[i]; + const int32 G1 = c.geom[0]; + const int32 G2 = c.geom[1]; + const int32 B1 = (G1 >= 0 && G1 < m->ngeom) ? m->geom_bodyid[G1] : -1; + const int32 B2 = (G2 >= 0 && G2 < m->ngeom) ? m->geom_bodyid[G2] : -1; + const FString G1Name = NameOrEmpty(mjOBJ_GEOM, G1); + const FString G2Name = NameOrEmpty(mjOBJ_GEOM, G2); + const FString B1Name = NameOrEmpty(mjOBJ_BODY, B1); + const FString B2Name = NameOrEmpty(mjOBJ_BODY, B2); + + if (!FGeom1.IsEmpty() && !G1Name.Equals(FGeom1)) + continue; + if (!FGeom2.IsEmpty() && !G2Name.Equals(FGeom2)) + continue; + if (!FBody1.IsEmpty() && !B1Name.Equals(FBody1)) + continue; + if (!FBody2.IsEmpty() && !B2Name.Equals(FBody2)) + continue; + + if (Matched >= MaxContacts) + { + bTruncated = true; + break; + } + + mjtNum Force[6] = {0}; + mj_contactForce(m, d, i, Force); + + TArray> Pos; + for (int32 k = 0; k < 3; ++k) + Pos.Add(MakeShared(c.pos[k])); + // First row of the contact frame is the contact normal. + TArray> Normal; + for (int32 k = 0; k < 3; ++k) + Normal.Add(MakeShared(c.frame[k])); + TArray> ForceArr; + for (int32 k = 0; k < 6; ++k) + ForceArr.Add(MakeShared(Force[k])); + + TSharedPtr CObj = MakeShared(); + CObj->SetStringField(TEXT("geom1"), G1Name); + CObj->SetStringField(TEXT("geom2"), G2Name); + CObj->SetStringField(TEXT("body1"), B1Name); + CObj->SetStringField(TEXT("body2"), B2Name); + CObj->SetArrayField(TEXT("pos"), Pos); + CObj->SetArrayField(TEXT("normal"), Normal); + CObj->SetNumberField(TEXT("dist"), c.dist); + CObj->SetArrayField(TEXT("force"), ForceArr); + Out.Add(MakeShared(CObj)); + ++Matched; + } + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("get_contacts_ok")); + Reply->SetNumberField(TEXT("n_contacts"), Matched); + Reply->SetBoolField(TEXT("truncated"), bTruncated); + Reply->SetArrayField(TEXT("contacts"), Out); + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleListKeyframes(const TSharedPtr& /*Req*/) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->IsInitialized()) + return MakeError(URLabError::NotReady, TEXT("Manager not initialised")); + + mjModel* m = Mgr->PhysicsEngine->GetModel(); + if (!m) + return MakeError(URLabError::NotReady, TEXT("MjModel missing")); + + auto Slice = [](const mjtNum* src, int32 stride, int32 idx, int32 width) { + TArray> Out; + if (!src || width <= 0) + return Out; + for (int32 k = 0; k < width; ++k) + Out.Add(MakeShared(src[idx * stride + k])); + return Out; + }; + + TArray> Keys; + for (int32 i = 0; i < m->nkey; ++i) + { + const char* NameC = mj_id2name(m, mjOBJ_KEY, i); + TSharedPtr K = MakeShared(); + K->SetStringField(TEXT("name"), NameC ? UTF8_TO_TCHAR(NameC) : TEXT("")); + K->SetNumberField(TEXT("time"), m->key_time ? m->key_time[i] : 0.0); + K->SetArrayField(TEXT("qpos"), Slice(m->key_qpos, m->nq, i, m->nq)); + K->SetArrayField(TEXT("qvel"), Slice(m->key_qvel, m->nv, i, m->nv)); + K->SetArrayField(TEXT("ctrl"), Slice(m->key_ctrl, m->nu, i, m->nu)); + K->SetArrayField(TEXT("mocap_pos"), Slice(m->key_mpos, m->nmocap * 3, i, m->nmocap * 3)); + K->SetArrayField(TEXT("mocap_quat"), Slice(m->key_mquat, m->nmocap * 4, i, m->nmocap * 4)); + Keys.Add(MakeShared(K)); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("list_keyframes_ok")); + Reply->SetArrayField(TEXT("keyframes"), Keys); + return Reply; +} + +// ============================================================================= +// recording_* / replay_* delegate to AMjReplayManager +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleRecording(const FString& Op, const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + // Use the game-thread-cached pointer; TActorIterator from this worker + // thread would assert IsInGameThread() and crash. + AMjReplayManager* RM = CachedReplayManager.Get(); + if (!RM) + return MakeError(URLabError::NotReady, TEXT("AMjReplayManager not present in scene")); + + if (Op.Equals(TEXT("recording_start"))) + { + if (RM->bIsRecording) + return MakeError(URLabError::RecordingAlreadyActive, TEXT("Recording already active")); + double MaxDur = 0.0; + if (Req->TryGetNumberField(TEXT("max_duration_s"), MaxDur)) + RM->MaxRecordDuration = (float)MaxDur; + else + RM->MaxRecordDuration = FLT_MAX; + RM->StartRecording(); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("recording_start_ok")); + Reply->SetStringField(TEXT("name"), AMjReplayManager::LiveSessionName); + Reply->SetNumberField(TEXT("max_duration_s"), RM->MaxRecordDuration); + return Reply; + } + if (Op.Equals(TEXT("recording_stop"))) + { + if (!RM->bIsRecording) + return MakeError(URLabError::RecordingNotActive, TEXT("Recording is not active")); + RM->StopRecording(); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("recording_stop_ok")); + Reply->SetStringField(TEXT("name"), AMjReplayManager::LiveSessionName); + // Populate the summary fields the Python client maps to + // RecordingSummary. Recording has been stopped (StopRecording + // above set bIsRecording=false) so the OnPostStep hook isn't + // mutating Frames in parallel. + Reply->SetNumberField(TEXT("frame_count"), static_cast(RM->GetLiveFrameCount())); + Reply->SetNumberField(TEXT("sim_duration_s"), RM->GetLiveSimDurationS()); + return Reply; + } + if (Op.Equals(TEXT("recording_save"))) + { + FString Path; + Req->TryGetStringField(TEXT("path"), Path); + FString FileName = Path.IsEmpty() ? TEXT("recording.json") : FPaths::GetCleanFilename(Path); + if (Path.IsEmpty()) + Path = FileName; + bool bOk = RM->SaveRecordingToFile(FileName); + // ResolveReplayPath now matches the manager's + // ProjectSavedDir/URLab/Replays/ output dir, so the absolute_path + // returned to the client points at the file the manager actually wrote. + FString Abs = ResolveReplayPath(Path); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), bOk ? TEXT("recording_save_ok") : TEXT("error")); + Reply->SetStringField(TEXT("absolute_path"), Abs); + if (!bOk) + Reply->SetStringField(TEXT("code"), URLabError::PathNotWritable); + return Reply; + } + if (Op.Equals(TEXT("recording_clear"))) + { + RM->ClearRecording(); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("recording_clear_ok")); + return Reply; + } + return MakeError(URLabError::UnknownOp, Op); +} + +TSharedPtr FURLabRpcDispatcher::HandleReplay(const FString& Op, const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + AMjReplayManager* RM = CachedReplayManager.Get(); + if (!RM) + return MakeError(URLabError::NotReady, TEXT("AMjReplayManager not present in scene")); + + if (Op.Equals(TEXT("replay_load"))) + { + FString P; + if (!Req->TryGetStringField(TEXT("path"), P)) + return MakeError(URLabError::MissingField, TEXT("replay_load requires 'path'")); + FString FileName = FPaths::GetCleanFilename(P); + bool bOk = RM->LoadRecordingFromFile(FileName); + if (!bOk) + return MakeError(URLabError::PathNotReadable, P); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("replay_load_ok")); + Reply->SetStringField(TEXT("name"), FPaths::GetBaseFilename(FileName)); + return Reply; + } + if (Op.Equals(TEXT("replay_list_sessions"))) + { + TArray> Names; + for (const FString& N : RM->GetSessionNames()) + Names.Add(MakeShared(N)); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("replay_list_sessions_ok")); + Reply->SetArrayField(TEXT("sessions"), Names); + return Reply; + } + if (Op.Equals(TEXT("replay_set_active"))) + { + FString N; + if (!Req->TryGetStringField(TEXT("name"), N)) + return MakeError(URLabError::MissingField, TEXT("replay_set_active requires 'name'")); + if (!RM->Sessions.Contains(N)) + return MakeError(URLabError::ReplaySessionNotFound, N); + RM->SetActiveSession(N); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("replay_set_active_ok")); + return Reply; + } + if (Op.Equals(TEXT("replay_start"))) + { + if (ActiveStepMode == EStepMode::Live) + return MakeError(URLabError::ReplayRequiresStepped, + TEXT("Switch to direct or puppet before starting replay")); + RM->StartReplay(); + int32 Total = RM->Sessions.Contains(RM->GetActiveSessionName()) + ? RM->Sessions[RM->GetActiveSessionName()].Frames.Num() + : 0; + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("replay_start_ok")); + Reply->SetStringField(TEXT("active_session"), RM->GetActiveSessionName()); + Reply->SetNumberField(TEXT("total_frames"), Total); + return Reply; + } + if (Op.Equals(TEXT("replay_stop"))) + { + RM->StopReplay(); + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("replay_stop_ok")); + return Reply; + } + return MakeError(URLabError::UnknownOp, Op); +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_SimOptions.cpp b/Source/URLab/Private/Bridge/RpcHandlers_SimOptions.cpp new file mode 100644 index 00000000..2b3b8c18 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_SimOptions.cpp @@ -0,0 +1,586 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/OpRegistry.h" +#include "Bridge/MsgpackHelpers.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Controllers/MjArticulationController.h" +#include "MuJoCo/Input/MjPerturbation.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Transport/NetworkManager.h" +#include "Transport/ShmPublishTransport.h" +#include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Transport/ShmRegion.h" // FMjShmHeader (header_size in shm_rpc block) +#include "Bridge/BridgeServer.h" +#include "Replay/MjReplayManager.h" +#include "Kismet/GameplayStatics.h" +#include "Misc/Base64.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "Internationalization/Regex.h" +#include "HAL/FileManager.h" +#include "EngineUtils.h" +#include "Engine/World.h" +#include "Misc/Guid.h" +#include "Utils/URLabLogging.h" + +// ============================================================================= +// configure_controller +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleConfigureController(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + FString ArtName; + if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) + return MakeError(URLabError::MissingField, TEXT("configure_controller requires 'articulation'")); + + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + return MakeError(URLabError::UnknownArticulation, ArtName); + + UMjArticulationController* Ctrl = Art->FindComponentByClass(); + if (!Ctrl) + return MakeError(URLabError::NoController, FString::Printf(TEXT("Articulation '%s' has no controller"), *ArtName)); + + const TSharedPtr* Params = nullptr; + if (Req->TryGetObjectField(TEXT("params"), Params) && Params && Params->IsValid()) + { + Ctrl->ApplyConfig(*Params); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("configure_controller_ok")); + Reply->SetStringField(TEXT("articulation"), ArtName); + + TSharedPtr Out = MakeShared(); + Ctrl->GetCurrentConfig(Out); + Reply->SetObjectField(TEXT("params"), Out); + return Reply; +} + +// ============================================================================= +// set_sim_options +// ============================================================================= + +namespace +{ +bool ParseIntegrator(const FString& S, EMjIntegrator& Out) +{ + if (S.Equals(TEXT("euler"), ESearchCase::IgnoreCase)) + { + Out = EMjIntegrator::Euler; + return true; + } + if (S.Equals(TEXT("rk4"), ESearchCase::IgnoreCase)) + { + Out = EMjIntegrator::RK4; + return true; + } + if (S.Equals(TEXT("implicit"), ESearchCase::IgnoreCase)) + { + Out = EMjIntegrator::Implicit; + return true; + } + if (S.Equals(TEXT("implicitfast"), ESearchCase::IgnoreCase)) + { + Out = EMjIntegrator::ImplicitFast; + return true; + } + return false; +} +FString IntegratorToString(EMjIntegrator I) +{ + switch (I) + { + case EMjIntegrator::Euler: + return TEXT("euler"); + case EMjIntegrator::RK4: + return TEXT("rk4"); + case EMjIntegrator::Implicit: + return TEXT("implicit"); + case EMjIntegrator::ImplicitFast: + return TEXT("implicitfast"); + } + return TEXT("euler"); +} +bool ParseCone(const FString& S, EMjCone& Out) +{ + if (S.Equals(TEXT("pyramidal"), ESearchCase::IgnoreCase)) + { + Out = EMjCone::Pyramidal; + return true; + } + if (S.Equals(TEXT("elliptic"), ESearchCase::IgnoreCase)) + { + Out = EMjCone::Elliptic; + return true; + } + return false; +} +FString ConeToString(EMjCone C) +{ + return C == EMjCone::Elliptic ? TEXT("elliptic") : TEXT("pyramidal"); +} +bool ParseSolver(const FString& S, EMjSolver& Out) +{ + if (S.Equals(TEXT("pgs"), ESearchCase::IgnoreCase)) + { + Out = EMjSolver::PGS; + return true; + } + if (S.Equals(TEXT("cg"), ESearchCase::IgnoreCase)) + { + Out = EMjSolver::CG; + return true; + } + if (S.Equals(TEXT("newton"), ESearchCase::IgnoreCase)) + { + Out = EMjSolver::Newton; + return true; + } + return false; +} +FString SolverToString(EMjSolver S) +{ + switch (S) + { + case EMjSolver::PGS: + return TEXT("pgs"); + case EMjSolver::CG: + return TEXT("cg"); + case EMjSolver::Newton: + return TEXT("newton"); + } + return TEXT("newton"); +} + +bool TryReadVec3(const TSharedPtr& Obj, const TCHAR* Key, double Out[3]) +{ + const TArray>* Arr = nullptr; + if (!Obj->TryGetArrayField(Key, Arr) || !Arr || Arr->Num() != 3) + return false; + Out[0] = (*Arr)[0]->AsNumber(); + Out[1] = (*Arr)[1]->AsNumber(); + Out[2] = (*Arr)[2]->AsNumber(); + return true; +} +} // namespace + +TSharedPtr FURLabRpcDispatcher::HandleSetSimOptions(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + + const TSharedPtr* OptsPtr = nullptr; + if (!Req->TryGetObjectField(TEXT("options"), OptsPtr) || !OptsPtr || !(*OptsPtr).IsValid()) + return MakeError(URLabError::MissingField, TEXT("set_sim_options requires 'options' object")); + const TSharedPtr& Opts = *OptsPtr; + + FMjOptionGenerated& O = Mgr->PhysicsEngine->Options; + + double DNum = 0.0; + if (Opts->TryGetNumberField(TEXT("timestep"), DNum)) + { + O.Timestep = (float)DNum; + O.bOverride_Timestep = true; + } + + // Wire is MJ-native SI; FMjOptionGenerated stores UE cm/s² with Y-flip and + // ApplyOverridesToModel reverses that, so pre-bake the inverse here. + double V3[3]; + if (TryReadVec3(Opts, TEXT("gravity"), V3)) + { + O.Gravity = FVector((float)(V3[0] * 100.0), (float)(-V3[1] * 100.0), (float)(V3[2] * 100.0)); + O.bOverride_Gravity = true; + } + if (TryReadVec3(Opts, TEXT("wind"), V3)) + { + O.Wind = FVector((float)(V3[0] * 100.0), (float)(-V3[1] * 100.0), (float)(V3[2] * 100.0)); + O.bOverride_Wind = true; + } + if (TryReadVec3(Opts, TEXT("magnetic"), V3)) + { + O.Magnetic = FVector((float)V3[0], (float)-V3[1], (float)V3[2]); + O.bOverride_Magnetic = true; + } + + if (Opts->TryGetNumberField(TEXT("density"), DNum)) + { + O.Density = (float)DNum; + O.bOverride_Density = true; + } + if (Opts->TryGetNumberField(TEXT("viscosity"), DNum)) + { + O.Viscosity = (float)DNum; + O.bOverride_Viscosity = true; + } + if (Opts->TryGetNumberField(TEXT("impratio"), DNum)) + { + O.Impratio = (float)DNum; + O.bOverride_Impratio = true; + } + if (Opts->TryGetNumberField(TEXT("tolerance"), DNum)) + { + O.Tolerance = (float)DNum; + O.bOverride_Tolerance = true; + } + + int32 INum = 0; + if (Opts->TryGetNumberField(TEXT("iterations"), INum)) + { + O.Iterations = INum; + O.bOverride_Iterations = true; + } + if (Opts->TryGetNumberField(TEXT("ls_iterations"), INum)) + { + O.LsIterations = INum; + O.bOverride_LsIterations = true; + } + + FString SNum; + if (Opts->TryGetStringField(TEXT("integrator"), SNum)) + { + EMjIntegrator E; + if (!ParseIntegrator(SNum, E)) + return MakeError(URLabError::BadValue, FString::Printf(TEXT("unknown integrator '%s'"), *SNum)); + O.Integrator = E; + O.bOverride_Integrator = true; + } + if (Opts->TryGetStringField(TEXT("cone"), SNum)) + { + EMjCone E; + if (!ParseCone(SNum, E)) + return MakeError(URLabError::BadValue, FString::Printf(TEXT("unknown cone '%s'"), *SNum)); + O.Cone = E; + O.bOverride_Cone = true; + } + if (Opts->TryGetStringField(TEXT("solver"), SNum)) + { + EMjSolver E; + if (!ParseSolver(SNum, E)) + return MakeError(URLabError::BadValue, FString::Printf(TEXT("unknown solver '%s'"), *SNum)); + O.Solver = E; + O.bOverride_Solver = true; + } + + if (Opts->TryGetNumberField(TEXT("noslip_iterations"), INum)) + { + O.NoslipIterations = INum; + O.bOverride_NoslipIterations = true; + } + if (Opts->TryGetNumberField(TEXT("noslip_tolerance"), DNum)) + { + O.NoslipTolerance = (float)DNum; + O.bOverride_NoslipTolerance = true; + } + if (Opts->TryGetNumberField(TEXT("ccd_iterations"), INum)) + { + O.CCD_Iterations = INum; + O.bOverride_CCD_Iterations = true; + } + if (Opts->TryGetNumberField(TEXT("ccd_tolerance"), DNum)) + { + O.CCD_Tolerance = (float)DNum; + O.bOverride_CCD_Tolerance = true; + } + + bool BNum = false; + if (Opts->TryGetBoolField(TEXT("enable_multiccd"), BNum)) + { + O.bEnableMultiCCD = BNum; + } + if (Opts->TryGetBoolField(TEXT("enable_sleep"), BNum)) + { + O.bEnableSleep = BNum; + } + if (Opts->TryGetNumberField(TEXT("sleep_tolerance"), DNum)) + { + O.SleepTolerance = (float)DNum; + } + + // The physics worker may be mid mj_step on this same m/d on another thread. + // Serialise every live-model write below (disable/enable flags, + // ApplyOverridesToModel) and the threadpool rebuild against it under the + // worker's step lock, and hold the lock across the reply's m->opt reads so + // the echoed snapshot is coherent. Rebuilding mju_threadpool while a step is + // in flight is otherwise a crash. + FScopeLock ModelLock(&Mgr->PhysicsEngine->CallbackMutex); + + // Fetch the live model under the lock: a concurrent CompileModel frees and + // reallocates m/d, so a pointer captured before the lock could dangle. + mjModel* m = Mgr->PhysicsEngine->GetModel(); + if (!m) + return MakeError(URLabError::NotReady, TEXT("mjModel not compiled")); + + // Raw disable / enable bit masks. Values are bitwise-ORs of + // mujoco/mjmodel.h mjtDisableBit / mjtEnableBit constants. + // Applied BEFORE FMjOptionGenerated::ApplyOverridesToModel so any named + // bits the caller also set (enable_sleep / enable_multiccd) win on + // top of the raw mask. Treat the raw masks as a coarse baseline. + int32 DisableMask = 0; + if (Opts->TryGetNumberField(TEXT("disableflags"), DisableMask)) + { + m->opt.disableflags = DisableMask; + } + int32 EnableMask = 0; + if (Opts->TryGetNumberField(TEXT("enableflags"), EnableMask)) + { + m->opt.enableflags = EnableMask; + } + + O.ApplyOverridesToModel(m); + + // Worker thread pool (mju_threadpool). Not a MuJoCo option-struct field — + // it's a URLab engine setting applied to the live mjData. Clamped to the + // detected CPU core count; ApplyThreadPool is idempotent. + int32 NumThreads = 0; + if (Opts->TryGetNumberField(TEXT("num_worker_threads"), NumThreads)) + { + Mgr->PhysicsEngine->NumWorkerThreads = + FMath::Clamp(NumThreads, 0, UMjPhysicsEngine::MaxWorkerThreads()); + Mgr->PhysicsEngine->ApplyThreadPool(); + } + + UE_LOG(LogURLabNet, Log, + TEXT("FURLabRpcDispatcher: set_sim_options applied (timestep=%.5fs, gravity=[%.3f %.3f %.3f] m/s²)"), + m->opt.timestep, m->opt.gravity[0], m->opt.gravity[1], m->opt.gravity[2]); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_sim_options_ok")); + + TSharedPtr Out = MakeShared(); + Out->SetNumberField(TEXT("timestep"), m->opt.timestep); + Out->SetNumberField(TEXT("num_worker_threads"), Mgr->PhysicsEngine->NumWorkerThreads); + Out->SetNumberField(TEXT("max_worker_threads"), UMjPhysicsEngine::MaxWorkerThreads()); + { + TArray> G; + G.Add(MakeShared(m->opt.gravity[0])); + G.Add(MakeShared(m->opt.gravity[1])); + G.Add(MakeShared(m->opt.gravity[2])); + Out->SetArrayField(TEXT("gravity"), G); + TArray> W; + W.Add(MakeShared(m->opt.wind[0])); + W.Add(MakeShared(m->opt.wind[1])); + W.Add(MakeShared(m->opt.wind[2])); + Out->SetArrayField(TEXT("wind"), W); + TArray> Mg; + Mg.Add(MakeShared(m->opt.magnetic[0])); + Mg.Add(MakeShared(m->opt.magnetic[1])); + Mg.Add(MakeShared(m->opt.magnetic[2])); + Out->SetArrayField(TEXT("magnetic"), Mg); + } + Out->SetNumberField(TEXT("density"), m->opt.density); + Out->SetNumberField(TEXT("viscosity"), m->opt.viscosity); + Out->SetNumberField(TEXT("impratio"), m->opt.impratio); + Out->SetNumberField(TEXT("tolerance"), m->opt.tolerance); + Out->SetNumberField(TEXT("iterations"), m->opt.iterations); + Out->SetNumberField(TEXT("ls_iterations"), m->opt.ls_iterations); + Out->SetStringField(TEXT("integrator"), IntegratorToString((EMjIntegrator)m->opt.integrator)); + Out->SetStringField(TEXT("cone"), ConeToString((EMjCone)m->opt.cone)); + Out->SetStringField(TEXT("solver"), SolverToString((EMjSolver)m->opt.solver)); + Out->SetNumberField(TEXT("noslip_iterations"), m->opt.noslip_iterations); + Out->SetNumberField(TEXT("noslip_tolerance"), m->opt.noslip_tolerance); + Out->SetNumberField(TEXT("ccd_iterations"), m->opt.ccd_iterations); + Out->SetNumberField(TEXT("ccd_tolerance"), m->opt.ccd_tolerance); + + Out->SetBoolField(TEXT("enable_multiccd"), (m->opt.disableflags & mjDSBL_MULTICCD) == 0); + Out->SetBoolField(TEXT("enable_sleep"), (m->opt.enableflags & mjENBL_SLEEP) != 0); + Out->SetNumberField(TEXT("sleep_tolerance"), m->opt.sleep_tolerance); + + // Echo the raw bit masks so callers using disableflags / enableflags + // can verify the final composed state (named overrides + raw mask). + Out->SetNumberField(TEXT("disableflags"), (int32)m->opt.disableflags); + Out->SetNumberField(TEXT("enableflags"), (int32)m->opt.enableflags); + + Reply->SetObjectField(TEXT("options"), Out); + return Reply; +} + +// ============================================================================= +// set_sim_speed +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleSetSimSpeed(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + + double Pct = 0.0; + if (!Req->TryGetNumberField(TEXT("percent"), Pct)) + return MakeError(URLabError::MissingField, TEXT("set_sim_speed requires 'percent'")); + + // Engine clamps internally (5..100); echo back so the caller sees what stuck. + Mgr->PhysicsEngine->SetSimSpeed((float)Pct); + const float Effective = FMath::Clamp((float)Pct, 5.0f, 100.0f); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_sim_speed_ok")); + Reply->SetNumberField(TEXT("percent"), Effective); + return Reply; +} + +// ============================================================================= +// set_control_source +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleSetControlSource(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + + FString SourceStr; + if (!Req->TryGetStringField(TEXT("source"), SourceStr)) + return MakeError(URLabError::MissingField, TEXT("set_control_source requires 'source' (\"zmq\" | \"ui\")")); + + EControlSource NewSource; + if (SourceStr.Equals(TEXT("zmq"), ESearchCase::IgnoreCase)) + NewSource = EControlSource::ZMQ; + else if (SourceStr.Equals(TEXT("ui"), ESearchCase::IgnoreCase)) + NewSource = EControlSource::UI; + else + return MakeError(URLabError::BadValue, FString::Printf(TEXT("unknown source '%s'"), *SourceStr)); + + FString ArtName; + Req->TryGetStringField(TEXT("articulation"), ArtName); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_control_source_ok")); + Reply->SetStringField(TEXT("source"), SourceStr.ToLower()); + + if (ArtName.IsEmpty()) + { + // Global flip is a control write across every art, so the source must + // own each currently-claimed art (or none may be claimed). + const FString Source = ResolveControlSource(Req); + for (const auto& Owned : ControlOwnership.GetActiveOwners()) + { + if (!Owned.Value.Equals(Source)) + { + TSharedPtr Err = MakeError(TEXT("not_control_owner"), + FString::Printf(TEXT("%s owned by %s"), *Owned.Key.ToString(), *Owned.Value)); + Err->SetStringField(TEXT("owner"), Owned.Value); + return Err; + } + } + + // Global: update engine + every articulation so the per-actor field + // doesn't keep stale state after a global flip. + Mgr->PhysicsEngine->SetControlSource(NewSource); + for (AMjArticulation* Art : Mgr->GetAllArticulations()) + { + if (Art) + Art->ControlSource = (uint8)NewSource; + } + Reply->SetStringField(TEXT("scope"), TEXT("global")); + } + else + { + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + return MakeError(URLabError::UnknownArticulation, ArtName); + if (TSharedPtr Denied = RejectIfNotControlOwner(FName(*Art->GetName()), Req)) + return Denied; + Art->ControlSource = (uint8)NewSource; + Reply->SetStringField(TEXT("scope"), TEXT("articulation")); + Reply->SetStringField(TEXT("articulation"), ArtName); + } + return Reply; +} + +// ============================================================================= +// set_twist +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleSetTwist(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + FString ArtName; + if (!Req->TryGetStringField(TEXT("articulation"), ArtName)) + return MakeError(URLabError::MissingField, TEXT("set_twist requires 'articulation'")); + + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + return MakeError(URLabError::UnknownArticulation, ArtName); + + if (TSharedPtr Denied = RejectIfNotControlOwner(FName(*Art->GetName()), Req)) + return Denied; + + UMjTwistController* TC = Art->FindComponentByClass(); + if (!TC) + return MakeError(URLabError::NoTwistController, + FString::Printf(TEXT("Articulation '%s' has no UMjTwistController"), *ArtName)); + + // Wire format mirrors how the bridge already reads twist: linear is + // (vx, vy, _) m/s, angular is (_, _, yaw_rate) rad/s. Tuple slots + // beyond the ones used are accepted but ignored. + auto ReadAxis = [](const TArray>* Arr, int32 Idx, float& Out) { + if (Arr && Arr->IsValidIndex(Idx)) + Out = (float)(*Arr)[Idx]->AsNumber(); + }; + + float Vx = 0.f, Vy = 0.f, YawRate = 0.f; + const TArray>* LinArr = nullptr; + const TArray>* AngArr = nullptr; + Req->TryGetArrayField(TEXT("linear"), LinArr); + Req->TryGetArrayField(TEXT("angular"), AngArr); + ReadAxis(LinArr, 0, Vx); + ReadAxis(LinArr, 1, Vy); + ReadAxis(AngArr, 2, YawRate); + + TC->SetTwist(Vx, Vy, YawRate); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_twist_ok")); + Reply->SetStringField(TEXT("articulation"), ArtName); + { + TArray> L; + L.Add(MakeShared(Vx)); + L.Add(MakeShared(Vy)); + L.Add(MakeShared(0.0)); + Reply->SetArrayField(TEXT("linear"), L); + TArray> A; + A.Add(MakeShared(0.0)); + A.Add(MakeShared(0.0)); + A.Add(MakeShared(YawRate)); + Reply->SetArrayField(TEXT("angular"), A); + } + return Reply; +} diff --git a/Source/URLab/Private/Bridge/RpcHandlers_Step.cpp b/Source/URLab/Private/Bridge/RpcHandlers_Step.cpp new file mode 100644 index 00000000..03b62c58 --- /dev/null +++ b/Source/URLab/Private/Bridge/RpcHandlers_Step.cpp @@ -0,0 +1,1086 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" +#include "Bridge/OpRegistry.h" +#include "Bridge/StepCommands.h" +#include "Bridge/MsgpackHelpers.h" +#include "State/MjStateCollector.h" +#include "State/MjMsgpackEncoder.h" +#include "State/MjCanonicalName.h" +#include "State/MjStateTypes.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Controllers/MjArticulationController.h" +#include "MuJoCo/Input/MjPerturbation.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Transport/NetworkManager.h" +#include "Transport/ShmPublishTransport.h" +#include "Transport/ShmRpcTransport.h" +#include "Transport/RpcTransport.h" +#include "Transport/ShmRegion.h" // FMjShmHeader (header_size in shm_rpc block) +#include "Bridge/BridgeServer.h" +#include "Replay/MjReplayManager.h" +#include "Kismet/GameplayStatics.h" +#include "Misc/Base64.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "Internationalization/Regex.h" +#include "HAL/FileManager.h" +#include "EngineUtils.h" +#include "Engine/World.h" +#include "Misc/Guid.h" +#include "Utils/URLabLogging.h" + +namespace +{ +/** Map enum to wire-format string, matching the Python StepMode enum values. */ +FString StepModeToString(EStepMode Mode) +{ + switch (Mode) + { + case EStepMode::Live: + return TEXT("live"); + case EStepMode::Direct: + return TEXT("direct"); + case EStepMode::Puppet: + return TEXT("puppet"); + case EStepMode::Auto: + return TEXT("auto"); + } + return TEXT("live"); +} + +bool StepModeFromString(const FString& Str, EStepMode& OutMode) +{ + if (Str.Equals(TEXT("live"), ESearchCase::IgnoreCase) || Str.Equals(TEXT("streaming"), ESearchCase::IgnoreCase)) + { + OutMode = EStepMode::Live; + return true; + } + if (Str.Equals(TEXT("direct"), ESearchCase::IgnoreCase)) + { + OutMode = EStepMode::Direct; + return true; + } + if (Str.Equals(TEXT("puppet"), ESearchCase::IgnoreCase)) + { + OutMode = EStepMode::Puppet; + return true; + } + if (Str.Equals(TEXT("auto"), ESearchCase::IgnoreCase)) + { + OutMode = EStepMode::Auto; + return true; + } + return false; +} + +/** Write a client-pushed integration state (qpos/qvel/ctrl/time) into + * (m,d), recompute derived quantities, and fire OnPostStep. Shared by the + * puppet inline path and the puppet step handler. The caller must hold the + * engine's CallbackMutex. */ +void ApplyPushedState(UMjPhysicsEngine* Engine, const FMjPushStateRequest& Push, mjModel* m, mjData* d) +{ + if (Push.QPos.Num() == m->nq) + FMemory::Memcpy(d->qpos, Push.QPos.GetData(), m->nq * sizeof(mjtNum)); + if (Push.QVel.Num() == m->nv) + FMemory::Memcpy(d->qvel, Push.QVel.GetData(), m->nv * sizeof(mjtNum)); + if (Push.bIncludeCtrl && Push.Ctrl.Num() == m->nu) + FMemory::Memcpy(d->ctrl, Push.Ctrl.GetData(), m->nu * sizeof(mjtNum)); + d->time = Push.Time; + mj_forward(m, d); + if (Engine->OnPostStep) + Engine->OnPostStep(m, d); +} +} // namespace + +TSharedPtr FURLabRpcDispatcher::HandleSetPaused(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + + bool bPause = false; + if (!Req->TryGetBoolField(TEXT("paused"), bPause)) + return MakeError(URLabError::MissingField, TEXT("set_paused requires 'paused' bool")); + + Mgr->PhysicsEngine->SetPaused(bPause); + UE_LOG(LogURLabNet, Log, TEXT("FURLabRpcDispatcher: set_paused -> %s"), + bPause ? TEXT("true") : TEXT("false")); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_paused_ok")); + Reply->SetBoolField(TEXT("paused"), Mgr->PhysicsEngine->bIsPaused); + return Reply; +} + +// ============================================================================= +// step +// ============================================================================= + +// Parse the per_articulation control payload -- control_mode, positional ctrl +// array, named ctrl_map, and xfrc_applied -- into an FMjStepRequest. Shared by +// the live and direct step paths so both apply the full payload; the live +// branch previously parsed only the positional ctrl array and silently dropped +// ctrl_map / xfrc_applied. +static void ParseStepPerArticulation(const TSharedPtr& Req, + AAMjManager* Mgr, FMjStepRequest& Out) +{ + const TSharedPtr* PerArt = nullptr; + if (!Req->TryGetObjectField(TEXT("per_articulation"), PerArt) || !PerArt || !PerArt->IsValid()) + { + return; + } + for (auto& Pair : (*PerArt)->Values) + { + const TSharedPtr* ArtObj = nullptr; + if (!Pair.Value->TryGetObject(ArtObj) || !ArtObj || !ArtObj->IsValid()) + continue; + + // control_mode override + FString CtlMode; + if ((*ArtObj)->TryGetStringField(TEXT("control_mode"), CtlMode)) + { + Out.PerArticulationControlMode.Add(Pair.Key, CtlMode); + } + + // Positional ctrl array: indexed in articulation actuator order. + const TArray>* CtrlList = nullptr; + if ((*ArtObj)->TryGetArrayField(TEXT("ctrl"), CtrlList) && CtrlList) + { + if (AMjArticulation* Art = Cast(Mgr->GetArticulation(Pair.Key))) + { + TArray Acts = Art->GetActuators(); + for (int32 i = 0; i < CtrlList->Num() && i < Acts.Num(); ++i) + { + UMjActuator* A = Acts[i]; + if (!A) + continue; + FString LocalName = A->GetMjName(); + FString Prefix = Art->GetName() + TEXT("_"); + if (LocalName.StartsWith(Prefix)) + LocalName = LocalName.Mid(Prefix.Len()); + Out.PerArticulationCtrl.FindOrAdd(Pair.Key).Add( + {LocalName, (float)(*CtrlList)[i]->AsNumber()}); + } + } + } + + // Named ctrl map alternative. + const TSharedPtr* CtrlMap = nullptr; + if ((*ArtObj)->TryGetObjectField(TEXT("ctrl_map"), CtrlMap) && CtrlMap && CtrlMap->IsValid()) + { + for (auto& KV : (*CtrlMap)->Values) + { + Out.PerArticulationCtrl.FindOrAdd(Pair.Key).Add( + {KV.Key, (float)KV.Value->AsNumber()}); + } + } + + // xfrc_applied: { body_name: [fx,fy,fz,tx,ty,tz] }. Cleared after step. + const TSharedPtr* XfrcMap = nullptr; + if ((*ArtObj)->TryGetObjectField(TEXT("xfrc_applied"), XfrcMap) && XfrcMap && XfrcMap->IsValid()) + { + TMap>& BodyMap = Out.PerArticulationXfrc.FindOrAdd(Pair.Key); + for (auto& KV : (*XfrcMap)->Values) + { + const TArray>* Arr = nullptr; + if (KV.Value->TryGetArray(Arr) && Arr && Arr->Num() == 6) + { + TArray& Six = BodyMap.FindOrAdd(KV.Key); + Six.SetNum(6); + for (int i = 0; i < 6; ++i) + Six[i] = (*Arr)[i]->AsNumber(); + } + } + } + } +} + +void FURLabRpcDispatcher::ParseStepCommon(const TSharedPtr& Req, + AAMjManager* Mgr, FStepRequestCommon& Out) const +{ + // Per-step observations override: applies to THIS request only. Defaults to + // the session level; the session default is only changed at hello, never + // per-step, so a one-off `observations` field can't leak into later steps. + Out.ObservationLevel = ActiveObservationLevel.load(std::memory_order_acquire); + FString StepObs; + if (Req->TryGetStringField(TEXT("observations"), StepObs)) + { + if (StepObs.Equals(TEXT("minimal"), ESearchCase::IgnoreCase)) + Out.ObservationLevel = EObservationLevel::Minimal; + else if (StepObs.Equals(TEXT("full"), ESearchCase::IgnoreCase)) + Out.ObservationLevel = EObservationLevel::Full; + else if (StepObs.Equals(TEXT("standard"), ESearchCase::IgnoreCase)) + Out.ObservationLevel = EObservationLevel::Standard; + } + + // Parse include_cameras. Per-camera value forms: + // "latest" / "sync" -> latest available frame from the camera's history + // -> the frame showing post-step state >= that frame_id + // { "frame_id": N } -> same as + // Or include_cameras: true -> all registered cameras, latest. + // Retrieval is non-blocking: a frame that isn't ready yet is omitted and + // the client retries (or passes the step's frame_id to wait client-side). + { + const TSharedPtr* CamObj = nullptr; + if (Req->TryGetObjectField(TEXT("include_cameras"), CamObj) && CamObj && CamObj->IsValid()) + { + for (const auto& Kv : (*CamObj)->Values) + { + if (!Kv.Value.IsValid()) + continue; + FString Mode; + double Num = 0.0; + const TSharedPtr* Obj = nullptr; + if (Kv.Value->TryGetString(Mode)) + { + ECameraInclude E = Mode.Equals(TEXT("sync"), ESearchCase::IgnoreCase) + ? ECameraInclude::Sync + : ECameraInclude::Latest; + Out.CameraSpec.Add(Kv.Key, E); + } + else if (Kv.Value->TryGetNumber(Num)) + { + Out.CameraSpec.Add(Kv.Key, ECameraInclude::Latest); + if (Num > 0.0) + Out.CameraMinFrameIds.Add(Kv.Key, static_cast(Num)); + } + else if (Kv.Value->TryGetObject(Obj) && Obj && Obj->IsValid()) + { + Out.CameraSpec.Add(Kv.Key, ECameraInclude::Latest); + double Fid = 0.0; + if ((*Obj)->TryGetNumberField(TEXT("frame_id"), Fid) && Fid > 0.0) + Out.CameraMinFrameIds.Add(Kv.Key, static_cast(Fid)); + } + } + } + else + { + bool bAll = false; + if (Req->TryGetBoolField(TEXT("include_cameras"), bAll) && bAll && Mgr) + { + auto AddCamera = [&Out](UMjCamera* C) { + if (!C || C->bIsDefault) + return; + Out.CameraSpec.Add(C->GetCanonicalName(), ECameraInclude::Latest); + }; + for (AMjArticulation* Art : Mgr->GetAllArticulations()) + { + if (!Art) + continue; + TArray Cams; + Art->GetComponents(Cams); + for (UMjCamera* C : Cams) + AddCamera(C); + } + // Global / manager-level cameras: components attached directly to + // the manager actor rather than an articulation. The per-art walk + // alone dropped these, so `include_cameras: true` covered only + // robot-mounted cameras. + TArray GlobalCams; + Mgr->GetComponents(GlobalCams); + for (UMjCamera* C : GlobalCams) + AddCamera(C); + } + } + } + + // wait_cameras: block the reply server-side until the requested cameras + // have the frame this step produced, instead of the client polling with a + // min_frame_id and eating a round trip per miss. + Req->TryGetBoolField(TEXT("wait_cameras"), Out.bWaitCameras); + { + double T = 0.0; + if (Req->TryGetNumberField(TEXT("camera_timeout_ms"), T) && T > 0.0) + Out.CameraTimeoutMs = static_cast(T); + } + // render: "sync" drives an immediate capture and waits for the fresh frame + // (frame_id == this step). render: "async" kicks the capture but returns the + // most-recently-completed frame instead of waiting, so back-to-back requests + // overlap render and readback for higher throughput at the cost of a + // one-step-stale frame. Both are for eval, not interactive use. + { + FString R; + if (Req->TryGetStringField(TEXT("render"), R)) + { + Out.bRenderSync = R.Equals(TEXT("sync"), ESearchCase::IgnoreCase); + Out.bRenderAsync = R.Equals(TEXT("async"), ESearchCase::IgnoreCase); + } + } +} + +TSharedPtr FURLabRpcDispatcher::HandleStep(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) + { + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + } + + // Gate step-carried control: any articulation whose payload carries + // ctrl / ctrl_map / xfrc_applied must be owned by this request's control + // source. Observation-only steps (no control payload) are never gated. + { + const TSharedPtr* PerArt = nullptr; + if (Req->TryGetObjectField(TEXT("per_articulation"), PerArt) && PerArt && PerArt->IsValid()) + { + const FString Source = ResolveControlSource(Req); + for (const auto& Pair : (*PerArt)->Values) + { + const TSharedPtr* ArtObj = nullptr; + if (!Pair.Value->TryGetObject(ArtObj) || !ArtObj || !ArtObj->IsValid()) + continue; + const bool bCarriesControl = + (*ArtObj)->HasField(TEXT("ctrl")) || (*ArtObj)->HasField(TEXT("ctrl_map")) || (*ArtObj)->HasField(TEXT("xfrc_applied")); + if (!bCarriesControl) + continue; + + const AMjArticulation* Art = Mgr->GetArticulation(Pair.Key); + const FName Key(Art ? *Art->GetName() : *Pair.Key); + FString CurrentOwner; + if (ControlOwnership.CheckWrite(Key, Source, CurrentOwner) + != FMjControlOwnership::EWriteCheck::Ok) + { + TSharedPtr Err = MakeError(TEXT("not_control_owner"), + FString::Printf(TEXT("%s owned by %s"), *Key.ToString(), *CurrentOwner)); + Err->SetStringField(TEXT("owner"), CurrentOwner); + return Err; + } + } + } + } + + FStepRequestCommon Common; + ParseStepCommon(Req, Mgr, Common); + + // Snapshot the strategy under DispatchMutex, then dispatch outside it. The + // TSharedPtr copy keeps the strategy alive even if a concurrent set_mode + // swaps CurrentStepStrategy mid-step; strategies are stateless, so an + // in-flight step finishing on the prior strategy is correct. + TSharedPtr Strategy; + { + FScopeLock Lock(&DispatchMutex); + Strategy = CurrentStepStrategy; + } + if (!Strategy) + return MakeError(URLabError::NotReady, TEXT("no active step strategy")); + return Strategy->HandleStep(*this, Req, Common); +} + +TSharedPtr FURLabRpcDispatcher::BuildStepReply(const FMjStateSnapshot& Snapshot, + uint64 FrameId, EObservationLevel Level) +{ + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("step_ok")); + Reply->SetNumberField(TEXT("time"), Snapshot.Time); + Reply->SetNumberField(TEXT("step"), static_cast(Snapshot.Step)); + AppendClockFields(Reply, Snapshot.Time); + Reply->SetNumberField(TEXT("frame_id"), static_cast(FrameId)); + Reply->SetObjectField(TEXT("arts"), FMjMsgpackEncoder::EncodeArts(Snapshot, Level)); + Reply->SetObjectField(TEXT("scene"), FMjMsgpackEncoder::EncodeScene(Snapshot)); + return Reply; +} + +void FURLabRpcDispatcher::AppendCamerasBlock(TSharedPtr& Reply, AAMjManager* Mgr, + const TMap& CameraSpec, + const TMap& CameraMinFrameIds) +{ + if (!Reply.IsValid() || CameraSpec.Num() == 0) + return; + TSharedPtr Cams = BuildCamerasBlock(Mgr, CameraSpec, CameraMinFrameIds); + if (Cams.IsValid() && Cams->Values.Num() > 0) + Reply->SetObjectField(TEXT("cameras"), Cams); +} + +void FURLabRpcDispatcher::ApplyStepCtrl(AAMjManager* Manager, const FMjStepRequest& Req, + mjModel* m, mjData* d) +{ + if (!Manager) + return; + for (auto& Pair : Req.PerArticulationCtrl) + { + AMjArticulation* Art = Manager->GetArticulation(Pair.Key); + if (!Art) + continue; + + const FString Prefix = Art->GetName() + TEXT("_"); + TMap ByName; + ByName.Reserve(Art->GetActuators().Num() * 2); + for (UMjActuator* A : Art->GetActuators()) + { + if (!A) + continue; + FString FullName = A->GetMjName(); + FString Local = FullName.StartsWith(Prefix) ? FullName.Mid(Prefix.Len()) : FullName; + ByName.Add(Local, A); + ByName.Add(FullName, A); + } + + // Stage to actuator NetworkValue; AMjArticulation::ApplyControls + // copies it into d->ctrl every sub-step. Raw-mode articulations + // bypass NetworkValue entirely: d->ctrl is written directly here + // and ApplyControls skips its default path for bSkipController. + bool bRaw = false; + { + const FString* Mode = Req.PerArticulationControlMode.Find( + FMjCanonicalName::ArtSegment(Art).ToString()); + if (!Mode) + Mode = Req.PerArticulationControlMode.Find(Art->GetName()); + bRaw = Mode && Mode->Equals(TEXT("raw"), ESearchCase::IgnoreCase); + } + for (const TPair& KV : Pair.Value) + { + UMjActuator** Found = ByName.Find(KV.Key); + if (!Found || !*Found) + continue; + if (bRaw && m && d) + { + int id = (*Found)->GetMjID(); + if (id >= 0 && id < m->nu) + d->ctrl[id] = (mjtNum)KV.Value; + } + else + { + (*Found)->SetNetworkControl(KV.Value); + } + } + } + + // xfrc_applied writes: per_articulation -> body_name -> 6-vec. + // MuJoCo clears d->xfrc_applied on every mj_step, so this is a one-shot + // impulse for the next mj_step n_steps loop. Body name lookup tries both + // the local (no-prefix) form and the prefixed full name. + if (m && d) + { + for (auto& APair : Req.PerArticulationXfrc) + { + FString ArtPrefix = APair.Key + TEXT("_"); + for (auto& BPair : APair.Value) + { + if (BPair.Value.Num() != 6) + continue; + FString FullName = ArtPrefix + BPair.Key; + int Bid = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*FullName)); + if (Bid < 0) + Bid = mj_name2id(m, mjOBJ_BODY, TCHAR_TO_UTF8(*BPair.Key)); + if (Bid < 0 || Bid >= m->nbody) + continue; + for (int i = 0; i < 6; ++i) + d->xfrc_applied[6 * Bid + i] = (mjtNum)BPair.Value[i]; + } + } + } +} + +// ============================================================================= +// reset / set_mode +// ============================================================================= + +TSharedPtr FURLabRpcDispatcher::HandleReset(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) + { + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + } + + int32 SeedVal = 0; + if (Req->TryGetNumberField(TEXT("seed"), SeedVal)) + { + Mgr->Seed = SeedVal; + // Modern mjOption has no "seed" field; mj_step is deterministic and + // doesn't depend on a stored seed (random elements come from + // user-set noise inputs, not an integrator-internal RNG). The seed + // is recorded on the manager so any RNG used by client code or by + // the recording layer can mirror it for reproducibility. UE itself + // does not reseed the integrator here. + } + + TSharedPtr Reply = MakeShared(); + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + + // Fetch model/data under the lock: a concurrent CompileModel frees them + // under CallbackMutex. + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + + FString KfName; + if (Req->TryGetStringField(TEXT("keyframe_name"), KfName) && !KfName.IsEmpty()) + { + int Kid = mj_name2id(m, mjOBJ_KEY, TCHAR_TO_UTF8(*KfName)); + if (Kid < 0) + return MakeError(URLabError::UnknownKeyframe, KfName); + mj_resetDataKeyframe(m, d, Kid); + } + else + { + mj_resetData(m, d); + } + + // Per-articulation qpos overrides (joint-name -> value). + const TSharedPtr* PerArt = nullptr; + if (Req->TryGetObjectField(TEXT("per_articulation_qpos"), PerArt) && PerArt && PerArt->IsValid()) + { + for (auto& APair : (*PerArt)->Values) + { + AMjArticulation* Art = Mgr->GetArticulation(APair.Key); + if (!Art) + continue; + const TSharedPtr* QObj = nullptr; + if (!APair.Value->TryGetObject(QObj) || !QObj || !QObj->IsValid()) + continue; + + FString Prefix = Art->GetName() + TEXT("_"); + for (auto& JPair : (*QObj)->Values) + { + FString FullName = Prefix + JPair.Key; + int Jid = mj_name2id(m, mjOBJ_JOINT, TCHAR_TO_UTF8(*FullName)); + if (Jid < 0) + Jid = mj_name2id(m, mjOBJ_JOINT, TCHAR_TO_UTF8(*JPair.Key)); + if (Jid < 0) + continue; + int QAddr = m->jnt_qposadr[Jid]; + d->qpos[QAddr] = (mjtNum)JPair.Value->AsNumber(); + } + } + } + mj_forward(m, d); + + StepCounter.store(0, std::memory_order_relaxed); + + // Build the reply fields under the lock: the worker wakes on its idle + // timeout and can mutate d, tearing reads done after the lock releases. + Reply->SetStringField(TEXT("op"), TEXT("reset_ok")); + Reply->SetNumberField(TEXT("time"), d->time); + Reply->SetNumberField(TEXT("step"), 0); + AppendClockFields(Reply, d->time); + const FMjStateSnapshot& Snap = Mgr->GetStateCollector().Collect(m, d, 0); + Reply->SetObjectField(TEXT("arts"), + FMjMsgpackEncoder::EncodeArts(Snap, ActiveObservationLevel.load(std::memory_order_acquire))); + } + return Reply; +} + +// Run mj_forward (kinematics + dynamics, no integration) and return +// observations. Lets a client write qpos / qvel then read consistent +// derived state (xpos, sensors, contacts, ...) without advancing time. +TSharedPtr FURLabRpcDispatcher::HandleForward(const TSharedPtr& /*Req*/) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine || !Mgr->PhysicsEngine->m_model) + { + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + } + + TSharedPtr Reply = MakeShared(); + { + FScopeLock Lock(&Mgr->PhysicsEngine->CallbackMutex); + // Fetch model/data under the lock: a concurrent CompileModel frees them + // under CallbackMutex. + mjModel* m = Mgr->PhysicsEngine->GetModel(); + mjData* d = Mgr->PhysicsEngine->GetData(); + if (!m || !d) + return MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + mj_forward(m, d); + + // Build the reply fields under the lock so the worker's idle-timeout + // drain can't tear a read done after the lock releases. + Reply->SetStringField(TEXT("op"), TEXT("forward_ok")); + Reply->SetNumberField(TEXT("time"), d->time); + Reply->SetNumberField(TEXT("step"), StepCounter.load(std::memory_order_relaxed)); + AppendClockFields(Reply, d->time); + const FMjStateSnapshot& Snap = + Mgr->GetStateCollector().Collect(m, d, StepCounter.load(std::memory_order_relaxed)); + Reply->SetObjectField(TEXT("arts"), + FMjMsgpackEncoder::EncodeArts(Snap, ActiveObservationLevel.load(std::memory_order_acquire))); + } + return Reply; +} + +TSharedPtr FURLabRpcDispatcher::HandleSetMode(const TSharedPtr& Req) +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return MakeError(URLabError::NotReady, TEXT("Manager missing")); + + if (Mgr->StepMode != EStepMode::Auto) + { + return MakeError(URLabError::ModeLockedByServer, + FString::Printf(TEXT("Project pinned StepMode to %s"), *StepModeToString(Mgr->StepMode))); + } + + FString ModeStr; + if (!Req->TryGetStringField(TEXT("mode"), ModeStr)) + return MakeError(URLabError::MissingField, TEXT("set_mode requires 'mode'")); + + EStepMode NewMode; + if (!StepModeFromString(ModeStr, NewMode)) + return MakeError(URLabError::BadMode, FString::Printf(TEXT("Unknown mode '%s'"), *ModeStr)); + + EStepMode Prev = ActiveStepMode; + SetActiveStepMode(NewMode); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("set_mode_ok")); + Reply->SetStringField(TEXT("previous_mode"), StepModeToString(Prev)); + Reply->SetStringField(TEXT("current_mode"), StepModeToString(ActiveStepMode)); + return Reply; +} + +// Per-mode lifecycle + step body. OnEnter pauses/unpauses the state+ctrl +// publishers, sets the engine step mode (which unpauses the worker for +// client-driven modes), and installs the step handler; OnExit uninstalls it. +// HandleStep runs the per-step work. Camera publishers stream in every mode, so +// the caller clears FCameraZmqWorker::bPublishersPaused. These are named +// (not anonymous) so RpcDispatcher.h can friend them for internal access. +struct FLiveStepMode : FStepModeStrategy +{ + EStepMode Mode() const override { return EStepMode::Live; } + void OnEnter(FURLabRpcDispatcher& /*D*/, AAMjManager& Mgr) override + { + Mgr.bPublishersPaused.store(false, std::memory_order_release); + if (Mgr.PhysicsEngine) + Mgr.PhysicsEngine->SetStepMode(EStepMode::Live); + } + void OnExit(FURLabRpcDispatcher& /*D*/, AAMjManager& /*Mgr*/) override {} + + // UE drives its own physics: apply ctrl and read current state. n_steps is + // ignored (UE steps at its own rate). Cameras are served against the latest + // render snapshot id since live has no discrete stepped frame. + TSharedPtr HandleStep(FURLabRpcDispatcher& D, + const TSharedPtr& Req, const FStepRequestCommon& Common) override + { + AAMjManager* Mgr = D.OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; + + FMjStepRequest TmpReq; + ParseStepPerArticulation(Req, Mgr, TmpReq); + + TSharedPtr Reply; + uint64 FrameId = 0; + { + // Fetch model/data AFTER acquiring CallbackMutex: a concurrent + // CompileModel frees them under this lock, so a fetch before it + // would dangle. + FScopeLock Lock(&Engine->CallbackMutex); + mjModel* m = Engine->GetModel(); + mjData* d = Engine->GetData(); + if (!m || !d) + return FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + FURLabRpcDispatcher::ApplyStepCtrl(Mgr, TmpReq, m, d); + // Most recently published snapshot id (UE's autonomous physics owns + // stepping here), so the client can wait for a streamed frame >= this. + FrameId = Engine->GetRenderFrameId(); + const int64 StepIdx = D.StepCounter.load(std::memory_order_relaxed); + const FMjStateSnapshot& Snap = Mgr->GetStateCollector().Collect(m, d, StepIdx); + Reply = D.BuildStepReply(Snap, FrameId, Common.ObservationLevel); + } + + TMap CameraMinFrameIds = Common.CameraMinFrameIds; + if (Common.bRenderSync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, FrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + else if (Common.bRenderAsync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, 0, Common.CameraTimeoutMs, CameraMinFrameIds, /*bWait=*/false); + else if (Common.bWaitCameras && Common.CameraSpec.Num() > 0) + D.WaitForCameraFrames(Mgr, Common.CameraSpec, FrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + + D.AppendCamerasBlock(Reply, Mgr, Common.CameraSpec, CameraMinFrameIds); + return Reply; + } +}; + +struct FDirectStepMode : FStepModeStrategy +{ + EStepMode Mode() const override { return EStepMode::Direct; } + void OnEnter(FURLabRpcDispatcher& D, AAMjManager& Mgr) override + { + Mgr.bPublishersPaused.store(true, std::memory_order_release); + if (Mgr.PhysicsEngine) + Mgr.PhysicsEngine->SetStepMode(EStepMode::Direct); + D.InstallDirectHandler(); + } + void OnExit(FURLabRpcDispatcher& D, AAMjManager& /*Mgr*/) override + { + D.UninstallDirectHandler(); + } + + // Enqueue an FMjDirectStepCommand for the physics worker's step handler and + // wait for completion. If the worker isn't running (test / editor path), pump + // the handler inline under the engine lock. + TSharedPtr HandleStep(FURLabRpcDispatcher& D, + const TSharedPtr& Req, const FStepRequestCommon& Common) override + { + AAMjManager* Mgr = D.OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; + + TSharedPtr Cmd = MakeShared(); + int32 NSteps = 1; + Req->TryGetNumberField(TEXT("n_steps"), NSteps); + Cmd->Request.NSteps = NSteps > 0 ? NSteps : 1; + Cmd->ObservationLevel = Common.ObservationLevel; + ParseStepPerArticulation(Req, Mgr, Cmd->Request); + + Cmd->Completion = FPlatformProcess::GetSynchEventFromPool(true); + + const bool bWorkerRunning = Engine->bWorkerRunning.load(std::memory_order_acquire); + D.StepQueue.Enqueue(Cmd); + if (Engine->StepRequestEvent) + Engine->StepRequestEvent->Trigger(); + + if (!bWorkerRunning) + { + // Test / editor path: pump the handler synchronously so we don't + // block forever waiting for an engine that isn't ticking. The handler + // mutates mjData, so hold the engine lock the handler contract + // requires (the worker path already runs it under CallbackMutex). + if (Engine->CustomStepHandler) + { + FScopeLock Lock(&Engine->CallbackMutex); + Engine->CustomStepHandler(Engine->GetModel(), Engine->GetData()); + } + } + + // 5-second hard cap so a wedged engine returns an error rather than + // wedging the RPC thread. Polled in 50ms slices so the bDraining flag + // (set when the bridge is being stopped) can short-circuit the wait. + bool bSignaled = false; + { + const double Deadline = FPlatformTime::Seconds() + 5.0; + while (FPlatformTime::Seconds() < Deadline) + { + if (D.bDraining.load(std::memory_order_acquire)) + break; + if (Cmd->Completion->Wait(FTimespan::FromMilliseconds(50))) + { + bSignaled = true; + break; + } + } + } + + if (bSignaled && Cmd->bDone) + { + TMap CameraMinFrameIds = Common.CameraMinFrameIds; + if (Common.bRenderSync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, Cmd->ResultFrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + else if (Common.bRenderAsync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, 0, Common.CameraTimeoutMs, CameraMinFrameIds, /*bWait=*/false); + else if (Common.bWaitCameras && Common.CameraSpec.Num() > 0) + D.WaitForCameraFrames(Mgr, Common.CameraSpec, Cmd->ResultFrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + // The base reply (arts/scene/time/step/frame_id) was built on the + // physics thread from the state IR while the just-stepped mjData was + // valid; append any requested cameras here. + D.AppendCamerasBlock(Cmd->Reply, Mgr, Common.CameraSpec, CameraMinFrameIds); + return Cmd->Reply; + } + + // We stop waiting but the command is still queued. Mark it abandoned so + // the handler discards it instead of stepping physics for a request the + // client already saw fail (and may retry) -- otherwise the step executes + // twice. + Cmd->bAbandoned.store(true, std::memory_order_release); + if (D.bDraining.load(std::memory_order_acquire)) + return FURLabRpcDispatcher::MakeError(URLabError::ShuttingDown, + TEXT("Bridge stopping; Direct-mode step abandoned")); + return FURLabRpcDispatcher::MakeError(URLabError::StepTimeout, + TEXT("Direct-mode step did not complete within 5s")); + } +}; + +struct FPuppetStepMode : FStepModeStrategy +{ + EStepMode Mode() const override { return EStepMode::Puppet; } + void OnEnter(FURLabRpcDispatcher& /*D*/, AAMjManager& Mgr) override + { + Mgr.bPublishersPaused.store(true, std::memory_order_release); + if (Mgr.PhysicsEngine) + Mgr.PhysicsEngine->SetStepMode(EStepMode::Puppet); + } + void OnExit(FURLabRpcDispatcher& /*D*/, AAMjManager& /*Mgr*/) override {} + + // Client owns the integrator: write the pushed qpos/qvel/ctrl/time into + // mjData, run mj_forward, and return the derived state. + TSharedPtr HandleStep(FURLabRpcDispatcher& D, + const TSharedPtr& Req, const FStepRequestCommon& Common) override + { + AAMjManager* Mgr = D.OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; + + FMjPushStateRequest Push; + const TArray>* QPosArr = nullptr; + const TArray>* QVelArr = nullptr; + const TArray>* CtrlArr = nullptr; + if (Req->TryGetArrayField(TEXT("qpos"), QPosArr)) + { + Push.QPos.Reserve(QPosArr->Num()); + for (auto& V : *QPosArr) + Push.QPos.Add(V->AsNumber()); + } + if (Req->TryGetArrayField(TEXT("qvel"), QVelArr)) + { + Push.QVel.Reserve(QVelArr->Num()); + for (auto& V : *QVelArr) + Push.QVel.Add(V->AsNumber()); + } + if (Req->TryGetArrayField(TEXT("ctrl"), CtrlArr)) + { + Push.bIncludeCtrl = true; + Push.Ctrl.Reserve(CtrlArr->Num()); + for (auto& V : *CtrlArr) + Push.Ctrl.Add(V->AsNumber()); + } + double TimeVal = 0.0; + Req->TryGetNumberField(TEXT("time"), TimeVal); + Push.Time = TimeVal; + + TSharedPtr Reply; + uint64 PostFrameId = 0; + { + // Fetch model/data AFTER the lock: a concurrent CompileModel frees + // them under CallbackMutex. + FScopeLock Lock(&Engine->CallbackMutex); + mjModel* m = Engine->GetModel(); + mjData* d = Engine->GetData(); + if (!m || !d) + return FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("PhysicsEngine not initialised")); + ApplyPushedState(Engine, Push, m, d); + + // Publish the just-pushed pose to the render snapshot now, while we + // still hold CallbackMutex (lock order CallbackMutex -> + // RenderStateMutex). Without this the snapshot only refreshes on the + // physics loop's idle timeout, so cameras and any render consumer + // would lag the pushed state. The synchronous camera path below + // relies on this snapshot being current. + Engine->PushRenderState(); + + // Build the reply from the state IR while still holding the lock. The + // puppet-mode worker wakes on its idle timeout and can mutate d + // (mocap/wrench drain), which would tear a read done after release. + PostFrameId = Engine->GetRenderFrameId(); + const int64 StepIdx = D.StepCounter.fetch_add(1, std::memory_order_relaxed) + 1; + const FMjStateSnapshot& Snap = Mgr->GetStateCollector().Collect(m, d, StepIdx); + Reply = D.BuildStepReply(Snap, PostFrameId, Common.ObservationLevel); + } + + // frame_id is the post-step state id: the client passes it back as a + // camera frame_id to fetch the image showing this exact step's state. + TMap CameraMinFrameIds = Common.CameraMinFrameIds; + if (Common.bRenderSync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, PostFrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + else if (Common.bRenderAsync && Common.CameraSpec.Num() > 0) + D.RenderCamerasSync(Mgr, Common.CameraSpec, 0, Common.CameraTimeoutMs, CameraMinFrameIds, /*bWait=*/false); + else if (Common.bWaitCameras && Common.CameraSpec.Num() > 0) + D.WaitForCameraFrames(Mgr, Common.CameraSpec, PostFrameId, Common.CameraTimeoutMs, CameraMinFrameIds); + + D.AppendCamerasBlock(Reply, Mgr, Common.CameraSpec, CameraMinFrameIds); + // Puppet-mode perturbation: include the latest sample so the client can + // apply the editor click-drag widget's force to its own MjData. + if (Mgr->Perturbation) + { + FMjPerturbationSample Sample = Mgr->Perturbation->GetLatestPerturbationSample(); + if (Sample.BodyId > 0) + { + TSharedPtr Pert = MakeShared(); + Pert->SetNumberField(TEXT("body_id"), Sample.BodyId); + Pert->SetNumberField(TEXT("version"), Sample.Version); + TArray> Six; + for (int i = 0; i < 6; ++i) + Six.Add(MakeShared(Sample.Xfrc[i])); + Pert->SetArrayField(TEXT("xfrc"), Six); + Reply->SetObjectField(TEXT("perturbation"), Pert); + } + } + return Reply; + } +}; + +TSharedPtr FURLabRpcDispatcher::MakeStepStrategy(EStepMode Mode) +{ + switch (Mode) + { + case EStepMode::Direct: + return MakeShared(); + case EStepMode::Puppet: + return MakeShared(); + case EStepMode::Live: + case EStepMode::Auto: + default: + return MakeShared(); + } +} + +void FURLabRpcDispatcher::SetActiveStepMode(EStepMode NewMode) +{ + // Serialises install/uninstall side effects against concurrent set_mode + // calls; Dispatch releases DispatchMutex before handlers. + FScopeLock Lock(&DispatchMutex); + + const EStepMode Mode = (NewMode == EStepMode::Auto) ? EStepMode::Live : NewMode; + const EStepMode CurMode = ActiveStepMode.load(std::memory_order_acquire); + if (Mode == CurMode) + return; + + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr) + return; + + if (CurrentStepStrategy) + CurrentStepStrategy->OnExit(*this, *Mgr); + DrainQueues(); + + ActiveStepMode.store(Mode, std::memory_order_release); + // Camera publishers stream in every mode (frames are decoupled from the + // step reply), so they are never paused by mode. + FCameraZmqWorker::bPublishersPaused.store(false, std::memory_order_release); + + CurrentStepStrategy = MakeStepStrategy(Mode); + CurrentStepStrategy->OnEnter(*this, *Mgr); + + UE_LOG(LogURLabNet, Log, TEXT("FURLabRpcDispatcher: step mode -> %s"), + *StepModeToString(Mode)); +} + +void FURLabRpcDispatcher::ReapplyActiveStepMode() +{ + FScopeLock Lock(&DispatchMutex); + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !CurrentStepStrategy) + return; + // OnExit before OnEnter so the direct handler's install guard + // (bDirectHandlerInstalled) doesn't skip reinstalling onto the fresh engine + // handler slot after a recompile. + CurrentStepStrategy->OnExit(*this, *Mgr); + CurrentStepStrategy->OnEnter(*this, *Mgr); +} + +void FURLabRpcDispatcher::InstallDirectHandler() +{ + AAMjManager* Mgr = OwnerMgr.Get(); + if (!Mgr || !Mgr->PhysicsEngine) + return; + if (bDirectHandlerInstalled) + return; + + UMjPhysicsEngine* Engine = Mgr->PhysicsEngine; + DirectStepHandler = [this, Engine, Mgr](mjModel* m, mjData* d) -> bool { + // Only the physics-engine async worker thread runs this handler, + // so d is exclusively owned for its duration. + TSharedPtr Cmd; + if (!StepQueue.Dequeue(Cmd) || !Cmd.IsValid()) + return false; // idle wake, no step requested — no advance + + // The RPC thread gave up on this command (timeout / draining) before we + // got to it. Discard without stepping so a request the client already saw + // fail does not also advance physics here (double step). + if (Cmd->bAbandoned.load(std::memory_order_acquire)) + return false; + + ApplyStepCtrl(Mgr, Cmd->Request, m, d); + + // control_mode="raw" per articulation bypasses the UE controller + // (NetworkValue treated as direct ctrl setpoint). Name-keyed so + // adding/removing articulations doesn't shift the mapping. The set is + // fixed for this command (registration is compile-time), so read it once. + const TArray& Arts = Mgr->GetAllArticulations(); + TMap SkipController; + for (AMjArticulation* Art : Arts) + { + if (!Art) + continue; + // Clients key control mode by the public segment (ActorId-based); accept + // the raw UE name too for callers that still address by it. + const FString* Mode = Cmd->Request.PerArticulationControlMode.Find( + FMjCanonicalName::ArtSegment(Art).ToString()); + if (!Mode) + Mode = Cmd->Request.PerArticulationControlMode.Find(Art->GetName()); + const bool bRaw = Mode && Mode->Equals(TEXT("raw"), ESearchCase::IgnoreCase); + SkipController.Add(Art, bRaw); + } + + for (int32 i = 0; i < Cmd->Request.NSteps; ++i) + { + for (AMjArticulation* Art : Arts) + { + if (!Art) + continue; + const bool* bSkip = SkipController.Find(Art); + Art->ApplyControls(bSkip != nullptr && *bSkip); + } + mj_step(m, d); + if (Engine->OnPostStep) + Engine->OnPostStep(m, d); + } + Cmd->ResultTime = d->time; + Cmd->ResultStep = StepCounter.fetch_add(Cmd->Request.NSteps, std::memory_order_relaxed) + + Cmd->Request.NSteps; + // Publish the just-stepped state to the render snapshot now, while this + // handler still owns d (it runs inside the engine's CallbackMutex), and + // capture the resulting frame_id for the reply. Without this the RPC + // thread would read GetRenderFrameId() after we Trigger() below but + // before the physics loop's own PushRenderState() runs, returning the + // previous step's id and breaking camera frame association. + Engine->PushRenderState(); + Engine->bRenderStatePublishedThisStep = true; // loop tail must not republish + Cmd->ResultFrameId = Engine->GetRenderFrameId(); + // Build the base reply (arts/scene/time/step/frame_id) from the state IR + // here, where the just-stepped mjData is valid and the persistent snapshot + // buffer is not racing another Collect (all callers hold CallbackMutex). + const FMjStateSnapshot& Snap = Mgr->GetStateCollector().Collect(m, d, Cmd->ResultStep); + Cmd->Reply = BuildStepReply(Snap, Cmd->ResultFrameId, Cmd->ObservationLevel); + Cmd->bDone = true; + if (Cmd->Completion) + Cmd->Completion->Trigger(); + return true; + }; + Engine->SetCustomStepHandler(DirectStepHandler); + bDirectHandlerInstalled = true; +} + +void FURLabRpcDispatcher::UninstallDirectHandler() +{ + if (!bDirectHandlerInstalled) + return; + if (AAMjManager* Mgr = OwnerMgr.Get()) + { + if (Mgr->PhysicsEngine) + Mgr->PhysicsEngine->ClearCustomStepHandler(); + } + bDirectHandlerInstalled = false; + DirectStepHandler = nullptr; +} diff --git a/Source/URLab/Public/Bridge/AssetCache.h b/Source/URLab/Public/Bridge/AssetCache.h new file mode 100644 index 00000000..0059fe61 --- /dev/null +++ b/Source/URLab/Public/Bridge/AssetCache.h @@ -0,0 +1,86 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "CoreMinimal.h" + +/** + * @class FURLabAssetCache + * @brief Content-addressed, on-disk blob store shared across farm instances. + * + * A blob lives at `{root}/blobs/{sha[:2]}/{sha}`; its content is its key, so + * reads need no lock and are immutable. Writes are atomic (temp file in the + * same directory, then rename), which makes two processes storing the same + * hash safe — the bytes are identical, so whichever rename wins, the result + * is correct. The manifest step of the upload protocol resolves each asset + * hash here to skip re-sending bytes an instance already holds. + * + * The default root comes from `URLAB_ASSET_CACHE`, else + * `%LOCALAPPDATA%/URLab/cache` on Windows and `~/.cache/URLab/cache` + * (honouring `XDG_CACHE_HOME`) on Linux. + */ +class URLAB_API FURLabAssetCache +{ +public: + /** Cache rooted at an explicit directory (used by tests). */ + explicit FURLabAssetCache(const FString& InRoot); + + /** Process-wide cache rooted at ResolveCacheRoot(). */ + static FURLabAssetCache& Get(); + + /** True if a verified blob for this hash is already stored. Lock-free. */ + bool Has(const FString& Sha256Hex) const; + + /** Resolve a stored blob to its on-disk path. Returns false if absent. */ + bool GetPath(const FString& Sha256Hex, FString& OutPath) const; + + /** Store bytes under their (already-computed) content hash. Atomic: writes + * a temp file in the blob's directory then renames it into place, so a + * concurrent writer of the same hash is harmless. Returns false only on a + * filesystem error. */ + bool Put(const FString& Sha256Hex, const uint8* Data, int32 Size); + bool Put(const FString& Sha256Hex, const TArray& Data) + { + return Put(Sha256Hex, Data.GetData(), Data.Num()); + } + + /** Root directory of this cache instance. */ + const FString& GetRoot() const { return Root; } + + /** Best-effort LRU eviction: if the total blob size exceeds MaxBytes, delete + * the least-recently-used blobs (by mtime) until back under the cap. A + * MaxBytes <= 0 reads URLAB_ASSET_CACHE_MAX_BYTES (0/unset = no eviction). + * Returns the number of bytes freed. */ + int64 EvictToBudget(int64 MaxBytes = 0); + + /** Default cache root (env override, else per-OS user cache dir). */ + static FString ResolveCacheRoot(); + + /** Lowercase-hex SHA-256 of a byte range. */ + static FString Sha256Hex(const uint8* Data, int32 Size); + static FString Sha256Hex(const TArray& Data) + { + return Sha256Hex(Data.GetData(), Data.Num()); + } + + /** True if the string is a syntactically valid lowercase SHA-256 hex digest + * (64 chars, [0-9a-f]). */ + static bool IsValidSha256Hex(const FString& Candidate); + +private: + FString BlobPath(const FString& Sha256Hex) const; + + FString Root; +}; diff --git a/Source/URLab/Public/Bridge/BridgeServer.h b/Source/URLab/Public/Bridge/BridgeServer.h index 34366fe2..2f0a7ed1 100644 --- a/Source/URLab/Public/Bridge/BridgeServer.h +++ b/Source/URLab/Public/Bridge/BridgeServer.h @@ -8,6 +8,7 @@ #include "CoreMinimal.h" #include "UObject/Object.h" #include "Bridge/RpcDispatcher.h" +#include "Bridge/BridgeServerConfig.h" #include "BridgeServer.generated.h" class AAMjManager; @@ -51,9 +52,22 @@ class URLAB_API UURLabBridgeServer : public UObject * Empty string means "live". */ bool EnsureShmBound(const FString& SessionId = TEXT("")); + /** Bring up the optional out-of-core transport surface if not already up: the + * state publish transport (registered with the active manager's fan-out) and + * the control RPC transport (stored in `RpcTransports`), both created through + * the FMjExternalTransportProvider factory hooks. No-op returning false when + * no external transport module is loaded or its runtime is unavailable. */ + bool EnsureExternalTransportsBound(); + /** Dispatcher when running, nullptr otherwise. */ FURLabRpcDispatcher* GetDispatcher() const { return Dispatcher.Get(); } + /** Resolved per-instance config (ports, bind address, instance identity). + * Set by the owner before Start so the handshake `instance` block and the + * manager-owned state/camera endpoints read one source of truth. */ + void SetInstanceConfig(const FURLabBridgeServerConfig& InConfig) { InstanceConfig = InConfig; } + const FURLabBridgeServerConfig& GetInstanceConfig() const { return InstanceConfig; } + /** True when AAMjManager owns this server (cooked path, or editor * without subsystem auto-start). EndPlay tears it down only when so. */ bool IsOwnedByManager() const { return bOwnedByManager; } @@ -71,11 +85,85 @@ class URLAB_API UURLabBridgeServer : public UObject * doesn't need to inspect these directly. */ const TArray>& GetRpcTransports() const { return RpcTransports; } + // --- Cooperative render-farm lease (not a security boundary) --- + // One lease per process. A pool client claims this instance so the pool + // won't hand the same editor process to a second client. Guarded by + // LeaseMutex since RPC threads on multiple transports may touch it. + + /** Claim the lease if free. On success returns true and fills OutLeaseId + * with a fresh id; when already held returns false and fills + * OutExistingLeaseId with the current holder's id. Lazily expires an idle + * lease past its TTL before deciding. */ + bool TryAcquireLease(const FString& Owner, double TtlSeconds, + FString& OutLeaseId, FString& OutExistingLeaseId); + + /** Release the lease when InLeaseId matches the current holder. Returns + * false when no lease is held or the id doesn't match. */ + bool ReleaseLease(const FString& InLeaseId); + + /** Refresh the activity timestamp so an active client keeps its lease. + * No-op when no lease is held. */ + void TouchLease(); + + /** True when a lease is currently held. Performs lazy TTL expiry: an idle + * lease past its TTL is auto-released before the state is reported. */ + bool IsLeaseHeld(); + + /** Current lease id, or empty when none is held. */ + FString GetLeaseId() const; + + /** Test seam: pin the lease clock to a fixed value so TTL expiry is + * deterministic without sleeping. Every lease op consults it — including + * TouchLease on the dispatch path — so a driven Dispatch() sees injected + * time. A negative value restores the wall clock. */ + void SetLeaseClockForTest(double NowSeconds); + private: + /** Construct the dispatcher if needed and wire its back-pointer to this + * server so no-manager ops (e.g. leasing) can reach per-instance state. */ + void EnsureDispatcher(); + + /** Current lease clock: the test override when set (>= 0), else the wall + * clock. Callers hold LeaseMutex. */ + double LeaseNow() const; + + /** Lazy-expire then report lease state. Assumes LeaseMutex is held. */ + bool IsLeaseHeldInternal(double NowSeconds); + + mutable FCriticalSection LeaseMutex; + bool bLeaseHeld = false; + FString LeaseId; + FString LeaseOwner; + double LeaseTtlSeconds = 0.0; + double LeaseLastActivitySeconds = 0.0; + double LeaseClockOverrideForTest = -1.0; + TUniquePtr Dispatcher; TWeakObjectPtr ActiveManager; bool bOwnedByManager = false; + /** Resolved config, set via SetInstanceConfig. Defaults reproduce the + * single-editor behaviour when no owner supplies one. */ + FURLabBridgeServerConfig InstanceConfig; + + // Disable editor frame pacing (VSync + FPS cap) while a real transport is + // bound, so camera frame delivery isn't capped at the monitor refresh. + // Prior cvar values are saved and restored on Stop. + void ApplyPerformanceOverrides(); + void RestorePerformanceOverrides(); + + bool bPacingOverridden = false; + bool bHadVSync = false; + bool bHadVSyncEditor = false; + bool bHadMaxFPS = false; + bool bHadSlateThrottle = false; + bool bHadIdleWhenNotForeground = false; + float SavedVSync = 1.0f; + float SavedVSyncEditor = 1.0f; + float SavedMaxFPS = 0.0f; + float SavedSlateThrottle = 1.0f; + float SavedIdleWhenNotForeground = 0.0f; + /** Every bound RPC transport. Survives PIE transitions. Transient so * UE GC won't try to serialise these alongside the bridge UObject. */ UPROPERTY(Transient) diff --git a/Source/URLab/Public/Bridge/BridgeServerConfig.h b/Source/URLab/Public/Bridge/BridgeServerConfig.h index 56a1819e..0dc9860d 100644 --- a/Source/URLab/Public/Bridge/BridgeServerConfig.h +++ b/Source/URLab/Public/Bridge/BridgeServerConfig.h @@ -31,6 +31,38 @@ struct URLAB_API FURLabBridgeServerConfig UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") int32 StatePort = 5555; + /** Human-readable instance name that seeds every namespaced resource + * (SHM session dir, registry file, wake objects). Empty resolves to the + * SHM layer's default ("live") for a single editor, or "instance_{index}" + * when a farm index is set. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + FString InstanceId; + + /** Zero-based ordinal in a render farm. -1 means "not a farm member": + * ports keep their single-editor values. When >= 0 the step/state/camera + * ports derive from PortBase + Index*PortStride unless overridden. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + int32 InstanceIndex = -1; + + /** First port of the strided per-instance block. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + int32 PortBase = 5559; + + /** Port span reserved to each instance (step, state, several camera + * ports, and headroom for future channels). */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + int32 PortStride = 10; + + /** First camera stream port; cameras allocate upward from here. 0 means + * "derive" (StepPort + 2, or PortBase + Index*PortStride + 2 for a farm). */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + int32 CamBasePort = 0; + + /** Interface the RPC, state, and camera sockets bind. Default 0.0.0.0 so + * remote clients can connect; pin a NIC or loopback to restrict reach. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|Bridge") + FString BindAddress = TEXT("0.0.0.0"); + /** When true, EndPlay tears the server down even if it was started by * the editor subsystem. Default false: an editor-spawned server stays * up across PIE cycles. */ diff --git a/Source/URLab/Public/Bridge/BridgeServerConfigUtils.h b/Source/URLab/Public/Bridge/BridgeServerConfigUtils.h index 6406935a..b9cb2f7b 100644 --- a/Source/URLab/Public/Bridge/BridgeServerConfigUtils.h +++ b/Source/URLab/Public/Bridge/BridgeServerConfigUtils.h @@ -21,6 +21,33 @@ URLAB_API void LoadFromIni(FURLabBridgeServerConfig& Out); * parent directories if missing. */ URLAB_API void SaveToIni(const FURLabBridgeServerConfig& In); +/** Apply per-instance overrides on top of an already INI-loaded config. + * Precedence, highest first: command line, environment, INI. + * Command line: -URLabInstanceId=, -URLabInstanceIndex=, -URLabStepPort=, + * -URLabStatePort=, -URLabCamBasePort=, -URLabPortBase=, -URLabPortStride=, + * -URLabBindAddress=. Environment: the URLAB_* equivalents. When a farm + * index is set and a port was not explicitly overridden, the port derives + * from PortBase + Index*PortStride (see DerivePorts). Pure function of the + * passed struct plus this process's environment and command line. */ +URLAB_API void ApplyEnvAndCommandLineOverrides(FURLabBridgeServerConfig& Cfg); + +/** Resolve the derived fields (step/state/camera ports, instance id) from + * InstanceIndex, honouring any port that an override already set explicitly. + * Separated from ApplyEnvAndCommandLineOverrides so the derivation is unit + * testable without touching process env or command line: + * - InstanceIndex >= 0: StepPort/StatePort/CamBasePort = + * PortBase + Index*PortStride + {0,1,2} for each port not flagged explicit. + * - CamBasePort still 0 afterwards: defaults to StepPort + 2. + * - Empty InstanceId with a farm index: becomes "instance_{index}". */ +URLAB_API void DerivePorts(FURLabBridgeServerConfig& Cfg, + bool bStepExplicit, bool bStateExplicit, bool bCamExplicit); + +/** Build the ZMQ endpoint for the camera at CameraIndex within this + * instance's camera port block: tcp://{BindAddress}:{CamBasePort + CameraIndex}. + * Cameras allocate one port each, upward from CamBasePort, so N instances on + * distinct CamBasePort blocks never collide. CameraIndex is clamped to >= 0. */ +URLAB_API FString BuildCameraEndpoint(const FURLabBridgeServerConfig& Cfg, int32 CameraIndex); + /** Section name used in the INI. Exposed for tests. */ URLAB_API extern const TCHAR* SectionName; } // namespace URLabBridgeServerConfigUtils diff --git a/Source/URLab/Public/Bridge/ControlOwnership.h b/Source/URLab/Public/Bridge/ControlOwnership.h new file mode 100644 index 00000000..b29feb3f --- /dev/null +++ b/Source/URLab/Public/Bridge/ControlOwnership.h @@ -0,0 +1,98 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "CoreMinimal.h" + +/** One articulation's control claim: who owns it, its TTL, and the last time + * the owner touched it. A zero TTL never expires. */ +struct FMjControlClaim +{ + FString Owner; // source id + double TtlSeconds = 0.0; // 0 = no expiry + double LastActivitySeconds = 0.0; +}; + +/** + * Per-articulation control arbitration. Every control write (ctrl, xfrc, + * twist, qpos, mocap, control-source flip) carries a source id and must own + * the target articulation before it applies; observation is never gated. + * + * An unclaimed articulation rejects control writes — control must always be + * explicitly owned so RPC, Python, and ROS surfaces coexist without silently + * fighting over the same robot. Claims are cooperative, not a security + * boundary: a TTL auto-frees a crashed owner, and `bForce` steals for operator + * override / crash recovery. + * + * Multiple RPC transport threads (ZMQ + SHM, and later a ROS executor) reach + * this concurrently, so every operation takes the mutex. Expiry is lazy — + * evaluated on the next Claim / CheckWrite for an articulation rather than on a + * timer. + */ +class URLAB_API FMjControlOwnership +{ +public: + enum class EClaimResult : uint8 + { + Ok, + AlreadyOwned + }; + + /** Claim `Art` for `Source`. `bForce` steals an existing claim (operator + * override). Returns AlreadyOwned and fills OutCurrentOwner when the art is + * held by another live source and force is not set. */ + EClaimResult Claim(FName Art, const FString& Source, double TtlSeconds, + bool bForce, FString& OutCurrentOwner); + + /** Release `Art` when `Source` owns it. Returns false when the source is + * not the current owner (including an already-expired or absent claim). */ + bool Release(FName Art, const FString& Source); + + enum class EWriteCheck : uint8 + { + Ok, + NotOwner + }; + + /** The gate for every control write. Ok refreshes LastActivity (the + * heartbeat). Unclaimed art => NotOwner (unclaimed = external control + * ignored). Fills OutCurrentOwner on NotOwner when another source holds it. */ + EWriteCheck CheckWrite(FName Art, const FString& Source, FString& OutCurrentOwner); + + /** Snapshot of currently-held (non-expired) claims, art -> owner. Lazily + * expires stale claims first. Used by the global control-source flip, which + * must own every claimed art (or none may be claimed). */ + TMap GetActiveOwners(); + + /** Drop every claim. Called on OnManagerGone / Shutdown: claims are per-PIE + * because the articulations die with the world. */ + void Reset(); + + /** Pin the clock to a fixed value so TTL expiry is deterministic without + * sleeping. A negative value restores the wall clock. */ + void SetClockOverrideForTest(double NowSeconds); + +private: + /** Current time: the test override when set (>= 0), else the wall clock. + * Callers hold Mutex. */ + double Now() const; + + /** True when a claim has a live TTL and has been idle past it. */ + static bool IsExpired(const FMjControlClaim& Claim, double NowSeconds); + + FCriticalSection Mutex; // RPC threads: ZMQ + SHM (+ ROS later) + TMap Claims; + double ClockOverrideForTest = -1.0; +}; diff --git a/Source/URLab/Public/Bridge/InstanceRegistry.h b/Source/URLab/Public/Bridge/InstanceRegistry.h new file mode 100644 index 00000000..e955cf0d --- /dev/null +++ b/Source/URLab/Public/Bridge/InstanceRegistry.h @@ -0,0 +1,46 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. + +#pragma once + +#include "CoreMinimal.h" + +struct FURLabBridgeServerConfig; + +/** + * @class FURLabInstanceRegistry + * @brief File-based, broker-less discovery for render-farm instances. + * + * Each editor process writes {registry_dir}/{instance_id}_{pid}.json on + * server start with the same fields as the `hello` instance block plus a + * `registry_written_at` timestamp, and deletes it on clean shutdown. A + * client reads the directory to discover live instances without any daemon. + */ +class URLAB_API FURLabInstanceRegistry +{ +public: + /** Feature flags advertised by this build. Single source shared by the + * handshake `instance` block and the registry file. */ + static const TArray& Capabilities(); + + /** Registry directory: URLAB_REGISTRY_DIR override, else + * %LOCALAPPDATA%/URLab/registry on Windows and ~/.cache/URLab/registry + * (honouring XDG_CACHE_HOME) on Linux. */ + static FString ResolveRegistryDir(); + + /** Path of this instance's entry file within ResolveRegistryDir(). */ + static FString ResolveEntryPath(const FURLabBridgeServerConfig& Cfg); + + /** Write (or overwrite) this instance's entry file. */ + static void WriteEntry(const FURLabBridgeServerConfig& Cfg, const FString& UrlabVersion, + bool bManagerPresent, bool bBusy); + + /** Rewrite the entry to refresh its mtime for the staleness heartbeat. */ + static void RefreshEntry(const FURLabBridgeServerConfig& Cfg, const FString& UrlabVersion, + bool bManagerPresent, bool bBusy); + + /** Delete this instance's entry file. Safe when it was never written. */ + static void RemoveEntry(const FURLabBridgeServerConfig& Cfg); +}; diff --git a/Source/URLab/Public/Bridge/MsgpackHelpers.h b/Source/URLab/Public/Bridge/MsgpackHelpers.h index d2a7d2c6..abba55b4 100644 --- a/Source/URLab/Public/Bridge/MsgpackHelpers.h +++ b/Source/URLab/Public/Bridge/MsgpackHelpers.h @@ -50,9 +50,16 @@ class URLAB_API FURLabMsgpackUtil * need a separate code path for binary fields. */ static bool UnpackToJsonObject(const uint8* Data, int32 Size, TSharedPtr& OutObject); - /** Pack-and-set a binary blob under a JSON object's field. Stores as - * base64 string with a `__b64__` suffix on the key so PackJsonObject - * knows to re-emit it as a real msgpack `bin` rather than a string. - * Used by handshake (MJB) and camera replies (pixel buffers). */ + /** Pack-and-set a binary blob under a JSON object's field so PackJsonObject + * emits it as a real msgpack `bin`. Owns a single copy of the bytes (no + * base64), so the caller's buffer need not outlive the reply. Used by the + * handshake (MJB blob) and any caller without a shareable source buffer. */ static void SetBinaryField(TSharedPtr& Obj, const FString& Field, const uint8* Data, int32 Size); + + /** Zero-copy variant of SetBinaryField: packs `Data`/`Size` straight through + * as msgpack `bin` with no base64 and no intermediate copy. `Keeper` must own + * the buffer that `Data` points into; it is retained until the reply is packed, + * so a caller can hand raw pixel bytes directly from a shared frame. */ + static void SetBinaryFieldShared(TSharedPtr& Obj, const FString& Field, + const uint8* Data, int32 Size, TSharedPtr Keeper); }; diff --git a/Source/URLab/Public/Bridge/RpcDispatcher.h b/Source/URLab/Public/Bridge/RpcDispatcher.h index b1c63fd4..b0e6aec4 100644 --- a/Source/URLab/Public/Bridge/RpcDispatcher.h +++ b/Source/URLab/Public/Bridge/RpcDispatcher.h @@ -16,15 +16,22 @@ #include "CoreMinimal.h" #include "MuJoCo/Core/MjPhysicsEngine.h" +#include "Bridge/ControlOwnership.h" +#include "State/MjObservationLevel.h" #include "Dom/JsonObject.h" #include "Containers/Queue.h" #include class AAMjManager; class AMjReplayManager; +class UURLabBridgeServer; +struct FMjStateSnapshot; struct FMjStepRequest; struct FMjDirectStepCommand; -struct FMjPushStateRequest; +struct FStepModeStrategy; +struct FLiveStepMode; +struct FDirectStepMode; +struct FPuppetStepMode; /** * @brief Transport-agnostic step-server core. @@ -35,22 +42,23 @@ struct FMjPushStateRequest; * same dispatcher; they only differ in how bytes get from the wire to * `Dispatch()` and back. * - * Lives on `AAMjManager` so all transports share one instance; the - * dispatcher's serializing mutex makes concurrent dispatches from - * multiple transports safe. + * Lives on `AAMjManager` so all transports share one instance. DispatchMutex + * serialises only the session / mode / step-handler mutation; it is released + * before the handler bodies, so those run concurrently across transports + * (ZMQ + SHM). The direct-mode step queue is therefore MPSC and each command's + * completion / abandon flag is atomic. */ class URLAB_API FURLabRpcDispatcher { + // The per-mode strategies own the step body (RpcHandlers_Step.cpp) and reach + // into dispatcher internals (queue, counters, camera helpers) to run it. + friend struct FLiveStepMode; + friend struct FDirectStepMode; + friend struct FPuppetStepMode; + public: /** Observation verbosity. minimal=qpos+qvel; standard=+ctrl+act+sensors; * full=+body xpos/xquat+actuator forces. */ - enum class EObservationLevel : uint8 - { - Minimal, - Standard, - Full - }; - /** Per-camera include mode for step replies. */ enum class ECameraInclude : uint8 { @@ -64,6 +72,11 @@ class URLAB_API FURLabRpcDispatcher /** Bind the dispatcher to a manager (game thread, called from BeginPlay). */ void Init(AAMjManager* InManager); + /** Back-pointer to the owning bridge server, set once at construction so + * no-manager ops (leasing) can reach per-instance state even before a + * scene/manager exists. */ + void SetOwningBridge(UURLabBridgeServer* InBridge); + /** Per-manager teardown: drop the manager pointer, uninstall step * handlers, drain queues, reset per-PIE state (mode, step counter). * KEEPS the bridge-level session id, encoding flag, and observation @@ -83,7 +96,7 @@ class URLAB_API FURLabRpcDispatcher bool IsDraining() const { return bDraining.load(std::memory_order_acquire); } /** Wire format echoed to the client as the urlab plugin version. */ - FString URLabVersion = TEXT("urlab/0.1"); + FString URLabVersion = TEXT("urlab/0.2"); // --- Top-level dispatch --- @@ -114,6 +127,10 @@ class URLAB_API FURLabRpcDispatcher int64 GetStepCounter() const { return StepCounter.load(std::memory_order_acquire); } + /** Per-articulation control arbitration shared by every control write. + * Exposed so tests can drive claims and the deterministic clock seam. */ + FMjControlOwnership& GetControlOwnership() { return ControlOwnership; } + /** Worker threads read the replay manager via this cache instead of * TActorIterator (which asserts IsInGameThread). */ void SetCachedReplayManager(AMjReplayManager* RM); @@ -121,8 +138,11 @@ class URLAB_API FURLabRpcDispatcher // --- Test seams (lifted from UURLabZmqRpcTransport) --- void EnqueueStepRequestForTest(FMjStepRequest&& Req); - void EnqueuePushStateRequestForTest(FMjPushStateRequest&& Req); - void DrainQueuesForTest(); + + /** Empty the direct-mode step queue, abandoning + waking every pending + * command so a blocked RPC thread returns at once. Called on mode switch, + * PIE-end, and from tests. */ + void DrainQueues(); // --- Static helpers (transport-agnostic, callable from anywhere) --- @@ -144,17 +164,71 @@ class URLAB_API FURLabRpcDispatcher static void ApplyStepCtrl(AAMjManager* Manager, const FMjStepRequest& Req, mjModel* m, mjData* d); - static TSharedPtr BuildStepObservations(AAMjManager* Manager, - mjModel* m, mjData* d, - EObservationLevel Level = EObservationLevel::Standard); - - static TSharedPtr BuildEntitiesBlock(AAMjManager* Manager, - mjModel* m, mjData* d); - + /** + * Non-blocking camera retrieval from each camera's frame-history ring. + * CameraSpec selects the cameras; MinFrameIds optionally requests, per + * camera, the frame showing state >= that id (0 / absent = latest + * available). Requested cameras are touched so per-camera capture gating + * keeps them live. Each emitted camera carries its frame's `frame_id` / + * `sim_time` so the bridge can associate an image with a step. + */ static TSharedPtr BuildCamerasBlock(AAMjManager* Manager, const TMap& CameraSpec, + const TMap& MinFrameIds = TMap(), int32 TimeoutMs = 1000); + /** Assemble the base step_ok reply (op/time/step/clock/frame_id plus the + * encoded `arts` / `scene` blocks) from the state IR. Must be called while + * the snapshot is valid (under the engine's CallbackMutex). Cameras and any + * mode-specific fields (e.g. puppet perturbation) are appended by the caller + * after the lock is released. */ + TSharedPtr BuildStepReply(const FMjStateSnapshot& Snapshot, + uint64 FrameId, EObservationLevel Level); + + /** Append a `cameras` block to a step reply for the requested cameras, if any + * produced a frame. No-op when CameraSpec is empty. */ + void AppendCamerasBlock(TSharedPtr& Reply, AAMjManager* Mgr, + const TMap& CameraSpec, + const TMap& CameraMinFrameIds); + + /** Block the calling (RPC) thread until each requested camera has a frame + * with id >= MinFrameId, or the timeout elapses, or the bridge drains. + * For cameras that reach it, records MinFrameId as their floor in + * CameraMinFrameIds so the reply returns the frame showing this step's + * state; cameras that time out are left to return their latest frame + * (its smaller frame_id signals staleness to the client). Returns true if + * every requested camera reached MinFrameId. */ + bool WaitForCameraFrames(AAMjManager* Mgr, + const TMap& CameraSpec, + uint64 MinFrameId, int32 TimeoutMs, + TMap& CameraMinFrameIds); + + /** Render-on-demand: on the game thread, apply the latest physics snapshot, + * capture + read back every requested camera, flush the render thread once, + * and harvest — so the frame for MinFrameId exists within a single pump + * instead of over several ticks. Blocks the RPC thread until done or + * TimeoutMs elapses. Records MinFrameId as the floor for cameras that + * produced it. Opt-in (`render: "sync"`); flushes the game thread, so it is + * for eval, not interactive use. */ + bool RenderCamerasSync(AAMjManager* Mgr, + const TMap& CameraSpec, + uint64 MinFrameId, int32 TimeoutMs, + TMap& CameraMinFrameIds, + bool bWait = true); + + /** Build the name->camera lookup used to resolve include_cameras / + * set_camera_streaming keys. Keyed solely by the canonical "/" + * name (FMjCanonicalName), matching the handshake zmq_topic. First writer + * wins per key so a sanitize-collision can't hide a distinct camera. */ + static void BuildCameraNameMap(AAMjManager* Manager, + TMap& OutByName); + + /** Game-thread: apply per-camera streaming requests (key -> {zmq, shm}), + * toggling the broadcast flags + SetStreamingEnabled, and return the + * per-camera reply object (streaming flag + bound zmq endpoint/topic). */ + static TSharedPtr ApplyCameraStreamingGameThread(AAMjManager* Manager, + const TMap>& Requests); + static TSharedPtr MakeError(const FString& Code, const FString& Message); /** Stamps `sim_time` + `wall_time` blocks (ROS `builtin_interfaces/Time` @@ -172,12 +246,20 @@ class URLAB_API FURLabRpcDispatcher * weak via TWeakObjectPtr rather than UPROPERTY. */ TWeakObjectPtr OwnerMgr; + /** Owning bridge server (set at construction). Weak so a torn-down + * server leaves this null rather than dangling. Reached by the lease + * ops, which are per-process and independent of any manager. */ + TWeakObjectPtr OwningBridge; + /** Guards ActiveSessionId + step-handler install/uninstall. NOT held * across handler bodies — Dispatch releases it before invoking. */ FCriticalSection DispatchMutex; FString ActiveSessionId; + /** Per-articulation control ownership. Reset per PIE from OnManagerGone. */ + FMjControlOwnership ControlOwnership; + std::atomic ActiveStepMode{EStepMode::Live}; std::atomic ActiveObservationLevel{EObservationLevel::Standard}; std::atomic StepCounter{0}; @@ -190,25 +272,42 @@ class URLAB_API FURLabRpcDispatcher TWeakObjectPtr CachedReplayManager; - /** SPSC. Shared ownership so both RPC + physics threads can drop their - * ref independently — avoids UAF on RPC-side timeout while the handler - * is still processing. */ - TQueue, EQueueMode::Spsc> StepQueue; - - /** Puppet-mode request queue. */ - TQueue PushStateQueue; + /** MPSC. Two transport threads (ZMQ + SHM) can be inside the direct-mode + * step body at once (DispatchMutex is released before handler bodies), so + * both may enqueue. Shared ownership so the RPC + physics threads can drop + * their ref independently — avoids UAF on an RPC-side timeout while the + * handler is still processing. */ + TQueue, EQueueMode::Mpsc> StepQueue; - /** Custom step handlers installed on the engine for Direct / Puppet. */ - UMjPhysicsEngine::FMujocoStepCallback PuppetStepHandler; + /** Custom step handler installed on the engine for Direct mode. */ UMjPhysicsEngine::FMujocoStepCallback DirectStepHandler; - bool bPuppetHandlerInstalled = false; bool bDirectHandlerInstalled = false; - void InstallPuppetHandler(); - void UninstallPuppetHandler(); +public: + // Public so the per-mode strategy objects can drive them on enter/exit. void InstallDirectHandler(); void UninstallDirectHandler(); + /** After a mid-session recompile rebuilt mjModel/mjData, re-run the active + * strategy's OnEnter (under DispatchMutex) so its step handler is + * reinstalled onto the fresh engine and the pause / pacing invariants are + * restored. Called from the engine's recompile path. */ + void ReapplyActiveStepMode(); + +private: + /** Per-mode lifecycle + step body strategy: OnEnter installs the handler + + * sets pause / publisher state, OnExit uninstalls, HandleStep runs the + * mode's per-step work. Swapped by SetActiveStepMode. Shared so an in-flight + * HandleStep keeps the strategy alive across a concurrent set_mode swap. */ + TSharedPtr CurrentStepStrategy; + static TSharedPtr MakeStepStrategy(EStepMode Mode); + + /** Parse the request-scoped step fields (observation override, camera spec, + * wait / render flags) shared by every mode into Out. Does NOT mutate any + * session-level state. */ + void ParseStepCommon(const TSharedPtr& Req, AAMjManager* Mgr, + struct FStepRequestCommon& Out) const; + /** Register every dispatcher-owned op (manager-required + * no-manager) on the URLabOpRegistry with the right Category and * Namespace metadata. Bound `this` lambdas — paired with @@ -221,18 +320,41 @@ class URLAB_API FURLabRpcDispatcher void UnregisterDispatcherOps(); TArray RegisteredOpNames; + /** The control source for a request: the optional `source` field, falling + * back to `session_id`. Lets one session act as several sources in tests. */ + FString ResolveControlSource(const TSharedPtr& Req) const; + + /** Consult the control gate for a write to `ArtKey`. Returns nullptr when + * the request's source owns it; otherwise a `not_control_owner` error reply + * carrying the current owner. */ + TSharedPtr RejectIfNotControlOwner(FName ArtKey, + const TSharedPtr& Req); + // Op handlers TSharedPtr HandleHello(const TSharedPtr& Req); TSharedPtr HandleMeta(const TSharedPtr& Req); + TSharedPtr HandleAcquireLease(const TSharedPtr& Req); + TSharedPtr HandleReleaseLease(const TSharedPtr& Req); + // Network model upload (RpcHandlers_ModelUpload.cpp). Manifest + chunk are + // pure data staging on the RPC thread; commit materialises to a temp dir and + // drives the existing import_xml editor job. + TSharedPtr HandleUploadModelManifest(const TSharedPtr& Req); + TSharedPtr HandleUploadModelChunk(const TSharedPtr& Req); + TSharedPtr HandleUploadModelCommit(const TSharedPtr& Req); TSharedPtr HandleStep(const TSharedPtr& Req); TSharedPtr HandleReset(const TSharedPtr& Req); TSharedPtr HandleForward(const TSharedPtr& Req); TSharedPtr HandleSetMode(const TSharedPtr& Req); TSharedPtr HandleSetPaused(const TSharedPtr& Req); + TSharedPtr HandleSetCameraStreaming(const TSharedPtr& Req); + TSharedPtr HandleSetCameraDelay(const TSharedPtr& Req); TSharedPtr HandleConfigureController(const TSharedPtr& Req); TSharedPtr HandleSetSimOptions(const TSharedPtr& Req); TSharedPtr HandleSetSimSpeed(const TSharedPtr& Req); TSharedPtr HandleSetControlSource(const TSharedPtr& Req); + TSharedPtr HandleClaimControl(const TSharedPtr& Req); + TSharedPtr HandleReleaseControl(const TSharedPtr& Req); + TSharedPtr HandleSetUserChannels(const TSharedPtr& Req); TSharedPtr HandleSetTwist(const TSharedPtr& Req); TSharedPtr HandleSetQpos(const TSharedPtr& Req); TSharedPtr HandleSetMocapPose(const TSharedPtr& Req); @@ -242,3 +364,32 @@ class URLAB_API FURLabRpcDispatcher TSharedPtr HandleRecording(const FString& Op, const TSharedPtr& Req); TSharedPtr HandleReplay(const FString& Op, const TSharedPtr& Req); }; + +/** Request-scoped step fields parsed once per step (ParseStepCommon) and handed + * to the active strategy's HandleStep. Nothing here is session state — the + * observation override applies to this step only. */ +struct FStepRequestCommon +{ + EObservationLevel ObservationLevel = EObservationLevel::Standard; + TMap CameraSpec; + TMap CameraMinFrameIds; + bool bWaitCameras = false; + bool bRenderSync = false; + bool bRenderAsync = false; + int32 CameraTimeoutMs = 200; +}; + +/** Per-mode step lifecycle + body. Concrete Live/Direct/Puppet strategies (in + * RpcHandlers_Step.cpp) install/uninstall the step handler and set the engine + * pause + publisher state on transition, and own the per-step work in + * HandleStep, so the mode logic isn't a growing if-chain in HandleStep / + * SetActiveStepMode. */ +struct FStepModeStrategy +{ + virtual ~FStepModeStrategy() = default; + virtual EStepMode Mode() const = 0; + virtual void OnEnter(FURLabRpcDispatcher& Dispatcher, AAMjManager& Mgr) = 0; + virtual void OnExit(FURLabRpcDispatcher& Dispatcher, AAMjManager& Mgr) = 0; + virtual TSharedPtr HandleStep(FURLabRpcDispatcher& D, + const TSharedPtr& Req, const FStepRequestCommon& Common) = 0; +}; diff --git a/Source/URLab/Public/Bridge/RpcErrorCodes.h b/Source/URLab/Public/Bridge/RpcErrorCodes.h new file mode 100644 index 00000000..0665c214 --- /dev/null +++ b/Source/URLab/Public/Bridge/RpcErrorCodes.h @@ -0,0 +1,55 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "CoreMinimal.h" + +namespace URLabError +{ + // Dispatcher-level errors + inline const TCHAR* BadRequest = TEXT("bad_request"); + inline const TCHAR* MissingOp = TEXT("missing_op"); + inline const TCHAR* UnknownOp = TEXT("unknown_op"); + inline const TCHAR* NotInEditor = TEXT("not_in_editor"); + inline const TCHAR* NoActiveManager = TEXT("no_active_manager"); + inline const TCHAR* SessionExpired = TEXT("session_expired"); + inline const TCHAR* MissingField = TEXT("missing_field"); + inline const TCHAR* NotReady = TEXT("not_ready"); + inline const TCHAR* BadMode = TEXT("bad_mode"); + inline const TCHAR* BadValue = TEXT("bad_value"); + inline const TCHAR* UnknownArticulation = TEXT("unknown_articulation"); + inline const TCHAR* UnknownBody = TEXT("unknown_body"); + inline const TCHAR* NotMocapBody = TEXT("not_mocap_body"); + inline const TCHAR* UnknownKeyframe = TEXT("unknown_keyframe"); + inline const TCHAR* DimMismatch = TEXT("dim_mismatch"); + inline const TCHAR* NoJoints = TEXT("no_joints"); + inline const TCHAR* NoTwistController = TEXT("no_twist_controller"); + inline const TCHAR* NoController = TEXT("no_controller"); + inline const TCHAR* ControllerSchemaViolation = TEXT("controller_schema_violation"); + inline const TCHAR* ModeLockedByServer = TEXT("mode_locked_by_server"); + inline const TCHAR* StepTimeout = TEXT("step_timeout"); + inline const TCHAR* Timeout = TEXT("timeout"); + inline const TCHAR* ReplyTooLarge = TEXT("reply_too_large"); + inline const TCHAR* WrongTransport = TEXT("wrong_transport"); + inline const TCHAR* UnknownJob = TEXT("unknown_job"); + inline const TCHAR* ShuttingDown = TEXT("shutting_down"); + // Recording + inline const TCHAR* RecordingAlreadyActive = TEXT("recording_already_active"); + inline const TCHAR* RecordingNotActive = TEXT("recording_not_active"); + inline const TCHAR* PathNotWritable = TEXT("path_not_writable"); + inline const TCHAR* PathNotReadable = TEXT("path_not_readable"); + inline const TCHAR* ReplaySessionNotFound = TEXT("replay_session_not_found"); + inline const TCHAR* ReplayRequiresStepped = TEXT("replay_requires_stepped"); +} diff --git a/Source/URLab/Public/Bridge/StepCommands.h b/Source/URLab/Public/Bridge/StepCommands.h new file mode 100644 index 00000000..fe486427 --- /dev/null +++ b/Source/URLab/Public/Bridge/StepCommands.h @@ -0,0 +1,103 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "CoreMinimal.h" +#include "State/MjObservationLevel.h" +#include "Dom/JsonObject.h" +#include "HAL/Event.h" +#include "HAL/PlatformProcess.h" +#include + +/** + * @struct FMjStepRequest + * @brief One Direct-mode step request, parsed from a client RPC and pushed to + * the physics-thread queue. Owns the per-articulation ctrl writes and + * the n_steps count. + */ +struct FMjStepRequest +{ + int32 NSteps = 1; + /** prefix -> array of (actuator_name, value). Names are local (no prefix). */ + TMap>> PerArticulationCtrl; + /** Per-articulation control mode: "ue_controller" (default) or "raw". */ + TMap PerArticulationControlMode; + /** Per-articulation xfrc_applied: prefix -> body_name -> [fx,fy,fz,tx,ty,tz]. */ + TMap>> PerArticulationXfrc; + /** Echo'd request envelope for downstream reply building. */ + FString Op; +}; + +/** + * @struct FMjDirectStepCommand + * @brief Heap-allocated command passed as a shared pointer through the SPSC + * queue in Direct mode. The RPC thread enqueues, the physics-thread + * custom step handler dequeues, drains the request, and signals via FEvent. + * Captures observations inline so the reply can be built off the + * physics thread without re-touching d. + */ +struct FMjDirectStepCommand +{ + FMjStepRequest Request; + /** Set true by the handler when mj_step has completed. */ + bool bDone = false; + /** Set true by the RPC thread if it gives up (timeout / draining) before the + * handler ran this command. The handler checks it after dequeue and discards + * the command instead of stepping, so a timed-out request the client will + * retry does not also execute here -- avoiding a double physics step. */ + std::atomic bAbandoned{false}; + /** Request-scoped observation verbosity. Threaded from the step request so + * the worker builds this command's observations at the level THIS step + * asked for, not whatever session level is current when the handler runs. */ + EObservationLevel ObservationLevel = EObservationLevel::Standard; + /** Base step reply (op/time/step/clock/frame_id/arts/scene) built by the + * handler under the engine's CallbackMutex from the state IR, so the reply's + * observations reflect the just-stepped mjData. The RPC thread appends any + * requested cameras before returning it. */ + TSharedPtr Reply; + double ResultTime = 0.0; + int64 ResultStep = 0; + /** Post-step render-snapshot id, captured by the handler under + * CallbackMutex right after the step. The reply returns this as the + * camera frame_id; reading the engine's live counter from the RPC thread + * would race the physics loop and yield the previous step's id. */ + uint64 ResultFrameId = 0; + /** Physics thread signals this when the step has completed. */ + FEvent* Completion = nullptr; + + ~FMjDirectStepCommand() + { + if (Completion) + { + FPlatformProcess::ReturnSynchEventToPool(Completion); + Completion = nullptr; + } + } +}; + +/** + * @struct FMjPushStateRequest + * @brief One Puppet-mode push-state request. The client owns the integrator; + * UE writes qpos/qvel and calls mj_forward. + */ +struct FMjPushStateRequest +{ + TArray QPos; + TArray QVel; + TArray Ctrl; // optional informational ctrl + bool bIncludeCtrl = false; + double Time = 0.0; + int32 NSteps = 1; // informational only in puppet +}; From 1930e43f334f94dc570f9889971d36fc7a6ebb15 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:38 +0100 Subject: [PATCH 04/32] Carry state and frames over shared memory as well as ZMQ An oversize reply falls back to ZMQ instead of stalling, and the shared-memory RPC contract is advertised in the handshake rather than assumed. --- .../Transport/MjExternalTransportProvider.cpp | 31 +++ .../Private/Transport/NetworkManager.cpp | 45 +++- .../URLab/Private/Transport/RpcTransport.cpp | 40 +++- .../Private/Transport/ShmPublishTransport.cpp | 10 + .../Private/Transport/ShmRpcTransport.cpp | 118 ++++++++-- .../Private/Transport/SnapshotProducer.cpp | 46 ---- .../Private/Transport/ZmqPublishTransport.cpp | 202 ------------------ .../Private/Transport/ZmqRpcTransport.cpp | 113 +++++++--- .../Transport/ZmqSubscribeTransport.cpp | 16 ++ .../Transport/MjExternalTransportProvider.h | 64 ++++++ .../URLab/Public/Transport/NetworkManager.h | 11 +- .../URLab/Public/Transport/PublishTransport.h | 6 + Source/URLab/Public/Transport/RpcTransport.h | 13 ++ .../Public/Transport/ShmPublishTransport.h | 7 +- .../URLab/Public/Transport/ShmRpcTransport.h | 62 +++++- .../URLab/Public/Transport/SnapshotProducer.h | 52 ----- .../Public/Transport/SnapshotPublisher.h | 7 +- .../Public/Transport/SubscribeTransport.h | 2 + .../Public/Transport/ZmqPublishTransport.h | 50 +---- .../URLab/Public/Transport/ZmqRpcTransport.h | 77 ++----- .../Public/Transport/ZmqSubscribeTransport.h | 1 + 21 files changed, 478 insertions(+), 495 deletions(-) create mode 100644 Source/URLab/Private/Transport/MjExternalTransportProvider.cpp delete mode 100644 Source/URLab/Private/Transport/SnapshotProducer.cpp create mode 100644 Source/URLab/Public/Transport/MjExternalTransportProvider.h delete mode 100644 Source/URLab/Public/Transport/SnapshotProducer.h diff --git a/Source/URLab/Private/Transport/MjExternalTransportProvider.cpp b/Source/URLab/Private/Transport/MjExternalTransportProvider.cpp new file mode 100644 index 00000000..7839f4a0 --- /dev/null +++ b/Source/URLab/Private/Transport/MjExternalTransportProvider.cpp @@ -0,0 +1,31 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/MjExternalTransportProvider.h" + +FMjMakeExternalRpcTransport FMjExternalTransportProvider::MakeControlRpcTransport; +FMjMakeExternalPublishTransport FMjExternalTransportProvider::MakeStatePublishTransport; + +bool FMjExternalTransportProvider::HasControlRpcTransport() +{ + return MakeControlRpcTransport.IsBound(); +} diff --git a/Source/URLab/Private/Transport/NetworkManager.cpp b/Source/URLab/Private/Transport/NetworkManager.cpp index 3ed602a2..d4030b69 100644 --- a/Source/URLab/Private/Transport/NetworkManager.cpp +++ b/Source/URLab/Private/Transport/NetworkManager.cpp @@ -40,11 +40,19 @@ void UMjNetworkManager::UpdateCameraStreamingState() { if (Cam) { - Cam->bEnableZmqBroadcast = bEnableAllCameras; - Cam->bEnableShmBroadcast = bEnableAllCameras; - Cam->SetStreamingEnabled(bEnableAllCameras); + // bEnableAllCameras forces every camera to broadcast (legacy + // "stream all"). Otherwise respect each camera's own broadcast + // flags; cameras with neither set stay dormant and only capture + // when requested (per-camera capture gating in TickComponent). + if (bEnableAllCameras) + { + Cam->bEnableZmqBroadcast = true; + Cam->bEnableShmBroadcast = true; + } + const bool bStream = Cam->bEnableZmqBroadcast || Cam->bEnableShmBroadcast; + Cam->SetStreamingEnabled(bStream); Count++; - UE_LOG(LogURLabNet, Log, TEXT(" - %s Camera: %s on Actor: %s"), bEnableAllCameras ? TEXT("Enabled") : TEXT("Disabled"), *Cam->GetName(), Cam->GetOwner() ? *Cam->GetOwner()->GetName() : TEXT("None")); + UE_LOG(LogURLabNet, Log, TEXT(" - %s Camera: %s on Actor: %s"), bStream ? TEXT("Streaming") : TEXT("Dormant"), *Cam->GetName(), Cam->GetOwner() ? *Cam->GetOwner()->GetName() : TEXT("None")); } } UE_LOG(LogURLabNet, Log, TEXT("Global camera toggle processed %d cameras."), Count); @@ -55,14 +63,29 @@ void UMjNetworkManager::RegisterCamera(UMjCamera* Cam) if (!Cam) return; FScopeLock Lock(&CameraMutex); - ActiveCameras.AddUnique(Cam); + const int32 PortIndex = ActiveCameras.AddUnique(Cam); + // Each camera seeds a distinct port upward from the instance's CamBasePort + // (CamBasePort + index), so co-located cameras and co-located editor render + // servers never share a port. Assigned before any SetStreamingEnabled below. + Cam->SetStreamPortIndex(PortIndex); - // Sync newly registered camera to the current global toggle state - Cam->bEnableZmqBroadcast = bEnableAllCameras; - Cam->bEnableShmBroadcast = bEnableAllCameras; - Cam->SetStreamingEnabled(bEnableAllCameras); + // bEnableAllCameras forces broadcast on (legacy "stream all"). Otherwise + // honour the camera's authored broadcast flags. A camera with no broadcast + // stays dormant (no GPU capture) until a client requests it via + // include_cameras, at which point per-camera gating spins it up. + if (bEnableAllCameras) + { + Cam->bEnableZmqBroadcast = true; + Cam->bEnableShmBroadcast = true; + } + const bool bStream = Cam->bEnableZmqBroadcast || Cam->bEnableShmBroadcast; + if (bStream) + { + Cam->SetStreamingEnabled(true); + } - UE_LOG(LogURLabNet, Log, TEXT("UMjNetworkManager: Registered Camera %s. Total: %d"), *Cam->GetName(), ActiveCameras.Num()); + UE_LOG(LogURLabNet, Log, TEXT("UMjNetworkManager: Registered Camera %s (%s). Total: %d"), + *Cam->GetName(), bStream ? TEXT("streaming") : TEXT("dormant"), ActiveCameras.Num()); } void UMjNetworkManager::UnregisterCamera(UMjCamera* Cam) @@ -74,7 +97,7 @@ void UMjNetworkManager::UnregisterCamera(UMjCamera* Cam) UE_LOG(LogURLabNet, Log, TEXT("UMjNetworkManager: Unregistered Camera %s. Total: %d"), *Cam->GetName(), ActiveCameras.Num()); } -TArray UMjNetworkManager::GetActiveCameras() +TArray> UMjNetworkManager::GetActiveCameras() { FScopeLock Lock(&CameraMutex); return ActiveCameras; diff --git a/Source/URLab/Private/Transport/RpcTransport.cpp b/Source/URLab/Private/Transport/RpcTransport.cpp index 21f46353..ddd5301e 100644 --- a/Source/URLab/Private/Transport/RpcTransport.cpp +++ b/Source/URLab/Private/Transport/RpcTransport.cpp @@ -6,6 +6,7 @@ #include "Transport/RpcTransport.h" #include "Bridge/BridgeServer.h" #include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" #include "Bridge/MsgpackHelpers.h" #include "Bridge/OpRegistry.h" #include "Serialization/JsonSerializer.h" @@ -39,6 +40,25 @@ FURLabRpcDispatcher* UURLabRpcTransport::ResolveDispatcher() const return Bridge ? Bridge->GetDispatcher() : nullptr; } +void UURLabRpcTransport::EncodeReply(const TSharedPtr& Reply, + TArray& OutBytes) const +{ + OutBytes.Reset(); + FURLabRpcDispatcher* Disp = ResolveDispatcher(); + const bool bUseJson = Disp ? Disp->GetUseJsonEncoding() : false; + if (!bUseJson) + { + FURLabMsgpackUtil::PackJsonObject(Reply, OutBytes); + } + else + { + const FString Out = SerializeRpcReplyJson(Reply); + FTCHARToUTF8 OutUtf8(*Out); + OutBytes.SetNumUninitialized(OutUtf8.Length()); + FMemory::Memcpy(OutBytes.GetData(), OutUtf8.Get(), OutUtf8.Length()); + } +} + bool UURLabRpcTransport::ProcessRequestBytes(const TArray& InBytes, TArray& OutReplyBytes) { @@ -60,8 +80,13 @@ bool UURLabRpcTransport::ProcessRequestBytes(const TArray& InBytes, } if (!Req.IsValid()) { - FString JsonStr = FString(InBytes.Num(), - UTF8_TO_TCHAR((const char*)InBytes.GetData())); + // The wire buffer is not NUL-terminated. Convert with an explicit + // byte length so the UTF-8 decode stops at the end of the request + // instead of reading past it (a heap overread on any JSON or + // otherwise-unparseable request). + FUTF8ToTCHAR Conv(reinterpret_cast(InBytes.GetData()), + InBytes.Num()); + FString JsonStr(Conv.Length(), Conv.Get()); TSharedRef> Reader = TJsonReaderFactory<>::Create(JsonStr); if (!FJsonSerializer::Deserialize(Reader, Req)) Req.Reset(); @@ -74,19 +99,20 @@ bool UURLabRpcTransport::ProcessRequestBytes(const TArray& InBytes, TSharedPtr Reply; if (!Disp) { - Reply = FURLabRpcDispatcher::MakeError(TEXT("not_ready"), + Reply = FURLabRpcDispatcher::MakeError(URLabError::NotReady, TEXT("Bridge / dispatcher missing")); } else if (!AcceptsEditorOps() && Req.IsValid()) { - // SHM scope narrowing: editor-only ops never reach the - // dispatcher on a runtime-only transport. The bridge-side client - // re-routes such ops to ZMQ on receipt of `wrong_transport`. + // SHM scope narrowing: editor-only ops never reach the dispatcher on + // a runtime-only transport. They are rejected with `wrong_transport` + // so the client can re-route them to ZMQ. The request is never + // executed here, which is what makes re-routing safe. FString Op; Req->TryGetStringField(TEXT("op"), Op); if (URLabOpRegistry::IsEditorOnlyOp(Op)) { - Reply = FURLabRpcDispatcher::MakeError(TEXT("wrong_transport"), + Reply = FURLabRpcDispatcher::MakeError(URLabError::WrongTransport, FString::Printf(TEXT("op '%s' not accepted on %s; use zmq"), *Op, *GetTransportName())); } diff --git a/Source/URLab/Private/Transport/ShmPublishTransport.cpp b/Source/URLab/Private/Transport/ShmPublishTransport.cpp index db64f7fb..ce687869 100644 --- a/Source/URLab/Private/Transport/ShmPublishTransport.cpp +++ b/Source/URLab/Private/Transport/ShmPublishTransport.cpp @@ -120,3 +120,13 @@ void UURLabShmPublishTransport::Publish(const FString& /*Topic*/, Hdr->LatestIdx.store(Target, std::memory_order_release); Hdr->Sequence.fetch_add(1, std::memory_order_release); } + +void UURLabShmPublishTransport::AppendHandshakeBlock(TSharedPtr& Reply) const +{ + const FString ShmDir = FPaths::GetPath(GetStatePath()); + if (!ShmDir.IsEmpty()) + { + Reply->SetStringField(TEXT("shm_session_dir"), + FPaths::ConvertRelativePathToFull(ShmDir)); + } +} diff --git a/Source/URLab/Private/Transport/ShmRpcTransport.cpp b/Source/URLab/Private/Transport/ShmRpcTransport.cpp index 2443e5ab..8dabfa3b 100644 --- a/Source/URLab/Private/Transport/ShmRpcTransport.cpp +++ b/Source/URLab/Private/Transport/ShmRpcTransport.cpp @@ -15,6 +15,8 @@ #include "Transport/ShmRpcTransport.h" #include "Transport/ShmPublishTransport.h" // ResolveSessionDir #include "Bridge/BridgeServer.h" +#include "Bridge/RpcDispatcher.h" // MakeError +#include "Bridge/RpcErrorCodes.h" #include "Misc/Paths.h" #include "HAL/FileManager.h" #include "HAL/RunnableThread.h" @@ -61,10 +63,24 @@ bool UURLabShmRpcTransport::TransportInit() if (bInitialized) return true; - FString Sid = SessionId; - if (Sid.IsEmpty()) - Sid = TEXT("live"); - const FString Dir = UURLabShmPublishTransport::ResolveSessionDir(Sid); + const FString BaseSid = SessionId.IsEmpty() ? FString(TEXT("live")) : SessionId; + // Make the session globally unique per editor process so many render-server + // instances on one host never share SHM files or Windows event names (a + // shared event name lets one instance eat another's wakeup and degrade to + // the 100 ms poll timeout). The process id guarantees uniqueness; the step + // port, when the bridge supplies it, keeps the name traceable to the + // instance. Every resolved name and path is advertised in the hello + // shm_rpc block, so the bridge opens exactly these objects instead of + // re-deriving them from a fixed "live" session. + const uint32 Pid = FPlatformProcess::GetCurrentProcessId(); + ResolvedSessionId = InstancePort > 0 + ? FString::Printf(TEXT("%s_p%d_%u"), *BaseSid, InstancePort, Pid) + : FString::Printf(TEXT("%s_%u"), *BaseSid, Pid); + // Event names match what the worker creates below; advertised in hello so + // the bridge opens the exact objects rather than assuming "live". + ReqEventName = MakeEventName(ResolvedSessionId, TEXT("req")); + RepEventName = MakeEventName(ResolvedSessionId, TEXT("rep")); + const FString Dir = UURLabShmPublishTransport::ResolveSessionDir(ResolvedSessionId); IFileManager::Get().MakeDirectory(*Dir, /*Tree=*/true); ReqPath = FPaths::Combine(Dir, TEXT("req.shm")); RepPath = FPaths::Combine(Dir, TEXT("rep.shm")); @@ -75,7 +91,7 @@ bool UURLabShmRpcTransport::TransportInit() TEXT("UURLabShmRpcTransport: failed to open req.shm at %s"), *ReqPath); return false; } - if (!RepRegion.Open(RepPath, static_cast(BufferStride), /*NBuffers=*/2)) + if (!RepRegion.Open(RepPath, static_cast(ReplyBufferStride), /*NBuffers=*/2)) { UE_LOG(LogURLabNet, Error, TEXT("UURLabShmRpcTransport: failed to open rep.shm at %s"), *RepPath); @@ -89,14 +105,25 @@ bool UURLabShmRpcTransport::TransportInit() // means a single SetEvent unblocks exactly one waiter and self-clears. // Initial state = unsignaled; the first signal comes from the producer. { - const FString ReqName = MakeEventName(Sid, TEXT("req")); - const FString RepName = MakeEventName(Sid, TEXT("rep")); // UE's Windows wrappers hide the TRUE/FALSE macros; pass integers // directly (BOOL is int). ReqReadyEvent = ::CreateEventW(nullptr, /*bManualReset=*/0, - /*bInitialState=*/0, *ReqName); + /*bInitialState=*/0, *ReqEventName); + const DWORD ReqErr = ::GetLastError(); RepReadyEvent = ::CreateEventW(nullptr, /*bManualReset=*/0, - /*bInitialState=*/0, *RepName); + /*bInitialState=*/0, *RepEventName); + const DWORD RepErr = ::GetLastError(); + // Per-process naming should make a pre-existing event impossible. If one + // exists anyway, another instance resolved the same identity; warn + // rather than silently share a wake object, which would let that + // instance steal this one's request/reply signals. + if ((ReqReadyEvent && ReqErr == ERROR_ALREADY_EXISTS) || (RepReadyEvent && RepErr == ERROR_ALREADY_EXISTS)) + { + UE_LOG(LogURLabNet, Warning, + TEXT("UURLabShmRpcTransport: kernel event name collision (req=%s rep=%s); " + "another instance may steal wakeups"), + *ReqEventName, *RepEventName); + } if (!ReqReadyEvent || !RepReadyEvent) { UE_LOG(LogURLabNet, Warning, @@ -119,8 +146,8 @@ bool UURLabShmRpcTransport::TransportInit() bStop = false; bInitialized = true; - FSmStepTransportRunnable* Runner = new FSmStepTransportRunnable(this); - WorkerThread = FRunnableThread::Create(Runner, TEXT("URLabSmStepTransport")); + WorkerRunnable = new FSmStepTransportRunnable(this); + WorkerThread = FRunnableThread::Create(WorkerRunnable, TEXT("URLabSmStepTransport")); UE_LOG(LogURLabNet, Log, TEXT("UURLabShmRpcTransport: req=%s, rep=%s, sync=%s"), @@ -149,6 +176,10 @@ void UURLabShmRpcTransport::TransportShutdown() delete WorkerThread; WorkerThread = nullptr; } + // FRunnableThread never owns the runnable; delete it explicitly so the + // bind/unbind cycle does not leak one runnable each time. + delete WorkerRunnable; + WorkerRunnable = nullptr; #if PLATFORM_WINDOWS if (ReqReadyEvent) @@ -228,7 +259,11 @@ void UURLabShmRpcTransport::RunPollLoop() } uint32 Size = 0; FMemory::Memcpy(&Size, Slot, sizeof(uint32)); - if (Size == 0 || Size + sizeof(uint32) > ReqStride) + // Size is written by any local process that can map the region, so it + // is untrusted. Compare against the remaining slot space without adding + // to Size first: `Size + sizeof(uint32)` would wrap for a hostile Size + // near UINT32_MAX and pass the check, then over-read the slot. + if (Size == 0 || Size > ReqStride - sizeof(uint32)) { UE_LOG(LogURLab, Warning, TEXT("ShmRpcTransport: dropping request seq=%llu with invalid size=%u (stride=%u)"), @@ -242,9 +277,11 @@ void UURLabShmRpcTransport::RunPollLoop() FMemory::Memcpy(ReqBytes.GetData(), Slot + sizeof(uint32), Size); const uint64 SeqAfter = ReqHdr->Sequence.load(std::memory_order_acquire); - if (SeqAfter - CurSeq > ReqHdr->NBuffers) + if (SeqAfter - CurSeq >= ReqHdr->NBuffers) { - // Producer wrapped past our read. Skip. + // Producer advanced by at least NBuffers slots while we copied, so + // the slot we read has already been reused: the read is torn. Reject + // `== NBuffers` too, since that already reuses our slot exactly once. LastSeenReqSeq = SeqAfter; continue; } @@ -259,15 +296,37 @@ void UURLabShmRpcTransport::RunPollLoop() if (static_cast(RepBytes.Num()) + sizeof(uint32) > RepStride) { + // Reply doesn't fit the fixed SHM reply slot — e.g. a multi-camera + // include_cameras frame or a large hello MJB. By design SHM hands + // oversize replies to ZMQ. Return an explicit `wrong_transport` + // reply NOW so the bridge re-routes this one request to ZMQ + // immediately, instead of dropping it and forcing the client to + // wait out its full recv timeout (the 5s stall). The fast image + // path for SHM consumers is the per-camera cam_*.shm streams, not + // this RPC reply slot; raise ReplyBufferStride only if you want + // large inline replies carried over SHM. UE_LOG(LogURLabNet, Warning, - TEXT("UURLabShmRpcTransport: reply payload %d bytes exceeds slot stride %u; dropping"), + TEXT("UURLabShmRpcTransport: reply %d bytes exceeds reply slot %u; routing to zmq " + "(raise ReplyBufferStride to carry it over shm)"), RepBytes.Num(), RepStride); - // Manager-required ops have bounded reply sizes (step replies, - // sensor readouts, qpos snapshots) — overflow here means a - // bug in the dispatcher's reply construction. Drop the reply - // rather than corrupting the slot; the bridge times out, which - // is the correct signal that something is wrong. - continue; + + // `reply_too_large` is the code the bridge's ShmTransport already + // sticky-routes to its ZMQ fallback. (Distinct from the editor-op + // `wrong_transport` rejection.) + TSharedPtr Err = FURLabRpcDispatcher::MakeError( + URLabError::ReplyTooLarge, + FString::Printf( + TEXT("reply %d bytes exceeds shm reply slot %u; use zmq for this request"), + RepBytes.Num(), RepStride)); + TArray ErrBytes; + EncodeReply(Err, ErrBytes); + if (static_cast(ErrBytes.Num()) + sizeof(uint32) > RepStride) + { + // Error envelope itself won't fit (pathologically tiny stride). + // Nothing safe to write; skip and let the bridge time out. + continue; + } + RepBytes = MoveTemp(ErrBytes); } const uint32 CurLatest = RepHdr->LatestIdx.load(std::memory_order_acquire); @@ -293,3 +352,20 @@ void UURLabShmRpcTransport::RunPollLoop() #endif } } + +void UURLabShmRpcTransport::AppendHandshakeBlock(TSharedPtr& Reply) const +{ + TSharedPtr Rpc = MakeShared(); + Rpc->SetStringField(TEXT("session"), GetSessionId()); + Rpc->SetStringField(TEXT("req_path"), + FPaths::ConvertRelativePathToFull(GetReqPath())); + Rpc->SetStringField(TEXT("rep_path"), + FPaths::ConvertRelativePathToFull(GetRepPath())); + Rpc->SetStringField(TEXT("req_event"), GetReqEventName()); + Rpc->SetStringField(TEXT("rep_event"), GetRepEventName()); + Rpc->SetNumberField(TEXT("req_stride"), GetReqStride()); + Rpc->SetNumberField(TEXT("rep_stride"), GetRepStride()); + Rpc->SetNumberField(TEXT("n_buffers"), GetNumBuffers()); + Rpc->SetNumberField(TEXT("header_size"), static_cast(sizeof(FMjShmHeader))); + Reply->SetObjectField(TEXT("shm_rpc"), Rpc); +} diff --git a/Source/URLab/Private/Transport/SnapshotProducer.cpp b/Source/URLab/Private/Transport/SnapshotProducer.cpp deleted file mode 100644 index ea7ac82d..00000000 --- a/Source/URLab/Private/Transport/SnapshotProducer.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "Transport/SnapshotProducer.h" -#include "Bridge/RpcDispatcher.h" -#include "Bridge/MsgpackHelpers.h" -#include "Dom/JsonObject.h" -#include "mujoco/mujoco.h" - -TArray FMjSnapshotProducer::BuildStateSnapshot(AAMjManager* Manager, - mjModel* m, mjData* d, - int64 StepIndex) -{ - if (!Manager || !m || !d) - return {}; - - TSharedPtr Snapshot = MakeShared(); - Snapshot->SetStringField(TEXT("op"), TEXT("state_full")); - Snapshot->SetNumberField(TEXT("time"), d->time); - Snapshot->SetNumberField(TEXT("step"), static_cast(StepIndex)); - FURLabRpcDispatcher::AppendClockFields(Snapshot, d->time); - - TSharedPtr Obs = FURLabRpcDispatcher::BuildStepObservations( - Manager, m, d, FURLabRpcDispatcher::EObservationLevel::Standard); - if (Obs.IsValid()) - Snapshot->SetObjectField(TEXT("per_articulation"), Obs); - - TSharedPtr Entities = FURLabRpcDispatcher::BuildEntitiesBlock(Manager, m, d); - if (Entities.IsValid()) - Snapshot->SetObjectField(TEXT("entities"), Entities); - - TArray Buf; - FURLabMsgpackUtil::PackJsonObject(Snapshot, Buf); - return Buf; -} diff --git a/Source/URLab/Private/Transport/ZmqPublishTransport.cpp b/Source/URLab/Private/Transport/ZmqPublishTransport.cpp index 1829c86f..79ddc3cb 100644 --- a/Source/URLab/Private/Transport/ZmqPublishTransport.cpp +++ b/Source/URLab/Private/Transport/ZmqPublishTransport.cpp @@ -20,15 +20,7 @@ #if PLATFORM_WINDOWS #include "Windows/HideWindowsPlatformTypes.h" #endif -#include "MuJoCo/Core/MjArticulation.h" #include "MuJoCo/Core/AMjManager.h" -#include "MuJoCo/Core/MjPhysicsEngine.h" -#include "MuJoCo/Components/MjComponent.h" -#include "MuJoCo/Input/MjTwistController.h" -#include "Bridge/MsgpackHelpers.h" -#include "Serialization/BufferArchive.h" -#include "Async/Async.h" -#include "Misc/ScopeExit.h" #include "Utils/URLabLogging.h" namespace @@ -66,9 +58,6 @@ void UURLabZmqPublishTransport::TransportShutdown() { Mgr->UnregisterSnapshotPublisher(this); } - // Make sure the physics thread stops reading our cache before tear-down. - bCacheBuilt.store(false, std::memory_order_release); - CachedRecords.Reset(); ShutdownZmqSocket(); } @@ -108,197 +97,6 @@ void UURLabZmqPublishTransport::ShutdownZmqSocket() bIsInitialized = false; } -void UURLabZmqPublishTransport::RequestGameThreadCacheBuild() -{ - bool Expected = false; - if (!bCacheBuildScheduled.compare_exchange_strong(Expected, true, - std::memory_order_acq_rel)) - { - return; // already scheduled - } - TWeakObjectPtr WeakSelf(this); - AsyncTask(ENamedThreads::GameThread, [WeakSelf]() { - if (UURLabZmqPublishTransport* Self = WeakSelf.Get()) - { - Self->BuildBroadcastCacheGameThread(); - } - }); -} - -void UURLabZmqPublishTransport::BuildBroadcastCacheGameThread() -{ - // Always reset the scheduled flag on exit so a future call can - // re-schedule (e.g. articulations were added after we ran). - ON_SCOPE_EXIT - { - bCacheBuildScheduled.store(false, std::memory_order_release); - }; - - AAMjManager* Manager = OwningManager.Get(); - if (!Manager) - return; - - TArray Articulations = Manager->GetAllArticulations(); - if (Articulations.Num() == 0) - return; - - CachedRecords.Reset(Articulations.Num()); - for (AMjArticulation* Art : Articulations) - { - if (!Art) - continue; - - FArticulationBroadcastRecord Rec; - Rec.Articulation = Art; - Rec.ArticPrefix = Art->GetName(); - Art->GetComponents(Rec.TelemetryComponents); - Rec.TwistCtrl = Art->FindComponentByClass(); - CachedRecords.Add(MoveTemp(Rec)); - } - - bCacheBuilt.store(true, std::memory_order_release); - - UE_LOG(LogURLabNet, Log, - TEXT("UURLabZmqPublishTransport: built broadcast cache (%d articulations)."), - CachedRecords.Num()); -} - -void UURLabZmqPublishTransport::PostStep(mjModel* m, mjData* d) -{ - static constexpr int32 kLogInterval = 500; - bool bShouldLog = (FrameCounter++ % kLogInterval == 0); - - if (!bIsInitialized) - return; - - // Single source of truth for "publishers paused" — flipped by - // UURLabZmqRpcTransport on Direct / Puppet mode entry so we don't - // double-write to the wire while the step server drives cadence. - if (AAMjManager* Mgr = OwningManager.Get()) - { - if (Mgr->bPublishersPaused.load(std::memory_order_acquire)) - { - return; - } - } - - // Acquire-load: if the game thread hasn't published the cache yet, - // schedule a build (idempotent) and skip this step. We DO NOT touch - // AActor::OwnedComponents from this thread. - if (!bCacheBuilt.load(std::memory_order_acquire)) - { - RequestGameThreadCacheBuild(); - return; - } - - int BroadcastCount = 0; - if (bShouldLog) - { - UE_LOG(LogURLabNet, Verbose, - TEXT("UURLabZmqPublishTransport PostStep: broadcasting %d cached articulations"), - CachedRecords.Num()); - } - - for (const FArticulationBroadcastRecord& Rec : CachedRecords) - { - if (!Rec.Articulation) - continue; - - for (UMjComponent* Comp : Rec.TelemetryComponents) - { - if (!Comp || Comp->bIsDefault) - continue; - - FString TopicSuffix = Comp->GetTelemetryTopicName(); - if (TopicSuffix.IsEmpty()) - continue; - - FString FullTopic = FString::Printf(TEXT("%s/%s"), *Rec.ArticPrefix, *TopicSuffix); - - FBufferArchive Payload; - Comp->BuildBinaryPayload(Payload); - - if (Payload.Num() > 0) - { - SendTopic(ZmqPublisher, FullTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, Payload.GetData(), Payload.Num(), 0); - BroadcastCount++; - } - } - - if (Rec.TwistCtrl) - { - FVector Twist = Rec.TwistCtrl->GetTwist(); - FString TwistTopic = FString::Printf(TEXT("%s/twist"), *Rec.ArticPrefix); - float TwistData[3] = {(float)Twist.X, (float)Twist.Y, (float)Twist.Z}; - SendTopic(ZmqPublisher, TwistTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, TwistData, sizeof(TwistData), 0); - BroadcastCount++; - - int32 Actions = Rec.TwistCtrl->GetActiveActions(); - if (Actions != 0) - { - FString ActionTopic = FString::Printf(TEXT("%s/actions"), *Rec.ArticPrefix); - SendTopic(ZmqPublisher, ActionTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, &Actions, sizeof(Actions), 0); - BroadcastCount++; - } - } - } - - // Non-articulation dynamic bodies (free-jointed props, heightfields, etc). - if (AAMjManager* Mgr = OwningManager.Get()) - { - const TArray& Entities = Mgr->GetEntities(); - for (const FMjEntityRecord& Rec : Entities) - { - if (Rec.MjId < 0 || Rec.MjId >= m->nbody) - continue; - - FString XposTopic = FString::Printf(TEXT("scene/%s/xpos"), *Rec.Name); - SendTopic(ZmqPublisher, XposTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, &d->xpos[Rec.MjId * 3], 3 * sizeof(mjtNum), 0); - BroadcastCount++; - - FString XquatTopic = FString::Printf(TEXT("scene/%s/xquat"), *Rec.Name); - SendTopic(ZmqPublisher, XquatTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, &d->xquat[Rec.MjId * 4], 4 * sizeof(mjtNum), 0); - BroadcastCount++; - - if (Rec.bHasFreeBase && m->body_jntnum && m->body_jntadr && m->jnt_type && m->jnt_qposadr && m->jnt_dofadr) - { - int FirstJnt = m->body_jntadr[Rec.MjId]; - int NumJnt = m->body_jntnum[Rec.MjId]; - if (FirstJnt >= 0 && NumJnt > 0 && FirstJnt < m->njnt && m->jnt_type[FirstJnt] == mjJNT_FREE) - { - int QAddr = m->jnt_qposadr[FirstJnt]; - int VAddr = m->jnt_dofadr[FirstJnt]; - FString QposTopic = FString::Printf(TEXT("scene/%s/qpos"), *Rec.Name); - SendTopic(ZmqPublisher, QposTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, &d->qpos[QAddr], 7 * sizeof(mjtNum), 0); - FString QvelTopic = FString::Printf(TEXT("scene/%s/qvel"), *Rec.Name); - SendTopic(ZmqPublisher, QvelTopic, ZMQ_SNDMORE); - zmq_send(ZmqPublisher, &d->qvel[VAddr], 6 * sizeof(mjtNum), 0); - BroadcastCount += 2; - } - } - } - } - - // state/full snapshots are built once per step by AAMjManager and - // fanned out via PublishSnapshot to every IMjSnapshotPublisher. - if (bShouldLog && BroadcastCount == 0) - { - UE_LOG(LogURLabNet, Warning, - TEXT("UURLabZmqPublishTransport: Found components but NONE produced a valid binary payload!")); - } - else if (bShouldLog) - { - UE_LOG(LogURLabNet, Verbose, - TEXT("UURLabZmqPublishTransport: broadcast %d messages to ZMQ."), BroadcastCount); - } -} - void UURLabZmqPublishTransport::Publish(const FString& Topic, const TArray& Payload) { if (!ZmqPublisher || Payload.Num() == 0) diff --git a/Source/URLab/Private/Transport/ZmqRpcTransport.cpp b/Source/URLab/Private/Transport/ZmqRpcTransport.cpp index 0f531387..ce802b61 100644 --- a/Source/URLab/Private/Transport/ZmqRpcTransport.cpp +++ b/Source/URLab/Private/Transport/ZmqRpcTransport.cpp @@ -20,53 +20,73 @@ #include "Utils/URLabLogging.h" #include "zmq.h" +class FStepServerRunnable : public FRunnable +{ +public: + UURLabZmqRpcTransport* Server; + explicit FStepServerRunnable(UURLabZmqRpcTransport* S) + : Server(S) {} + virtual uint32 Run() override + { + Server->RunPollLoop(); + return 0; + } + virtual void Stop() override { Server->bStop = true; } +}; + UURLabZmqRpcTransport::UURLabZmqRpcTransport() = default; -bool UURLabZmqRpcTransport::TransportInit() +bool UURLabZmqRpcTransport::CreateAndBindRep() { - if (bIsInitialized) - return true; - - ZmqContext = zmq_ctx_new(); ZmqRep = zmq_socket(ZmqContext, ZMQ_REP); + if (!ZmqRep) + return false; + int Timeout = PollTimeoutMs; zmq_setsockopt(ZmqRep, ZMQ_RCVTIMEO, &Timeout, sizeof(Timeout)); zmq_setsockopt(ZmqRep, ZMQ_SNDTIMEO, &Timeout, sizeof(Timeout)); + // LINGER 0: on close, drop any unsent reply immediately instead of the + // libzmq default (infinite), so zmq_ctx_term can never block editor exit + // on a client that stopped reading. + int Linger = 0; + zmq_setsockopt(ZmqRep, ZMQ_LINGER, &Linger, sizeof(Linger)); - int rc = zmq_bind(ZmqRep, TCHAR_TO_UTF8(*StepEndpoint)); - if (rc != 0) + if (zmq_bind(ZmqRep, TCHAR_TO_UTF8(*StepEndpoint)) != 0) + { + zmq_close(ZmqRep); + ZmqRep = nullptr; + return false; + } + return true; +} + +bool UURLabZmqRpcTransport::TransportInit() +{ + if (bIsInitialized) + return true; + + ZmqContext = zmq_ctx_new(); + if (!ZmqContext || !CreateAndBindRep()) { UE_LOG(LogURLabNet, Error, TEXT("UURLabZmqRpcTransport: failed to bind REP at %s"), *StepEndpoint); if (GEngine) { GEngine->AddOnScreenDebugMessage(-1, 10.f, FColor::Red, - FString::Printf(TEXT("URLab: ZMQ bind failed on %s — port conflict?"), *StepEndpoint)); + FString::Printf(TEXT("URLab: ZMQ bind failed on %s, port conflict?"), *StepEndpoint)); + } + if (ZmqContext) + { + zmq_ctx_term(ZmqContext); + ZmqContext = nullptr; } - zmq_close(ZmqRep); - zmq_ctx_term(ZmqContext); - ZmqRep = nullptr; - ZmqContext = nullptr; return false; } bIsInitialized = true; bStop = false; - class FStepServerRunnable : public FRunnable - { - public: - UURLabZmqRpcTransport* Server; - explicit FStepServerRunnable(UURLabZmqRpcTransport* S) - : Server(S) {} - virtual uint32 Run() override - { - Server->RunPollLoop(); - return 0; - } - virtual void Stop() override { Server->bStop = true; } - }; - FStepServerRunnable* Runner = new FStepServerRunnable(this); - WorkerThread = FRunnableThread::Create(Runner, TEXT("URLabStepServer")); + WorkerRunnable = new FStepServerRunnable(this); + WorkerThread = FRunnableThread::Create(WorkerRunnable, TEXT("URLabStepServer")); UE_LOG(LogURLabNet, Log, TEXT("UURLabZmqRpcTransport initialised at %s"), *StepEndpoint); return true; @@ -84,6 +104,10 @@ void UURLabZmqRpcTransport::TransportShutdown() delete WorkerThread; WorkerThread = nullptr; } + // FRunnableThread never owns the runnable; delete it explicitly so the + // bind/unbind cycle does not leak one runnable each time. + delete WorkerRunnable; + WorkerRunnable = nullptr; if (ZmqRep) { @@ -122,6 +146,39 @@ void UURLabZmqRpcTransport::RunPollLoop() // Wire detect / parse / dispatch / encode all live on the base. TArray OutBytes; ProcessRequestBytes(InBytes, OutBytes); - zmq_send(ZmqRep, OutBytes.GetData(), OutBytes.Num(), 0); + + // REP is a strict recv-then-send state machine: having received, we + // MUST send before the next recv. If the send fails (EAGAIN under the + // send timeout, or a peer that vanished) the socket stays stuck in the + // must-send state and every later recv returns EFSM, spinning a core. + // Retry a few times while progress is still possible, then rebuild the + // socket to reset the state machine instead of busy-spinning. + constexpr int MaxSendAttempts = 3; + bool bSent = false; + for (int Attempt = 0; Attempt < MaxSendAttempts && !bStop.load(std::memory_order_acquire); ++Attempt) + { + if (zmq_send(ZmqRep, OutBytes.GetData(), OutBytes.Num(), 0) >= 0) + { + bSent = true; + break; + } + if (zmq_errno() != EAGAIN) + break; // hard error; rebuild rather than keep retrying + } + if (!bSent && !bStop.load(std::memory_order_acquire)) + { + UE_LOG(LogURLabNet, Warning, + TEXT("UURLabZmqRpcTransport: reply send failed (errno=%d); resetting REP socket at %s"), + zmq_errno(), *StepEndpoint); + zmq_close(ZmqRep); + ZmqRep = nullptr; + if (!CreateAndBindRep()) + { + UE_LOG(LogURLabNet, Error, + TEXT("UURLabZmqRpcTransport: failed to rebind REP at %s after send error; stopping poll loop"), + *StepEndpoint); + break; + } + } } } diff --git a/Source/URLab/Private/Transport/ZmqSubscribeTransport.cpp b/Source/URLab/Private/Transport/ZmqSubscribeTransport.cpp index ffa160ec..ae00006d 100644 --- a/Source/URLab/Private/Transport/ZmqSubscribeTransport.cpp +++ b/Source/URLab/Private/Transport/ZmqSubscribeTransport.cpp @@ -135,6 +135,7 @@ void UURLabZmqSubscribeTransport::ShutdownZmqSocket() void UURLabZmqSubscribeTransport::BuildCache(mjModel* m) { ActuatorCache.Empty(); + ActuatorToArticulationName.Empty(); if (!m) return; @@ -147,6 +148,7 @@ void UURLabZmqSubscribeTransport::BuildCache(mjModel* m) if (!Articulation) continue; + const FName ArtName(*Articulation->GetName()); TArray ArticActuators = Articulation->GetActuators(); for (UMjActuator* Actuator : ArticActuators) { @@ -157,6 +159,7 @@ void UURLabZmqSubscribeTransport::BuildCache(mjModel* m) { ActuatorCache.Add(Actuator->GetMjName(), id); ActuatorComponentCache.Add(id, Actuator); + ActuatorToArticulationName.Add(id, ArtName); } } } @@ -430,7 +433,20 @@ void UURLabZmqSubscribeTransport::PreStep(mjModel* m, mjData* d) if (UMjActuator** ActuatorPtr = ActuatorComponentCache.Find(Idx)) { if (*ActuatorPtr) + { + FURLabRpcDispatcher* Dispatcher = Manager->GetStepDispatcher(); + if (Dispatcher) + { + const FName* ArtName = ActuatorToArticulationName.Find(Idx); + if (ArtName && Dispatcher->GetControlOwnership().GetActiveOwners().Contains(*ArtName)) + { + IDPtr = (int32*)((char*)IDPtr + 8); + ValPtr = (float*)((char*)ValPtr + 8); + continue; + } + } (*ActuatorPtr)->SetNetworkControl(Value); + } } else if (bShouldLog) { diff --git a/Source/URLab/Public/Transport/MjExternalTransportProvider.h b/Source/URLab/Public/Transport/MjExternalTransportProvider.h new file mode 100644 index 00000000..3a9274b0 --- /dev/null +++ b/Source/URLab/Public/Transport/MjExternalTransportProvider.h @@ -0,0 +1,64 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +class UURLabRpcTransport; +class UURLabPublishTransport; +class UURLabBridgeServer; +class AAMjManager; + +/** + * @struct FMjExternalTransportProvider + * @brief Factory hooks an optional, out-of-core transport module installs at + * startup so the core can create its transports without naming their + * concrete types. + * + * The baseline transports (ZMQ, SHM) are created directly by the core. An + * additional transport module that ships as a separate, optional UE module + * depends on the core and installs these hooks in its StartupModule; the core + * invokes them through the abstract base pointers (UURLabRpcTransport / + * UURLabPublishTransport) it already owns. When the optional module is absent + * the hooks stay unbound and the core simply has no such transport. + * + * The factory is responsible for NewObject-ing its transport (with the supplied + * outer) and any owner wiring (SetOwningBridge, consumer registration); the core + * only calls TransportInit and adds the result to the appropriate owner list. + */ +DECLARE_DELEGATE_RetVal_OneParam(UURLabRpcTransport*, FMjMakeExternalRpcTransport, UURLabBridgeServer*); +DECLARE_DELEGATE_RetVal_OneParam(UURLabPublishTransport*, FMjMakeExternalPublishTransport, AAMjManager*); + +struct URLAB_API FMjExternalTransportProvider +{ + /** Creates an external request/reply + control-in transport bound to the + * bridge. Unbound when no external module is loaded. */ + static FMjMakeExternalRpcTransport MakeControlRpcTransport; + + /** Creates an external per-step state consumer transport owned by the + * manager. Unbound when no external module is loaded. */ + static FMjMakeExternalPublishTransport MakeStatePublishTransport; + + /** True when an external module has installed the control RPC factory. */ + static bool HasControlRpcTransport(); +}; diff --git a/Source/URLab/Public/Transport/NetworkManager.h b/Source/URLab/Public/Transport/NetworkManager.h index 2fd3b920..81811788 100644 --- a/Source/URLab/Public/Transport/NetworkManager.h +++ b/Source/URLab/Public/Transport/NetworkManager.h @@ -42,18 +42,21 @@ class URLAB_API UMjNetworkManager : public UActorComponent public: UMjNetworkManager(); - /** Forces all UMjCameras to enable ZMQ broadcasting. */ + /** Forces every UMjCamera to broadcast (legacy "stream all"). Default + * false: cameras stream only if their own bEnableZmqBroadcast / + * bEnableShmBroadcast is set, or once a client requests them + * (per-camera capture gating keeps idle cameras off the GPU). */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Network") - bool bEnableAllCameras = true; + bool bEnableAllCameras = false; void UpdateCameraStreamingState(); // Thread-safe camera registry. void RegisterCamera(UMjCamera* Cam); void UnregisterCamera(UMjCamera* Cam); - TArray GetActiveCameras(); + TArray> GetActiveCameras(); private: - TArray ActiveCameras; + TArray> ActiveCameras; FCriticalSection CameraMutex; }; diff --git a/Source/URLab/Public/Transport/PublishTransport.h b/Source/URLab/Public/Transport/PublishTransport.h index 51a271af..65254c81 100644 --- a/Source/URLab/Public/Transport/PublishTransport.h +++ b/Source/URLab/Public/Transport/PublishTransport.h @@ -60,6 +60,10 @@ class URLAB_API UURLabPublishTransport : public UObject virtual FString GetTransportName() const PURE_VIRTUAL(UURLabPublishTransport::GetTransportName, return FString();); + /** Append transport-specific fields to the hello handshake reply. + * Default no-op; SHM override advertises the session dir. */ + virtual void AppendHandshakeBlock(TSharedPtr& Reply) const {} + // --- Per-step physics hooks (Async thread) ---------------------------- // Default-empty so transports that don't tie to the physics step (e.g. // sensor-shaped publishers that publish on demand) don't need to opt in. @@ -73,5 +77,7 @@ class URLAB_API UURLabPublishTransport : public UObject virtual void PostStep(struct mjModel_* /*m*/, struct mjData_* /*d*/) {} protected: + // Owning bridge; same pattern as UURLabRpcTransport — UE single-inheritance + // prevents extracting a shared UObject base for all three transport ABCs. TWeakObjectPtr OwningBridge; }; diff --git a/Source/URLab/Public/Transport/RpcTransport.h b/Source/URLab/Public/Transport/RpcTransport.h index b9245f75..43a17e65 100644 --- a/Source/URLab/Public/Transport/RpcTransport.h +++ b/Source/URLab/Public/Transport/RpcTransport.h @@ -61,6 +61,11 @@ class URLAB_API UURLabRpcTransport : public UObject * Default true so new transports are universal unless they opt out. */ virtual bool AcceptsEditorOps() const { return true; } + /** Append transport-specific fields to the hello handshake reply. + * Called from BuildHandshakePayload for each bound transport. + * Default no-op; SHM overrides to advertise paths/events/strides. */ + virtual void AppendHandshakeBlock(TSharedPtr& Reply) const {} + /** Shared request handler. Concrete transports call this from their * worker loop with raw inbound bytes; receives encoded reply bytes * ready to ship back. Handles: @@ -80,6 +85,14 @@ class URLAB_API UURLabRpcTransport : public UObject * caller should ship a `not_ready` error reply. */ FURLabRpcDispatcher* ResolveDispatcher() const; + /** Encode a reply object to wire bytes using the live session's encoding + * (msgpack by default, JSON when the handshake selected it). For + * transports that synthesize a reply outside the normal + * ProcessRequestBytes path — e.g. a fixed-size transport rejecting an + * oversize reply with a fast `wrong_transport`/`reply_too_large` error + * instead of dropping it. */ + void EncodeReply(const TSharedPtr& Reply, TArray& OutBytes) const; + protected: TWeakObjectPtr OwningBridge; }; diff --git a/Source/URLab/Public/Transport/ShmPublishTransport.h b/Source/URLab/Public/Transport/ShmPublishTransport.h index 6cc1a0e5..d6979d2f 100644 --- a/Source/URLab/Public/Transport/ShmPublishTransport.h +++ b/Source/URLab/Public/Transport/ShmPublishTransport.h @@ -32,8 +32,8 @@ class AAMjManager; * via `TransportShutdown`. * * Wire layout (per slot): [u32 size][bytes payload]. Payload is the - * msgpack-encoded snapshot built by FMjSnapshotProducer. Producer pattern - * is the standard double-buffer + sequence fence. + * msgpack-encoded state IR built by FMjStateCollector + FMjMsgpackEncoder. + * Producer pattern is the standard double-buffer + sequence fence. */ UCLASS() class URLAB_API UURLabShmPublishTransport : public UURLabPublishTransport @@ -84,6 +84,9 @@ class URLAB_API UURLabShmPublishTransport : public UURLabPublishTransport /** Convenience: directory holding all SHM files for this session. */ static FString ResolveSessionDir(const FString& InSessionId); + // UURLabPublishTransport contract + virtual void AppendHandshakeBlock(TSharedPtr& Reply) const override; + private: TWeakObjectPtr OwningManager; FMjShmRegion StateRegion; diff --git a/Source/URLab/Public/Transport/ShmRpcTransport.h b/Source/URLab/Public/Transport/ShmRpcTransport.h index 3189cd22..3e43856c 100644 --- a/Source/URLab/Public/Transport/ShmRpcTransport.h +++ b/Source/URLab/Public/Transport/ShmRpcTransport.h @@ -21,6 +21,7 @@ #include "ShmRpcTransport.generated.h" class FRunnableThread; +class FSmStepTransportRunnable; /** * @class UURLabShmRpcTransport @@ -37,8 +38,8 @@ class FRunnableThread; * inner-loop transport (1 kHz controller channels). Editor-only ops * (`import_xml`, `spawn_actor`, `list_actors`, etc.) get a * `wrong_transport: use_zmq` reply via the base class's - * `AcceptsEditorOps()=false` short-circuit. The bridge-side client - * auto-routes editor ops to ZMQ; nothing needs to be re-tried. + * `AcceptsEditorOps()=false` short-circuit. The request is never executed + * here, so the client can safely re-route such ops to ZMQ. */ UCLASS() class URLAB_API UURLabShmRpcTransport : public UURLabRpcTransport @@ -48,19 +49,39 @@ class URLAB_API UURLabShmRpcTransport : public UURLabRpcTransport public: UURLabShmRpcTransport(); - /** Per-buffer slot size. Step replies are tiny (~few KB) but the - * hello reply embeds the MJB, which can be many MB for mesh-heavy - * scenes. 1 MiB is a workable default; bump higher for large MJBs. - * Replies that exceed the stride get a `reply_too_large` error so - * the bridge can fall back to ZMQ for that specific RPC. */ + /** Request-slot size (bridge -> UE). Step requests are small (qpos / + * qvel / ctrl arrays), so 1 MiB is ample. */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|SHM") int32 BufferStride = 1024 * 1024; - /** Optional explicit session id (defaults to "live"; mirrors the - * publisher's path scheme). */ + /** Reply-slot size (UE -> bridge). The ring is a fixed mmap sized once at + * open. Replies that exceed it are NOT dropped — they get an immediate + * `wrong_transport` reply so the bridge re-routes that request to ZMQ + * (the designed fallback for oversize replies). 16 MiB comfortably holds + * the hello MJB and a single HD frame over SHM; multi-camera or 4K + * `include_cameras` replies exceed it and fall back to ZMQ. Note the + * fast image path is the per-camera cam_*.shm / ZMQ streams, NOT this + * reply slot. Raise this only if you specifically want large inline + * camera replies carried over the SHM RPC channel. The bridge reads the + * actual stride from the SHM header (offset 8), so it adapts without a + * hardcoded size. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|SHM") + int32 ReplyBufferStride = 16 * 1024 * 1024; + + /** Optional explicit session label. The resolved session id used for the + * SHM files and kernel event names is always made unique per editor + * process (see TransportInit), so many render-server instances on one + * host never collide; this label just prefixes that unique id. Defaults + * to "live". */ UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "URLab|SHM") FString SessionId; + /** Step-RPC port of the owning instance. Set by the bridge before + * TransportInit purely to keep the resolved session name traceable back + * to the instance; 0 means the name relies on the process id alone for + * uniqueness. */ + int32 InstancePort = 0; + /** How long the worker thread waits between sequence checks * (microseconds). On Windows the worker waits on a named event, so * this only matters when the event isn't usable (other platforms, @@ -76,11 +97,25 @@ class URLAB_API UURLabShmRpcTransport : public UURLabRpcTransport /** SHM scope narrowing: editor ops never reach the dispatcher on * this transport. */ virtual bool AcceptsEditorOps() const override { return false; } + virtual void AppendHandshakeBlock(TSharedPtr& Reply) const override; /** Resolved on-disk paths (set after TransportInit). */ FString GetReqPath() const { return ReqPath; } FString GetRepPath() const { return RepPath; } + // --- Explicit contract for the hello handshake --- + // The bridge must use these verbatim instead of guessing: the req/rep + // file paths, the per-direction Windows event names, the slot strides and + // buffer count. With these it can open exactly the regions UE created and + // poll the rep sequence as a fallback if the named-event wakeup doesn't + // cross its process/session boundary (see project_shm_rpc_5s_followup). + FString GetSessionId() const { return ResolvedSessionId; } + FString GetReqEventName() const { return ReqEventName; } + FString GetRepEventName() const { return RepEventName; } + int32 GetReqStride() const { return BufferStride; } + int32 GetRepStride() const { return ReplyBufferStride; } + int32 GetNumBuffers() const { return 2; } + private: FMjShmRegion ReqRegion; // bridge writes, UE reads FMjShmRegion RepRegion; // UE writes, bridge reads @@ -88,6 +123,12 @@ class URLAB_API UURLabShmRpcTransport : public UURLabRpcTransport FString ReqPath; FString RepPath; + /** Session id actually used (defaults to "live") and the resolved Windows + * event names, captured in TransportInit so the hello can advertise them. */ + FString ResolvedSessionId; + FString ReqEventName; + FString RepEventName; + /** Named-event handles for kernel-wakeup signalling. Bridge calls * SetEvent on `ReqReadyEvent` after writing req.shm; UE's worker * waits on it. Symmetric for `RepReadyEvent`. Stored as void* to @@ -97,6 +138,9 @@ class URLAB_API UURLabShmRpcTransport : public UURLabRpcTransport void* RepReadyEvent = nullptr; FRunnableThread* WorkerThread = nullptr; + /** Runnable driving WorkerThread. FRunnableThread does not own it, so the + * transport keeps the pointer and deletes it at shutdown. */ + FSmStepTransportRunnable* WorkerRunnable = nullptr; std::atomic bStop{false}; bool bInitialized = false; diff --git a/Source/URLab/Public/Transport/SnapshotProducer.h b/Source/URLab/Public/Transport/SnapshotProducer.h deleted file mode 100644 index 912b841e..00000000 --- a/Source/URLab/Public/Transport/SnapshotProducer.h +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "CoreMinimal.h" - -class AAMjManager; -struct mjModel_; -struct mjData_; -typedef mjModel_ mjModel; -typedef mjData_ mjData; - -/** - * @brief Build the per-step state snapshot that drives live clients. - * - * The bytes returned here are the same shape the step server emits for - * direct/puppet step replies, so a live client that subscribes to a - * snapshot publisher sees the same wire format as a stepped client. Pure - * builder -- no transport, no I/O. AAMjManager calls this once per - * physics step and fans the bytes out to every IMjSnapshotPublisher. - */ -class URLAB_API FMjSnapshotProducer -{ -public: - /** - * Build a msgpack-encoded `state_full` snapshot for the current step. - * - * The reply shape mirrors the step server's direct/puppet replies: - * { op: "state_full", time, step, per_articulation, entities } - * - * @param Manager Manager owning the articulation list / scene cache. - * @param m Compiled mjModel (read only here). - * @param d Live mjData (read only here). - * @param StepIndex Monotonic frame counter to embed in the reply. - * @return msgpack bytes; empty if Manager / m / d are not ready. - */ - static TArray BuildStateSnapshot(AAMjManager* Manager, - mjModel* m, mjData* d, - int64 StepIndex); -}; diff --git a/Source/URLab/Public/Transport/SnapshotPublisher.h b/Source/URLab/Public/Transport/SnapshotPublisher.h index f3b73ef9..61ec299f 100644 --- a/Source/URLab/Public/Transport/SnapshotPublisher.h +++ b/Source/URLab/Public/Transport/SnapshotPublisher.h @@ -23,9 +23,10 @@ * with UCLASSes is straightforward. Implementations: UURLabZmqPublishTransport * (PUB on tcp://0.0.0.0:5555), UURLabShmPublishTransport (SHM ring buffer). * - * The publisher does NOT decide what to ship -- `FMjSnapshotProducer` - * builds the msgpack bytes once per step (in `AAMjManager`'s PostStep - * callback); each publisher calls `PublishSnapshot(Bytes)` to fan out. + * The publisher does NOT decide what to ship -- the manager's PostStep + * callback builds the state IR via `FMjStateCollector` and encodes the msgpack + * bytes once via `FMjMsgpackEncoder`; each publisher calls + * `PublishSnapshot(Bytes)` to fan out. */ class URLAB_API IMjSnapshotPublisher { diff --git a/Source/URLab/Public/Transport/SubscribeTransport.h b/Source/URLab/Public/Transport/SubscribeTransport.h index 81acebc8..095d7f24 100644 --- a/Source/URLab/Public/Transport/SubscribeTransport.h +++ b/Source/URLab/Public/Transport/SubscribeTransport.h @@ -56,5 +56,7 @@ class URLAB_API UURLabSubscribeTransport : public UObject virtual void PostStep(struct mjModel_* /*m*/, struct mjData_* /*d*/) {} protected: + // Owning bridge; same pattern as UURLabRpcTransport — UE single-inheritance + // prevents extracting a shared UObject base for all three transport ABCs. TWeakObjectPtr OwningBridge; }; diff --git a/Source/URLab/Public/Transport/ZmqPublishTransport.h b/Source/URLab/Public/Transport/ZmqPublishTransport.h index bec9b1a1..4c6669c4 100644 --- a/Source/URLab/Public/Transport/ZmqPublishTransport.h +++ b/Source/URLab/Public/Transport/ZmqPublishTransport.h @@ -22,27 +22,21 @@ #pragma once -#include - #include "CoreMinimal.h" #include "Transport/PublishTransport.h" #include "Transport/SnapshotPublisher.h" #include "ZmqPublishTransport.generated.h" class AAMjManager; -class AMjArticulation; -class UMjComponent; -class UMjTwistController; /** * @class UURLabZmqPublishTransport - * @brief ZMQ PUB transport broadcasting per-articulation telemetry + - * the `state/full` snapshot. + * @brief ZMQ PUB transport broadcasting the `state/full` snapshot. * * Plain UObject deriving from UURLabPublishTransport, created via - * `NewObject` + `SetOwningManager` + `TransportInit`. The game-thread - * cache build runs lazily on the first PostStep via `AsyncTask` to the - * game thread. + * `NewObject` + `SetOwningManager` + `TransportInit`. It registers as an + * IMjSnapshotPublisher; the manager's post-step callback builds + encodes + * the snapshot once and fans the bytes out via PublishSnapshot. */ UCLASS() class URLAB_API UURLabZmqPublishTransport : public UURLabPublishTransport @@ -68,11 +62,8 @@ class URLAB_API UURLabZmqPublishTransport : public UURLabPublishTransport virtual void Publish(const FString& Topic, const TArray& Payload) override; - // Per-step hook (Async / physics thread). - virtual void PostStep(struct mjModel_* m, struct mjData_* d) override; - // IMjSnapshotPublisher: route through to Publish("state/full", bytes) - // so the manager's existing snapshot fan-out keeps working. + // so the manager's snapshot fan-out delivers to the wire. virtual void PublishSnapshot(const TArray& Bytes) override { Publish(TEXT("state/full"), Bytes); @@ -82,39 +73,8 @@ class URLAB_API UURLabZmqPublishTransport : public UURLabPublishTransport TWeakObjectPtr OwningManager; void* ZmqContext = nullptr; void* ZmqPublisher = nullptr; - int32 FrameCounter = 0; bool bIsInitialized = false; - /** Per-articulation snapshot built once on the game thread and read - * repeatedly from the physics thread in PostStep. Iterating - * OwnedComponents on the physics thread is unsafe (the game thread - * can mutate it during actor BeginPlay — e.g. auto-created twist - * controllers), and tripping the sparse-array range-for ensure - * corrupts nearby heap state, producing seemingly-unrelated RHI - * crashes further along. */ - struct FArticulationBroadcastRecord - { - AMjArticulation* Articulation = nullptr; - FString ArticPrefix; - TArray TelemetryComponents; - UMjTwistController* TwistCtrl = nullptr; - }; - - /** Populated once on the game thread. bCacheBuilt (with acquire/release - * ordering) publishes visibility to the physics thread. No mid-play - * refresh — broadcaster assumes articulations and their components are - * stable across a single play session. */ - TArray CachedRecords; - std::atomic bCacheBuilt{false}; - std::atomic bCacheBuildScheduled{false}; - - /** Schedule a one-shot AsyncTask(GameThread) to build the cache. - * Idempotent: only the first call goes through. */ - void RequestGameThreadCacheBuild(); - - /** Game-thread-only: enumerate articulations + components into CachedRecords. */ - void BuildBroadcastCacheGameThread(); - void InitZmqSocket(); void ShutdownZmqSocket(); }; diff --git a/Source/URLab/Public/Transport/ZmqRpcTransport.h b/Source/URLab/Public/Transport/ZmqRpcTransport.h index 779c40ce..b691fe55 100644 --- a/Source/URLab/Public/Transport/ZmqRpcTransport.h +++ b/Source/URLab/Public/Transport/ZmqRpcTransport.h @@ -24,6 +24,7 @@ #include "CoreMinimal.h" #include "Transport/RpcTransport.h" +#include "Bridge/StepCommands.h" #include "Dom/JsonObject.h" #include "HAL/Event.h" #include "HAL/PlatformProcess.h" @@ -31,71 +32,7 @@ #include "ZmqRpcTransport.generated.h" class FRunnableThread; - -/** - * @struct FMjStepRequest - * @brief One Direct-mode step request, parsed from a client RPC and pushed to - * the physics-thread queue. Owns the per-articulation ctrl writes and - * the n_steps count. - */ -struct FMjStepRequest -{ - int32 NSteps = 1; - /** prefix -> array of (actuator_name, value). Names are local (no prefix). */ - TMap>> PerArticulationCtrl; - /** Per-articulation control mode: "ue_controller" (default) or "raw". */ - TMap PerArticulationControlMode; - /** Per-articulation xfrc_applied: prefix -> body_name -> [fx,fy,fz,tx,ty,tz]. */ - TMap>> PerArticulationXfrc; - /** Echo'd request envelope for downstream reply building. */ - FString Op; -}; - -/** - * @struct FMjDirectStepCommand - * @brief Heap-allocated wrapper passed by raw pointer through the SPSC queue - * in Direct mode. The RPC thread enqueues, the physics-thread custom - * step handler dequeues, drains the request, and signals via FEvent. - * Captures observations inline so the reply can be built off the - * physics thread without re-touching d. - */ -struct FMjDirectStepCommand -{ - FMjStepRequest Request; - /** Set true by the handler when mj_step has completed. */ - bool bDone = false; - /** Observations captured under the engine's CallbackMutex. */ - TSharedPtr Observations; - TSharedPtr Entities; - double ResultTime = 0.0; - int64 ResultStep = 0; - /** Physics thread signals this when the step has completed. */ - FEvent* Completion = nullptr; - - ~FMjDirectStepCommand() - { - if (Completion) - { - FPlatformProcess::ReturnSynchEventToPool(Completion); - Completion = nullptr; - } - } -}; - -/** - * @struct FMjPushStateRequest - * @brief One Puppet-mode push-state request. The client owns the integrator; - * UE writes qpos/qvel and calls mj_forward. - */ -struct FMjPushStateRequest -{ - TArray QPos; - TArray QVel; - TArray Ctrl; // optional informational ctrl - bool bIncludeCtrl = false; - double Time = 0.0; - int32 NSteps = 1; // informational only in puppet -}; +class FRunnable; /** * @class UURLabZmqRpcTransport @@ -129,10 +66,20 @@ class URLAB_API UURLabZmqRpcTransport : public UURLabRpcTransport void* ZmqContext = nullptr; void* ZmqRep = nullptr; FRunnableThread* WorkerThread = nullptr; + /** Runnable driving WorkerThread. FRunnableThread does not own it, so the + * transport keeps the pointer and deletes it at shutdown. */ + FRunnable* WorkerRunnable = nullptr; std::atomic bStop{false}; bool bIsInitialized = false; + /** Create the REP socket, apply timeouts + LINGER, and bind StepEndpoint. + * Shared by TransportInit and the send-error recovery path so a wedged + * REP state machine can be reset without duplicating socket setup. */ + bool CreateAndBindRep(); + /** Worker thread loop. Runs zmq_poll on the REP socket and forwards each * parsed request to the dispatcher; sends the reply back to the wire. */ void RunPollLoop(); + + friend class FStepServerRunnable; }; diff --git a/Source/URLab/Public/Transport/ZmqSubscribeTransport.h b/Source/URLab/Public/Transport/ZmqSubscribeTransport.h index 1f6908d0..54f8df0d 100644 --- a/Source/URLab/Public/Transport/ZmqSubscribeTransport.h +++ b/Source/URLab/Public/Transport/ZmqSubscribeTransport.h @@ -71,6 +71,7 @@ class URLAB_API UURLabZmqSubscribeTransport : public UURLabSubscribeTransport int TotalStepCount = 0; TMap ActuatorCache; TMap ActuatorComponentCache; + TMap ActuatorToArticulationName; bool bCacheBuilt = false; int32 ControlLogCounter = 0; From 15bc7aff5835a49119a200dcc22e432fc2b15875 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:38 +0100 Subject: [PATCH 05/32] Unify state serialization behind one IR Two serialization paths had drifted. There is now a single intermediate representation carrying raw MuJoCo SI values, one msgpack encoder over it, and user-authored payload channels that are transport-neutral by construction. --- .../URLab/Private/State/MjCanonicalName.cpp | 74 ++++ .../URLab/Private/State/MjMsgpackEncoder.cpp | 241 ++++++++++ .../URLab/Private/State/MjStateCollector.cpp | 397 +++++++++++++++++ .../UserChannels/MjUserChannelComponent.cpp | 412 ++++++++++++++++++ Source/URLab/Public/State/MjCanonicalName.h | 52 +++ Source/URLab/Public/State/MjMsgpackEncoder.h | 57 +++ .../URLab/Public/State/MjObservationLevel.h | 28 ++ Source/URLab/Public/State/MjStateCollector.h | 115 +++++ Source/URLab/Public/State/MjStateConsumer.h | 53 +++ Source/URLab/Public/State/MjStateProducer.h | 64 +++ Source/URLab/Public/State/MjStateTypes.h | 289 ++++++++++++ .../UserChannels/MjUserChannelComponent.h | 195 +++++++++ 12 files changed, 1977 insertions(+) create mode 100644 Source/URLab/Private/State/MjCanonicalName.cpp create mode 100644 Source/URLab/Private/State/MjMsgpackEncoder.cpp create mode 100644 Source/URLab/Private/State/MjStateCollector.cpp create mode 100644 Source/URLab/Private/UserChannels/MjUserChannelComponent.cpp create mode 100644 Source/URLab/Public/State/MjCanonicalName.h create mode 100644 Source/URLab/Public/State/MjMsgpackEncoder.h create mode 100644 Source/URLab/Public/State/MjObservationLevel.h create mode 100644 Source/URLab/Public/State/MjStateCollector.h create mode 100644 Source/URLab/Public/State/MjStateConsumer.h create mode 100644 Source/URLab/Public/State/MjStateProducer.h create mode 100644 Source/URLab/Public/State/MjStateTypes.h create mode 100644 Source/URLab/Public/UserChannels/MjUserChannelComponent.h diff --git a/Source/URLab/Private/State/MjCanonicalName.cpp b/Source/URLab/Private/State/MjCanonicalName.cpp new file mode 100644 index 00000000..3738ec59 --- /dev/null +++ b/Source/URLab/Private/State/MjCanonicalName.cpp @@ -0,0 +1,74 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "State/MjCanonicalName.h" +#include "MuJoCo/Core/MjArticulation.h" + +FString FMjCanonicalName::Sanitize(const FString& Segment) +{ + if (Segment.IsEmpty()) + return Segment; + + FString Out; + Out.Reserve(Segment.Len() + 1); + for (TCHAR C : Segment) + { + const bool bLegal = (C >= TEXT('A') && C <= TEXT('Z')) || (C >= TEXT('a') && C <= TEXT('z')) + || (C >= TEXT('0') && C <= TEXT('9')) || C == TEXT('_'); + Out.AppendChar(bLegal ? C : TEXT('_')); + } + if (Out[0] >= TEXT('0') && Out[0] <= TEXT('9')) + Out = FString(TEXT("_")) + Out; + return Out; +} + +FName FMjCanonicalName::ArtSegment(const AMjArticulation* Art) +{ + if (!Art) + return FName(); + // The art's public identity for topics, tf frames, and control-ownership keys. + // Prefer the stable, user-supplied ActorId ("franka") over the UE object name, + // which is auto-generated and regenerated per spawn (panda_C_UAID_...); the + // latter makes ROS topic/frame names unstable across runs. GetName() is the + // fallback for arts spawned without an ActorId (e.g. placed in-editor). + const FString Public = Art->ActorId.IsEmpty() ? Art->GetName() : Art->ActorId; + return FName(*Sanitize(Public)); +} + +FName FMjCanonicalName::PartSegment(const AMjArticulation* Art, const FString& MjName) +{ + // Child mj names are compiled with the UE object-name prefix (not ActorId), so + // prefix stripping stays keyed on GetName() even though ArtSegment is ActorId. + FString Local = MjName; + if (Art) + { + const FString Prefix = Art->GetName() + TEXT("_"); + if (Local.StartsWith(Prefix)) + Local = Local.Mid(Prefix.Len()); + } + return FName(*Sanitize(Local)); +} + +FString FMjCanonicalName::Full(FName Art, FName Part) +{ + return FString::Printf(TEXT("%s/%s"), *Art.ToString(), *Part.ToString()); +} diff --git a/Source/URLab/Private/State/MjMsgpackEncoder.cpp b/Source/URLab/Private/State/MjMsgpackEncoder.cpp new file mode 100644 index 00000000..1fba7afd --- /dev/null +++ b/Source/URLab/Private/State/MjMsgpackEncoder.cpp @@ -0,0 +1,241 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "State/MjMsgpackEncoder.h" +#include "State/MjStateTypes.h" +#include "Bridge/MsgpackHelpers.h" + +namespace +{ +TSharedPtr NumArray(const TArray& Values) +{ + TArray> Out; + Out.Reserve(Values.Num()); + for (double V : Values) + Out.Add(MakeShared(V)); + return MakeShared(Out); +} + +TSharedPtr NumArrayN(const double* Values, int32 Count) +{ + TArray> Out; + Out.Reserve(Count); + for (int32 i = 0; i < Count; ++i) + Out.Add(MakeShared(Values[i])); + return MakeShared(Out); +} + +/** Encode one user channel into its self-describing msgpack value. The shape is + * keyed on Kind so a Python client reads typed values with no schema. */ +TSharedPtr EncodeUserValue(const FMjUserChannel& C) +{ + switch (C.Kind) + { + case EMjUserChannelKind::Bool: + return MakeShared(C.Values.Num() > 0 && C.Values[0] != 0.0); + case EMjUserChannelKind::Int: + case EMjUserChannelKind::Scalar: + return MakeShared(C.Values.Num() > 0 ? C.Values[0] : 0.0); + case EMjUserChannelKind::Vec3: + case EMjUserChannelKind::Quat: + case EMjUserChannelKind::Array: + return NumArray(C.Values); + case EMjUserChannelKind::Transform: + { + TSharedPtr Obj = MakeShared(); + const int32 N = C.Values.Num(); + Obj->SetField(TEXT("pos"), NumArrayN(C.Values.GetData(), FMath::Min(3, N))); + Obj->SetField(TEXT("quat"), N > 3 ? NumArrayN(C.Values.GetData() + 3, FMath::Min(4, N - 3)) + : NumArrayN(nullptr, 0)); + return MakeShared(Obj); + } + case EMjUserChannelKind::String: + return MakeShared(C.Text); + case EMjUserChannelKind::Struct: + { + // Packed carries a pre-built msgpack map; re-inflate it so it splices + // into the snapshot as a nested object rather than an opaque blob. + TSharedPtr Parsed; + if (C.Packed.Num() > 0 + && FURLabMsgpackUtil::UnpackToJsonObject(C.Packed.GetData(), C.Packed.Num(), Parsed) + && Parsed.IsValid()) + { + return MakeShared(Parsed); + } + return MakeShared(MakeShared()); + } + } + return MakeShared(); +} + +/** Emit a `user` map keyed by channel name, if any channels exist. */ +void EncodeUserChannels(const TArray& Channels, const TSharedPtr& Out) +{ + if (Channels.Num() == 0) + return; + TSharedPtr User = MakeShared(); + for (const FMjUserChannel& C : Channels) + User->SetField(C.Name.ToString(), EncodeUserValue(C)); + Out->SetObjectField(TEXT("user"), User); +} + +/** Encode one articulation into the per-art block for the requested level. */ +TSharedPtr EncodeArticulation(const FMjArticulationState& Art, EObservationLevel Level) +{ + const bool bStandard = (Level == EObservationLevel::Standard) || (Level == EObservationLevel::Full); + const bool bFull = (Level == EObservationLevel::Full); + + TSharedPtr Obj = MakeShared(); + + // qpos / qvel -- present at every level, concatenated in joint order. + { + TArray> QPos; + TArray> QVel; + for (const FMjJointState& J : Art.Joints) + { + for (double V : J.QPos) + QPos.Add(MakeShared(V)); + for (double V : J.QVel) + QVel.Add(MakeShared(V)); + } + Obj->SetArrayField(TEXT("qpos"), QPos); + Obj->SetArrayField(TEXT("qvel"), QVel); + } + + if (bStandard) + { + TArray> Ctrl; + TArray> Act; + for (const FMjActuatorState& A : Art.Actuators) + { + Ctrl.Add(MakeShared(A.Ctrl)); + Act.Add(MakeShared(A.Act)); + } + Obj->SetArrayField(TEXT("ctrl"), Ctrl); + Obj->SetArrayField(TEXT("act"), Act); + + TSharedPtr Sensors = MakeShared(); + for (const FMjSensorState& Sen : Art.Sensors) + Sensors->SetField(Sen.Name.ToString(), NumArray(Sen.Values)); + Obj->SetObjectField(TEXT("sensors"), Sensors); + + // User channels emit at Standard and Full, beside sensors. + EncodeUserChannels(Art.UserChannels, Obj); + } + + if (bFull) + { + TSharedPtr Bodies = MakeShared(); + for (const FMjBodyState& B : Art.Bodies) + { + TSharedPtr Bo = MakeShared(); + Bo->SetField(TEXT("xpos"), NumArrayN(B.Xpos, 3)); + Bo->SetField(TEXT("xquat"), NumArrayN(B.Xquat, 4)); + Bodies->SetObjectField(B.Name.ToString(), Bo); + } + Obj->SetObjectField(TEXT("bodies"), Bodies); + + TArray> Force; + for (const FMjActuatorState& A : Art.Actuators) + Force.Add(MakeShared(A.Force)); + Obj->SetArrayField(TEXT("actuator_force"), Force); + } + + // Twist -- emitted whenever a twist controller is attached, regardless of + // level. geometry_msgs/Twist layout. + if (Art.Twist.IsSet()) + { + const FMjTwistState& T = Art.Twist.GetValue(); + TSharedPtr TwistObj = MakeShared(); + TwistObj->SetField(TEXT("linear"), NumArrayN(T.Linear, 3)); + TwistObj->SetField(TEXT("angular"), NumArrayN(T.Angular, 3)); + Obj->SetObjectField(TEXT("twist"), TwistObj); + Obj->SetNumberField(TEXT("actions"), static_cast(T.Actions)); + } + + return Obj; +} + +/** sim_time / wall_time blocks, matching FURLabRpcDispatcher::AppendClockFields. */ +void EncodeClock(const FMjClock& Clock, const TSharedPtr& Out) +{ + TSharedPtr Sim = MakeShared(); + Sim->SetNumberField(TEXT("sec"), Clock.SimSec); + Sim->SetNumberField(TEXT("nsec"), Clock.SimNsec); + Out->SetObjectField(TEXT("sim_time"), Sim); + + TSharedPtr Wall = MakeShared(); + Wall->SetNumberField(TEXT("sec"), static_cast(Clock.WallSec)); + Wall->SetNumberField(TEXT("nsec"), static_cast(Clock.WallNsec)); + Out->SetObjectField(TEXT("wall_time"), Wall); +} +} // namespace + +TSharedPtr FMjMsgpackEncoder::EncodeArts(const FMjStateSnapshot& S, + EObservationLevel Level) +{ + TSharedPtr Arts = MakeShared(); + for (const FMjArticulationState& Art : S.Articulations) + Arts->SetObjectField(Art.Name.ToString(), EncodeArticulation(Art, Level)); + return Arts; +} + +TSharedPtr FMjMsgpackEncoder::EncodeScene(const FMjStateSnapshot& S) +{ + TSharedPtr Scene = MakeShared(); + for (const FMjEntityState& E : S.Entities) + { + TSharedPtr Obj = MakeShared(); + Obj->SetField(TEXT("xpos"), NumArrayN(E.Xpos, 3)); + Obj->SetField(TEXT("xquat"), NumArrayN(E.Xquat, 4)); + if (E.bFreeBase && E.QPos.Num() == 7 && E.QVel.Num() == 6) + { + Obj->SetField(TEXT("qpos"), NumArray(E.QPos)); + Obj->SetField(TEXT("qvel"), NumArray(E.QVel)); + } + Scene->SetObjectField(E.Name.ToString(), Obj); + } + return Scene; +} + +TSharedPtr FMjMsgpackEncoder::EncodeSnapshot(const FMjStateSnapshot& S, + EObservationLevel Level) +{ + TSharedPtr Snap = MakeShared(); + Snap->SetStringField(TEXT("op"), TEXT("state_full")); + Snap->SetNumberField(TEXT("time"), S.Time); + Snap->SetNumberField(TEXT("step"), static_cast(S.Step)); + EncodeClock(S.Clock, Snap); + Snap->SetObjectField(TEXT("arts"), EncodeArts(S, Level)); + Snap->SetObjectField(TEXT("scene"), EncodeScene(S)); + // Scene-scoped user channels (producers not owned by any articulation). + EncodeUserChannels(S.UserChannels, Snap); + return Snap; +} + +TArray FMjMsgpackEncoder::EncodeSnapshotBytes(const FMjStateSnapshot& S, + EObservationLevel Level) +{ + TArray Buf; + FURLabMsgpackUtil::PackJsonObject(EncodeSnapshot(S, Level), Buf); + return Buf; +} diff --git a/Source/URLab/Private/State/MjStateCollector.cpp b/Source/URLab/Private/State/MjStateCollector.cpp new file mode 100644 index 00000000..8482dadd --- /dev/null +++ b/Source/URLab/Private/State/MjStateCollector.cpp @@ -0,0 +1,397 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "State/MjStateCollector.h" +#include "State/MjCanonicalName.h" +#include "State/MjStateProducer.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "MuJoCo/Components/MjComponent.h" +#include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "Components/ActorComponent.h" +#include "GameFramework/Actor.h" +#include "Async/Async.h" +#include "Misc/ScopeExit.h" +#include "mujoco/mujoco.h" + +void FMjStateCollector::Init(AAMjManager* InManager) +{ + Manager = InManager; + bCacheValid.store(false, std::memory_order_release); +} + +void FMjStateCollector::MarkProducerCacheDirty() +{ + bCacheValid.store(false, std::memory_order_release); + RequestGameThreadRebuild(); +} + +void FMjStateCollector::RequestGameThreadRebuild() +{ + bool bExpected = false; + if (!bRebuildScheduled.compare_exchange_strong(bExpected, true)) + return; // a rebuild is already queued + + TWeakObjectPtr WeakMgr = Manager; + AsyncTask(ENamedThreads::GameThread, [this, WeakMgr]() { + if (WeakMgr.IsValid()) + RebuildProducerCacheGameThread(); + else + bRebuildScheduled.store(false, std::memory_order_release); + }); +} + +void FMjStateCollector::RebuildProducerCacheGameThread() +{ + ON_SCOPE_EXIT + { + bRebuildScheduled.store(false, std::memory_order_release); + }; + + AAMjManager* Mgr = Manager.Get(); + if (!Mgr) + return; + + // Assemble into a local with no lock held; the physics thread keeps reading + // the previous cache meanwhile. Only the swap below is guarded. + TArray NewCache; + const TArray& Arts = Mgr->GetAllArticulations(); + NewCache.Reserve(Arts.Num()); + for (AMjArticulation* Art : Arts) + { + if (!Art) + continue; + + FCachedArticulation Rec; + Rec.Art = Art; + Rec.ArtSegment = FMjCanonicalName::ArtSegment(Art); + + TArray Components; + Art->GetComponents(Components); + Rec.Producers.Reserve(Components.Num()); + for (UMjComponent* Comp : Components) + { + if (!Comp || Comp->bIsDefault) + continue; + Rec.Producers.Add(Comp); + } + + Rec.TwistCtrl = Art->FindComponentByClass(); + NewCache.Add(MoveTemp(Rec)); + } + + // Registered IMjStateProducers the art walk cannot discover (user channel + // components, scene-level actors). Scope is resolved here on the game thread: + // a producer owned by an articulation caches under that art; everything else + // is a scene producer. The physics-thread step never does scope logic. + TArray> NewSceneProducers; + { + TArray> Registered; + Mgr->GetStateProducers(Registered); + for (const TWeakObjectPtr& WeakProducer : Registered) + { + UObject* Obj = WeakProducer.Get(); + if (!Obj) + continue; + + AActor* OwnerActor = Cast(Obj); + if (!OwnerActor) + { + if (UActorComponent* Comp = Cast(Obj)) + OwnerActor = Comp->GetOwner(); + } + + AMjArticulation* OwningArt = Cast(OwnerActor); + FCachedArticulation* Rec = OwningArt + ? NewCache.FindByPredicate([OwningArt](const FCachedArticulation& R) { + return R.Art.Get() == OwningArt; + }) + : nullptr; + if (Rec) + Rec->InterfaceProducers.Add(Obj); + else + NewSceneProducers.Add(Obj); + } + } + + // World geometry cache: every geom not on a robot body, resolved once here + // (shapes are static) so the per-step physics-thread build only reads the + // parent body's live pose. Mesh geoms collapse to their AABB box; very large + // geoms (the ground / environment shell) and planes are skipped. + TArray NewWorldGeoms; + if (Mgr->PhysicsEngine) + { + if (const mjModel* m = Mgr->PhysicsEngine->GetModel()) + { + // Robot bodies are compiled with the articulation's raw-name prefix; match + // by name (the actor name is stable on the game thread) rather than mj ids, + // which may not be bound yet when the cache first rebuilds. + TArray RobotPrefixes; + RobotPrefixes.Reserve(Arts.Num()); + for (AMjArticulation* Art : Arts) + { + if (Art) + RobotPrefixes.Add(Art->GetName() + TEXT("_")); + } + + // Skip geoms whose (half-extent) box is large enough to be environment + // shell rather than a discrete object: as an AABB it would engulf the + // robot and flag every start state in collision. Discrete graspable / + // avoidable objects are well under this. (A future mesh path can carry + // the real concave geometry instead of a box.) + const double MaxWorldExtent = 1.0; + for (int g = 0; g < m->ngeom; ++g) + { + const int b = m->geom_bodyid[g]; + if (b <= 0) + continue; + const char* BodyRaw = mj_id2name(const_cast(m), mjOBJ_BODY, b); + const FString BodyName = BodyRaw ? FString(UTF8_TO_TCHAR(BodyRaw)) : FString(); + bool bRobot = false; + for (const FString& Prefix : RobotPrefixes) + { + if (BodyName.StartsWith(Prefix)) + { + bRobot = true; + break; + } + } + if (bRobot) + continue; + + FCachedWorldGeom W; + W.BodyId = b; + const char* Raw = mj_id2name(const_cast(m), mjOBJ_GEOM, g); + W.Name = Raw ? FName(UTF8_TO_TCHAR(Raw)) + : FName(*FString::Printf(TEXT("geom%d"), g)); + const mjtNum* Gp = &m->geom_pos[3 * g]; + const mjtNum* Gq = &m->geom_quat[4 * g]; + for (int k = 0; k < 3; ++k) + W.LocalPos[k] = Gp[k]; + for (int k = 0; k < 4; ++k) + W.LocalQuat[k] = Gq[k]; + W.bStatic = (m->body_dofnum[b] == 0); + + const int gt = m->geom_type[g]; + const mjtNum* Sz = &m->geom_size[3 * g]; + if (gt == mjGEOM_SPHERE) + { + W.Shape = EMjWorldGeomShape::Sphere; + W.Size[0] = Sz[0]; + } + else if (gt == mjGEOM_CYLINDER || gt == mjGEOM_CAPSULE) + { + W.Shape = EMjWorldGeomShape::Cylinder; + W.Size[0] = Sz[0]; + W.Size[1] = Sz[1]; + } + else if (gt == mjGEOM_BOX) + { + W.Shape = EMjWorldGeomShape::Box; + W.Size[0] = Sz[0]; + W.Size[1] = Sz[1]; + W.Size[2] = Sz[2]; + } + else if (gt == mjGEOM_MESH) + { + // Real triangle geometry (same mesh_vert / mesh_face tables the + // URDF export uses), so concave obstacles reach MoveIt faithfully + // instead of an engulfing AABB. + const int did = m->geom_dataid[g]; + if (did < 0) + continue; + const int va = m->mesh_vertadr[did]; + const int vn = m->mesh_vertnum[did]; + const int fa = m->mesh_faceadr[did]; + const int fn = m->mesh_facenum[did]; + if (vn <= 0 || fn <= 0) + continue; + TSharedPtr Mesh = MakeShared(); + Mesh->Verts.Reserve(vn); + for (int v = 0; v < vn; ++v) + { + const float* P = &m->mesh_vert[3 * (va + v)]; + Mesh->Verts.Emplace(P[0], P[1], P[2]); + } + Mesh->Tris.Reserve(fn * 3); + for (int f = 0; f < fn; ++f) + { + const int* T = &m->mesh_face[3 * (fa + f)]; + Mesh->Tris.Add(T[0]); + Mesh->Tris.Add(T[1]); + Mesh->Tris.Add(T[2]); + } + W.Shape = EMjWorldGeomShape::Mesh; + W.Mesh = Mesh; + } + else + { + continue; // plane / hfield / ellipsoid: unsupported for now + } + + // Primitive extent cap skips the ground / environment shell, whose AABB + // would engulf the robot. Meshes carry real geometry, so they pass through. + if (W.Shape != EMjWorldGeomShape::Mesh && FMath::Max3(W.Size[0], W.Size[1], W.Size[2]) > MaxWorldExtent) + continue; + NewWorldGeoms.Add(W); + } + } + } + + { + FScopeLock Lock(&CacheMutex); + Cache = MoveTemp(NewCache); + SceneProducers = MoveTemp(NewSceneProducers); + WorldGeomCache = MoveTemp(NewWorldGeoms); + } + ++StructureVersion; + bCacheValid.store(true, std::memory_order_release); +} + +const FMjStateSnapshot& FMjStateCollector::Collect(mjModel* m, mjData* d, int64 StepIdx) +{ + Snapshot.Reset(); + if (!m || !d) + return Snapshot; + + AAMjManager* Mgr = Manager.Get(); + + Snapshot.Time = d->time; + Snapshot.Step = StepIdx; + Snapshot.StructureVersion = StructureVersion; + + // Clock: ROS builtin_interfaces/Time sec/nsec pairs, matching AppendClockFields. + const int32 SimSec = static_cast(d->time); + Snapshot.Clock.SimSec = SimSec; + Snapshot.Clock.SimNsec = static_cast((d->time - SimSec) * 1.0e9); + const FTimespan Delta = FDateTime::UtcNow() - FDateTime(1970, 1, 1); + Snapshot.Clock.WallSec = Delta.GetTotalSeconds(); + Snapshot.Clock.WallNsec = (Delta.GetTicks() % ETimespan::TicksPerSecond) * 100; + + if (!bCacheValid.load(std::memory_order_acquire)) + RequestGameThreadRebuild(); + + bool bStaleRef = false; + { + FScopeLock Lock(&CacheMutex); + Snapshot.Articulations.Reserve(Cache.Num()); + for (const FCachedArticulation& Rec : Cache) + { + AMjArticulation* Art = Rec.Art.Get(); + if (!Art) + { + bStaleRef = true; + continue; + } + FMjArticulationState& ArtState = Snapshot.Articulations.AddDefaulted_GetRef(); + ArtState.Name = Rec.ArtSegment; + for (const TWeakObjectPtr& WeakComp : Rec.Producers) + { + if (UMjComponent* Comp = WeakComp.Get()) + Comp->DescribeState(ArtState); + } + if (UMjTwistController* Twist = Rec.TwistCtrl.Get()) + Twist->DescribeState(ArtState); + for (const TWeakObjectPtr& WeakProducer : Rec.InterfaceProducers) + { + if (IMjStateProducer* Producer = Cast(WeakProducer.Get())) + Producer->DescribeState(ArtState); + } + } + + // Scene-scoped producers fill the snapshot's own blocks (e.g. scene + // user channels), after the art loop but still under the cache lock. + for (const TWeakObjectPtr& WeakProducer : SceneProducers) + { + if (IMjStateProducer* Producer = Cast(WeakProducer.Get())) + Producer->DescribeSceneState(Snapshot); + } + + // World geometry: cached (static) shapes composed with the parent body's + // live world pose. World pose = body pose * geom-local offset. + Snapshot.WorldGeoms.Reserve(WorldGeomCache.Num()); + for (const FCachedWorldGeom& W : WorldGeomCache) + { + if (W.BodyId < 0 || W.BodyId >= m->nbody) + continue; + FMjWorldGeom G; + G.Name = W.Name; + G.Shape = W.Shape; + G.Size[0] = W.Size[0]; + G.Size[1] = W.Size[1]; + G.Size[2] = W.Size[2]; + G.bStatic = W.bStatic; + G.Mesh = W.Mesh; + const mjtNum* Bp = &d->xpos[3 * W.BodyId]; + const mjtNum* Bq = &d->xquat[4 * W.BodyId]; + mjtNum Rotated[3]; + mju_rotVecQuat(Rotated, W.LocalPos, Bq); + G.Xpos[0] = Bp[0] + Rotated[0]; + G.Xpos[1] = Bp[1] + Rotated[1]; + G.Xpos[2] = Bp[2] + Rotated[2]; + mju_mulQuat(G.Xquat, Bq, W.LocalQuat); + Snapshot.WorldGeoms.Add(MoveTemp(G)); + } + } + + // Non-articulation entities: raw MjIds with no component, read straight from + // mjData. Prefer the manager's entity cache (built at PostCompile). + if (Mgr) + { + const TArray& Entities = Mgr->GetEntities(); + Snapshot.Entities.Reserve(Entities.Num()); + for (const FMjEntityRecord& Ent : Entities) + { + if (Ent.MjId < 0 || Ent.MjId >= m->nbody) + continue; + FMjEntityState& E = Snapshot.Entities.AddDefaulted_GetRef(); + E.Name = FName(*Ent.Name); + E.bFreeBase = Ent.bHasFreeBase; + for (int i = 0; i < 3; ++i) + E.Xpos[i] = d->xpos[Ent.MjId * 3 + i]; + for (int i = 0; i < 4; ++i) + E.Xquat[i] = d->xquat[Ent.MjId * 4 + i]; + if (Ent.bHasFreeBase && m->body_jntnum && m->body_jntadr) + { + const int FirstJnt = m->body_jntadr[Ent.MjId]; + if (FirstJnt >= 0 && FirstJnt < m->njnt && m->jnt_type[FirstJnt] == mjJNT_FREE) + { + const int QAddr = m->jnt_qposadr[FirstJnt]; + const int VAddr = m->jnt_dofadr[FirstJnt]; + E.QPos.SetNumUninitialized(7); + E.QVel.SetNumUninitialized(6); + for (int i = 0; i < 7; ++i) + E.QPos[i] = d->qpos[QAddr + i]; + for (int i = 0; i < 6; ++i) + E.QVel[i] = d->qvel[VAddr + i]; + } + } + } + } + + if (bStaleRef) + MarkProducerCacheDirty(); + + return Snapshot; +} diff --git a/Source/URLab/Private/UserChannels/MjUserChannelComponent.cpp b/Source/URLab/Private/UserChannels/MjUserChannelComponent.cpp new file mode 100644 index 00000000..454c9dc8 --- /dev/null +++ b/Source/URLab/Private/UserChannels/MjUserChannelComponent.cpp @@ -0,0 +1,412 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "UserChannels/MjUserChannelComponent.h" +#include "State/MjCanonicalName.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Utils/URLabAxisConv.h" +#include "Async/Async.h" + +namespace +{ +/** Map the Blueprint-facing input kind onto the IR kind (1:1). */ +EMjUserChannelKind ToChannelKind(EMjUserInputKind Kind) +{ + switch (Kind) + { + case EMjUserInputKind::Bool: + return EMjUserChannelKind::Bool; + case EMjUserInputKind::Int: + return EMjUserChannelKind::Int; + case EMjUserInputKind::Scalar: + return EMjUserChannelKind::Scalar; + case EMjUserInputKind::Vec3: + return EMjUserChannelKind::Vec3; + case EMjUserInputKind::Quat: + return EMjUserChannelKind::Quat; + case EMjUserInputKind::Transform: + return EMjUserChannelKind::Transform; + case EMjUserInputKind::Array: + return EMjUserChannelKind::Array; + case EMjUserInputKind::String: + return EMjUserChannelKind::String; + } + return EMjUserChannelKind::Scalar; +} + +/** String and Struct are the text family; everything else is numeric. An input's + * provided kind must share a family with its declared kind to be accepted. */ +bool IsTextKind(EMjUserChannelKind Kind) +{ + return Kind == EMjUserChannelKind::String || Kind == EMjUserChannelKind::Struct; +} +} // namespace + +UMjUserChannelComponent::UMjUserChannelComponent() +{ + PrimaryComponentTick.bCanEverTick = false; +} + +FName UMjUserChannelComponent::MakeChannelName(FName Raw) +{ + return FName(*FMjCanonicalName::Sanitize(Raw.ToString())); +} + +void UMjUserChannelComponent::StoreChannel(FMjUserChannel&& Channel) +{ + const FName Key = Channel.Name; + bool bStructureChanged = false; + { + FScopeLock Lock(&MailboxMutex); + if (const FMjUserChannel* Existing = Mailbox.Find(Key)) + bStructureChanged = (Existing->Kind != Channel.Kind); + else + bStructureChanged = true; + Mailbox.Add(Key, MoveTemp(Channel)); + } + + // First publish of a name, or a kind change, is a structure change: bump the + // collector's StructureVersion so schema-caching consumers rebuild. Steady + // state publishes touch nothing but the mailbox. + if (bStructureChanged) + { + if (AAMjManager* Mgr = ResolveManager()) + Mgr->GetStateCollector().MarkProducerCacheDirty(); + } +} + +void UMjUserChannelComponent::PublishBool(FName Channel, bool bValue) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Bool; + C.Values = {bValue ? 1.0 : 0.0}; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishInt(FName Channel, int64 Value) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Int; + C.Values = {static_cast(Value)}; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishFloat(FName Channel, double Value) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Scalar; + C.Values = {Value}; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishVector(FName Channel, FVector Value, bool bConvertFromUESpace) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Vec3; + C.Values.SetNumUninitialized(3); + if (bConvertFromUESpace) + { + double Out[3]; + URLabAxisConv::UePositionToMj(Value, Out); + C.Values[0] = Out[0]; + C.Values[1] = Out[1]; + C.Values[2] = Out[2]; + } + else + { + C.Values[0] = Value.X; + C.Values[1] = Value.Y; + C.Values[2] = Value.Z; + } + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishQuat(FName Channel, FQuat Value, bool bConvertFromUESpace) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Quat; + C.Values.SetNumUninitialized(4); + if (bConvertFromUESpace) + { + double Out[4]; + URLabAxisConv::UeQuatToMj(Value, Out); + C.Values[0] = Out[0]; + C.Values[1] = Out[1]; + C.Values[2] = Out[2]; + C.Values[3] = Out[3]; + } + else + { + // Already MuJoCo wxyz. + C.Values[0] = Value.W; + C.Values[1] = Value.X; + C.Values[2] = Value.Y; + C.Values[3] = Value.Z; + } + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishTransform(FName Channel, FTransform Value, bool bConvertFromUESpace) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Transform; + C.Values.SetNumUninitialized(7); + const FVector Pos = Value.GetLocation(); + const FQuat Rot = Value.GetRotation(); + if (bConvertFromUESpace) + { + double P[3]; + double Q[4]; + URLabAxisConv::UePositionToMj(Pos, P); + URLabAxisConv::UeQuatToMj(Rot, Q); + C.Values[0] = P[0]; + C.Values[1] = P[1]; + C.Values[2] = P[2]; + C.Values[3] = Q[0]; + C.Values[4] = Q[1]; + C.Values[5] = Q[2]; + C.Values[6] = Q[3]; + } + else + { + C.Values[0] = Pos.X; + C.Values[1] = Pos.Y; + C.Values[2] = Pos.Z; + C.Values[3] = Rot.W; + C.Values[4] = Rot.X; + C.Values[5] = Rot.Y; + C.Values[6] = Rot.Z; + } + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishFloatArray(FName Channel, const TArray& Values) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Array; + C.Values = Values; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishString(FName Channel, const FString& Value) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::String; + C.Text = Value; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::PublishStructBytes(FName Channel, const TArray& PackedMsgpackMap) +{ + FMjUserChannel C; + C.Name = MakeChannelName(Channel); + C.Kind = EMjUserChannelKind::Struct; + C.Packed = PackedMsgpackMap; + StoreChannel(MoveTemp(C)); +} + +void UMjUserChannelComponent::DeclareInputChannel(FName Channel, EMjUserInputKind Kind) +{ + const FName Key = MakeChannelName(Channel); + bool bChanged = false; + { + FScopeLock Lock(&InputMutex); + const EMjUserChannelKind NewKind = ToChannelKind(Kind); + if (const EMjUserChannelKind* Existing = InputDecls.Find(Key)) + bChanged = (*Existing != NewKind); + else + bChanged = true; + InputDecls.Add(Key, NewKind); + } + // A new declared input is a structure change: ROS rebuilds its per-channel + // subscriptions on the StructureVersion bump. + if (bChanged) + { + if (AAMjManager* Mgr = ResolveManager()) + Mgr->GetStateCollector().MarkProducerCacheDirty(); + } +} + +bool UMjUserChannelComponent::GetDeclaredInputKind(FName Channel, EMjUserChannelKind& OutKind) const +{ + const FName Key = MakeChannelName(Channel); + FScopeLock Lock(&InputMutex); + if (const EMjUserChannelKind* Found = InputDecls.Find(Key)) + { + OutKind = *Found; + return true; + } + return false; +} + +void UMjUserChannelComponent::GetDeclaredInputChannels( + TArray>& Out) const +{ + FScopeLock Lock(&InputMutex); + Out.Reserve(Out.Num() + InputDecls.Num()); + for (const TPair& Pair : InputDecls) + Out.Add(Pair); +} + +bool UMjUserChannelComponent::ApplyInput(FName Channel, const FMjUserChannel& Value) +{ + const FName Key = MakeChannelName(Channel); + EMjUserChannelKind Declared; + { + FScopeLock Lock(&InputMutex); + const EMjUserChannelKind* Found = InputDecls.Find(Key); + if (!Found) + return false; // undeclared: not on the allowlist + Declared = *Found; + } + + // Kind-family check: a text value cannot fill a numeric channel or vice versa. + if (IsTextKind(Declared) != IsTextKind(Value.Kind)) + return false; + + FMjUserChannel Stored; + Stored.Name = Key; + Stored.Kind = Declared; + if (IsTextKind(Declared)) + Stored.Text = Value.Text; + else + Stored.Values = Value.Values; + + { + FScopeLock Lock(&InputMutex); + InputMailbox.Add(Key, MoveTemp(Stored)); + } + + // OnUserInput is a Blueprint delegate; broadcast on the game thread so graphs + // never run on a transport thread. + TWeakObjectPtr WeakThis(this); + AsyncTask(ENamedThreads::GameThread, [WeakThis, Key]() { + if (UMjUserChannelComponent* Self = WeakThis.Get()) + Self->OnUserInput.Broadcast(Key); + }); + return true; +} + +bool UMjUserChannelComponent::GetInputBool(FName Channel, bool bDefault) const +{ + FScopeLock Lock(&InputMutex); + if (const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel))) + return C->Values.Num() > 0 && C->Values[0] != 0.0; + return bDefault; +} + +double UMjUserChannelComponent::GetInputFloat(FName Channel, double Default) const +{ + FScopeLock Lock(&InputMutex); + if (const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel))) + return C->Values.Num() > 0 ? C->Values[0] : Default; + return Default; +} + +FVector UMjUserChannelComponent::GetInputVector(FName Channel, bool bConvertToUESpace) const +{ + FScopeLock Lock(&InputMutex); + const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel)); + if (!C || C->Values.Num() < 3) + return FVector::ZeroVector; + const double V[3] = {C->Values[0], C->Values[1], C->Values[2]}; + return bConvertToUESpace ? URLabAxisConv::MjPositionToUe(V) + : FVector(V[0], V[1], V[2]); +} + +FTransform UMjUserChannelComponent::GetInputTransform(FName Channel, bool bConvertToUESpace) const +{ + FScopeLock Lock(&InputMutex); + const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel)); + if (!C || C->Values.Num() < 7) + return FTransform::Identity; + const double P[3] = {C->Values[0], C->Values[1], C->Values[2]}; + const double Q[4] = {C->Values[3], C->Values[4], C->Values[5], C->Values[6]}; // wxyz + if (bConvertToUESpace) + return FTransform(URLabAxisConv::MjQuatToUe(Q), URLabAxisConv::MjPositionToUe(P)); + return FTransform(FQuat(Q[1], Q[2], Q[3], Q[0]), FVector(P[0], P[1], P[2])); +} + +TArray UMjUserChannelComponent::GetInputFloatArray(FName Channel) const +{ + FScopeLock Lock(&InputMutex); + if (const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel))) + return C->Values; + return TArray(); +} + +FString UMjUserChannelComponent::GetInputString(FName Channel) const +{ + FScopeLock Lock(&InputMutex); + if (const FMjUserChannel* C = InputMailbox.Find(MakeChannelName(Channel))) + return C->Text; + return FString(); +} + +void UMjUserChannelComponent::DescribeState(FMjArticulationState& Out) const +{ + CopyMailboxInto(Out.UserChannels); +} + +void UMjUserChannelComponent::DescribeSceneState(FMjStateSnapshot& Out) const +{ + CopyMailboxInto(Out.UserChannels); +} + +void UMjUserChannelComponent::CopyMailboxInto(TArray& OutChannels) const +{ + FScopeLock Lock(&MailboxMutex); + OutChannels.Reserve(OutChannels.Num() + Mailbox.Num()); + for (const TPair& Pair : Mailbox) + OutChannels.Add(Pair.Value); +} + +AAMjManager* UMjUserChannelComponent::ResolveManager() +{ + if (AAMjManager* M = CachedManager.Get()) + return M; + AAMjManager* M = AAMjManager::GetManager(); + CachedManager = M; + return M; +} + +void UMjUserChannelComponent::BeginPlay() +{ + Super::BeginPlay(); + if (AAMjManager* Mgr = ResolveManager()) + Mgr->RegisterStateProducer(this); +} + +void UMjUserChannelComponent::EndPlay(const EEndPlayReason::Type EndPlayReason) +{ + if (AAMjManager* Mgr = CachedManager.Get()) + Mgr->UnregisterStateProducer(this); + Super::EndPlay(EndPlayReason); +} diff --git a/Source/URLab/Public/State/MjCanonicalName.h b/Source/URLab/Public/State/MjCanonicalName.h new file mode 100644 index 00000000..c0c65666 --- /dev/null +++ b/Source/URLab/Public/State/MjCanonicalName.h @@ -0,0 +1,52 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +class AMjArticulation; + +/** + * The single owner of every name assembled or stripped for the state IR. A + * canonical segment is legal as a ROS name segment, a tf2 frame_id, and a future + * URDF link/joint name, so the tf2 tree a later phase publishes drops in with no + * renaming. + */ +struct URLAB_API FMjCanonicalName +{ + /** Replace every char outside [A-Za-z0-9_] with '_', prefixing '_' when the + * first char is a digit. Empty input returns empty. */ + static FString Sanitize(const FString& Segment); + + /** Canonical articulation segment. If a stable LogicalId ever lands it swaps + * in here and nowhere else. */ + static FName ArtSegment(const AMjArticulation* Art); + + /** Canonical part segment: the compiled name with the compile-time + * "_" prefix stripped, then sanitized. This is the one place that + * strip happens. Art may be null (falls back to Sanitize(MjName)). */ + static FName PartSegment(const AMjArticulation* Art, const FString& MjName); + + /** "/" (e.g. "go2/imu_gyro"). */ + static FString Full(FName Art, FName Part); +}; diff --git a/Source/URLab/Public/State/MjMsgpackEncoder.h b/Source/URLab/Public/State/MjMsgpackEncoder.h new file mode 100644 index 00000000..f901ee2d --- /dev/null +++ b/Source/URLab/Public/State/MjMsgpackEncoder.h @@ -0,0 +1,57 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "Dom/JsonObject.h" +#include "State/MjObservationLevel.h" + +struct FMjStateSnapshot; + +/** + * The msgpack encoder: IR -> FJsonObject (packed to bytes via FURLabMsgpackUtil). + * One writer serves both consumers -- the streamed state/full snapshot packs the + * object to bytes, and RPC step replies embed the same arts/scene objects in the + * reply tree. + * + * Canonical schema. The full snapshot is + * { op:"state_full", time, step, sim_time, wall_time, arts, scene }. + * Per articulation, by observation level: + * minimal -> { qpos, qvel } + * standard -> +{ ctrl, act, sensors:{name:[...]} } + * full -> +{ bodies:{name:{xpos,xquat}}, actuator_force } + * twist -> { twist:{linear,angular}, actions } (whenever a twist controller + * is attached; independent of level) + * The scene block is { name:{ xpos, xquat, [qpos, qvel] } } for free-based props. + */ +class URLAB_API FMjMsgpackEncoder +{ +public: + static TSharedPtr EncodeSnapshot(const FMjStateSnapshot& S, + EObservationLevel Level); + static TSharedPtr EncodeArts(const FMjStateSnapshot& S, + EObservationLevel Level); + static TSharedPtr EncodeScene(const FMjStateSnapshot& S); + static TArray EncodeSnapshotBytes(const FMjStateSnapshot& S, + EObservationLevel Level); +}; diff --git a/Source/URLab/Public/State/MjObservationLevel.h b/Source/URLab/Public/State/MjObservationLevel.h new file mode 100644 index 00000000..3d95393d --- /dev/null +++ b/Source/URLab/Public/State/MjObservationLevel.h @@ -0,0 +1,28 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "CoreMinimal.h" + +/** Observation verbosity for step replies and state snapshots. + * minimal = qpos + qvel; + * standard = +ctrl + act + sensors; + * full = +body xpos/xquat + actuator forces. */ +enum class EObservationLevel : uint8 +{ + Minimal, + Standard, + Full +}; diff --git a/Source/URLab/Public/State/MjStateCollector.h b/Source/URLab/Public/State/MjStateCollector.h new file mode 100644 index 00000000..a428dba9 --- /dev/null +++ b/Source/URLab/Public/State/MjStateCollector.h @@ -0,0 +1,115 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "State/MjStateTypes.h" +#include + +class AAMjManager; +class AMjArticulation; +class UMjComponent; +class UMjTwistController; +struct mjModel_; +struct mjData_; +typedef mjModel_ mjModel; +typedef mjData_ mjData; + +/** + * The one concrete collector that builds the per-step IR. No producer interface, + * no registration: it has exactly three fixed steps. + * + * - Producer cache (game thread): per articulation, the canonical name and weak + * ptrs to its UMjComponents; rebuilt on the same triggers as the old broadcast + * cache (registry change / recompile / stale weak ptr). Bumps StructureVersion. + * - Per-step build (physics thread, post-step): reset the persistent snapshot, + * stamp time/step/clock, call DescribeState on each cached component and the + * twist controller, then fill Entities from the manager's entity cache indexing + * mjData directly. + * - On-demand build (RPC step replies): the same Collect(), called under the + * engine's CallbackMutex by the dispatcher. + * + * Collect() reuses one persistent snapshot and returns a reference to it. Every + * caller holds the engine's CallbackMutex (the post-step fan-out runs inside it; + * the reply paths acquire it explicitly), so builds never overlap and the single + * buffer is safe. + */ +class URLAB_API FMjStateCollector +{ +public: + void Init(AAMjManager* InManager); + + /** Mark the producer cache stale (registry change / recompile). A rebuild is + * scheduled on the game thread; safe to call from any thread. */ + void MarkProducerCacheDirty(); + + /** Rebuild the producer cache from the manager's live articulation list. + * Game thread only (walks components). Bumps StructureVersion. */ + void RebuildProducerCacheGameThread(); + + /** Build the IR for the current step and return the persistent snapshot. */ + const FMjStateSnapshot& Collect(mjModel* m, mjData* d, int64 StepIdx); + + uint32 GetStructureVersion() const { return StructureVersion; } + +private: + struct FCachedArticulation + { + FName ArtSegment; + TWeakObjectPtr Art; + TArray> Producers; // DescribeState per step + TWeakObjectPtr TwistCtrl; // UActorComponent; called separately + /** IMjStateProducer implementers registered under this art (e.g. user + * channel components). Any UObject; the collector Casts to the interface. */ + TArray> InterfaceProducers; + }; + + // A non-robot geom resolved once on the game thread (shape is static); the + // physics thread only reads the parent body's live pose each step. + struct FCachedWorldGeom + { + FName Name; + EMjWorldGeomShape Shape = EMjWorldGeomShape::Box; + double Size[3] = {0.0, 0.0, 0.0}; + int32 BodyId = 0; + double LocalPos[3] = {0.0, 0.0, 0.0}; + double LocalQuat[4] = {1.0, 0.0, 0.0, 0.0}; + bool bStatic = true; + TSharedPtr Mesh; // set when Shape == Mesh + }; + + TWeakObjectPtr Manager; + TArray Cache; // built game thread, read physics thread + TArray WorldGeomCache; // built game thread, read physics thread + /** Registered IMjStateProducers not owned by any articulation; their + * DescribeSceneState fills the snapshot's scene-scoped blocks. */ + TArray> SceneProducers; + FCriticalSection CacheMutex; + std::atomic bCacheValid{false}; + std::atomic bRebuildScheduled{false}; + FMjStateSnapshot Snapshot; // persistent; Reset() keeps top-level capacity + uint32 StructureVersion = 0; + + /** Post a game-thread rebuild if one is not already queued. */ + void RequestGameThreadRebuild(); +}; diff --git a/Source/URLab/Public/State/MjStateConsumer.h b/Source/URLab/Public/State/MjStateConsumer.h new file mode 100644 index 00000000..03b4d189 --- /dev/null +++ b/Source/URLab/Public/State/MjStateConsumer.h @@ -0,0 +1,53 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +struct FMjStateSnapshot; + +/** + * @class IMjStateConsumer + * @brief The transport-agnostic seam a typed consumer of the per-step state IR + * implements to receive the full snapshot after the collector builds it. + * + * Plain C++ abstract class (NOT a UE UINTERFACE) so multiple inheritance with a + * UCLASS transport is straightforward, mirroring IMjSnapshotPublisher. Where + * IMjSnapshotPublisher receives already-encoded msgpack bytes, a state consumer + * receives the typed FMjStateSnapshot and does its own encoding; it is the seam + * an out-of-core encoder (e.g. an optional message-bus module) registers against + * instead of the manager naming the concrete transport type. + * + * ConsumeState runs on the physics thread inside the engine's CallbackMutex, + * once per step, from AAMjManager::FanOutStateSnapshot. It is called in every + * step mode regardless of the byte-fan-out pause, so a distinct consumer keeps + * receiving state while Direct / Puppet mode suppresses the msgpack streams. + */ +class URLAB_API IMjStateConsumer +{ +public: + virtual ~IMjStateConsumer() = default; + + /** Consume one per-step state snapshot. Must be fast and non-blocking. */ + virtual void ConsumeState(const FMjStateSnapshot& Snapshot) = 0; +}; diff --git a/Source/URLab/Public/State/MjStateProducer.h b/Source/URLab/Public/State/MjStateProducer.h new file mode 100644 index 00000000..e624cbad --- /dev/null +++ b/Source/URLab/Public/State/MjStateProducer.h @@ -0,0 +1,64 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "UObject/Interface.h" +#include "MjStateProducer.generated.h" + +struct FMjArticulationState; +struct FMjStateSnapshot; + +UINTERFACE(MinimalAPI) +class UMjStateProducer : public UInterface +{ + GENERATED_BODY() +}; + +/** + * The seam any UObject implements to contribute typed values to the per-step + * state IR. It is transport-agnostic: producers fill the IR and never know which + * encoders (msgpack, ...) read it afterwards. + * + * Both methods run on the physics thread inside the engine's CallbackMutex, the + * same contract UMjComponent::DescribeState has today. Implementers must not + * allocate UObjects, must not touch game-thread-only state, and must stay cheap; + * they run once per physics step. Values are exposed through atomics or a lock + * (see UMjUserChannelComponent's mailbox), not by reading arbitrary game state. + * + * Scope is resolved by the collector at cache-rebuild time on the game thread: a + * producer owned by (or attached under) an AMjArticulation contributes through + * DescribeState; any other producer contributes through DescribeSceneState. A + * given producer only ever has one of the two called per step. + */ +class URLAB_API IMjStateProducer +{ + GENERATED_BODY() + +public: + /** Art-scoped: called when the producer is cached under an articulation. */ + virtual void DescribeState(FMjArticulationState& Out) const {} + + /** Scene-scoped: called for producers not owned by any articulation. */ + virtual void DescribeSceneState(FMjStateSnapshot& Out) const {} +}; diff --git a/Source/URLab/Public/State/MjStateTypes.h b/Source/URLab/Public/State/MjStateTypes.h new file mode 100644 index 00000000..a2703c0d --- /dev/null +++ b/Source/URLab/Public/State/MjStateTypes.h @@ -0,0 +1,289 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "MuJoCo/Components/Joints/MjJoint.h" // EMjJointType + +/** + * Semantically-typed intermediate representation of one physics step. Producers + * declare into it once (UMjComponent::DescribeState); encoders read it. There is + * one IR and, downstream, two encoders: msgpack (state/full + step replies) and, + * later, ROS. Semantics and grouping are first-class because typed ROS messages + * need them; msgpack ignores the extra typing harmlessly. + * + * Names are FName so the per-step copies the collector makes are index-cheap and + * never allocate. Numeric slots are double so no precision is lost relative to + * mjData's mjtNum. + */ + +/** + * Coarse grouping derived from EMjSensorType. It is what lets a ROS publisher + * pair gyro+accel into one Imu and route pose-like sensors to tf2. msgpack does + * not read it. + */ +enum class EMjSensorSemantic : uint8 +{ + Generic, + Gyro, + Accel, + Velocity, + Force, + Torque, + Touch, + Rangefinder, + Magnetometer, + JointPos, + JointVel, + ActuatorPos, + ActuatorVel, + ActuatorFrc, + FramePos, + FrameQuat, + FrameAxis, + FrameLinVel, + FrameAngVel, + FrameLinAcc, + FrameAngAcc, + SubtreeCom, + SubtreeLinVel, + SubtreeAngMom, + Clock +}; + +/** + * Sim + wall clock, stored as ROS builtin_interfaces/Time sec/nsec pairs so the + * projection to the wire matches AppendClockFields exactly. int32 sec + int32 + * nsec both fit a double's 2^53 mantissa; wall seconds since epoch need int64. + */ +struct FMjClock +{ + int32 SimSec = 0; + int32 SimNsec = 0; + int64 WallSec = 0; + int64 WallNsec = 0; +}; + +/** One joint's position/velocity slices. Slot widths follow the joint type. */ +struct FMjJointState +{ + FName Name; + EMjJointType Type = EMjJointType::Hinge; + TArray QPos; + TArray QVel; + /** The joint's qpos0 (reference) slice. Filled for 1-DOF joints (hinge/slide) + * so the ROS /joint_states shift can emit `qpos - qpos0`, matching the URDF + * zero pose (URDF q=0 == MuJoCo qpos0). Empty means no shift. */ + TArray RefPos; + + void Reset() + { + Name = FName(); + QPos.Reset(); + QVel.Reset(); + RefPos.Reset(); + } +}; + +/** One actuator's control setpoint, activation state, and applied force. */ +struct FMjActuatorState +{ + FName Name; + // Canonical segment of the joint this actuator drives, for joint-transmission + // actuators; None for tendon/site/body transmissions. Lets consumers map an + // actuator to the URDF/JointState joint it commands without the actuator's own + // name having to match the joint's. + FName TargetJoint; + double Ctrl = 0.0; + double Act = 0.0; + double Force = 0.0; +}; + +/** One sensor's reading, as raw MuJoCo SI values (MuJoCo frame, double + * precision) copied straight from d->sensordata. The MuJoCo -> UE coordinate/ + * unit transform lives on the display-facing UMjSensor::GetReading() accessor, + * not here; encoders own their own target conventions. */ +struct FMjSensorState +{ + FName Name; + EMjSensorSemantic Semantic = EMjSensorSemantic::Generic; + TArray Values; + + void Reset() + { + Name = FName(); + Semantic = EMjSensorSemantic::Generic; + Values.Reset(); + } +}; + +/** One body's world pose. The body name is its own tf2 frame id. */ +struct FMjBodyState +{ + FName Name; + double Xpos[3] = {0.0, 0.0, 0.0}; + double Xquat[4] = {1.0, 0.0, 0.0, 0.0}; +}; + +/** A twist command (linear + angular) plus the active-action bitmask. */ +struct FMjTwistState +{ + double Linear[3] = {0.0, 0.0, 0.0}; + double Angular[3] = {0.0, 0.0, 0.0}; + int32 Actions = 0; +}; + +/** Kind tag for one user channel value. Closed set; encoders switch on it. */ +enum class EMjUserChannelKind : uint8 +{ + Bool, // Values[0] != 0.0 + Int, // Values[0], integral + Scalar, // Values[0] + Vec3, // Values[0..2] + Quat, // Values[0..3], wxyz (MuJoCo order, matching xquat) + Transform, // Values[0..2] pos, Values[3..6] quat wxyz + Array, // Values[0..N-1], free length + String, // Text + Struct // Packed: a msgpack-map blob converted at publish time +}; + +/** + * One user-declared payload value. Scoped by which container holds it + * (FMjArticulationState = art scope, FMjStateSnapshot = scene scope). At most one + * of Values / Text / Packed is populated per kind; the empty fields cost two idle + * TArrays, negligible at the unit-to-tens channel counts this targets. + * + * Spatial kinds (Vec3/Quat/Transform) carry raw MuJoCo SI values (metres, wxyz), + * converted from UE space in the publish path so the IR stays transport-neutral + * and matches every other pose in the snapshot. + */ +struct FMjUserChannel +{ + FName Name; // sanitized, unique within its scope + EMjUserChannelKind Kind = EMjUserChannelKind::Scalar; + TArray Values; // numeric kinds + FString Text; // String kind + TArray Packed; // Struct kind: msgpack map bytes +}; + +/** All per-step state for one articulation, grouped by element kind. */ +struct FMjArticulationState +{ + FName Name; // canonical art segment + TArray Joints; + TArray Actuators; + TArray Sensors; + TArray Bodies; + TOptional Twist; + TArray UserChannels; // art-scoped user payloads + + void Reset() + { + Name = FName(); + Joints.Reset(); + Actuators.Reset(); + Sensors.Reset(); + Bodies.Reset(); + Twist.Reset(); + UserChannels.Reset(); + } +}; + +/** + * A non-articulation dynamic body (prop, free-jointed scene object). These are + * raw MjIds with no owning component, so the collector fills them directly from + * mjData rather than through a DescribeState producer. + */ +struct FMjEntityState +{ + FName Name; + double Xpos[3] = {0.0, 0.0, 0.0}; + double Xquat[4] = {1.0, 0.0, 0.0, 0.0}; + bool bFreeBase = false; + TArray QPos; + TArray QVel; +}; + +/** + * The whole per-step snapshot. Built full every step; the msgpack encoder omits + * blocks per the requested observation level. StructureVersion bumps whenever + * the producer set changes so consumers can cache derived state (key tables, ROS + * publisher handles) and invalidate only on a registry change. + */ +// A non-robot collision shape in the world (obstacle / table / manipulable +// object). Poses are world-frame; shapes are a primitive or a full mesh +// (FMjWorldMesh). Robot links are excluded (they reach ROS via the URDF). +enum class EMjWorldGeomShape : uint8 +{ + Box, // Size = half-extents (x, y, z) + Sphere, // Size = (radius, _, _) + Cylinder, // Size = (radius, half-height, _) + Mesh, // triangle geometry in Mesh; Size unused +}; + +// Triangle mesh in the geom-local frame (vertices as stored by MuJoCo's compiled +// mesh_vert, which already fold in the mesh centring so composing them with the +// geom's world pose reproduces the authored shape). Shared so the per-step +// snapshot carries only a ref-counted pointer, not a vertex copy. +struct FMjWorldMesh +{ + TArray Verts; // geom-local vertex positions + TArray Tris; // 3 vertex indices per triangle, flattened +}; + +struct FMjWorldGeom +{ + FName Name; + EMjWorldGeomShape Shape = EMjWorldGeomShape::Box; + double Size[3] = {0.0, 0.0, 0.0}; + double Xpos[3] = {0.0, 0.0, 0.0}; + double Xquat[4] = {1.0, 0.0, 0.0, 0.0}; // world orientation, wxyz + bool bStatic = true; // worldbody-fixed vs movable + TSharedPtr Mesh; // set when Shape == Mesh +}; + +struct FMjStateSnapshot +{ + double Time = 0.0; + int64 Step = 0; + FMjClock Clock; + uint32 StructureVersion = 0; + TArray Articulations; + TArray Entities; + TArray UserChannels; // scene-scoped user payloads + TArray WorldGeoms; // non-robot collision geometry + + /** Clears the payload while keeping the top-level array capacity so the + * steady-state per-step build does not reallocate the outer arrays. */ + void Reset() + { + Time = 0.0; + Step = 0; + Clock = FMjClock(); + StructureVersion = 0; + Articulations.Reset(); + Entities.Reset(); + UserChannels.Reset(); + WorldGeoms.Reset(); + } +}; diff --git a/Source/URLab/Public/UserChannels/MjUserChannelComponent.h b/Source/URLab/Public/UserChannels/MjUserChannelComponent.h new file mode 100644 index 00000000..ade98639 --- /dev/null +++ b/Source/URLab/Public/UserChannels/MjUserChannelComponent.h @@ -0,0 +1,195 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "Components/ActorComponent.h" +#include "State/MjStateProducer.h" +#include "State/MjStateTypes.h" +#include "MjUserChannelComponent.generated.h" + +class AAMjManager; + +/** Blueprint-facing kind selector for declaring an input channel. Maps 1:1 onto + * the IR's EMjUserChannelKind (Struct is not an input kind in v1). */ +UENUM(BlueprintType) +enum class EMjUserInputKind : uint8 +{ + Bool, + Int, + Scalar, + Vec3, + Quat, + Transform, + Array, + String +}; + +/** Fires on the game thread when a declared input channel receives a value. */ +DECLARE_DYNAMIC_MULTICAST_DELEGATE_OneParam(FMjUserInputReceived, FName, Channel); + +/** + * The single authoring surface for user-declared payload channels, for Blueprint + * and convenience C++ alike. Drop it on an actor and call a Publish* node; the + * value flows out over every enabled byte transport (ZMQ PUB, SHM ring, RPC step + * replies) with no per-transport work, because the channel lives in the state IR. + * + * Threading. Publish* runs on the game thread (or any thread) and writes a + * double-mailbox TMap under a light lock. DescribeState / DescribeSceneState run + * on the physics thread and copy the latest mailbox values into the IR under the + * same lock. Values are sample-and-hold: physics steps between publishes re-emit + * the last value. + * + * Scope by attachment. On an AMjArticulation actor the channels land in that + * art's block (arts//user); on any other actor they land in the scene block + * (top-level user). The collector resolves this at cache-rebuild time, so this + * component implements both producer entry points and fills whichever the + * collector hands it. + * + * Spatial kinds convert UE space (cm, left-handed) to raw MuJoCo SI (metres, + * wxyz) in the publish path when bConvertFromUESpace is set, keeping the IR + * consistent with every other pose in the snapshot. + */ +UCLASS(ClassGroup = (URLab), meta = (BlueprintSpawnableComponent)) +class URLAB_API UMjUserChannelComponent : public UActorComponent + , public IMjStateProducer +{ + GENERATED_BODY() + +public: + UMjUserChannelComponent(); + + // --- Output: publish a value into a named channel (game thread typical) --- + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishBool(FName Channel, bool bValue); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishInt(FName Channel, int64 Value); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishFloat(FName Channel, double Value); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishVector(FName Channel, FVector Value, bool bConvertFromUESpace = true); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishQuat(FName Channel, FQuat Value, bool bConvertFromUESpace = true); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishTransform(FName Channel, FTransform Value, bool bConvertFromUESpace = true); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishFloatArray(FName Channel, const TArray& Values); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void PublishString(FName Channel, const FString& Value); + + /** Publish a Struct-kind channel from pre-packed msgpack-map bytes. The + * encoder splices the map verbatim into the snapshot. This is the C++ entry + * for the Struct kind; the wildcard-pin Blueprint PublishStruct (reflection + * packing via FJsonObjectConverter) is the P8 follow-up. */ + void PublishStructBytes(FName Channel, const TArray& PackedMsgpackMap); + + // --- Input: declare + read a named channel (game thread typical) --- + + /** Declare an input channel so its topic / RPC allowlist entry exists before + * data can flow. Input channels must be declared (unlike lazy outputs): ROS + * needs the kind to create a subscription, and the declaration is the allowlist + * that stops writes into undeclared names. Redeclaring updates the kind. */ + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + void DeclareInputChannel(FName Channel, EMjUserInputKind Kind); + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + bool GetInputBool(FName Channel, bool bDefault = false) const; + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + double GetInputFloat(FName Channel, double Default = 0.0) const; + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + FVector GetInputVector(FName Channel, bool bConvertToUESpace = true) const; + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + FTransform GetInputTransform(FName Channel, bool bConvertToUESpace = true) const; + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + TArray GetInputFloatArray(FName Channel) const; + + UFUNCTION(BlueprintCallable, Category = "URLab|User Channels") + FString GetInputString(FName Channel) const; + + /** Broadcast on the game thread whenever a declared input channel is written by + * any transport (the set_user_channels RPC op or a ROS subscription). */ + UPROPERTY(BlueprintAssignable, Category = "URLab|User Channels") + FMjUserInputReceived OnUserInput; + + /** Look up a declared input channel's kind. Returns false if not declared. + * Thread-safe; used by the manager's input router and the ROS subscription + * builder. */ + bool GetDeclaredInputKind(FName Channel, EMjUserChannelKind& OutKind) const; + + /** Copy the declared input channels out (thread-safe). */ + void GetDeclaredInputChannels(TArray>& Out) const; + + /** Apply an inbound value to a declared input channel from any transport thread. + * Validates the channel is declared and the value is compatible with its + * declared kind, stores it in the input mailbox under the declared kind, and + * queues an OnUserInput broadcast on the game thread. Returns false when the + * channel is undeclared or the value's kind is incompatible. */ + bool ApplyInput(FName Channel, const FMjUserChannel& Value); + + // --- IMjStateProducer: physics thread, under the engine CallbackMutex --- + virtual void DescribeState(FMjArticulationState& Out) const override; + virtual void DescribeSceneState(FMjStateSnapshot& Out) const override; + +protected: + virtual void BeginPlay() override; + virtual void EndPlay(const EEndPlayReason::Type EndPlayReason) override; + +private: + /** Sanitize a raw channel name once into a canonical FName (msgpack key and, + * later, ROS topic segment are the same string on every transport). */ + static FName MakeChannelName(FName Raw); + + /** Store a channel into the mailbox; marks the producer cache dirty when the + * channel set changes (new name or kind change) so consumers rebuild. */ + void StoreChannel(FMjUserChannel&& Channel); + + /** Copy the mailbox into OutChannels under MailboxMutex. Shared by + * DescribeState (articulation-scoped) and DescribeSceneState (scene-scoped). */ + void CopyMailboxInto(TArray& OutChannels) const; + + /** Resolve (and cache) the owning MuJoCo manager. */ + AAMjManager* ResolveManager(); + + mutable FCriticalSection MailboxMutex; + TMap Mailbox; + TWeakObjectPtr CachedManager; + + /** Declared input channels (allowlist + kind) and the latest received value per + * channel. Guarded by InputMutex; written by transport threads, read on the + * game thread by the GetInput* nodes. */ + mutable FCriticalSection InputMutex; + TMap InputDecls; + TMap InputMailbox; +}; From d5ba79aa3274d3df82f3c1f161c24d349caae623 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:39 +0100 Subject: [PATCH 06/32] Add ROS 2 as an optional module over the state IR URLabRos links the rcl C API directly -- no rclcpp, no shim -- and registers providers that read the same IR the other transports do. JointState, IMU, twist, tf2, clock, camera image, odometry, point cloud, occupancy grid and octomap. The core never references ROS, so the plugin builds and runs without it. --- Source/URLab/Private/Urdf/UrdfExporter.cpp | 736 +++++ Source/URLab/Public/Urdf/UrdfExporter.h | 150 + Source/URLab/URLab.Build.cs | 2 + Source/URLabRos/Private/Ros/UrlabRclCore.cpp | 2609 +++++++++++++++++ Source/URLabRos/Private/Ros/UrlabRclCore.h | 343 +++ .../Providers/RosCameraInfoProvider.cpp | 135 + .../Transport/Providers/RosClockProvider.cpp | 50 + .../Transport/Providers/RosImuProvider.cpp | 97 + .../Providers/RosJointStateProvider.cpp | 97 + .../Providers/RosOccupancyGridProvider.cpp | 246 ++ .../Providers/RosOctomapProvider.cpp | 386 +++ .../Providers/RosOdomFramesProvider.cpp | 67 + .../Providers/RosOdometryProvider.cpp | 101 + .../Providers/RosPlanningSceneProvider.cpp | 221 ++ .../Providers/RosPointCloudProvider.cpp | 363 +++ .../Transport/Providers/RosPoseProvider.cpp | 85 + .../Transport/Providers/RosProviderCommon.h | 52 + .../Providers/RosRobotDescriptionProvider.cpp | 72 + .../Transport/Providers/RosSensorProvider.cpp | 199 ++ .../Transport/Providers/RosTfProvider.cpp | 68 + .../Transport/Providers/RosTwistProvider.cpp | 83 + .../Providers/RosUserChannelProvider.cpp | 243 ++ .../URLabRos/Private/Transport/RosContext.cpp | 88 + .../Private/Transport/RosOutputProvider.cpp | 693 +++++ .../Private/Transport/RosPublishTransport.cpp | 300 ++ .../Private/Transport/RosRpcTransport.cpp | 766 +++++ .../Private/Transport/RosSensorRouting.cpp | 129 + .../Private/Transport/RosStateEstimation.cpp | 129 + Source/URLabRos/Private/URLabRos.cpp | 187 ++ Source/URLabRos/Private/URLabRosLog.h | 30 + Source/URLabRos/Public/Transport/RosContext.h | 85 + .../Public/Transport/RosOutputProvider.h | 250 ++ .../Public/Transport/RosPublishTransport.h | 155 + .../Public/Transport/RosRpcTransport.h | 193 ++ .../Public/Transport/RosSensorRouting.h | 84 + .../Public/Transport/RosStateEstimation.h | 75 + Source/URLabRos/Public/URLabRos.h | 43 + Source/URLabRos/URLabRos.Build.cs | 246 ++ UnrealRoboticsLab.uplugin | 5 + ros/urlab_jog/.gitignore | 2 + ros/urlab_jog/README.md | 109 + ros/urlab_jog/launch/franka_jog.launch.py | 161 + ros/urlab_jog/rviz/franka.rviz | 86 + ros/urlab_ros_ws/.gitignore | 4 + ros/urlab_ros_ws/CMakeLists.txt | 99 + ros/urlab_ros_ws/build_and_test.ps1 | 161 + ros/urlab_ros_ws/build_and_test.sh | 123 + ros/urlab_ros_ws/scripts/test_maps.py | 180 ++ ros/urlab_ros_ws/src/test_main.cpp | 464 +++ 49 files changed, 11252 insertions(+) create mode 100644 Source/URLab/Private/Urdf/UrdfExporter.cpp create mode 100644 Source/URLab/Public/Urdf/UrdfExporter.h create mode 100644 Source/URLabRos/Private/Ros/UrlabRclCore.cpp create mode 100644 Source/URLabRos/Private/Ros/UrlabRclCore.h create mode 100644 Source/URLabRos/Private/Transport/Providers/RosCameraInfoProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosClockProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosImuProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosJointStateProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosOccupancyGridProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosOctomapProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosOdomFramesProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosOdometryProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosPlanningSceneProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosPointCloudProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosPoseProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosProviderCommon.h create mode 100644 Source/URLabRos/Private/Transport/Providers/RosRobotDescriptionProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosSensorProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosTfProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosTwistProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/Providers/RosUserChannelProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/RosContext.cpp create mode 100644 Source/URLabRos/Private/Transport/RosOutputProvider.cpp create mode 100644 Source/URLabRos/Private/Transport/RosPublishTransport.cpp create mode 100644 Source/URLabRos/Private/Transport/RosRpcTransport.cpp create mode 100644 Source/URLabRos/Private/Transport/RosSensorRouting.cpp create mode 100644 Source/URLabRos/Private/Transport/RosStateEstimation.cpp create mode 100644 Source/URLabRos/Private/URLabRos.cpp create mode 100644 Source/URLabRos/Private/URLabRosLog.h create mode 100644 Source/URLabRos/Public/Transport/RosContext.h create mode 100644 Source/URLabRos/Public/Transport/RosOutputProvider.h create mode 100644 Source/URLabRos/Public/Transport/RosPublishTransport.h create mode 100644 Source/URLabRos/Public/Transport/RosRpcTransport.h create mode 100644 Source/URLabRos/Public/Transport/RosSensorRouting.h create mode 100644 Source/URLabRos/Public/Transport/RosStateEstimation.h create mode 100644 Source/URLabRos/Public/URLabRos.h create mode 100644 Source/URLabRos/URLabRos.Build.cs create mode 100644 ros/urlab_jog/.gitignore create mode 100644 ros/urlab_jog/README.md create mode 100644 ros/urlab_jog/launch/franka_jog.launch.py create mode 100644 ros/urlab_jog/rviz/franka.rviz create mode 100644 ros/urlab_ros_ws/.gitignore create mode 100644 ros/urlab_ros_ws/CMakeLists.txt create mode 100644 ros/urlab_ros_ws/build_and_test.ps1 create mode 100644 ros/urlab_ros_ws/build_and_test.sh create mode 100644 ros/urlab_ros_ws/scripts/test_maps.py create mode 100644 ros/urlab_ros_ws/src/test_main.cpp diff --git a/Source/URLab/Private/Urdf/UrdfExporter.cpp b/Source/URLab/Private/Urdf/UrdfExporter.cpp new file mode 100644 index 00000000..23721908 --- /dev/null +++ b/Source/URLab/Private/Urdf/UrdfExporter.cpp @@ -0,0 +1,736 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Urdf/UrdfExporter.h" + +#include "State/MjCanonicalName.h" +#include "HAL/FileManager.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.h" + +#include "mujoco/mujoco.h" + +#include + +namespace +{ +// Match the Python prototype's %.9g formatting so numbers round-trip against the +// validated reference URDF. +FString FmtNum(double X) +{ + return FString::Printf(TEXT("%.9g"), X); +} + +FString Xyz(const FVector& V) +{ + return FString::Printf(TEXT("%s %s %s"), *FmtNum(V.X), *FmtNum(V.Y), *FmtNum(V.Z)); +} + +// MuJoCo quaternion (w,x,y,z) -> URDF rpy (fixed-axis / extrinsic XYZ), matching +// urdfdom's Rotation::getRPY. +FVector QuatWxyzToRpy(const mjtNum* Q) +{ + double W = Q[0], X = Q[1], Y = Q[2], Z = Q[3]; + const double Nrm = std::sqrt(W * W + X * X + Y * Y + Z * Z); + if (Nrm == 0.0) + return FVector::ZeroVector; + W /= Nrm; X /= Nrm; Y /= Nrm; Z /= Nrm; + + const double SinrCosp = 2.0 * (W * X + Y * Z); + const double CosrCosp = 1.0 - 2.0 * (X * X + Y * Y); + const double Roll = std::atan2(SinrCosp, CosrCosp); + + const double Sinp = 2.0 * (W * Y - Z * X); + const double Pitch = (std::abs(Sinp) >= 1.0) + ? std::copysign(PI / 2.0, Sinp) + : std::asin(Sinp); + + const double SinyCosp = 2.0 * (W * Z + X * Y); + const double CosyCosp = 1.0 - 2.0 * (Y * Y + Z * Z); + const double Yaw = std::atan2(SinyCosp, CosyCosp); + return FVector(Roll, Pitch, Yaw); +} + +// Rotate a vector by a MuJoCo (w,x,y,z) quaternion, no normalization (mjModel +// stores unit quaternions), matching the prototype's quat_rotate. +FVector RotateByQuatWxyz(const mjtNum* Q, const FVector& V) +{ + const double W = Q[0], X = Q[1], Y = Q[2], Z = Q[3]; + const double Vx = V.X, Vy = V.Y, Vz = V.Z; + const double Tx = 2.0 * (Y * Vz - Z * Vy); + const double Ty = 2.0 * (Z * Vx - X * Vz); + const double Tz = 2.0 * (X * Vy - Y * Vx); + return FVector( + Vx + W * Tx + (Y * Tz - Z * Ty), + Vy + W * Ty + (Z * Tx - X * Tz), + Vz + W * Tz + (X * Ty - Y * Tx)); +} + +double AbsMax(double A, double B) +{ + return FMath::Max(FMath::Abs(A), FMath::Abs(B)); +} + +bool IsPlainMotor(const mjModel* M, int A) +{ + // force = gain * ctrl: fixed gain, no bias, no activation dynamics. + return M->actuator_biastype[A] == mjBIAS_NONE + && M->actuator_gaintype[A] == mjGAIN_FIXED + && M->actuator_dyntype[A] == mjDYN_NONE; +} + +bool IsVelocityActuator(const mjModel* M, int A) +{ + // mjcf : affine bias with biasprm = [0, 0, -kv], so ctrl is a + // velocity target. A position servo's affine bias is [0, -kp, -kv] instead. + if (M->actuator_biastype[A] != mjBIAS_AFFINE) + return false; + const mjtNum* Bp = &M->actuator_biasprm[A * mjNBIAS]; + return std::abs(Bp[1]) < 1e-12 && std::abs(Bp[2]) > 0.0; +} + +// Effort ladder: actuator force range limit, else summed bounded +// joint-transmission actuator contributions, else the config default. +double JointEffort(const mjModel* M, int J, const FUrdfExportConfig& Cfg, + const FString& JointName, TArray& Warnings) +{ + if (M->jnt_actfrclimited[J]) + return AbsMax(M->jnt_actfrcrange[2 * J + 0], M->jnt_actfrcrange[2 * J + 1]); + + double Total = 0.0; + bool bFound = false; + bool bUnbounded = false; + for (int A = 0; A < M->nu; ++A) + { + if (M->actuator_trntype[A] != mjTRN_JOINT || M->actuator_trnid[2 * A + 0] != J) + continue; + bFound = true; + const double Gear = std::abs(M->actuator_gear[A * 6 + 0]); + double B; + if (M->actuator_forcelimited[A]) + { + B = AbsMax(M->actuator_forcerange[2 * A + 0], M->actuator_forcerange[2 * A + 1]); + } + else if (M->actuator_ctrllimited[A] && IsPlainMotor(M, A)) + { + B = M->actuator_gainprm[A * mjNGAIN + 0] + * AbsMax(M->actuator_ctrlrange[2 * A + 0], M->actuator_ctrlrange[2 * A + 1]); + } + else + { + bUnbounded = true; + continue; + } + Total += Gear * B; + } + if (bFound && !bUnbounded && Total > 0.0) + return Total; + + Warnings.Add(FString::Printf( + TEXT("joint '%s': effort defaulted to %s (no bounded joint-transmission " + "actuator; tendon/site transmissions do not count)"), + *JointName, *FmtNum(Cfg.DefaultEffort))); + return Cfg.DefaultEffort; +} + +// Velocity ladder: override map, else a velocity-actuator ctrl range, else the +// type-specific config default. MuJoCo has no joint velocity limit, so a +// position-servo arm falls through to the default. +double JointVelocity(const mjModel* M, int J, bool bIsSlide, + const FUrdfExportConfig& Cfg, const FString& JointName, TArray& Warnings) +{ + if (const double* Override = Cfg.VelocityOverrides.Find(JointName)) + return *Override; + + for (int A = 0; A < M->nu; ++A) + { + if (M->actuator_trntype[A] != mjTRN_JOINT || M->actuator_trnid[2 * A + 0] != J) + continue; + if (IsVelocityActuator(M, A) && M->actuator_ctrllimited[A]) + return AbsMax(M->actuator_ctrlrange[2 * A + 0], M->actuator_ctrlrange[2 * A + 1]); + } + + const double Default = bIsSlide ? Cfg.DefaultVelocityLinear : Cfg.DefaultVelocityAngular; + Warnings.Add(FString::Printf( + TEXT("joint '%s': velocity defaulted to %s (MuJoCo has no joint velocity " + "limit; no velocity actuator found)"), + *JointName, *FmtNum(Default))); + return Default; +} +struct FMimicJointInfo +{ + FString LeaderName; + double Multiplier = 1.0; + double Offset = 0.0; +}; +} // namespace + +FString FUrdfExporter::MeshBaseName(const mjModel* M, int32 MeshId) +{ + const char* Raw = mj_id2name(const_cast(M), mjOBJ_MESH, MeshId); + const FString Name = Raw ? FString(UTF8_TO_TCHAR(Raw)) : FString::Printf(TEXT("mesh%d"), MeshId); + return FMjCanonicalName::Sanitize(Name); +} + +TArray FUrdfExporter::BodyIdsForArt(const mjModel* M, const FString& ArtRawName) +{ + TArray Out; + if (!M) + return Out; + const FString Prefix = ArtRawName + TEXT("_"); + for (int32 i = 1; i < M->nbody; ++i) + { + if (ArtRawName.IsEmpty()) + { + Out.Add(i); + continue; + } + const char* Raw = mj_id2name(const_cast(M), mjOBJ_BODY, i); + const FString Name = Raw ? FString(UTF8_TO_TCHAR(Raw)) : FString(); + if (Name == ArtRawName || Name.StartsWith(Prefix)) + Out.Add(i); + } + return Out; +} + +FUrdfModel FUrdfExporter::Build(const mjModel* M, const FString& RobotName, + const FString& ArtRawName, const TArray& BodyIds, + const FString& MeshUriDir, const FUrdfExportConfig& Cfg) +{ + FUrdfModel Model; + Model.RobotName = RobotName; + if (!M) + return Model; + + const FString Prefix = ArtRawName.IsEmpty() ? FString() : (ArtRawName + TEXT("_")); + + // Compiled name -> canonical URDF segment (art prefix stripped, sanitized), + // mirroring FMjCanonicalName::PartSegment without needing the AMjArticulation. + auto LocalName = [&Prefix](const char* Raw) -> FString + { + FString Local = Raw ? FString(UTF8_TO_TCHAR(Raw)) : FString(); + if (!Prefix.IsEmpty() && Local.StartsWith(Prefix)) + Local = Local.Mid(Prefix.Len()); + return FMjCanonicalName::Sanitize(Local); + }; + auto BodyName = [M, &LocalName](int i) -> FString + { + return LocalName(mj_id2name(const_cast(M), mjOBJ_BODY, i)); + }; + + // Absolute file:// URI for a mesh id; also records the mesh id for STL export. + auto MeshUri = [&](int32 MeshId) -> FString + { + Model.MeshIds.AddUnique(MeshId); + const FString File = MeshBaseName(M, MeshId) + TEXT(".stl"); + FString Full = FPaths::ConvertRelativePathToFull(FPaths::Combine(MeshUriDir, File)); + Full.ReplaceInline(TEXT("\\"), TEXT("/")); + return FString(TEXT("file://")) + Full; + }; + + const TSet BodySet(BodyIds); + + // Per-body frame offset from the jnt_pos anchor shift (single-joint case). + TArray Offset; + Offset.Init(FVector::ZeroVector, M->nbody); + + int32 RootCount = 0; + for (int32 i : BodyIds) + if (M->body_parentid[i] == 0) + ++RootCount; + if (BodyIds.Num() > 0 && RootCount != 1) + { + Model.Warnings.Add(FString::Printf( + TEXT("%d world-rooted bodies; single-URDF export handles the first, a " + "synthetic-root welded ensemble is needed for multi-root arts"), + RootCount)); + } + + // Build a map of follower joint -> mimic info from mjEQ_JOINT equality + // constraints. Only linear relationships (polycoef[2..4] == 0) map cleanly + // to a URDF ; anything higher-order is dropped with a warning. + TMap MimicMap; + for (int e = 0; e < M->neq; ++e) + { + if (M->eq_type[e] != mjEQ_JOINT) + continue; + const int FollowerMjId = M->eq_obj1id[e]; + const int LeaderMjId = M->eq_obj2id[e]; + const mjtNum* Data = &M->eq_data[e * mjNEQDATA]; + const double Poly2 = (mjNEQDATA > 2) ? Data[2] : 0.0; + const double Poly3 = (mjNEQDATA > 3) ? Data[3] : 0.0; + const double Poly4 = (mjNEQDATA > 4) ? Data[4] : 0.0; + if (std::abs(Poly2) > 1e-12 || std::abs(Poly3) > 1e-12 || std::abs(Poly4) > 1e-12) + { + const char* Raw = mj_id2name(const_cast(M), mjOBJ_JOINT, FollowerMjId); + Model.Warnings.Add(FString::Printf( + TEXT("joint equality '%s': higher-order polycoef dropped (URDF mimic " + "is linear only)"), + Raw ? UTF8_TO_TCHAR(Raw) : TEXT("?"))); + continue; + } + const char* LeaderRaw = mj_id2name(const_cast(M), mjOBJ_JOINT, LeaderMjId); + FMimicJointInfo& Info = MimicMap.FindOrAdd(FollowerMjId); + Info.LeaderName = LocalName(LeaderRaw); + Info.Multiplier = Data[1]; + Info.Offset = Data[0]; + } + + // Find the leaf body (no children in BodySet) for the optional tool0 frame. + int32 LeafBodyId = -1; + if (Cfg.bAppendTool0) + { + for (int32 i : BodyIds) + { + bool bHasChild = false; + for (int32 j : BodyIds) + { + if (M->body_parentid[j] == i) { bHasChild = true; break; } + } + if (!bHasChild) + LeafBodyId = i; + } + } + + TArray LinkXml; + TArray JointXml; + + // Emit one joint (real, fixed, or a chained dummy segment) into JointXml and + // Model.Joints. ForceFixed synthesizes the parentless-child fixed joint. + auto EmitJoint = [&](int J, const FString& ParentLink, const FString& ChildLink, + const FVector& OriginXyz, const FVector& OriginRpy, bool bForceFixed) + { + FUrdfJoint Joint; + Joint.Parent = ParentLink; + Joint.Child = ChildLink; + Joint.OriginPos = OriginXyz; + Joint.OriginRpy = OriginRpy; + + if (bForceFixed) + { + Joint.Type = TEXT("fixed"); + Joint.Name = FString::Printf(TEXT("%s__to__%s"), *ParentLink, *ChildLink); + } + else + { + Joint.MjJointId = J; + const char* Raw = mj_id2name(const_cast(M), mjOBJ_JOINT, J); + Joint.Name = LocalName(Raw); + const int JType = M->jnt_type[J]; + if (JType == mjJNT_HINGE) + { + Joint.Type = M->jnt_limited[J] ? TEXT("revolute") : TEXT("continuous"); + } + else if (JType == mjJNT_SLIDE) + { + Joint.Type = TEXT("prismatic"); + } + else if (JType == mjJNT_BALL) + { + Model.Warnings.Add(FString::Printf( + TEXT("joint '%s': ball joint exported as fixed (URDF has no ball)"), *Joint.Name)); + Joint.Type = TEXT("fixed"); + } + else if (JType == mjJNT_FREE) + { + Model.Warnings.Add(FString::Printf( + TEXT("joint '%s': free joint omitted; floating base rides tf2"), *Joint.Name)); + Joint.Type = TEXT("fixed"); + } + else + { + Joint.Type = TEXT("fixed"); + } + } + + TArray Lines; + Lines.Add(FString::Printf(TEXT(" "), *Joint.Name, *Joint.Type)); + Lines.Add(FString::Printf(TEXT(" "), *ParentLink)); + Lines.Add(FString::Printf(TEXT(" "), *ChildLink)); + Lines.Add(FString::Printf(TEXT(" "), + *Xyz(OriginXyz), *FmtNum(OriginRpy.X), *FmtNum(OriginRpy.Y), *FmtNum(OriginRpy.Z))); + + if (Joint.Type == TEXT("revolute") || Joint.Type == TEXT("prismatic") + || Joint.Type == TEXT("continuous")) + { + const mjtNum* Ax = &M->jnt_axis[3 * J]; + Joint.Axis = FVector(Ax[0], Ax[1], Ax[2]); + Lines.Add(FString::Printf(TEXT(" "), + *FmtNum(Ax[0]), *FmtNum(Ax[1]), *FmtNum(Ax[2]))); + + const bool bIsSlide = (M->jnt_type[J] == mjJNT_SLIDE); + Joint.Effort = JointEffort(M, J, Cfg, Joint.Name, Model.Warnings); + Joint.Velocity = JointVelocity(M, J, bIsSlide, Cfg, Joint.Name, Model.Warnings); + + if (Joint.Type == TEXT("continuous")) + { + Lines.Add(FString::Printf(TEXT(" "), + *FmtNum(Joint.Effort), *FmtNum(Joint.Velocity))); + } + else + { + const double Q0 = M->qpos0[M->jnt_qposadr[J]]; + Joint.Lower = M->jnt_range[2 * J + 0] - Q0 - Cfg.LimitMargin; + Joint.Upper = M->jnt_range[2 * J + 1] - Q0 + Cfg.LimitMargin; + Joint.bHasLimit = true; + Lines.Add(FString::Printf( + TEXT(" "), + *FmtNum(Joint.Lower), *FmtNum(Joint.Upper), + *FmtNum(Joint.Effort), *FmtNum(Joint.Velocity))); + } + } + if (const FMimicJointInfo* Mimic = MimicMap.Find(J)) + { + Lines.Add(FString::Printf(TEXT(" "), + *Mimic->LeaderName, *FmtNum(Mimic->Multiplier), *FmtNum(Mimic->Offset))); + } + Lines.Add(TEXT(" ")); + JointXml.Add(FString::Join(Lines, TEXT("\n"))); + Model.Joints.Add(MoveTemp(Joint)); + }; + + for (int32 i : BodyIds) + { + const int NJnt = M->body_jntnum[i]; + const int Parent = M->body_parentid[i]; + + if (NJnt == 1) + { + const int J = M->body_jntadr[i]; + if (M->jnt_type[J] == mjJNT_HINGE || M->jnt_type[J] == mjJNT_SLIDE) + { + const mjtNum* Jp = &M->jnt_pos[3 * J]; + Offset[i] = FVector(Jp[0], Jp[1], Jp[2]); + } + } + + // --- link --- + FUrdfLink Link; + Link.Name = BodyName(i); + + TArray Parts; + Parts.Add(FString::Printf(TEXT(" "), *Link.Name)); + if (M->body_mass[i] > 0.0) + { + const mjtNum* Ip = &M->body_ipos[3 * i]; + const FVector IpV = FVector(Ip[0], Ip[1], Ip[2]) - Offset[i]; + const FVector Rpy = QuatWxyzToRpy(&M->body_iquat[4 * i]); + const mjtNum* In = &M->body_inertia[3 * i]; + Parts.Add(TEXT(" ")); + Parts.Add(FString::Printf(TEXT(" "), + *Xyz(IpV), *FmtNum(Rpy.X), *FmtNum(Rpy.Y), *FmtNum(Rpy.Z))); + Parts.Add(FString::Printf(TEXT(" "), *FmtNum(M->body_mass[i]))); + Parts.Add(FString::Printf( + TEXT(" "), + *FmtNum(In[0]), *FmtNum(In[1]), *FmtNum(In[2]))); + Parts.Add(TEXT(" ")); + } + + for (int g = 0; g < M->ngeom; ++g) + { + if (M->geom_bodyid[g] != i) + continue; + + FString GeoXml; + int32 UsedMeshId = -1; + const int GType = M->geom_type[g]; + const mjtNum* Sz = &M->geom_size[3 * g]; + if (GType == mjGEOM_SPHERE) + { + GeoXml = FString::Printf(TEXT(""), *FmtNum(Sz[0])); + } + else if (GType == mjGEOM_BOX) + { + GeoXml = FString::Printf(TEXT(""), + *FmtNum(2 * Sz[0]), *FmtNum(2 * Sz[1]), *FmtNum(2 * Sz[2])); + } + else if (GType == mjGEOM_CYLINDER) + { + GeoXml = FString::Printf(TEXT(""), + *FmtNum(Sz[0]), *FmtNum(2 * Sz[1])); + } + else if (GType == mjGEOM_CAPSULE) + { + Model.Warnings.Add(FString::Printf( + TEXT("geom %d: capsule approximated as a cylinder"), g)); + GeoXml = FString::Printf(TEXT(""), + *FmtNum(Sz[0]), *FmtNum(2 * Sz[1])); + } + else if (GType == mjGEOM_MESH) + { + UsedMeshId = M->geom_dataid[g]; + GeoXml = FString::Printf(TEXT(""), + *MeshUri(UsedMeshId)); + } + else + { + Model.Warnings.Add(FString::Printf( + TEXT("geom %d: type %d has no URDF equivalent; dropped"), g, GType)); + continue; + } + + const mjtNum* Gp = &M->geom_pos[3 * g]; + const FVector GpV = FVector(Gp[0], Gp[1], Gp[2]) - Offset[i]; + const FVector Rpy = QuatWxyzToRpy(&M->geom_quat[4 * g]); + + FUrdfGeomFrame Frame; + Frame.MjGeomId = g; + Frame.LocalPos = GpV; + Frame.LocalRpy = Rpy; + Link.Geoms.Add(Frame); + + const bool bCollision = M->geom_contype[g] != 0 || M->geom_conaffinity[g] != 0; + const TCHAR* Tag = bCollision ? TEXT("collision") : TEXT("visual"); + Parts.Add(FString::Printf(TEXT(" <%s>"), Tag)); + Parts.Add(FString::Printf(TEXT(" "), + *Xyz(GpV), *FmtNum(Rpy.X), *FmtNum(Rpy.Y), *FmtNum(Rpy.Z))); + Parts.Add(FString::Printf(TEXT(" %s"), *GeoXml)); + if (!bCollision) + { + const float* Rgba = &M->geom_rgba[4 * g]; + Parts.Add(FString::Printf( + TEXT(" "), + *Link.Name, g, *FmtNum(Rgba[0]), *FmtNum(Rgba[1]), *FmtNum(Rgba[2]), *FmtNum(Rgba[3]))); + } + Parts.Add(FString::Printf(TEXT(" "), Tag)); + } + Parts.Add(TEXT(" ")); + LinkXml.Add(FString::Join(Parts, TEXT("\n"))); + Model.Links.Add(MoveTemp(Link)); + + if (Parent == 0) + continue; // subtree root: no joint to world, placement rides tf2 + + const FVector ParentOff = Offset[Parent]; + const mjtNum* Bp = &M->body_pos[3 * i]; + const FVector BodyPos(Bp[0], Bp[1], Bp[2]); + const mjtNum* Bq = &M->body_quat[4 * i]; + const FVector ChildLinkInParentBody = BodyPos + RotateByQuatWxyz(Bq, Offset[i]); + const FVector OriginXyz = ChildLinkInParentBody - ParentOff; + const FVector OriginRpy = QuatWxyzToRpy(Bq); + + const FString ChildLink = BodyName(i); + const FString ParentLink = BodyName(Parent); + + if (NJnt == 0) + { + EmitJoint(-1, ParentLink, ChildLink, OriginXyz, OriginRpy, /*bForceFixed=*/true); + } + else if (NJnt == 1) + { + EmitJoint(M->body_jntadr[i], ParentLink, ChildLink, OriginXyz, OriginRpy, false); + } + else + { + // Multi-joint body: k-1 zero-inertia dummy links carry the first k-1 + // joints; the real link is carried by the last. Successive joints share + // the body frame, so intermediate origins are identity. + Model.Warnings.Add(FString::Printf( + TEXT("body '%s': %d joints -> %d dummy links inserted"), *ChildLink, NJnt, NJnt - 1)); + FString PrevLink = ParentLink; + FVector CurXyz = OriginXyz; + FVector CurRpy = OriginRpy; + for (int k = 0; k < NJnt; ++k) + { + const int J = M->body_jntadr[i] + k; + if (k < NJnt - 1) + { + const FString DLink = FString::Printf(TEXT("%s__j%d"), *ChildLink, k); + LinkXml.Add(FString::Printf(TEXT(" "), *DLink)); + FUrdfLink Dummy; + Dummy.Name = DLink; + Model.Links.Add(MoveTemp(Dummy)); + EmitJoint(J, PrevLink, DLink, CurXyz, CurRpy, false); + PrevLink = DLink; + CurXyz = FVector::ZeroVector; + CurRpy = FVector::ZeroVector; + } + else + { + EmitJoint(J, PrevLink, ChildLink, CurXyz, CurRpy, false); + } + } + } + } + + if (M->ntendon > 0) + { + Model.Warnings.Add(FString::Printf( + TEXT("%d tendon(s) dropped (no URDF representation)"), M->ntendon)); + } + { + int32 NonJointEq = 0; + for (int e = 0; e < M->neq; ++e) + if (M->eq_type[e] != mjEQ_JOINT) + ++NonJointEq; + if (NonJointEq > 0) + { + Model.Warnings.Add(FString::Printf( + TEXT("%d non-joint equality constraint(s) dropped (weld, connect, " + "tendon etc. have no URDF equivalent)"), NonJointEq)); + } + } + + // --- transmissions --- + TArray TransXml; + for (int A = 0; A < M->nu; ++A) + { + if (M->actuator_trntype[A] != mjTRN_JOINT) + continue; + const int JntId = M->actuator_trnid[2 * A + 0]; + const FUrdfJoint* Found = nullptr; + for (const FUrdfJoint& J : Model.Joints) + { + if (J.MjJointId == JntId) { Found = &J; break; } + } + if (!Found) + continue; + const char* ActRaw = mj_id2name(const_cast(M), mjOBJ_ACTUATOR, A); + if (!ActRaw) + continue; + const FString ActName = LocalName(ActRaw); + const double Gear = std::abs(M->actuator_gear[A * 6 + 0]); + + TransXml.Add(FString::Printf(TEXT(" "), *ActName)); + TransXml.Add(TEXT(" transmission_interface/SimpleTransmission")); + TransXml.Add(FString::Printf(TEXT(" "), *Found->Name)); + TransXml.Add(TEXT(" hardware_interface/EffortJointInterface")); + TransXml.Add(TEXT(" ")); + TransXml.Add(FString::Printf(TEXT(" "), *ActName)); + TransXml.Add(FString::Printf(TEXT(" %s"), *FmtNum(Gear))); + TransXml.Add(TEXT(" hardware_interface/EffortActuatorInterface")); + TransXml.Add(TEXT(" ")); + TransXml.Add(TEXT(" ")); + } + + // --- optional tool0 frame --- + if (Cfg.bAppendTool0 && LeafBodyId >= 0) + { + const FString LeafName = BodyName(LeafBodyId); + LinkXml.Add(FString::Printf(TEXT(" "))); + TArray T0J; + T0J.Add(TEXT(" ")); + T0J.Add(TEXT(" ")); + T0J.Add(FString::Printf(TEXT(" "), *LeafName)); + T0J.Add(TEXT(" ")); + T0J.Add(TEXT(" ")); + JointXml.Add(FString::Join(T0J, TEXT("\n"))); + } + + TArray Doc; + Doc.Add(TEXT("")); + Doc.Add(FString::Printf(TEXT(""), *RobotName)); + Doc.Append(LinkXml); + Doc.Append(JointXml); + Doc.Append(TransXml); + Doc.Add(TEXT("")); + Model.Xml = FString::Join(Doc, TEXT("\n")) + TEXT("\n"); + return Model; +} + +bool FUrdfExporter::WriteMeshStl(const mjModel* M, int32 MeshId, const FString& Dir, + FString& OutFilename) +{ + if (!M || MeshId < 0 || MeshId >= M->nmesh) + return false; + + const int VertAdr = M->mesh_vertadr[MeshId]; + const int VertNum = M->mesh_vertnum[MeshId]; + const int FaceAdr = M->mesh_faceadr[MeshId]; + const int FaceNum = M->mesh_facenum[MeshId]; + + TArray Bytes; + Bytes.Reserve(84 + FaceNum * 50); + + auto AppendU16 = [&Bytes](uint16 V) { + Bytes.Append(reinterpret_cast(&V), sizeof(V)); + }; + auto AppendU32 = [&Bytes](uint32 V) { + Bytes.Append(reinterpret_cast(&V), sizeof(V)); + }; + auto AppendF32 = [&Bytes](float V) { + Bytes.Append(reinterpret_cast(&V), sizeof(V)); + }; + + // 80-byte zero header + triangle count. + for (int i = 0; i < 80; ++i) + Bytes.Add(0); + AppendU32(static_cast(FaceNum)); + + auto Vert = [M, VertAdr](int Local) -> FVector { + const float* V = &M->mesh_vert[3 * (VertAdr + Local)]; + return FVector(V[0], V[1], V[2]); + }; + + for (int Tri = 0; Tri < FaceNum; ++Tri) + { + const int* F = &M->mesh_face[3 * (FaceAdr + Tri)]; + const FVector V0 = Vert(F[0]); + const FVector V1 = Vert(F[1]); + const FVector V2 = Vert(F[2]); + FVector N = FVector::CrossProduct(V1 - V0, V2 - V0); + const double Len = N.Size(); + N = (Len > 0.0) ? (N / Len) : FVector::ZeroVector; + + AppendF32(static_cast(N.X)); + AppendF32(static_cast(N.Y)); + AppendF32(static_cast(N.Z)); + for (const FVector& V : {V0, V1, V2}) + { + AppendF32(static_cast(V.X)); + AppendF32(static_cast(V.Y)); + AppendF32(static_cast(V.Z)); + } + AppendU16(0); + } + + OutFilename = MeshBaseName(M, MeshId) + TEXT(".stl"); + const FString Path = FPaths::Combine(Dir, OutFilename); + return FFileHelper::SaveArrayToFile(Bytes, *Path); +} + +FUrdfModel FUrdfExporter::ExportToDir(const mjModel* M, const FString& RobotName, + const FString& ArtRawName, const FString& OutDir, const FUrdfExportConfig& Cfg) +{ + FUrdfModel Model; + if (!M) + return Model; + + const FString MeshDir = FPaths::Combine(OutDir, TEXT("meshes")); + IFileManager::Get().MakeDirectory(*MeshDir, /*Tree=*/true); + + const TArray BodyIds = BodyIdsForArt(M, ArtRawName); + Model = Build(M, RobotName, ArtRawName, BodyIds, MeshDir, Cfg); + + for (int32 MeshId : Model.MeshIds) + { + FString Unused; + if (!WriteMeshStl(M, MeshId, MeshDir, Unused)) + { + Model.Warnings.Add(FString::Printf(TEXT("mesh %d: STL write failed"), MeshId)); + } + } + + const FString UrdfPath = FPaths::Combine(OutDir, TEXT("model.urdf")); + FFileHelper::SaveStringToFile(Model.Xml, *UrdfPath); + return Model; +} diff --git a/Source/URLab/Public/Urdf/UrdfExporter.h b/Source/URLab/Public/Urdf/UrdfExporter.h new file mode 100644 index 00000000..2e91fce8 --- /dev/null +++ b/Source/URLab/Public/Urdf/UrdfExporter.h @@ -0,0 +1,150 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +struct mjModel_; +typedef mjModel_ mjModel; + +/** + * Configuration for the mjModel -> URDF exporter. MuJoCo has no joint effort or + * velocity limits of its own, so the exporter derives what it can from the + * actuators and falls back to these defaults; consumers such as urdfdom and + * MoveIt TOTG reject a revolute/prismatic joint whose limit lacks a positive + * velocity, so the defaults must be non-zero. + */ +struct FUrdfExportConfig +{ + double DefaultEffort = 100.0; + double DefaultVelocityAngular = 3.14; // rad/s, for hinge joints + double DefaultVelocityLinear = 1.0; // m/s, for slide joints + /** Per-joint (compiled joint name) velocity override, highest priority. */ + TMap VelocityOverrides; + /** Widen exported position limits by this (rad / m) so MuJoCo's soft-limit + * overshoot stays inside the URDF hard limits. Without it a planner (MoveIt) + * rejects the sim's resting state as out of bounds on tight joints (the + * Franka's joint4). Small enough to be physically negligible. */ + double LimitMargin = 0.02; + /** When true, append a fixed "tool0" link at the leaf body of the + * articulation tree so MoveIt can attach an end-effector. */ + bool bAppendTool0 = true; +}; + +/** One emitted geom's frame data, kept for the forward-kinematics self-test. + * The link/collision classification and shape params live only in the XML. */ +struct FUrdfGeomFrame +{ + int32 MjGeomId = -1; + FVector LocalPos = FVector::ZeroVector; // origin xyz after the jnt_pos frame-shift, metres + FVector LocalRpy = FVector::ZeroVector; // origin rpy (extrinsic XYZ), radians +}; + +/** One URDF link. Dummy links (multi-joint bodies) carry no geoms. */ +struct FUrdfLink +{ + FString Name; + TArray Geoms; +}; + +/** One URDF joint. Origins are parent-link -> child-link at the URDF zero pose. */ +struct FUrdfJoint +{ + FString Name; + FString Parent; + FString Child; + FString Type; // revolute / continuous / prismatic / fixed + FVector OriginPos = FVector::ZeroVector; + FVector OriginRpy = FVector::ZeroVector; + FVector Axis = FVector::ZeroVector; + bool bHasLimit = false; // lower/upper present (revolute + prismatic) + double Lower = 0.0; + double Upper = 0.0; + double Effort = 0.0; + double Velocity = 0.0; + int32 MjJointId = -1; // -1 for synthesized fixed joints and dummy chains +}; + +/** Structured result of a build plus the serialized XML. */ +struct FUrdfModel +{ + FString RobotName; + TArray Links; + TArray Joints; + /** Compiled mesh ids referenced by the model, deduplicated (one STL each). */ + TArray MeshIds; + /** Human-readable warnings for every dropped or defaulted construct. */ + TArray Warnings; + /** The full URDF document. */ + FString Xml; +}; + +/** + * In-process exporter from a compiled `mjModel` to URDF + binary STL meshes. + * + * This is a direct port of the Python prototype validated field-by-field against + * the mujoco_menagerie Franka (see docs/plan_ros_urdf_port_spec.md). It is a + * pure mjModel reader with no external-transport dependency, so it always + * compiles as part of the core module. + * + * Frame conventions (validated to ~1e-9 m against mjModel geom_xpos): + * - URDF joint limits are `jnt_range - qpos0`, so URDF q=0 is the MuJoCo + * reference pose (qpos == qpos0). The matched joint_states shift lives in + * the state publisher's FillJointState. + * - A single-joint body with `jnt_pos != 0` has its link frame moved to the + * joint anchor; every body-local origin is re-expressed accordingly. + * - A body with k>1 joints becomes k-1 zero-inertia dummy links chained by the + * first k-1 joints, with the real link carried by the last. + * + * Dropped constructs (ball/free joints, tendons, equalities, unsupported geom + * types) are logged into FUrdfModel::Warnings. + */ +class URLAB_API FUrdfExporter +{ +public: + /** Body ids (i > 0) belonging to an art, in ascending (topological) order. + * When ArtRawName is empty every non-world body is returned (whole model). + * Otherwise the art's bodies are those whose compiled name equals ArtRawName + * or begins with "_", matching FMjCanonicalName::PartSegment. */ + static TArray BodyIdsForArt(const mjModel* M, const FString& ArtRawName); + + /** Build the structured model + XML for the given body set. MeshUriDir is the + * directory the emitted `file://` mesh URIs point at; no files are written. */ + static FUrdfModel Build(const mjModel* M, const FString& RobotName, + const FString& ArtRawName, const TArray& BodyIds, + const FString& MeshUriDir, const FUrdfExportConfig& Cfg); + + /** Full export to disk: build, write every referenced mesh as binary STL to + * OutDir/meshes, and dump OutDir/model.urdf. Returns the model (Xml filled). + * Returns an empty model (no links) on a null mjModel. */ + static FUrdfModel ExportToDir(const mjModel* M, const FString& RobotName, + const FString& ArtRawName, const FString& OutDir, const FUrdfExportConfig& Cfg); + + /** Serialize one compiled mesh id to a binary STL file at Dir/.stl. + * OutFilename receives the bare ".stl". Returns false on write failure. */ + static bool WriteMeshStl(const mjModel* M, int32 MeshId, const FString& Dir, + FString& OutFilename); + + /** Sanitized compiled mesh name (no extension), matching the STL file stem. */ + static FString MeshBaseName(const mjModel* M, int32 MeshId); +}; diff --git a/Source/URLab/URLab.Build.cs b/Source/URLab/URLab.Build.cs index 749d2d40..12b07251 100644 --- a/Source/URLab/URLab.Build.cs +++ b/Source/URLab/URLab.Build.cs @@ -21,6 +21,7 @@ // CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. using System; +using System.Collections.Generic; using System.Diagnostics; using UnrealBuildTool; using System.IO; @@ -446,4 +447,5 @@ protected void AddZeroMQ(ReadOnlyTargetRules Target) { AddThirdPartyLibrary("libzmq", Target); } + } diff --git a/Source/URLabRos/Private/Ros/UrlabRclCore.cpp b/Source/URLabRos/Private/Ros/UrlabRclCore.cpp new file mode 100644 index 00000000..6323ba50 --- /dev/null +++ b/Source/URLabRos/Private/Ros/UrlabRclCore.cpp @@ -0,0 +1,2609 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Everything below is fenced behind URLAB_WITH_ROS2 so that UBT, which compiles +// every .cpp under the module unconditionally, sees an empty translation unit in +// every UE build until the ROS build wiring defines the macro. The defined() +// guard keeps builds green while the macro does not exist at all. The standalone +// ROS workspace (ros/urlab_ros_ws) defines URLAB_WITH_ROS2=1 itself. +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "UrlabRclCore.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#ifndef URLAB_ROS_DISTRO_NAME +#define URLAB_ROS_DISTRO_NAME "humble" +#endif + +namespace +{ +// Last error string, captured from rcutils/rcl on each failed call. +char GLastError[1024] = {0}; + +void CaptureError() +{ + const rcutils_error_string_t Err = rcl_get_error_string(); + std::strncpy(GLastError, Err.str, sizeof(GLastError) - 1); + GLastError[sizeof(GLastError) - 1] = '\0'; + rcl_reset_error(); +} + +void ClearError() +{ + GLastError[0] = '\0'; +} + +void FillStamp(builtin_interfaces__msg__Time& Stamp, int64_t SimTimeNs) +{ + Stamp.sec = static_cast(SimTimeNs / 1000000000LL); + Stamp.nanosec = static_cast(SimTimeNs % 1000000000LL); +} + +// Subscription record kinds; SpinSome dispatches per kind. +enum class ESubKind : uint8_t +{ + Ctrl, + Twist, + JointState +}; + +struct FSubRecord +{ + ESubKind Kind; + rcl_subscription_t Sub; + UrlabRclContext* Ctx; + UrlabRclCtrlCallback CtrlCallback; + UrlabRclTwistCallback TwistCallback; + UrlabRclJointStateCallback JointStateCallback; + void* User; + std_msgs__msg__Float64MultiArray CtrlMsg; + geometry_msgs__msg__Twist TwistMsg; + sensor_msgs__msg__JointState JointStateMsg; +}; + +// Service record; SpinSome takes the request, runs the callback, sends the reply. +struct FSrvRecord +{ + rcl_service_t Srv; + UrlabRclContext* Ctx; + UrlabRclTriggerCallback Callback; + void* User; + std_srvs__srv__Trigger_Request Request; + std_srvs__srv__Trigger_Response Response; +}; +} // namespace + +// --- Opaque handle definitions --------------------------------------------- + +struct UrlabRclContext +{ + rcl_context_t Context; + rcl_init_options_t InitOptions; + rcl_node_t Node; + rcl_allocator_t Allocator; + bool bNodeValid; + // Subscriptions registered against this context, spun by UrlabRcl_SpinSome. + std::vector Subs; + // Services registered against this context, also served by UrlabRcl_SpinSome. + std::vector Srvs; +}; + +struct UrlabRclJointStatePub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__JointState Msg; +}; + +struct UrlabRclImuPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__Imu Msg; +}; + +struct UrlabRclTfPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + tf2_msgs__msg__TFMessage Msg; +}; + +struct UrlabRclTwistStampedPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + geometry_msgs__msg__TwistStamped Msg; +}; + +struct UrlabRclClockPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + rosgraph_msgs__msg__Clock Msg; +}; + +struct UrlabRclImagePub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__Image Msg; + int32_t Width; + int32_t Height; +}; + +struct UrlabRclCtrlPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + std_msgs__msg__Float64MultiArray Msg; +}; + +struct UrlabRclStringPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + std_msgs__msg__String Msg; +}; + +struct UrlabRclWrenchStampedPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + geometry_msgs__msg__WrenchStamped Msg; +}; + +struct UrlabRclRangePub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__Range Msg; +}; + +struct UrlabRclMagneticFieldPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__MagneticField Msg; +}; + +struct UrlabRclFloat64MultiArrayPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + std_msgs__msg__Float64MultiArray Msg; +}; + +struct UrlabRclOdometryPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + nav_msgs__msg__Odometry Msg; +}; + +struct UrlabRclPoseWithCovariancePub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + geometry_msgs__msg__PoseWithCovarianceStamped Msg; +}; + +struct UrlabRclCameraInfoPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__CameraInfo Msg; +}; + +struct UrlabRclBoolPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + std_msgs__msg__Bool Msg; +}; + +struct UrlabRclFloat64Pub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + std_msgs__msg__Float64 Msg; +}; + +struct UrlabRclVector3Pub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + geometry_msgs__msg__Vector3 Msg; +}; + +struct UrlabRclPoseStampedPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + geometry_msgs__msg__PoseStamped Msg; +}; + +struct UrlabRclCtrlSub +{ + FSubRecord Rec; +}; + +struct UrlabRclTwistSub +{ + FSubRecord Rec; +}; + +struct UrlabRclJointStateSub +{ + FSubRecord Rec; +}; + +struct UrlabRclTriggerService +{ + FSrvRecord Rec; +}; + +namespace +{ +// Publisher creation helper shared by every telemetry publisher: initialises the +// rcl publisher against the context node with the given type support and QoS. +bool InitPublisher(UrlabRclContext* Ctx, rcl_publisher_t& OutPub, + const rosidl_message_type_support_t* TypeSupport, const char* Topic, + const rmw_qos_profile_t& Qos) +{ + OutPub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t Options = rcl_publisher_get_default_options(); + Options.qos = Qos; + const rcl_ret_t Ret = rcl_publisher_init(&OutPub, &Ctx->Node, TypeSupport, Topic, &Options); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return false; + } + return true; +} + +void SetString(rosidl_runtime_c__String& Str, const char* Value) +{ + rosidl_runtime_c__String__assign(&Str, Value ? Value : ""); +} + +void DetachSub(FSubRecord* Rec) +{ + if (!Rec || !Rec->Ctx) + { + return; + } + std::vector& List = Rec->Ctx->Subs; + for (size_t i = 0; i < List.size(); ++i) + { + if (List[i] == Rec) + { + List.erase(List.begin() + i); + break; + } + } +} + +void DetachSrv(FSrvRecord* Rec) +{ + if (!Rec || !Rec->Ctx) + { + return; + } + std::vector& List = Rec->Ctx->Srvs; + for (size_t i = 0; i < List.size(); ++i) + { + if (List[i] == Rec) + { + List.erase(List.begin() + i); + break; + } + } +} +} // namespace + +// --- Context --------------------------------------------------------------- + +UrlabRclContext* UrlabRcl_Init(const char* NodeName, const char* NodeNamespace, int32_t DomainId) +{ + ClearError(); + UrlabRclContext* Ctx = new UrlabRclContext(); + Ctx->Context = rcl_get_zero_initialized_context(); + Ctx->InitOptions = rcl_get_zero_initialized_init_options(); + Ctx->Node = rcl_get_zero_initialized_node(); + Ctx->Allocator = rcl_get_default_allocator(); + Ctx->bNodeValid = false; + + rcl_ret_t Ret = rcl_init_options_init(&Ctx->InitOptions, Ctx->Allocator); + if (Ret != RCL_RET_OK) + { + CaptureError(); + delete Ctx; + return nullptr; + } + + const size_t Domain = (DomainId < 0) + ? static_cast(RCL_DEFAULT_DOMAIN_ID) + : static_cast(DomainId); + Ret = rcl_init_options_set_domain_id(&Ctx->InitOptions, Domain); + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_init_options_fini(&Ctx->InitOptions); + delete Ctx; + return nullptr; + } + + Ret = rcl_init(0, nullptr, &Ctx->InitOptions, &Ctx->Context); + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_init_options_fini(&Ctx->InitOptions); + delete Ctx; + return nullptr; + } + + rcl_node_options_t NodeOptions = rcl_node_get_default_options(); + Ret = rcl_node_init(&Ctx->Node, NodeName ? NodeName : "urlab", + NodeNamespace ? NodeNamespace : "", &Ctx->Context, &NodeOptions); + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_shutdown(&Ctx->Context); + rcl_context_fini(&Ctx->Context); + rcl_init_options_fini(&Ctx->InitOptions); + delete Ctx; + return nullptr; + } + Ctx->bNodeValid = true; + return Ctx; +} + +void UrlabRcl_Shutdown(UrlabRclContext* Ctx) +{ + if (!Ctx) + { + return; + } + for (FSubRecord* Rec : Ctx->Subs) + { + if (Rec) + { + rcl_subscription_fini(&Rec->Sub, &Ctx->Node); + if (Rec->Kind == ESubKind::Ctrl) + { + std_msgs__msg__Float64MultiArray__fini(&Rec->CtrlMsg); + } + else if (Rec->Kind == ESubKind::Twist) + { + geometry_msgs__msg__Twist__fini(&Rec->TwistMsg); + } + else + { + sensor_msgs__msg__JointState__fini(&Rec->JointStateMsg); + } + delete Rec; + } + } + Ctx->Subs.clear(); + + for (FSrvRecord* Rec : Ctx->Srvs) + { + if (Rec) + { + rcl_service_fini(&Rec->Srv, &Ctx->Node); + std_srvs__srv__Trigger_Request__fini(&Rec->Request); + std_srvs__srv__Trigger_Response__fini(&Rec->Response); + delete Rec; + } + } + Ctx->Srvs.clear(); + + if (Ctx->bNodeValid) + { + rcl_node_fini(&Ctx->Node); + Ctx->bNodeValid = false; + } + rcl_shutdown(&Ctx->Context); + rcl_context_fini(&Ctx->Context); + rcl_init_options_fini(&Ctx->InitOptions); + delete Ctx; +} + +const char* UrlabRcl_DistroName() +{ + return URLAB_ROS_DISTRO_NAME; +} + +const char* UrlabRcl_LastError() +{ + return GLastError; +} + +// --- JointState ------------------------------------------------------------ + +UrlabRclJointStatePub* UrlabRcl_CreateJointStatePub(UrlabRclContext* Ctx, + const char* Topic, const char** JointNames, int32_t JointCount) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclJointStatePub* Pub = new UrlabRclJointStatePub(); + Pub->Ctx = Ctx; + sensor_msgs__msg__JointState__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, ""); + + const int32_t Count = JointCount > 0 ? JointCount : 0; + if (Count > 0) + { + rosidl_runtime_c__String__Sequence__init(&Pub->Msg.name, Count); + for (int32_t i = 0; i < Count; ++i) + { + SetString(Pub->Msg.name.data[i], JointNames ? JointNames[i] : ""); + } + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.position, Count); + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.velocity, Count); + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.effort, Count); + } + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__JointState__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishJointState(UrlabRclJointStatePub* Pub, + const double* Positions, const double* Velocities, const double* Efforts, + int32_t Count, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + + const int32_t Cap = static_cast(Pub->Msg.position.capacity); + const int32_t N = Count < Cap ? Count : Cap; + + if (Positions) + { + std::memcpy(Pub->Msg.position.data, Positions, sizeof(double) * N); + Pub->Msg.position.size = N; + } + else + { + Pub->Msg.position.size = 0; + } + if (Velocities) + { + std::memcpy(Pub->Msg.velocity.data, Velocities, sizeof(double) * N); + Pub->Msg.velocity.size = N; + } + else + { + Pub->Msg.velocity.size = 0; + } + if (Efforts) + { + std::memcpy(Pub->Msg.effort.data, Efforts, sizeof(double) * N); + Pub->Msg.effort.size = N; + } + else + { + Pub->Msg.effort.size = 0; + } + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyJointStatePub(UrlabRclJointStatePub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__JointState__fini(&Pub->Msg); + delete Pub; +} + +// --- Imu ------------------------------------------------------------------- + +UrlabRclImuPub* UrlabRcl_CreateImuPub(UrlabRclContext* Ctx, const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclImuPub* Pub = new UrlabRclImuPub(); + Pub->Ctx = Ctx; + sensor_msgs__msg__Imu__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__Imu__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishImu(UrlabRclImuPub* Pub, const double AngularVel[3], + const double LinearAccel[3], const double OrientationXyzw[4], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + + if (OrientationXyzw) + { + Pub->Msg.orientation.x = OrientationXyzw[0]; + Pub->Msg.orientation.y = OrientationXyzw[1]; + Pub->Msg.orientation.z = OrientationXyzw[2]; + Pub->Msg.orientation.w = OrientationXyzw[3]; + Pub->Msg.orientation_covariance[0] = 0.0; + } + else + { + // REP 145: leading covariance element -1 marks orientation absent. + Pub->Msg.orientation.x = 0.0; + Pub->Msg.orientation.y = 0.0; + Pub->Msg.orientation.z = 0.0; + Pub->Msg.orientation.w = 1.0; + Pub->Msg.orientation_covariance[0] = -1.0; + } + if (AngularVel) + { + Pub->Msg.angular_velocity.x = AngularVel[0]; + Pub->Msg.angular_velocity.y = AngularVel[1]; + Pub->Msg.angular_velocity.z = AngularVel[2]; + Pub->Msg.angular_velocity_covariance[0] = 0.0; + } + else + { + Pub->Msg.angular_velocity.x = 0.0; + Pub->Msg.angular_velocity.y = 0.0; + Pub->Msg.angular_velocity.z = 0.0; + Pub->Msg.angular_velocity_covariance[0] = -1.0; + } + if (LinearAccel) + { + Pub->Msg.linear_acceleration.x = LinearAccel[0]; + Pub->Msg.linear_acceleration.y = LinearAccel[1]; + Pub->Msg.linear_acceleration.z = LinearAccel[2]; + Pub->Msg.linear_acceleration_covariance[0] = 0.0; + } + else + { + Pub->Msg.linear_acceleration.x = 0.0; + Pub->Msg.linear_acceleration.y = 0.0; + Pub->Msg.linear_acceleration.z = 0.0; + Pub->Msg.linear_acceleration_covariance[0] = -1.0; + } + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyImuPub(UrlabRclImuPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__Imu__fini(&Pub->Msg); + delete Pub; +} + +// --- Tf -------------------------------------------------------------------- + +UrlabRclTfPub* UrlabRcl_CreateTfPub(UrlabRclContext* Ctx, int32_t bStatic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclTfPub* Pub = new UrlabRclTfPub(); + Pub->Ctx = Ctx; + tf2_msgs__msg__TFMessage__init(&Pub->Msg); + + rmw_qos_profile_t Qos = rmw_qos_profile_default; + const char* Topic = "/tf"; + if (bStatic != 0) + { + // /tf_static: latch the last set so late joiners receive it. + Qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + Qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + Qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + Qos.depth = 1; + Topic = "/tf_static"; + } + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(tf2_msgs, msg, TFMessage); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, Qos)) + { + tf2_msgs__msg__TFMessage__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishTf(UrlabRclTfPub* Pub, const char** ParentFrameIds, + const char** ChildFrameIds, const double* TranslationsXyz, + const double* RotationsXyzw, int32_t Count, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + const int32_t N = Count > 0 ? Count : 0; + + // Resize the transform sequence to exactly N, reinitialising each element. + tf2_msgs__msg__TFMessage__fini(&Pub->Msg); + tf2_msgs__msg__TFMessage__init(&Pub->Msg); + if (N > 0) + { + geometry_msgs__msg__TransformStamped__Sequence__init(&Pub->Msg.transforms, N); + for (int32_t i = 0; i < N; ++i) + { + geometry_msgs__msg__TransformStamped& T = Pub->Msg.transforms.data[i]; + FillStamp(T.header.stamp, SimTimeNs); + SetString(T.header.frame_id, ParentFrameIds ? ParentFrameIds[i] : ""); + SetString(T.child_frame_id, ChildFrameIds ? ChildFrameIds[i] : ""); + if (TranslationsXyz) + { + T.transform.translation.x = TranslationsXyz[i * 3 + 0]; + T.transform.translation.y = TranslationsXyz[i * 3 + 1]; + T.transform.translation.z = TranslationsXyz[i * 3 + 2]; + } + if (RotationsXyzw) + { + T.transform.rotation.x = RotationsXyzw[i * 4 + 0]; + T.transform.rotation.y = RotationsXyzw[i * 4 + 1]; + T.transform.rotation.z = RotationsXyzw[i * 4 + 2]; + T.transform.rotation.w = RotationsXyzw[i * 4 + 3]; + } + else + { + T.transform.rotation.w = 1.0; + } + } + } + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyTfPub(UrlabRclTfPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + tf2_msgs__msg__TFMessage__fini(&Pub->Msg); + delete Pub; +} + +// --- TwistStamped ---------------------------------------------------------- + +UrlabRclTwistStampedPub* UrlabRcl_CreateTwistStampedPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclTwistStampedPub* Pub = new UrlabRclTwistStampedPub(); + Pub->Ctx = Ctx; + geometry_msgs__msg__TwistStamped__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, TwistStamped); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + geometry_msgs__msg__TwistStamped__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishTwistStamped(UrlabRclTwistStampedPub* Pub, + const double Linear[3], const double Angular[3], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + if (Linear) + { + Pub->Msg.twist.linear.x = Linear[0]; + Pub->Msg.twist.linear.y = Linear[1]; + Pub->Msg.twist.linear.z = Linear[2]; + } + if (Angular) + { + Pub->Msg.twist.angular.x = Angular[0]; + Pub->Msg.twist.angular.y = Angular[1]; + Pub->Msg.twist.angular.z = Angular[2]; + } + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyTwistStampedPub(UrlabRclTwistStampedPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + geometry_msgs__msg__TwistStamped__fini(&Pub->Msg); + delete Pub; +} + +// --- Clock ----------------------------------------------------------------- + +UrlabRclClockPub* UrlabRcl_CreateClockPub(UrlabRclContext* Ctx) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclClockPub* Pub = new UrlabRclClockPub(); + Pub->Ctx = Ctx; + rosgraph_msgs__msg__Clock__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(rosgraph_msgs, msg, Clock); + if (!InitPublisher(Ctx, Pub->Pub, Ts, "/clock", rmw_qos_profile_default)) + { + rosgraph_msgs__msg__Clock__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishClock(UrlabRclClockPub* Pub, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.clock, SimTimeNs); + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyClockPub(UrlabRclClockPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + rosgraph_msgs__msg__Clock__fini(&Pub->Msg); + delete Pub; +} + +// --- Image ----------------------------------------------------------------- + +UrlabRclImagePub* UrlabRcl_CreateImagePub(UrlabRclContext* Ctx, const char* Topic, + const char* FrameId, int32_t Width, int32_t Height, const char* Encoding) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclImagePub* Pub = new UrlabRclImagePub(); + Pub->Ctx = Ctx; + Pub->Width = Width > 0 ? Width : 0; + Pub->Height = Height > 0 ? Height : 0; + sensor_msgs__msg__Image__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + SetString(Pub->Msg.encoding, Encoding); + Pub->Msg.width = static_cast(Pub->Width); + Pub->Msg.height = static_cast(Pub->Height); + Pub->Msg.is_bigendian = 0; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Image); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__Image__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishImage(UrlabRclImagePub* Pub, const uint8_t* Data, + int32_t StrideBytes, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + Pub->Msg.step = static_cast(StrideBytes > 0 ? StrideBytes : 0); + + const size_t Total = static_cast(Pub->Msg.step) * static_cast(Pub->Height); + if (Data && Total > 0) + { + if (Pub->Msg.data.capacity < Total) + { + rosidl_runtime_c__uint8__Sequence__fini(&Pub->Msg.data); + rosidl_runtime_c__uint8__Sequence__init(&Pub->Msg.data, Total); + } + std::memcpy(Pub->Msg.data.data, Data, Total); + Pub->Msg.data.size = Total; + } + else + { + Pub->Msg.data.size = 0; + } + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyImagePub(UrlabRclImagePub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__Image__fini(&Pub->Msg); + delete Pub; +} + +// --- Ctrl publisher (control injection) ------------------------------------ + +UrlabRclCtrlPub* UrlabRcl_CreateCtrlPub(UrlabRclContext* Ctx, const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclCtrlPub* Pub = new UrlabRclCtrlPub(); + Pub->Ctx = Ctx; + std_msgs__msg__Float64MultiArray__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + std_msgs__msg__Float64MultiArray__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishCtrl(UrlabRclCtrlPub* Pub, const double* Values, int32_t Count) +{ + ClearError(); + if (!Pub) + { + return -1; + } + const int32_t N = Count > 0 ? Count : 0; + // Resize the data sequence to exactly N. + if (static_cast(Pub->Msg.data.capacity) < N) + { + rosidl_runtime_c__double__Sequence__fini(&Pub->Msg.data); + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.data, N); + } + if (Values && N > 0) + { + std::memcpy(Pub->Msg.data.data, Values, sizeof(double) * N); + } + Pub->Msg.data.size = N; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyCtrlPub(UrlabRclCtrlPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + std_msgs__msg__Float64MultiArray__fini(&Pub->Msg); + delete Pub; +} + +// --- String publisher (latched robot_description) -------------------------- + +UrlabRclStringPub* UrlabRcl_CreateStringPub(UrlabRclContext* Ctx, const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclStringPub* Pub = new UrlabRclStringPub(); + Pub->Ctx = Ctx; + std_msgs__msg__String__init(&Pub->Msg); + + // Latch the last document so late-joining subscribers (rviz, MoveIt) receive + // it without a re-publish, matching robot_state_publisher's QoS. + rmw_qos_profile_t Qos = rmw_qos_profile_default; + Qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + Qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + Qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + Qos.depth = 1; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, Qos)) + { + std_msgs__msg__String__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishString(UrlabRclStringPub* Pub, const char* Text) +{ + ClearError(); + if (!Pub) + { + return -1; + } + SetString(Pub->Msg.data, Text); + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyStringPub(UrlabRclStringPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + std_msgs__msg__String__fini(&Pub->Msg); + delete Pub; +} + +// --- WrenchStamped --------------------------------------------------------- + +UrlabRclWrenchStampedPub* UrlabRcl_CreateWrenchStampedPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclWrenchStampedPub* Pub = new UrlabRclWrenchStampedPub(); + Pub->Ctx = Ctx; + geometry_msgs__msg__WrenchStamped__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, WrenchStamped); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + geometry_msgs__msg__WrenchStamped__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishWrenchStamped(UrlabRclWrenchStampedPub* Pub, + const double Force[3], const double Torque[3], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + Pub->Msg.wrench.force.x = Force ? Force[0] : 0.0; + Pub->Msg.wrench.force.y = Force ? Force[1] : 0.0; + Pub->Msg.wrench.force.z = Force ? Force[2] : 0.0; + Pub->Msg.wrench.torque.x = Torque ? Torque[0] : 0.0; + Pub->Msg.wrench.torque.y = Torque ? Torque[1] : 0.0; + Pub->Msg.wrench.torque.z = Torque ? Torque[2] : 0.0; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyWrenchStampedPub(UrlabRclWrenchStampedPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + geometry_msgs__msg__WrenchStamped__fini(&Pub->Msg); + delete Pub; +} + +// --- Range ----------------------------------------------------------------- + +UrlabRclRangePub* UrlabRcl_CreateRangePub(UrlabRclContext* Ctx, const char* Topic, + const char* FrameId, uint8_t RadiationType, float FieldOfView, float MinRange, + float MaxRange) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclRangePub* Pub = new UrlabRclRangePub(); + Pub->Ctx = Ctx; + sensor_msgs__msg__Range__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + // Constant fields; only the reading + stamp change per publish. + Pub->Msg.radiation_type = RadiationType; + Pub->Msg.field_of_view = FieldOfView; + Pub->Msg.min_range = MinRange; + Pub->Msg.max_range = MaxRange; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Range); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__Range__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishRange(UrlabRclRangePub* Pub, float Range, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + Pub->Msg.range = Range; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyRangePub(UrlabRclRangePub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__Range__fini(&Pub->Msg); + delete Pub; +} + +// --- MagneticField --------------------------------------------------------- + +UrlabRclMagneticFieldPub* UrlabRcl_CreateMagneticFieldPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclMagneticFieldPub* Pub = new UrlabRclMagneticFieldPub(); + Pub->Ctx = Ctx; + sensor_msgs__msg__MagneticField__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + // Exact ground truth: leading covariance element 0 (not -1 "unknown"). + Pub->Msg.magnetic_field_covariance[0] = 0.0; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, MagneticField); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__MagneticField__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishMagneticField(UrlabRclMagneticFieldPub* Pub, const double Field[3], + int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + Pub->Msg.magnetic_field.x = Field ? Field[0] : 0.0; + Pub->Msg.magnetic_field.y = Field ? Field[1] : 0.0; + Pub->Msg.magnetic_field.z = Field ? Field[2] : 0.0; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyMagneticFieldPub(UrlabRclMagneticFieldPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__MagneticField__fini(&Pub->Msg); + delete Pub; +} + +// --- Float64MultiArray (total-coverage sensor fallback) -------------------- + +UrlabRclFloat64MultiArrayPub* UrlabRcl_CreateFloat64MultiArrayPub(UrlabRclContext* Ctx, + const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclFloat64MultiArrayPub* Pub = new UrlabRclFloat64MultiArrayPub(); + Pub->Ctx = Ctx; + std_msgs__msg__Float64MultiArray__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + std_msgs__msg__Float64MultiArray__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishFloat64MultiArray(UrlabRclFloat64MultiArrayPub* Pub, + const double* Values, int32_t Count) +{ + ClearError(); + if (!Pub) + { + return -1; + } + const int32_t N = Count > 0 ? Count : 0; + if (static_cast(Pub->Msg.data.capacity) < N) + { + rosidl_runtime_c__double__Sequence__fini(&Pub->Msg.data); + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.data, N); + } + if (Values && N > 0) + { + std::memcpy(Pub->Msg.data.data, Values, sizeof(double) * N); + } + Pub->Msg.data.size = N; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyFloat64MultiArrayPub(UrlabRclFloat64MultiArrayPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + std_msgs__msg__Float64MultiArray__fini(&Pub->Msg); + delete Pub; +} + +// --- Odometry -------------------------------------------------------------- + +namespace +{ +// Ground-truth pose/twist is exact; a small nonzero diagonal keeps EKF consumers +// (robot_localization) from rejecting the message while still reading as +// "essentially certain". +constexpr double GGroundTruthVariance = 1.0e-6; + +void SetCovarianceDiagonal(double Cov[36], double Value) +{ + std::memset(Cov, 0, sizeof(double) * 36); + for (int i = 0; i < 6; ++i) + { + Cov[i * 6 + i] = Value; + } +} +} // namespace + +UrlabRclOdometryPub* UrlabRcl_CreateOdometryPub(UrlabRclContext* Ctx, const char* Topic, + const char* FrameId, const char* ChildFrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclOdometryPub* Pub = new UrlabRclOdometryPub(); + Pub->Ctx = Ctx; + nav_msgs__msg__Odometry__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + SetString(Pub->Msg.child_frame_id, ChildFrameId); + SetCovarianceDiagonal(Pub->Msg.pose.covariance, GGroundTruthVariance); + SetCovarianceDiagonal(Pub->Msg.twist.covariance, GGroundTruthVariance); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + nav_msgs__msg__Odometry__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishOdometry(UrlabRclOdometryPub* Pub, const double PositionXyz[3], + const double OrientationXyzw[4], const double LinearBody[3], + const double AngularBody[3], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + geometry_msgs__msg__Pose& P = Pub->Msg.pose.pose; + P.position.x = PositionXyz ? PositionXyz[0] : 0.0; + P.position.y = PositionXyz ? PositionXyz[1] : 0.0; + P.position.z = PositionXyz ? PositionXyz[2] : 0.0; + P.orientation.x = OrientationXyzw ? OrientationXyzw[0] : 0.0; + P.orientation.y = OrientationXyzw ? OrientationXyzw[1] : 0.0; + P.orientation.z = OrientationXyzw ? OrientationXyzw[2] : 0.0; + P.orientation.w = OrientationXyzw ? OrientationXyzw[3] : 1.0; + geometry_msgs__msg__Twist& T = Pub->Msg.twist.twist; + T.linear.x = LinearBody ? LinearBody[0] : 0.0; + T.linear.y = LinearBody ? LinearBody[1] : 0.0; + T.linear.z = LinearBody ? LinearBody[2] : 0.0; + T.angular.x = AngularBody ? AngularBody[0] : 0.0; + T.angular.y = AngularBody ? AngularBody[1] : 0.0; + T.angular.z = AngularBody ? AngularBody[2] : 0.0; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyOdometryPub(UrlabRclOdometryPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + nav_msgs__msg__Odometry__fini(&Pub->Msg); + delete Pub; +} + +// --- PoseWithCovarianceStamped --------------------------------------------- + +UrlabRclPoseWithCovariancePub* UrlabRcl_CreatePoseWithCovariancePub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclPoseWithCovariancePub* Pub = new UrlabRclPoseWithCovariancePub(); + Pub->Ctx = Ctx; + geometry_msgs__msg__PoseWithCovarianceStamped__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + SetCovarianceDiagonal(Pub->Msg.pose.covariance, GGroundTruthVariance); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseWithCovarianceStamped); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + geometry_msgs__msg__PoseWithCovarianceStamped__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishPoseWithCovariance(UrlabRclPoseWithCovariancePub* Pub, + const double PositionXyz[3], const double OrientationXyzw[4], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + geometry_msgs__msg__Pose& P = Pub->Msg.pose.pose; + P.position.x = PositionXyz ? PositionXyz[0] : 0.0; + P.position.y = PositionXyz ? PositionXyz[1] : 0.0; + P.position.z = PositionXyz ? PositionXyz[2] : 0.0; + P.orientation.x = OrientationXyzw ? OrientationXyzw[0] : 0.0; + P.orientation.y = OrientationXyzw ? OrientationXyzw[1] : 0.0; + P.orientation.z = OrientationXyzw ? OrientationXyzw[2] : 0.0; + P.orientation.w = OrientationXyzw ? OrientationXyzw[3] : 1.0; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyPoseWithCovariancePub(UrlabRclPoseWithCovariancePub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + geometry_msgs__msg__PoseWithCovarianceStamped__fini(&Pub->Msg); + delete Pub; +} + +// --- CameraInfo ------------------------------------------------------------ + +UrlabRclCameraInfoPub* UrlabRcl_CreateCameraInfoPub(UrlabRclContext* Ctx, const char* Topic, + const char* FrameId, int32_t Width, int32_t Height, const double K9[9]) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclCameraInfoPub* Pub = new UrlabRclCameraInfoPub(); + Pub->Ctx = Ctx; + sensor_msgs__msg__CameraInfo__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + Pub->Msg.width = static_cast(Width > 0 ? Width : 0); + Pub->Msg.height = static_cast(Height > 0 ? Height : 0); + + // Zero-distortion pinhole: plumb_bob with five zero coefficients. + SetString(Pub->Msg.distortion_model, "plumb_bob"); + rosidl_runtime_c__double__Sequence__init(&Pub->Msg.d, 5); + for (int i = 0; i < 5; ++i) + { + Pub->Msg.d.data[i] = 0.0; + } + + // K (row-major 3x3) straight from the caller; R = identity; P = K with a zero + // translation column (monocular, no baseline). + for (int i = 0; i < 9; ++i) + { + Pub->Msg.k[i] = K9 ? K9[i] : 0.0; + } + for (int i = 0; i < 9; ++i) + { + Pub->Msg.r[i] = 0.0; + } + Pub->Msg.r[0] = Pub->Msg.r[4] = Pub->Msg.r[8] = 1.0; + for (int i = 0; i < 12; ++i) + { + Pub->Msg.p[i] = 0.0; + } + Pub->Msg.p[0] = Pub->Msg.k[0]; // fx + Pub->Msg.p[2] = Pub->Msg.k[2]; // cx + Pub->Msg.p[5] = Pub->Msg.k[4]; // fy + Pub->Msg.p[6] = Pub->Msg.k[5]; // cy + Pub->Msg.p[10] = 1.0; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, CameraInfo); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__CameraInfo__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishCameraInfo(UrlabRclCameraInfoPub* Pub, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyCameraInfoPub(UrlabRclCameraInfoPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__CameraInfo__fini(&Pub->Msg); + delete Pub; +} + +// --- Bool (typed user channel) --------------------------------------------- + +UrlabRclBoolPub* UrlabRcl_CreateBoolPub(UrlabRclContext* Ctx, const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclBoolPub* Pub = new UrlabRclBoolPub(); + Pub->Ctx = Ctx; + std_msgs__msg__Bool__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + std_msgs__msg__Bool__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishBool(UrlabRclBoolPub* Pub, int32_t bValue) +{ + ClearError(); + if (!Pub) + { + return -1; + } + Pub->Msg.data = bValue != 0; + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyBoolPub(UrlabRclBoolPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + std_msgs__msg__Bool__fini(&Pub->Msg); + delete Pub; +} + +// --- Float64 (typed user channel) ------------------------------------------ + +UrlabRclFloat64Pub* UrlabRcl_CreateFloat64Pub(UrlabRclContext* Ctx, const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclFloat64Pub* Pub = new UrlabRclFloat64Pub(); + Pub->Ctx = Ctx; + std_msgs__msg__Float64__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + std_msgs__msg__Float64__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishFloat64(UrlabRclFloat64Pub* Pub, double Value) +{ + ClearError(); + if (!Pub) + { + return -1; + } + Pub->Msg.data = Value; + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyFloat64Pub(UrlabRclFloat64Pub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + std_msgs__msg__Float64__fini(&Pub->Msg); + delete Pub; +} + +// --- Vector3 (typed user channel) ------------------------------------------ + +UrlabRclVector3Pub* UrlabRcl_CreateVector3Pub(UrlabRclContext* Ctx, const char* Topic) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclVector3Pub* Pub = new UrlabRclVector3Pub(); + Pub->Ctx = Ctx; + geometry_msgs__msg__Vector3__init(&Pub->Msg); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Vector3); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + geometry_msgs__msg__Vector3__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishVector3(UrlabRclVector3Pub* Pub, const double Xyz[3]) +{ + ClearError(); + if (!Pub) + { + return -1; + } + Pub->Msg.x = Xyz ? Xyz[0] : 0.0; + Pub->Msg.y = Xyz ? Xyz[1] : 0.0; + Pub->Msg.z = Xyz ? Xyz[2] : 0.0; + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyVector3Pub(UrlabRclVector3Pub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + geometry_msgs__msg__Vector3__fini(&Pub->Msg); + delete Pub; +} + +// --- PoseStamped (typed user channel) -------------------------------------- + +UrlabRclPoseStampedPub* UrlabRcl_CreatePoseStampedPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclPoseStampedPub* Pub = new UrlabRclPoseStampedPub(); + Pub->Ctx = Ctx; + geometry_msgs__msg__PoseStamped__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, PoseStamped); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + geometry_msgs__msg__PoseStamped__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishPoseStamped(UrlabRclPoseStampedPub* Pub, const double PositionXyz[3], + const double OrientationXyzw[4], int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + geometry_msgs__msg__Pose& P = Pub->Msg.pose; + P.position.x = PositionXyz ? PositionXyz[0] : 0.0; + P.position.y = PositionXyz ? PositionXyz[1] : 0.0; + P.position.z = PositionXyz ? PositionXyz[2] : 0.0; + P.orientation.x = OrientationXyzw ? OrientationXyzw[0] : 0.0; + P.orientation.y = OrientationXyzw ? OrientationXyzw[1] : 0.0; + P.orientation.z = OrientationXyzw ? OrientationXyzw[2] : 0.0; + P.orientation.w = OrientationXyzw ? OrientationXyzw[3] : 1.0; + + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyPoseStampedPub(UrlabRclPoseStampedPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + geometry_msgs__msg__PoseStamped__fini(&Pub->Msg); + delete Pub; +} + +// --- Subscriptions --------------------------------------------------------- + +UrlabRclCtrlSub* UrlabRcl_CreateCtrlSub(UrlabRclContext* Ctx, const char* Topic, + UrlabRclCtrlCallback Callback, void* User) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclCtrlSub* Sub = new UrlabRclCtrlSub(); + Sub->Rec.Kind = ESubKind::Ctrl; + Sub->Rec.Ctx = Ctx; + Sub->Rec.CtrlCallback = Callback; + Sub->Rec.TwistCallback = nullptr; + Sub->Rec.User = User; + std_msgs__msg__Float64MultiArray__init(&Sub->Rec.CtrlMsg); + + Sub->Rec.Sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t Options = rcl_subscription_get_default_options(); + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray); + const rcl_ret_t Ret = rcl_subscription_init(&Sub->Rec.Sub, &Ctx->Node, Ts, Topic, &Options); + if (Ret != RCL_RET_OK) + { + CaptureError(); + std_msgs__msg__Float64MultiArray__fini(&Sub->Rec.CtrlMsg); + delete Sub; + return nullptr; + } + Ctx->Subs.push_back(&Sub->Rec); + return Sub; +} + +void UrlabRcl_DestroyCtrlSub(UrlabRclCtrlSub* Sub) +{ + if (!Sub) + { + return; + } + DetachSub(&Sub->Rec); + rcl_subscription_fini(&Sub->Rec.Sub, &Sub->Rec.Ctx->Node); + std_msgs__msg__Float64MultiArray__fini(&Sub->Rec.CtrlMsg); + delete Sub; +} + +UrlabRclTwistSub* UrlabRcl_CreateTwistSub(UrlabRclContext* Ctx, const char* Topic, + UrlabRclTwistCallback Callback, void* User) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclTwistSub* Sub = new UrlabRclTwistSub(); + Sub->Rec.Kind = ESubKind::Twist; + Sub->Rec.Ctx = Ctx; + Sub->Rec.CtrlCallback = nullptr; + Sub->Rec.TwistCallback = Callback; + Sub->Rec.User = User; + geometry_msgs__msg__Twist__init(&Sub->Rec.TwistMsg); + + Sub->Rec.Sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t Options = rcl_subscription_get_default_options(); + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist); + const rcl_ret_t Ret = rcl_subscription_init(&Sub->Rec.Sub, &Ctx->Node, Ts, Topic, &Options); + if (Ret != RCL_RET_OK) + { + CaptureError(); + geometry_msgs__msg__Twist__fini(&Sub->Rec.TwistMsg); + delete Sub; + return nullptr; + } + Ctx->Subs.push_back(&Sub->Rec); + return Sub; +} + +void UrlabRcl_DestroyTwistSub(UrlabRclTwistSub* Sub) +{ + if (!Sub) + { + return; + } + DetachSub(&Sub->Rec); + rcl_subscription_fini(&Sub->Rec.Sub, &Sub->Rec.Ctx->Node); + geometry_msgs__msg__Twist__fini(&Sub->Rec.TwistMsg); + delete Sub; +} + +UrlabRclJointStateSub* UrlabRcl_CreateJointStateSub(UrlabRclContext* Ctx, const char* Topic, + UrlabRclJointStateCallback Callback, void* User) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclJointStateSub* Sub = new UrlabRclJointStateSub(); + Sub->Rec.Kind = ESubKind::JointState; + Sub->Rec.Ctx = Ctx; + Sub->Rec.CtrlCallback = nullptr; + Sub->Rec.TwistCallback = nullptr; + Sub->Rec.JointStateCallback = Callback; + Sub->Rec.User = User; + sensor_msgs__msg__JointState__init(&Sub->Rec.JointStateMsg); + + Sub->Rec.Sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t Options = rcl_subscription_get_default_options(); + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState); + const rcl_ret_t Ret = rcl_subscription_init(&Sub->Rec.Sub, &Ctx->Node, Ts, Topic, &Options); + if (Ret != RCL_RET_OK) + { + CaptureError(); + sensor_msgs__msg__JointState__fini(&Sub->Rec.JointStateMsg); + delete Sub; + return nullptr; + } + Ctx->Subs.push_back(&Sub->Rec); + return Sub; +} + +void UrlabRcl_DestroyJointStateSub(UrlabRclJointStateSub* Sub) +{ + if (!Sub) + { + return; + } + DetachSub(&Sub->Rec); + rcl_subscription_fini(&Sub->Rec.Sub, &Sub->Rec.Ctx->Node); + sensor_msgs__msg__JointState__fini(&Sub->Rec.JointStateMsg); + delete Sub; +} + +// --- Services -------------------------------------------------------------- + +UrlabRclTriggerService* UrlabRcl_CreateTriggerService(UrlabRclContext* Ctx, + const char* ServiceName, UrlabRclTriggerCallback Callback, void* User) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclTriggerService* Srv = new UrlabRclTriggerService(); + Srv->Rec.Ctx = Ctx; + Srv->Rec.Callback = Callback; + Srv->Rec.User = User; + std_srvs__srv__Trigger_Request__init(&Srv->Rec.Request); + std_srvs__srv__Trigger_Response__init(&Srv->Rec.Response); + + Srv->Rec.Srv = rcl_get_zero_initialized_service(); + rcl_service_options_t Options = rcl_service_get_default_options(); + const rosidl_service_type_support_t* Ts = + ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger); + const rcl_ret_t Ret = rcl_service_init(&Srv->Rec.Srv, &Ctx->Node, Ts, ServiceName, &Options); + if (Ret != RCL_RET_OK) + { + CaptureError(); + std_srvs__srv__Trigger_Request__fini(&Srv->Rec.Request); + std_srvs__srv__Trigger_Response__fini(&Srv->Rec.Response); + delete Srv; + return nullptr; + } + Ctx->Srvs.push_back(&Srv->Rec); + return Srv; +} + +void UrlabRcl_DestroyTriggerService(UrlabRclTriggerService* Srv) +{ + if (!Srv) + { + return; + } + DetachSrv(&Srv->Rec); + rcl_service_fini(&Srv->Rec.Srv, &Srv->Rec.Ctx->Node); + std_srvs__srv__Trigger_Request__fini(&Srv->Rec.Request); + std_srvs__srv__Trigger_Response__fini(&Srv->Rec.Response); + delete Srv; +} + +int UrlabRcl_SpinSome(UrlabRclContext* Ctx, int64_t TimeoutNs) +{ + ClearError(); + if (!Ctx) + { + return -1; + } + const size_t NSubs = Ctx->Subs.size(); + const size_t NSrvs = Ctx->Srvs.size(); + if (NSubs == 0 && NSrvs == 0) + { + return 0; + } + + rcl_wait_set_t WaitSet = rcl_get_zero_initialized_wait_set(); + rcl_ret_t Ret = rcl_wait_set_init(&WaitSet, NSubs, 0, 0, 0, NSrvs, 0, + &Ctx->Context, Ctx->Allocator); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + + Ret = rcl_wait_set_clear(&WaitSet); + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_wait_set_fini(&WaitSet); + return -static_cast(Ret); + } + for (FSubRecord* Rec : Ctx->Subs) + { + rcl_wait_set_add_subscription(&WaitSet, &Rec->Sub, nullptr); + } + for (FSrvRecord* Rec : Ctx->Srvs) + { + rcl_wait_set_add_service(&WaitSet, &Rec->Srv, nullptr); + } + + Ret = rcl_wait(&WaitSet, TimeoutNs); + if (Ret == RCL_RET_TIMEOUT) + { + rcl_wait_set_fini(&WaitSet); + return 0; + } + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_wait_set_fini(&WaitSet); + return -static_cast(Ret); + } + + for (size_t i = 0; i < NSubs; ++i) + { + if (WaitSet.subscriptions[i] == nullptr) + { + continue; + } + FSubRecord* Rec = Ctx->Subs[i]; + if (Rec->Kind == ESubKind::Ctrl) + { + rmw_message_info_t Info = rmw_get_zero_initialized_message_info(); + const rcl_ret_t Take = rcl_take(&Rec->Sub, &Rec->CtrlMsg, &Info, nullptr); + if (Take == RCL_RET_OK && Rec->CtrlCallback) + { + Rec->CtrlCallback(Rec->CtrlMsg.data.data, + static_cast(Rec->CtrlMsg.data.size), Rec->User); + } + } + else if (Rec->Kind == ESubKind::Twist) + { + rmw_message_info_t Info = rmw_get_zero_initialized_message_info(); + const rcl_ret_t Take = rcl_take(&Rec->Sub, &Rec->TwistMsg, &Info, nullptr); + if (Take == RCL_RET_OK && Rec->TwistCallback) + { + const double Linear[3] = { + Rec->TwistMsg.linear.x, Rec->TwistMsg.linear.y, Rec->TwistMsg.linear.z}; + const double Angular[3] = { + Rec->TwistMsg.angular.x, Rec->TwistMsg.angular.y, Rec->TwistMsg.angular.z}; + Rec->TwistCallback(Linear, Angular, Rec->User); + } + } + else // ESubKind::JointState + { + rmw_message_info_t Info = rmw_get_zero_initialized_message_info(); + const rcl_ret_t Take = rcl_take(&Rec->Sub, &Rec->JointStateMsg, &Info, nullptr); + if (Take == RCL_RET_OK && Rec->JointStateCallback) + { + const sensor_msgs__msg__JointState& Msg = Rec->JointStateMsg; + const size_t Count = Msg.name.size < Msg.position.size + ? Msg.name.size + : Msg.position.size; + std::vector Names(Count); + for (size_t j = 0; j < Count; ++j) + { + Names[j] = Msg.name.data[j].data ? Msg.name.data[j].data : ""; + } + Rec->JointStateCallback(Count > 0 ? Names.data() : nullptr, + Count > 0 ? Msg.position.data : nullptr, + static_cast(Count), Rec->User); + } + } + } + + for (size_t i = 0; i < NSrvs; ++i) + { + if (WaitSet.services[i] == nullptr) + { + continue; + } + FSrvRecord* Rec = Ctx->Srvs[i]; + rmw_request_id_t Header; + std::memset(&Header, 0, sizeof(Header)); + const rcl_ret_t Take = rcl_take_request(&Rec->Srv, &Header, &Rec->Request); + if (Take != RCL_RET_OK) + { + continue; + } + int32_t bSuccess = 0; + char MessageBuf[512] = {0}; + if (Rec->Callback) + { + Rec->Callback(Rec->User, &bSuccess, MessageBuf, static_cast(sizeof(MessageBuf))); + } + MessageBuf[sizeof(MessageBuf) - 1] = '\0'; + Rec->Response.success = bSuccess != 0; + SetString(Rec->Response.message, MessageBuf); + const rcl_ret_t Send = rcl_send_response(&Rec->Srv, &Header, &Rec->Response); + if (Send != RCL_RET_OK) + { + CaptureError(); + } + } + + rcl_wait_set_fini(&WaitSet); + return 0; +} + +// --- Zero-copy Clock ------------------------------------------------------- + +int UrlabRcl_ClockCanLoan(UrlabRclClockPub* Pub) +{ + if (!Pub) + { + return 0; + } + return rcl_publisher_can_loan_messages(&Pub->Pub) ? 1 : 0; +} + +int UrlabRcl_PublishClockLoaned(UrlabRclClockPub* Pub, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + if (!rcl_publisher_can_loan_messages(&Pub->Pub)) + { + return UrlabRcl_PublishClock(Pub, SimTimeNs); + } + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(rosgraph_msgs, msg, Clock); + void* Loaned = nullptr; + rcl_ret_t Ret = rcl_borrow_loaned_message(&Pub->Pub, Ts, &Loaned); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + rosgraph_msgs__msg__Clock* Msg = static_cast(Loaned); + FillStamp(Msg->clock, SimTimeNs); + Ret = rcl_publish_loaned_message(&Pub->Pub, Loaned, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + rcl_return_loaned_message_from_publisher(&Pub->Pub, Loaned); + return -static_cast(Ret); + } + return 0; +} + +// --- moveit_msgs/PlanningScene -------------------------------------------- +struct UrlabRclPlanningScenePub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + moveit_msgs__msg__PlanningScene Msg; +}; + +struct UrlabRclPointCloud2Pub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + sensor_msgs__msg__PointCloud2 Msg; + int32_t MaxPoints; +}; + +struct UrlabRclOccupancyGridPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + nav_msgs__msg__OccupancyGrid Msg; +}; + +struct UrlabRclOctomapPub +{ + UrlabRclContext* Ctx; + rcl_publisher_t Pub; + octomap_msgs__msg__Octomap Msg; +}; + +UrlabRclPlanningScenePub* UrlabRcl_CreatePlanningScenePub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, const char** Ids, + const int32_t* PrimTypes, const double* Dims, const int32_t* MeshVertCounts, + const double* MeshVerts, const int32_t* MeshTriCounts, const int32_t* MeshTris, + int32_t Count) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclPlanningScenePub* Pub = new UrlabRclPlanningScenePub(); + Pub->Ctx = Ctx; + moveit_msgs__msg__PlanningScene__init(&Pub->Msg); + Pub->Msg.is_diff = true; + + const int32_t N = Count > 0 ? Count : 0; + // Running offsets into the flattened mesh arrays (advanced past every object, + // primitive or mesh, so a primitive contributes 0 and mesh objects stay aligned). + int64_t VertOff = 0; + int64_t TriOff = 0; + moveit_msgs__msg__CollisionObject__Sequence__init(&Pub->Msg.world.collision_objects, N); + for (int32_t i = 0; i < N; ++i) + { + moveit_msgs__msg__CollisionObject* CO = &Pub->Msg.world.collision_objects.data[i]; + SetString(CO->header.frame_id, FrameId ? FrameId : "world"); + SetString(CO->id, (Ids && Ids[i]) ? Ids[i] : ""); + CO->operation = 0; // ADD + CO->pose.orientation.w = 1.0; // object frame; placement filled per-publish + + const uint8_t Type = PrimTypes ? (uint8_t)PrimTypes[i] : 1; + const int32_t VertCount = MeshVertCounts ? MeshVertCounts[i] : 0; + const int32_t TriCount = MeshTriCounts ? MeshTriCounts[i] : 0; + + if (Type == 4) // MESH + { + shape_msgs__msg__Mesh__Sequence__init(&CO->meshes, 1); + shape_msgs__msg__Mesh* Me = &CO->meshes.data[0]; + geometry_msgs__msg__Point__Sequence__init(&Me->vertices, VertCount); + for (int32_t v = 0; v < VertCount; ++v) + { + const double* Vp = &MeshVerts[(VertOff + v) * 3]; + Me->vertices.data[v].x = Vp[0]; + Me->vertices.data[v].y = Vp[1]; + Me->vertices.data[v].z = Vp[2]; + } + shape_msgs__msg__MeshTriangle__Sequence__init(&Me->triangles, TriCount); + for (int32_t t = 0; t < TriCount; ++t) + { + const int32_t* Tp = &MeshTris[(TriOff + t) * 3]; + Me->triangles.data[t].vertex_indices[0] = (uint32_t)Tp[0]; + Me->triangles.data[t].vertex_indices[1] = (uint32_t)Tp[1]; + Me->triangles.data[t].vertex_indices[2] = (uint32_t)Tp[2]; + } + geometry_msgs__msg__Pose__Sequence__init(&CO->mesh_poses, 1); + CO->mesh_poses.data[0].orientation.w = 1.0; // identity vs the object pose + } + else + { + shape_msgs__msg__SolidPrimitive__Sequence__init(&CO->primitives, 1); + shape_msgs__msg__SolidPrimitive* P = &CO->primitives.data[0]; + P->type = Type; + const int DimN = (Type == 1) ? 3 : (Type == 3 ? 2 : 1); // BOX 3, CYL 2, SPH 1 + rosidl_runtime_c__double__Sequence__init(&P->dimensions, DimN); + for (int k = 0; k < DimN; ++k) + { + P->dimensions.data[k] = Dims ? Dims[i * 3 + k] : 0.0; + } + + geometry_msgs__msg__Pose__Sequence__init(&CO->primitive_poses, 1); + CO->primitive_poses.data[0].orientation.w = 1.0; // identity vs the object pose + } + VertOff += VertCount; + TriOff += TriCount; + } + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(moveit_msgs, msg, PlanningScene); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + moveit_msgs__msg__PlanningScene__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishPlanningScene(UrlabRclPlanningScenePub* Pub, + const double* Poses, int32_t Count, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + const int32_t N = static_cast(Pub->Msg.world.collision_objects.size); + const int32_t M = Count < N ? Count : N; + for (int32_t i = 0; i < M; ++i) + { + moveit_msgs__msg__CollisionObject* CO = &Pub->Msg.world.collision_objects.data[i]; + FillStamp(CO->header.stamp, SimTimeNs); + CO->operation = 0; // ADD (re-add replaces, keeping the scene current) + const double* P = &Poses[i * 7]; + CO->pose.position.x = P[0]; + CO->pose.position.y = P[1]; + CO->pose.position.z = P[2]; + CO->pose.orientation.x = P[3]; + CO->pose.orientation.y = P[4]; + CO->pose.orientation.z = P[5]; + CO->pose.orientation.w = P[6]; + } + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyPlanningScenePub(UrlabRclPlanningScenePub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + moveit_msgs__msg__PlanningScene__fini(&Pub->Msg); + delete Pub; +} + +// --- PointCloud2 ---------------------------------------------------------- + +namespace +{ +constexpr int32_t GDefaultMaxPoints = 200'000; + +void InitPointField(sensor_msgs__msg__PointField& F, const char* Name, + uint32_t Offset, uint8_t DataType) +{ + SetString(F.name, Name); + F.offset = Offset; + F.datatype = DataType; + F.count = 1; +} +} // namespace + +UrlabRclPointCloud2Pub* UrlabRcl_CreatePointCloud2Pub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, int32_t MaxPoints) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclPointCloud2Pub* Pub = new UrlabRclPointCloud2Pub(); + Pub->Ctx = Ctx; + Pub->MaxPoints = MaxPoints > 0 ? MaxPoints : GDefaultMaxPoints; + sensor_msgs__msg__PointCloud2__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + Pub->Msg.height = 1; + Pub->Msg.is_bigendian = false; + Pub->Msg.is_dense = true; + Pub->Msg.point_step = 12; // 3 * float32 + sensor_msgs__msg__PointField__Sequence__init(&Pub->Msg.fields, 3); + InitPointField(Pub->Msg.fields.data[0], "x", 0, 7); + InitPointField(Pub->Msg.fields.data[1], "y", 4, 7); + InitPointField(Pub->Msg.fields.data[2], "z", 8, 7); + rosidl_runtime_c__uint8__Sequence__init(&Pub->Msg.data, + static_cast(Pub->MaxPoints) * 12); + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, PointCloud2); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + sensor_msgs__msg__PointCloud2__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishPointCloud2(UrlabRclPointCloud2Pub* Pub, + const float* Points, int32_t N, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + const int32_t Count = N > 0 ? N : 0; + const int32_t Capped = Count > Pub->MaxPoints ? Pub->MaxPoints : Count; + const size_t ByteSize = static_cast(Capped) * 12; + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + Pub->Msg.width = static_cast(Capped); + Pub->Msg.row_step = static_cast(ByteSize); + if (Pub->Msg.data.capacity < ByteSize) + { + rosidl_runtime_c__uint8__Sequence__fini(&Pub->Msg.data); + rosidl_runtime_c__uint8__Sequence__init(&Pub->Msg.data, ByteSize); + } + if (Points && Capped > 0) + { + std::memcpy(Pub->Msg.data.data, Points, ByteSize); + Pub->Msg.data.size = ByteSize; + } + else + { + Pub->Msg.data.size = 0; + Pub->Msg.width = 0; + Pub->Msg.row_step = 0; + } + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyPointCloud2Pub(UrlabRclPointCloud2Pub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + sensor_msgs__msg__PointCloud2__fini(&Pub->Msg); + delete Pub; +} + +// --- OccupancyGrid -------------------------------------------------------- + +UrlabRclOccupancyGridPub* UrlabRcl_CreateOccupancyGridPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, double Resolution, + int32_t Width, int32_t Height, double OriginX, double OriginY) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclOccupancyGridPub* Pub = new UrlabRclOccupancyGridPub(); + Pub->Ctx = Ctx; + nav_msgs__msg__OccupancyGrid__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + Pub->Msg.info.resolution = static_cast(Resolution); + Pub->Msg.info.width = static_cast(Width > 0 ? Width : 0); + Pub->Msg.info.height = static_cast(Height > 0 ? Height : 0); + Pub->Msg.info.origin.position.x = OriginX; + Pub->Msg.info.origin.position.y = OriginY; + Pub->Msg.info.origin.position.z = 0.0; + Pub->Msg.info.origin.orientation.w = 1.0; + const size_t CellCount = static_cast(Pub->Msg.info.width) * + static_cast(Pub->Msg.info.height); + rosidl_runtime_c__int8__Sequence__init(&Pub->Msg.data, CellCount); + if (CellCount > 0) + { + std::memset(Pub->Msg.data.data, 0xFF, CellCount); + Pub->Msg.data.size = CellCount; + } + + rmw_qos_profile_t Qos = rmw_qos_profile_default; + Qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + Qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + Qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + Qos.depth = 1; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, OccupancyGrid); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, Qos)) + { + nav_msgs__msg__OccupancyGrid__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishOccupancyGrid(UrlabRclOccupancyGridPub* Pub, + const int8_t* Data, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + const size_t CellCount = static_cast(Pub->Msg.info.width) * + static_cast(Pub->Msg.info.height); + if (Data && CellCount > 0) + { + std::memcpy(Pub->Msg.data.data, Data, CellCount); + Pub->Msg.data.size = CellCount; + } + else + { + Pub->Msg.data.size = 0; + } + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyOccupancyGridPub(UrlabRclOccupancyGridPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + nav_msgs__msg__OccupancyGrid__fini(&Pub->Msg); + delete Pub; +} + +// --- Octomap -------------------------------------------------------------- + +UrlabRclOctomapPub* UrlabRcl_CreateOctomapPub(UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, double Resolution) +{ + ClearError(); + if (!Ctx) + { + return nullptr; + } + UrlabRclOctomapPub* Pub = new UrlabRclOctomapPub(); + Pub->Ctx = Ctx; + octomap_msgs__msg__Octomap__init(&Pub->Msg); + SetString(Pub->Msg.header.frame_id, FrameId); + Pub->Msg.binary = true; + SetString(Pub->Msg.id, "OcTree"); + Pub->Msg.resolution = Resolution; + + const rosidl_message_type_support_t* Ts = + ROSIDL_GET_MSG_TYPE_SUPPORT(octomap_msgs, msg, Octomap); + if (!InitPublisher(Ctx, Pub->Pub, Ts, Topic, rmw_qos_profile_default)) + { + octomap_msgs__msg__Octomap__fini(&Pub->Msg); + delete Pub; + return nullptr; + } + return Pub; +} + +int UrlabRcl_PublishOctomap(UrlabRclOctomapPub* Pub, + const uint8_t* Data, int32_t Size, int64_t SimTimeNs) +{ + ClearError(); + if (!Pub) + { + return -1; + } + FillStamp(Pub->Msg.header.stamp, SimTimeNs); + const size_t S = static_cast(Size > 0 ? Size : 0); + if (Pub->Msg.data.capacity < S) + { + rosidl_runtime_c__int8__Sequence__fini(&Pub->Msg.data); + rosidl_runtime_c__int8__Sequence__init(&Pub->Msg.data, S); + } + if (Data && S > 0) + { + std::memcpy(Pub->Msg.data.data, Data, S); + Pub->Msg.data.size = S; + } + else + { + Pub->Msg.data.size = 0; + } + const rcl_ret_t Ret = rcl_publish(&Pub->Pub, &Pub->Msg, nullptr); + if (Ret != RCL_RET_OK) + { + CaptureError(); + return -static_cast(Ret); + } + return 0; +} + +void UrlabRcl_DestroyOctomapPub(UrlabRclOctomapPub* Pub) +{ + if (!Pub) + { + return; + } + rcl_publisher_fini(&Pub->Pub, &Pub->Ctx->Node); + octomap_msgs__msg__Octomap__fini(&Pub->Msg); + delete Pub; +} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Ros/UrlabRclCore.h b/Source/URLabRos/Private/Ros/UrlabRclCore.h new file mode 100644 index 00000000..d8523310 --- /dev/null +++ b/Source/URLabRos/Private/Ros/UrlabRclCore.h @@ -0,0 +1,343 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +// UrlabRclCore is the single UE-agnostic seam that owns every rcl / rosidl call +// in the project. This header exposes only opaque handles and POD/C-friendly +// free functions: no UE types, no rcl types, no rosidl types, no STL in any +// signature. It is includable from UE module code and from the standalone ROS +// workspace harness alike; its only include is . +// +// Return convention: functions returning int use 0 = ok, negative = a mapped +// rcl_ret_t. Create* functions return a handle pointer, null on failure. +// UrlabRcl_LastError() returns the rcutils error string from the most recent +// failed call, for logging. Every Create* has a matching Destroy that takes its +// handle; all destroy/shutdown functions are null-safe. +// +// Time is sim time in nanoseconds (int64_t); the core splits it into sec/nsec. +// Quaternions are xyzw ordered. +// +// Threading: the core takes no locks. Create and destroy all handles from one +// thread, serialize calls per handle, and note that UrlabRcl_SpinSome runs +// subscription callbacks on its caller's thread. + +#include + +#ifdef __cplusplus +extern "C" +{ +#endif + + // Opaque handles. Definitions live only in UrlabRclCore.cpp. + struct UrlabRclContext; // rcl init options + context + one node + struct UrlabRclJointStatePub; + struct UrlabRclImuPub; + struct UrlabRclTfPub; + struct UrlabRclTwistStampedPub; + struct UrlabRclClockPub; + struct UrlabRclImagePub; + struct UrlabRclCtrlPub; + struct UrlabRclStringPub; + struct UrlabRclWrenchStampedPub; + struct UrlabRclRangePub; + struct UrlabRclMagneticFieldPub; + struct UrlabRclFloat64MultiArrayPub; + struct UrlabRclOdometryPub; + struct UrlabRclPoseWithCovariancePub; + struct UrlabRclCameraInfoPub; + struct UrlabRclBoolPub; + struct UrlabRclFloat64Pub; + struct UrlabRclVector3Pub; + struct UrlabRclPoseStampedPub; + struct UrlabRclPlanningScenePub; + struct UrlabRclPointCloud2Pub; + struct UrlabRclOccupancyGridPub; + struct UrlabRclOctomapPub; + struct UrlabRclCtrlSub; + struct UrlabRclTwistSub; + struct UrlabRclJointStateSub; + struct UrlabRclTriggerService; + + // --- Context --------------------------------------------------------------- + // DomainId -1 = use the ROS_DOMAIN_ID environment variable. Returns null on + // failure (query UrlabRcl_LastError for the reason). + struct UrlabRclContext* UrlabRcl_Init(const char* NodeName, + const char* NodeNamespace, int32_t DomainId); + void UrlabRcl_Shutdown(struct UrlabRclContext* Ctx); // fini node/context, reverse order + const char* UrlabRcl_DistroName(); // compile-time pin, for the facts file + const char* UrlabRcl_LastError(); + + // --- Publishers ------------------------------------------------------------ + // Joint-name arrays are copied at create time (sized to the model); message + // structs are preallocated per handle. + struct UrlabRclJointStatePub* UrlabRcl_CreateJointStatePub(struct UrlabRclContext* Ctx, + const char* Topic, const char** JointNames, int32_t JointCount); + int UrlabRcl_PublishJointState(struct UrlabRclJointStatePub* Pub, + const double* Positions, const double* Velocities, const double* Efforts, + int32_t Count, int64_t SimTimeNs); // Velocities/Efforts may be null + void UrlabRcl_DestroyJointStatePub(struct UrlabRclJointStatePub* Pub); + + struct UrlabRclImuPub* UrlabRcl_CreateImuPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId); + int UrlabRcl_PublishImu(struct UrlabRclImuPub* Pub, const double AngularVel[3], + const double LinearAccel[3], const double OrientationXyzw[4], + int64_t SimTimeNs); // any array may be null (unpaired gyro) + void UrlabRcl_DestroyImuPub(struct UrlabRclImuPub* Pub); + + struct UrlabRclTfPub* UrlabRcl_CreateTfPub(struct UrlabRclContext* Ctx, int32_t bStatic); + // bStatic != 0: /tf_static with transient-local QoS; else /tf + int UrlabRcl_PublishTf(struct UrlabRclTfPub* Pub, const char** ParentFrameIds, + const char** ChildFrameIds, const double* TranslationsXyz /* 3*Count */, + const double* RotationsXyzw /* 4*Count */, int32_t Count, + int64_t SimTimeNs); + void UrlabRcl_DestroyTfPub(struct UrlabRclTfPub* Pub); + + // moveit_msgs/PlanningScene (is_diff) on /planning_scene: the world's non-robot + // collision geometry as CollisionObjects. Create fixes the object set (ids + + // shapes); Publish updates their world poses each step. PrimTypes are + // shape_msgs/SolidPrimitive.type constants (1=BOX, 2=SPHERE, 3=CYLINDER) with Dims + // 3 per object (BOX: full extents x,y,z; SPHERE: [radius,_,_]; CYLINDER: + // [height, radius, _]), or 4=MESH which reads geometry from the mesh arrays + // instead of Dims. Mesh arrays are flattened across all objects: for object i, + // MeshVertCounts[i] vertices (3 doubles each) and MeshTriCounts[i] triangles + // (3 int indices each), concatenated in object order (0 for primitive objects). + struct UrlabRclPlanningScenePub* UrlabRcl_CreatePlanningScenePub( + struct UrlabRclContext* Ctx, const char* Topic, const char* FrameId, + const char** Ids, const int32_t* PrimTypes, const double* Dims, + const int32_t* MeshVertCounts, const double* MeshVerts, + const int32_t* MeshTriCounts, const int32_t* MeshTris, int32_t Count); + int UrlabRcl_PublishPlanningScene(struct UrlabRclPlanningScenePub* Pub, + const double* Poses /* 7*Count: px,py,pz,qx,qy,qz,qw */, int32_t Count, + int64_t SimTimeNs); + void UrlabRcl_DestroyPlanningScenePub(struct UrlabRclPlanningScenePub* Pub); + + // sensor_msgs/PointCloud2, unordered (height=1), frame_id set at create. + // max_points caps the pre-allocated data array; 0 uses a default (200 kpts). + struct UrlabRclPointCloud2Pub* UrlabRcl_CreatePointCloud2Pub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, int32_t MaxPoints); + int UrlabRcl_PublishPointCloud2(struct UrlabRclPointCloud2Pub* Pub, + const float* Points, int32_t N, int64_t SimTimeNs); + void UrlabRcl_DestroyPointCloud2Pub(struct UrlabRclPointCloud2Pub* Pub); + + // nav_msgs/OccupancyGrid, latched (transient-local), fixed grid at create. + // OriginX/Y are the world-frame coordinate of the grid's bottom-left cell centre. + struct UrlabRclOccupancyGridPub* UrlabRcl_CreateOccupancyGridPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, double Resolution, + int32_t Width, int32_t Height, double OriginX, double OriginY); + int UrlabRcl_PublishOccupancyGrid(struct UrlabRclOccupancyGridPub* Pub, + const int8_t* Data, int64_t SimTimeNs); + void UrlabRcl_DestroyOccupancyGridPub(struct UrlabRclOccupancyGridPub* Pub); + + // octomap_msgs/Octomap (binary=true). Data is the serialised octree bytes; + // the tree id and resolution are fixed at create. + struct UrlabRclOctomapPub* UrlabRcl_CreateOctomapPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, double Resolution); + int UrlabRcl_PublishOctomap(struct UrlabRclOctomapPub* Pub, + const uint8_t* Data, int32_t Size, int64_t SimTimeNs); + void UrlabRcl_DestroyOctomapPub(struct UrlabRclOctomapPub* Pub); + + struct UrlabRclTwistStampedPub* UrlabRcl_CreateTwistStampedPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId); + int UrlabRcl_PublishTwistStamped(struct UrlabRclTwistStampedPub* Pub, + const double Linear[3], const double Angular[3], int64_t SimTimeNs); + void UrlabRcl_DestroyTwistStampedPub(struct UrlabRclTwistStampedPub* Pub); + + struct UrlabRclClockPub* UrlabRcl_CreateClockPub(struct UrlabRclContext* Ctx); // topic /clock + int UrlabRcl_PublishClock(struct UrlabRclClockPub* Pub, int64_t SimTimeNs); + void UrlabRcl_DestroyClockPub(struct UrlabRclClockPub* Pub); + + struct UrlabRclImagePub* UrlabRcl_CreateImagePub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, int32_t Width, int32_t Height, + const char* Encoding); // ROS encoding string, e.g. "rgb8"/"bgra8" + int UrlabRcl_PublishImage(struct UrlabRclImagePub* Pub, const uint8_t* Data, + int32_t StrideBytes, int64_t SimTimeNs); + void UrlabRcl_DestroyImagePub(struct UrlabRclImagePub* Pub); + + // Control injection: the publish counterpart to the //cmd_ctrl + // subscription below (std_msgs/Float64MultiArray). It exists so control can be + // injected in-process for loopback and tests without a second ROS node. + struct UrlabRclCtrlPub* UrlabRcl_CreateCtrlPub(struct UrlabRclContext* Ctx, + const char* Topic); + int UrlabRcl_PublishCtrl(struct UrlabRclCtrlPub* Pub, const double* Values, + int32_t Count); + void UrlabRcl_DestroyCtrlPub(struct UrlabRclCtrlPub* Pub); + + // Latched std_msgs/String publisher, transient-local + reliable + keep-last + // depth 1, for the per-art //robot_description URDF. The QoS matches + // robot_state_publisher so late-joining subscribers (rviz, MoveIt) receive the + // last published document. The text is copied on each publish. + struct UrlabRclStringPub* UrlabRcl_CreateStringPub(struct UrlabRclContext* Ctx, + const char* Topic); + int UrlabRcl_PublishString(struct UrlabRclStringPub* Pub, const char* Text); + void UrlabRcl_DestroyStringPub(struct UrlabRclStringPub* Pub); + + // geometry_msgs/WrenchStamped, for MuJoCo force + torque sensors paired on a + // site. Force and Torque are 3-vectors in the sensor frame; either may be null + // (an unpaired force or torque publishes its half, the other left zero). + struct UrlabRclWrenchStampedPub* UrlabRcl_CreateWrenchStampedPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId); + int UrlabRcl_PublishWrenchStamped(struct UrlabRclWrenchStampedPub* Pub, + const double Force[3], const double Torque[3], int64_t SimTimeNs); + void UrlabRcl_DestroyWrenchStampedPub(struct UrlabRclWrenchStampedPub* Pub); + + // sensor_msgs/Range, for MuJoCo rangefinder sensors. The constant fields + // (radiation type per sensor_msgs/Range: 0 = ultrasound, 1 = infrared; field of + // view; min/max range) are fixed at create time; publish sets only the reading. + struct UrlabRclRangePub* UrlabRcl_CreateRangePub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, uint8_t RadiationType, + float FieldOfView, float MinRange, float MaxRange); + int UrlabRcl_PublishRange(struct UrlabRclRangePub* Pub, float Range, int64_t SimTimeNs); + void UrlabRcl_DestroyRangePub(struct UrlabRclRangePub* Pub); + + // sensor_msgs/MagneticField, for MuJoCo magnetometer sensors. The field is a + // 3-vector in tesla; the covariance leading element is set to 0 (exact + // ground truth) per REP 145. + struct UrlabRclMagneticFieldPub* UrlabRcl_CreateMagneticFieldPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId); + int UrlabRcl_PublishMagneticField(struct UrlabRclMagneticFieldPub* Pub, + const double Field[3], int64_t SimTimeNs); + void UrlabRcl_DestroyMagneticFieldPub(struct UrlabRclMagneticFieldPub* Pub); + + // std_msgs/Float64MultiArray, the total-coverage fallback for any sensor with no + // standard typed message (touch, subtree, user, ...). The data sequence grows to + // fit on publish. Distinct from the ctrl publisher above so the two roles read + // clearly at the call site; it also serves the planned cmd_ctrl echo and typed + // user-channel array topics. + struct UrlabRclFloat64MultiArrayPub* UrlabRcl_CreateFloat64MultiArrayPub( + struct UrlabRclContext* Ctx, const char* Topic); + int UrlabRcl_PublishFloat64MultiArray(struct UrlabRclFloat64MultiArrayPub* Pub, + const double* Values, int32_t Count); + void UrlabRcl_DestroyFloat64MultiArrayPub(struct UrlabRclFloat64MultiArrayPub* Pub); + + // nav_msgs/Odometry, the ground-truth base odometry for a free-base articulation. + // FrameId is the header frame (REP-105 "odom"); ChildFrameId is the base link + // ("/"). Both are fixed at create. Per the MuJoCo free-joint + // convention the caller passes position + orientation (world) and the twist + // ALREADY resolved into the base frame (linear rotated world->body, angular is + // native body-frame qvel). Covariance is a small fixed ground-truth diagonal set + // at create so EKF consumers (robot_localization) accept the message. + struct UrlabRclOdometryPub* UrlabRcl_CreateOdometryPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, const char* ChildFrameId); + int UrlabRcl_PublishOdometry(struct UrlabRclOdometryPub* Pub, + const double PositionXyz[3], const double OrientationXyzw[4], + const double LinearBody[3], const double AngularBody[3], int64_t SimTimeNs); + void UrlabRcl_DestroyOdometryPub(struct UrlabRclOdometryPub* Pub); + + // geometry_msgs/PoseWithCovarianceStamped, the ground-truth base pose in the map + // frame (amcl_pose shape). FrameId is fixed at create ("map"); covariance is the + // same fixed ground-truth diagonal. + struct UrlabRclPoseWithCovariancePub* UrlabRcl_CreatePoseWithCovariancePub( + struct UrlabRclContext* Ctx, const char* Topic, const char* FrameId); + int UrlabRcl_PublishPoseWithCovariance(struct UrlabRclPoseWithCovariancePub* Pub, + const double PositionXyz[3], const double OrientationXyzw[4], int64_t SimTimeNs); + void UrlabRcl_DestroyPoseWithCovariancePub(struct UrlabRclPoseWithCovariancePub* Pub); + + // sensor_msgs/CameraInfo. Intrinsics are constant per camera, so the K matrix + // (row-major 3x3), width/height, frame id, a zero plumb_bob distortion model, the + // identity rectification R, and the projection matrix P (K with a zero 4th column) + // are all filled at create; publish only restamps and sends. K carries fx,fy,cx,cy + // at the standard pinhole slots (K[0]=fx, K[2]=cx, K[4]=fy, K[5]=cy, K[8]=1). + struct UrlabRclCameraInfoPub* UrlabRcl_CreateCameraInfoPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId, int32_t Width, int32_t Height, + const double K9[9]); + int UrlabRcl_PublishCameraInfo(struct UrlabRclCameraInfoPub* Pub, int64_t SimTimeNs); + void UrlabRcl_DestroyCameraInfoPub(struct UrlabRclCameraInfoPub* Pub); + + // --- Typed user-channel publishers ----------------------------------------- + // One triple per rosidl type the user-channel routing maps kinds to: + // Bool -> std_msgs/Bool, Int/Scalar -> std_msgs/Float64, Vec3 -> + // geometry_msgs/Vector3, Quat/Transform -> geometry_msgs/PoseStamped. Array / + // String / Struct reuse the Float64MultiArray / String triples above. + + struct UrlabRclBoolPub* UrlabRcl_CreateBoolPub(struct UrlabRclContext* Ctx, const char* Topic); + int UrlabRcl_PublishBool(struct UrlabRclBoolPub* Pub, int32_t bValue); // bValue != 0 + void UrlabRcl_DestroyBoolPub(struct UrlabRclBoolPub* Pub); + + struct UrlabRclFloat64Pub* UrlabRcl_CreateFloat64Pub(struct UrlabRclContext* Ctx, const char* Topic); + int UrlabRcl_PublishFloat64(struct UrlabRclFloat64Pub* Pub, double Value); + void UrlabRcl_DestroyFloat64Pub(struct UrlabRclFloat64Pub* Pub); + + struct UrlabRclVector3Pub* UrlabRcl_CreateVector3Pub(struct UrlabRclContext* Ctx, const char* Topic); + int UrlabRcl_PublishVector3(struct UrlabRclVector3Pub* Pub, const double Xyz[3]); + void UrlabRcl_DestroyVector3Pub(struct UrlabRclVector3Pub* Pub); + + // geometry_msgs/PoseStamped. FrameId is fixed at create; publish sets position + // + orientation (xyzw; the provider reorders MuJoCo wxyz) and the stamp. + struct UrlabRclPoseStampedPub* UrlabRcl_CreatePoseStampedPub(struct UrlabRclContext* Ctx, + const char* Topic, const char* FrameId); + int UrlabRcl_PublishPoseStamped(struct UrlabRclPoseStampedPub* Pub, + const double PositionXyz[3], const double OrientationXyzw[4], int64_t SimTimeNs); + void UrlabRcl_DestroyPoseStampedPub(struct UrlabRclPoseStampedPub* Pub); + + // --- Subscriptions --------------------------------------------------------- + // Callbacks fire inside UrlabRcl_SpinSome on its caller's thread; the core does + // no queuing beyond what the rmw layer holds. + typedef void (*UrlabRclCtrlCallback)(const double* Values, int32_t Count, + void* User); + struct UrlabRclCtrlSub* UrlabRcl_CreateCtrlSub(struct UrlabRclContext* Ctx, + const char* Topic, UrlabRclCtrlCallback Callback, void* User); + // std_msgs/Float64MultiArray, the //cmd_ctrl shape + void UrlabRcl_DestroyCtrlSub(struct UrlabRclCtrlSub* Sub); + + typedef void (*UrlabRclTwistCallback)(const double Linear[3], + const double Angular[3], void* User); + struct UrlabRclTwistSub* UrlabRcl_CreateTwistSub(struct UrlabRclContext* Ctx, + const char* Topic, UrlabRclTwistCallback Callback, void* User); + // geometry_msgs/Twist, the //cmd_vel shape + void UrlabRcl_DestroyTwistSub(struct UrlabRclTwistSub* Sub); + + // sensor_msgs/JointState, the //joint_command jog shape. Names and the + // paired position slice are handed to the callback; velocity / effort are + // ignored. Names point into the taken message and are valid only for the + // duration of the callback. + typedef void (*UrlabRclJointStateCallback)(const char** Names, + const double* Positions, int32_t Count, void* User); + struct UrlabRclJointStateSub* UrlabRcl_CreateJointStateSub(struct UrlabRclContext* Ctx, + const char* Topic, UrlabRclJointStateCallback Callback, void* User); + void UrlabRcl_DestroyJointStateSub(struct UrlabRclJointStateSub* Sub); + + int UrlabRcl_SpinSome(struct UrlabRclContext* Ctx, int64_t TimeoutNs); + + // --- Services -------------------------------------------------------------- + // A std_srvs/Trigger service (empty request; response {bool success, string + // message}), the standard type the claim_control / release_control services use + // so no custom .srv package is needed. The callback fills success + message on + // each request; the core sends the response. Callbacks fire inside + // UrlabRcl_SpinSome on its caller's thread, like subscriptions. This service + // area is kept separate from the message-publisher area of the seam. + typedef void (*UrlabRclTriggerCallback)(void* User, int32_t* OutSuccess, + char* OutMessage, int32_t OutMessageCap); + struct UrlabRclTriggerService* UrlabRcl_CreateTriggerService(struct UrlabRclContext* Ctx, + const char* ServiceName, UrlabRclTriggerCallback Callback, void* User); + void UrlabRcl_DestroyTriggerService(struct UrlabRclTriggerService* Srv); + + // --- Zero-copy (only Clock is loanable in our message set) ----------------- + // CanLoan wraps rcl_publisher_can_loan_messages. The loaned publish borrows, + // fills in place, publishes, and falls back to the plain publish when loaning + // is unavailable. + int UrlabRcl_ClockCanLoan(struct UrlabRclClockPub* Pub); // 1 = loanable, 0 = not + int UrlabRcl_PublishClockLoaned(struct UrlabRclClockPub* Pub, int64_t SimTimeNs); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/Source/URLabRos/Private/Transport/Providers/RosCameraInfoProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosCameraInfoProvider.cpp new file mode 100644 index 00000000..568125d1 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosCameraInfoProvider.cpp @@ -0,0 +1,135 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosStateEstimation.h" +#include "State/MjStateTypes.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" + +// sensor_msgs/CameraInfo on ///camera_info, one publisher per camera, +// carrying the pinhole intrinsics a vision node needs to interpret the paired +// image stream. The topic base and frame id are the camera's canonical identity +// ("/", the same key the image stream uses), so camera_info sits +// alongside the image. +// +// Intrinsic derivation: MuJoCo cameras carry a vertical FOV (fovy). The standard +// pinhole K is fy = (height/2) / tan(fovy/2), fx = fy (square pixels; the +// horizontal FOV follows from the width), principal point at the image centre. +// If the camera stores an explicit pixel focal length (focalpixel, MuJoCo's +// intrinsic override), that is used verbatim instead of the fovy derivation. +// Intrinsics are constant, so K / width / height / frame are fixed at create and +// Publish only restamps. +class FMjRosCameraInfoProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("camera_info"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& /*Snapshot*/) override + { + Pubs.Reset(); + const AAMjManager* Manager = Factory.GetManager(); + if (!Manager) + { + return; + } + + // One publisher per distinct camera (canonical name), first writer wins so a + // sanitize collision cannot hide a distinct camera - mirrors the camera name + // map the RPC / streaming paths build. + TSet Seen; + auto AddCamera = [this, &Factory, &Seen](UMjCamera* Cam) + { + if (!Cam || Cam->bIsDefault) + { + return; + } + const FString Canonical = Cam->GetCanonicalName(); + if (Canonical.IsEmpty() || Seen.Contains(Canonical)) + { + return; + } + Seen.Add(Canonical); + + const FIntPoint Res = Cam->GetResolution(); + double K[9]; + DeriveK(Cam, Res.X, Res.Y, K); + + const FString Topic = FString::Printf(TEXT("/%s/camera_info"), *Canonical); + FMjRosPub Pub = Factory.CreateCameraInfo(Topic, Canonical, Res.X, Res.Y, K); + if (Pub.IsValid()) + { + Pubs.Add(MoveTemp(Pub)); + } + }; + + for (AMjArticulation* Art : Manager->GetAllArticulations()) + { + if (!Art) + { + continue; + } + TArray Cameras; + Art->GetComponents(Cameras); + for (UMjCamera* Cam : Cameras) + { + AddCamera(Cam); + } + } + // Manager-owned (global) cameras not attached to any articulation. + TArray GlobalCameras; + Manager->GetComponents(GlobalCameras); + for (UMjCamera* Cam : GlobalCameras) + { + AddCamera(Cam); + } + } + + virtual void Publish(const FMjStateSnapshot& /*Snapshot*/, int64 SimTimeNs) override + { + for (FMjRosPub& Pub : Pubs) + { + Pub.PublishCameraInfo(SimTimeNs); + } + } + + virtual int32 GetPublisherCountForTest() const override { return Pubs.Num(); } + +private: + static void DeriveK(const UMjCamera* Cam, int32 Width, int32 Height, double OutK[9]) + { + MjRosStateEstimation::PinholeKFromFovy(Cam->fovy, Width, Height, OutK); + // Explicit pixel focal length overrides the fovy-derived focal length when + // the camera stores one (MuJoCo's focalpixel intrinsic). + if (Cam->bOverride_focalpixel && Cam->focalpixel.Num() >= 2 + && Cam->focalpixel[0] > 0 && Cam->focalpixel[1] > 0) + { + OutK[0] = static_cast(Cam->focalpixel[0]); // fx + OutK[4] = static_cast(Cam->focalpixel[1]); // fy + } + } + + TArray Pubs; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("camera_info", FMjRosCameraInfoProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosClockProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosClockProvider.cpp new file mode 100644 index 00000000..1939fafe --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosClockProvider.cpp @@ -0,0 +1,50 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +// rosgraph_msgs/Clock on /clock, one process-wide publisher. The transport +// projects the IR clock to nanoseconds once per step and passes it to every +// provider, so this one just forwards it. +class FMjRosClockProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("clock"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& /*Snapshot*/) override + { + ClockPub = Factory.CreateClock(); + } + + virtual void Publish(const FMjStateSnapshot& /*Snapshot*/, int64 SimTimeNs) override + { + ClockPub.PublishClock(SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return ClockPub.IsValid() ? 1 : 0; } + +private: + FMjRosPub ClockPub; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("clock", FMjRosClockProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosImuProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosImuProvider.cpp new file mode 100644 index 00000000..f57cd801 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosImuProvider.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" + +// sensor_msgs/Imu on //imu, one publisher per articulation that carries a +// gyro and/or accel. The gyro+accel pairing stays in the tested pure +// UURLabRosPublishTransport::FillImu. +class FMjRosImuProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("imu"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + double Ang[3]; + double Acc[3]; + bool bHasAng = false; + bool bHasAcc = false; + if (!UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc)) + { + continue; + } + const FString ArtName = Art.Name.ToString(); + const FString Topic = FString::Printf(TEXT("/%s/imu"), *ArtName); + FMjRosPub Pub = Factory.CreateImu(Topic, ArtName); + if (Pub.IsValid()) + { + Entries.Add({i, MoveTemp(Pub)}); + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + static constexpr int64 PublishIntervalNs = 10'000'000; // 100 Hz + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + const FMjArticulationState& Art = Snapshot.Articulations[Entry.ArtIndex]; + double Ang[3]; + double Acc[3]; + bool bHasAng = false; + bool bHasAcc = false; + UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc); + Entry.Pub.PublishImu(bHasAng ? Ang : nullptr, bHasAcc ? Acc : nullptr, + nullptr, SimTimeNs); + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + FMjRosPub Pub; + }; + TArray Entries; + int64 LastPublishNs = 0; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("imu", FMjRosImuProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosJointStateProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosJointStateProvider.cpp new file mode 100644 index 00000000..571409aa --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosJointStateProvider.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" + +// sensor_msgs/JointState on //joint_states, one publisher per articulation. +// The IR -> parallel-array transform stays in the tested pure UURLabRosPublish +// Transport::FillJointState; this provider only owns the per-art publishers. +class FMjRosJointStateProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("joint_state"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + Entries.Reserve(Snapshot.Articulations.Num()); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + const FString Topic = FString::Printf(TEXT("/%s/joint_states"), *Art.Name.ToString()); + FMjRosPub Pub = Factory.CreateJointState(Topic, Names); + if (Pub.IsValid()) + { + Entries.Add({i, MoveTemp(Pub)}); + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + static constexpr int64 PublishIntervalNs = 20'000'000; // 50 Hz + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + const FMjArticulationState& Art = Snapshot.Articulations[Entry.ArtIndex]; + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + Entry.Pub.PublishJointState(Positions.GetData(), Velocities.GetData(), + Efforts.Num() > 0 ? Efforts.GetData() : nullptr, Names.Num(), SimTimeNs); + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + FMjRosPub Pub; + }; + TArray Entries; + int64 LastPublishNs = 0; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("joint_state", FMjRosJointStateProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosOccupancyGridProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosOccupancyGridProvider.cpp new file mode 100644 index 00000000..5bb1e210 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosOccupancyGridProvider.cpp @@ -0,0 +1,246 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "Ros/UrlabRclCore.h" +#include "Transport/RosContext.h" + +namespace +{ +constexpr double GResolution = 0.05; +constexpr int32 GWidth = 400; +constexpr int32 GHeight = 400; +constexpr double GZSliceMin = -0.1; +constexpr double GZSliceMax = 2.0; +constexpr int32 GCellCount = GWidth * GHeight; + +constexpr double GOriginX = -static_cast(GWidth) * 0.5 * GResolution; +constexpr double GOriginY = -static_cast(GHeight) * 0.5 * GResolution; + +// Transform a local point by the geom's world pose. +void TransformPoint(const double* Xpos, const double* Xquat, + double& X, double& Y, double& Z) +{ + const double Qw = Xquat[0], Qx = Xquat[1], Qy = Xquat[2], Qz = Xquat[3]; + const double Xx = Qx * Qx, Yy = Qy * Qy, Zz = Qz * Qz; + const double Xy = Qx * Qy, Xz = Qx * Qz, Yz = Qy * Qz; + const double Wx = Qw * Qx, Wy = Qw * Qy, Wz = Qw * Qz; + + const double R00 = 1.0 - 2.0 * (Yy + Zz); + const double R01 = 2.0 * (Xy - Wz); + const double R02 = 2.0 * (Xz + Wy); + const double R10 = 2.0 * (Xy + Wz); + const double R11 = 1.0 - 2.0 * (Xx + Zz); + const double R12 = 2.0 * (Yz - Wx); + const double R20 = 2.0 * (Xz - Wy); + const double R21 = 2.0 * (Yz + Wx); + const double R22 = 1.0 - 2.0 * (Xx + Yy); + + const double Lx = X, Ly = Y, Lz = Z; + X = R00 * Lx + R01 * Ly + R02 * Lz + Xpos[0]; + Y = R10 * Lx + R11 * Ly + R12 * Lz + Xpos[1]; + Z = R20 * Lx + R21 * Ly + R22 * Lz + Xpos[2]; +} + +// Compute a conservative world-space AABB for a geom by transforming the 8 +// corners of its local AABB. +void ComputeWorldAABB(const FMjWorldGeom& G, double& MinX, double& MinY, + double& MaxX, double& MaxY) +{ + double HalfX, HalfY, HalfZ; + switch (G.Shape) + { + case EMjWorldGeomShape::Box: + HalfX = G.Size[0]; HalfY = G.Size[1]; HalfZ = G.Size[2]; + break; + case EMjWorldGeomShape::Sphere: + HalfX = HalfY = HalfZ = G.Size[0]; + break; + case EMjWorldGeomShape::Cylinder: + HalfX = HalfY = G.Size[0]; HalfZ = G.Size[1]; + break; + case EMjWorldGeomShape::Mesh: + if (G.Mesh.IsValid()) + { + const FMjWorldMesh& Me = *G.Mesh; + MinX = MinY = MaxX = MaxY = 0.0; + bool bFirst = true; + for (const FVector3f& V : Me.Verts) + { + double Px = V.X, Py = V.Y, Pz = V.Z; + TransformPoint(G.Xpos, G.Xquat, Px, Py, Pz); + if (bFirst) { MinX = MaxX = Px; MinY = MaxY = Py; bFirst = false; } + else + { + MinX = FMath::Min(MinX, Px); MaxX = FMath::Max(MaxX, Px); + MinY = FMath::Min(MinY, Py); MaxY = FMath::Max(MaxY, Py); + } + } + return; + } + return; + default: + return; + } + + MinX = MinY = MaxX = MaxY = 0.0; + bool bFirst = true; + for (int32 iz = 0; iz < 2; ++iz) + { + const double Z = (iz == 0) ? -HalfZ : HalfZ; + for (int32 iy = 0; iy < 2; ++iy) + { + const double Y = (iy == 0) ? -HalfY : HalfY; + for (int32 ix = 0; ix < 2; ++ix) + { + const double X = (ix == 0) ? -HalfX : HalfX; + double Px = X, Py = Y, Pz = Z; + TransformPoint(G.Xpos, G.Xquat, Px, Py, Pz); + if (bFirst) { MinX = MaxX = Px; MinY = MaxY = Py; bFirst = false; } + else + { + MinX = FMath::Min(MinX, Px); MaxX = FMath::Max(MaxX, Px); + MinY = FMath::Min(MinY, Py); MaxY = FMath::Max(MaxY, Py); + } + } + } + } +} + +// Rasterize a 2D bounding box into the grid, marking cells as occupied. +void RasterizeAABB(const int8_t* InGrid, int8_t* OutGrid, + double MinX, double MinY, double MaxX, double MaxY) +{ + const int32 X0 = FMath::Clamp( + FMath::FloorToInt32((MinX - GOriginX) / GResolution), 0, GWidth - 1); + const int32 X1 = FMath::Clamp( + FMath::FloorToInt32((MaxX - GOriginX) / GResolution), 0, GWidth - 1); + const int32 Y0 = FMath::Clamp( + FMath::FloorToInt32((MinY - GOriginY) / GResolution), 0, GHeight - 1); + const int32 Y1 = FMath::Clamp( + FMath::FloorToInt32((MaxY - GOriginY) / GResolution), 0, GHeight - 1); + + for (int32 Y = Y0; Y <= Y1; ++Y) + { + for (int32 X = X0; X <= X1; ++X) + { + OutGrid[Y * GWidth + X] = 100; + } + } +} +} // namespace + +// nav_msgs/OccupancyGrid on /map, world frame, latched. Rasterises world-geom +// AABBs into a fixed-resolution 2D grid. Publishes once on Build and again +// whenever StructureVersion changes. +class FMjRosOccupancyGridProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("map"); } + + virtual void Build(FMjRosPublisherFactory& /*Factory*/, const FMjStateSnapshot& Snapshot) override + { + Destroy(); + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return; + } + Pub = UrlabRcl_CreateOccupancyGridPub(Ctx, "/map", "world", + GResolution, GWidth, GHeight, GOriginX, GOriginY); + if (!Pub) + { + return; + } + LastStructureVersion = Snapshot.StructureVersion; + Grid.SetNumUninitialized(GCellCount); + FMemory::Memzero(Grid.GetData(), GCellCount); + + for (const FMjWorldGeom& G : Snapshot.WorldGeoms) + { + double MinX, MinY, MaxX, MaxY; + ComputeWorldAABB(G, MinX, MinY, MaxX, MaxY); + RasterizeAABB(Grid.GetData(), Grid.GetData(), MinX, MinY, MaxX, MaxY); + } + + TArray PackageData; + PackageData.SetNumUninitialized(GCellCount); + FMemory::Memcpy(PackageData.GetData(), Grid.GetData(), GCellCount); + UrlabRcl_PublishOccupancyGrid(Pub, PackageData.GetData(), 0); + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + if (!Pub) + { + return; + } + // Latched: only republish when the structure changes because occupancy + // is ground-truth static geometry; the grid origin and resolution are + // fixed at create. + if (Snapshot.StructureVersion == LastStructureVersion) + { + return; + } + LastStructureVersion = Snapshot.StructureVersion; + + FMemory::Memzero(Grid.GetData(), GCellCount); + for (const FMjWorldGeom& G : Snapshot.WorldGeoms) + { + double MinX, MinY, MaxX, MaxY; + ComputeWorldAABB(G, MinX, MinY, MaxX, MaxY); + RasterizeAABB(Grid.GetData(), Grid.GetData(), MinX, MinY, MaxX, MaxY); + } + + TArray PackageData; + PackageData.SetNumUninitialized(GCellCount); + FMemory::Memcpy(PackageData.GetData(), Grid.GetData(), GCellCount); + UrlabRcl_PublishOccupancyGrid(Pub, PackageData.GetData(), SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return Pub ? 1 : 0; } + + virtual ~FMjRosOccupancyGridProvider() { Destroy(); } + +private: + void Destroy() + { + if (Pub) + { + UrlabRcl_DestroyOccupancyGridPub(Pub); + Pub = nullptr; + } + LastStructureVersion = 0; + } + + UrlabRclOccupancyGridPub* Pub = nullptr; + uint32 LastStructureVersion = 0; + TArray Grid; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("map", FMjRosOccupancyGridProvider); + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/Providers/RosOctomapProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosOctomapProvider.cpp new file mode 100644 index 00000000..3f204374 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosOctomapProvider.cpp @@ -0,0 +1,386 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "Ros/UrlabRclCore.h" +#include "Transport/RosContext.h" + +namespace +{ +constexpr double GResolution = 0.05; +constexpr int32 GMaxDepth = 16; + +constexpr float GOccupiedLogOdds = 2.0f; +constexpr float GFreeLogOdds = -2.0f; + +// Transform a local point by the geom's world pose (wxyz quaternion). +void TransformPoint(const double* Xpos, const double* Xquat, + double& X, double& Y, double& Z) +{ + const double Qw = Xquat[0], Qx = Xquat[1], Qy = Xquat[2], Qz = Xquat[3]; + const double Xx = Qx * Qx, Yy = Qy * Qy, Zz = Qz * Qz; + const double Xy = Qx * Qy, Xz = Qx * Qz, Yz = Qy * Qz; + const double Wx = Qw * Qx, Wy = Qw * Qy, Wz = Qw * Qz; + + const double R00 = 1.0 - 2.0 * (Yy + Zz); + const double R01 = 2.0 * (Xy - Wz); + const double R02 = 2.0 * (Xz + Wy); + const double R10 = 2.0 * (Xy + Wz); + const double R11 = 1.0 - 2.0 * (Xx + Zz); + const double R12 = 2.0 * (Yz - Wx); + const double R20 = 2.0 * (Xz - Wy); + const double R21 = 2.0 * (Yz + Wx); + const double R22 = 1.0 - 2.0 * (Xx + Yy); + + const double Lx = X, Ly = Y, Lz = Z; + X = R00 * Lx + R01 * Ly + R02 * Lz + Xpos[0]; + Y = R10 * Lx + R11 * Ly + R12 * Lz + Xpos[1]; + Z = R20 * Lx + R21 * Ly + R22 * Lz + Xpos[2]; +} + +// Compute a conservative world-space AABB by transforming the 8 corners of +// the local AABB. Returns the result as an FBox. +FBox ComputeWorldAABB(const FMjWorldGeom& G) +{ + double HalfX, HalfY, HalfZ; + switch (G.Shape) + { + case EMjWorldGeomShape::Box: + HalfX = G.Size[0]; HalfY = G.Size[1]; HalfZ = G.Size[2]; + break; + case EMjWorldGeomShape::Sphere: + HalfX = HalfY = HalfZ = G.Size[0]; + break; + case EMjWorldGeomShape::Cylinder: + HalfX = HalfY = G.Size[0]; HalfZ = G.Size[1]; + break; + case EMjWorldGeomShape::Mesh: + if (G.Mesh.IsValid()) + { + const FMjWorldMesh& Me = *G.Mesh; + FBox Out; + for (const FVector3f& V : Me.Verts) + { + double Px = V.X, Py = V.Y, Pz = V.Z; + TransformPoint(G.Xpos, G.Xquat, Px, Py, Pz); + Out += FVector(Px, Py, Pz); + } + return Out; + } + return FBox(); + default: + return FBox(); + } + + FBox Out; + for (int32 iz = 0; iz < 2; ++iz) + { + const double Z = (iz == 0) ? -HalfZ : HalfZ; + for (int32 iy = 0; iy < 2; ++iy) + { + const double Y = (iy == 0) ? -HalfY : HalfY; + for (int32 ix = 0; ix < 2; ++ix) + { + const double X = (ix == 0) ? -HalfX : HalfX; + double Px = X, Py = Y, Pz = Z; + TransformPoint(G.Xpos, G.Xquat, Px, Py, Pz); + Out += FVector(Px, Py, Pz); + } + } + } + return Out; +} + +// Compute the global AABB enclosing all world geoms, expanded to a +// power-of-2 cube so it cleanly subdivides to the leaf resolution. +FBox ComputeGlobalCube(const TArray& Geoms, double& OutHalfSize) +{ + if (Geoms.Num() == 0) + { + OutHalfSize = 1.0; + return FBox(FVector(-1.0), FVector(1.0)); + } + FBox Global; + for (const FMjWorldGeom& G : Geoms) + { + FBox B = ComputeWorldAABB(G); + if (B.IsValid) + { + Global += B.Min; + Global += B.Max; + } + } + if (!Global.IsValid) + { + OutHalfSize = 1.0; + return FBox(FVector(-1.0), FVector(1.0)); + } + + const FVector Center = Global.GetCenter(); + const FVector Extent = Global.GetExtent(); + const double MaxExt = FMath::Max3(Extent.X, Extent.Y, Extent.Z); + + // Double until the cube side is at least MaxExt * 2 (to fully contain the + // AABB), but cap at a size that keeps the depth reasonable. + double HalfSize = 1.0; + while (HalfSize < MaxExt && HalfSize < 1.0e6) + { + HalfSize *= 2.0; + } + OutHalfSize = HalfSize; + return FBox(Center - FVector(HalfSize), Center + FVector(HalfSize)); +} + +// Test whether the node box (at arbitrary depth) intersects any geom AABB. +bool NodeIntersectsGeoms(const FBox& NodeBox, const TArray& GeomBoxes) +{ + for (const FBox& Gb : GeomBoxes) + { + if (NodeBox.Intersect(Gb)) + { + return true; + } + } + return false; +} + +// Test whether the node box is fully contained within some geom AABB. +bool NodeInsideGeom(const FBox& NodeBox, const TArray& GeomBoxes) +{ + for (const FBox& Gb : GeomBoxes) + { + if (NodeBox.Min.X >= Gb.Min.X && NodeBox.Max.X <= Gb.Max.X && + NodeBox.Min.Y >= Gb.Min.Y && NodeBox.Max.Y <= Gb.Max.Y && + NodeBox.Min.Z >= Gb.Min.Z && NodeBox.Max.Z <= Gb.Max.Z) + { + return true; + } + } + return false; +} + +void ClassifyNode(const FBox& NodeBox, const TArray& GeomBoxes, + bool& bOutIntersects, bool& bOutFullyInside) +{ + bOutIntersects = NodeIntersectsGeoms(NodeBox, GeomBoxes); + bOutFullyInside = bOutIntersects && NodeInsideGeom(NodeBox, GeomBoxes); +} + +// Count nodes for the size header in a pre-pass. +int32 CountNodes(const FBox& Box, int32 Depth, int32 MaxDepth, + const TArray& GeomBoxes) +{ + bool bIntersects, bFullyInside; + ClassifyNode(Box, GeomBoxes, bIntersects, bFullyInside); + if (!bIntersects || bFullyInside || Depth >= MaxDepth) + { + return 1; // leaf + } + int32 Count = 1; // this inner node + const FVector C = Box.GetCenter(); + const FVector HalfExt = Box.GetExtent() * 0.5; + for (int32 i = 0; i < 8; ++i) + { + FVector Corner = C; + Corner.X += (i & 1) ? HalfExt.X : -HalfExt.X; + Corner.Y += (i & 2) ? HalfExt.Y : -HalfExt.Y; + Corner.Z += (i & 4) ? HalfExt.Z : -HalfExt.Z; + FBox Child(Corner - HalfExt, Corner + HalfExt); + if (NodeIntersectsGeoms(Child, GeomBoxes)) + { + Count += CountNodes(Child, Depth + 1, MaxDepth, GeomBoxes); + } + } + return Count; +} + +// Serialize the ocTree in DFS order into OutBinary. Inner nodes get a +// placeholder children mask that is back-patched after recursion. +void SerializeNodes(const FBox& Box, int32 Depth, int32 MaxDepth, + const TArray& GeomBoxes, TArray& OutBinary) +{ + bool bIntersects, bFullyInside; + ClassifyNode(Box, GeomBoxes, bIntersects, bFullyInside); + + if (!bIntersects) + { + float V = GFreeLogOdds; + OutBinary.Append(reinterpret_cast(&V), sizeof(float)); + OutBinary.Add(0); // children mask = 0 + return; + } + if (bFullyInside || Depth >= MaxDepth) + { + float V = GOccupiedLogOdds; + OutBinary.Append(reinterpret_cast(&V), sizeof(float)); + OutBinary.Add(0); // children mask = 0 + return; + } + + // Inner node: logOdds = 0 (unknown at this resolution level), children follow. + const float InnerLogOdds = 0.0f; + OutBinary.Append(reinterpret_cast(&InnerLogOdds), sizeof(float)); + const int32 MaskPos = OutBinary.Num(); + OutBinary.Add(0); // placeholder + uint8 Mask = 0; + + const FVector C = Box.GetCenter(); + const FVector HalfExt = Box.GetExtent() * 0.5; + for (int32 i = 0; i < 8; ++i) + { + FVector Corner = C; + Corner.X += (i & 1) ? HalfExt.X : -HalfExt.X; + Corner.Y += (i & 2) ? HalfExt.Y : -HalfExt.Y; + Corner.Z += (i & 4) ? HalfExt.Z : -HalfExt.Z; + FBox Child(Corner - HalfExt, Corner + HalfExt); + if (NodeIntersectsGeoms(Child, GeomBoxes)) + { + Mask |= static_cast(1 << i); + SerializeNodes(Child, Depth + 1, MaxDepth, GeomBoxes, OutBinary); + } + } + OutBinary[MaskPos] = Mask; +} + +void AppendBinaryLE(TArray& Out, uint16_t Val) +{ + Out.Add(static_cast(Val & 0xFF)); + Out.Add(static_cast((Val >> 8) & 0xFF)); +} + +// Builds the full serialised Octomap message payload: text header line +// followed by the binary tree data. +TArray BuildOctomapPayload(const TArray& Geoms, + double Resolution, int32 MaxDepth) +{ + // Collect per-geom world-space AABBs. + TArray GeomBoxes; + GeomBoxes.Reserve(Geoms.Num()); + for (const FMjWorldGeom& G : Geoms) + { + FBox B = ComputeWorldAABB(G); + if (B.IsValid) + { + GeomBoxes.Add(B); + } + } + if (GeomBoxes.Num() == 0) + { + return TArray(); + } + + double HalfSize; + FBox RootBox = ComputeGlobalCube(Geoms, HalfSize); + + const int32 TotalNodes = CountNodes(RootBox, 0, MaxDepth, GeomBoxes); + + // Build text header. + const FString HeaderLine = FString::Printf( + TEXT("# Octomap OcTree\nid OcTree\nsize %d\nres %.4f\ndata\n"), + TotalNodes, Resolution); + FTCHARToUTF8 HeaderUtf8(*HeaderLine); + + TArray Payload; + Payload.Reserve(HeaderUtf8.Length() + 2 + TotalNodes * 7); + Payload.Append(reinterpret_cast(HeaderUtf8.Get()), + HeaderUtf8.Length()); + + // Binary data: uint16_t node count, then node records. + AppendBinaryLE(Payload, static_cast(TotalNodes > 65535 ? 65535 : TotalNodes)); + SerializeNodes(RootBox, 0, MaxDepth, GeomBoxes, Payload); + + return Payload; +} +} // namespace + +// octomap_msgs/Octomap on /octomap_binary, world frame. Builds a minimal +// OcTree from world-geom AABBs using recursive subdivision to the configured +// resolution. Publishes at ~5 Hz. +class FMjRosOctomapProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("octomap"); } + + virtual void Build(FMjRosPublisherFactory& /*Factory*/, const FMjStateSnapshot& Snapshot) override + { + Destroy(); + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return; + } + Pub = UrlabRcl_CreateOctomapPub(Ctx, "/octomap_binary", "world", GResolution); + LastStructureVersion = Snapshot.StructureVersion; + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + if (!Pub) + { + return; + } + static constexpr int64 PublishIntervalNs = 200'000'000; // 5 Hz + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + // Within rate limit, but republish if structure changed. + if (Snapshot.StructureVersion == LastStructureVersion) + { + return; + } + } + LastPublishNs = SimTimeNs; + LastStructureVersion = Snapshot.StructureVersion; + + const TArray Payload = BuildOctomapPayload( + Snapshot.WorldGeoms, GResolution, GMaxDepth); + UrlabRcl_PublishOctomap(Pub, Payload.GetData(), + static_cast(Payload.Num()), SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return Pub ? 1 : 0; } + + virtual ~FMjRosOctomapProvider() { Destroy(); } + +private: + void Destroy() + { + if (Pub) + { + UrlabRcl_DestroyOctomapPub(Pub); + Pub = nullptr; + } + LastPublishNs = 0; + LastStructureVersion = 0; + } + + UrlabRclOctomapPub* Pub = nullptr; + int64 LastPublishNs = 0; + uint32 LastStructureVersion = 0; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("octomap", FMjRosOctomapProvider); + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/Providers/RosOdomFramesProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosOdomFramesProvider.cpp new file mode 100644 index 00000000..293ecadf --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosOdomFramesProvider.cpp @@ -0,0 +1,67 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +// REP-105 ground-truth frame chain on /tf_static: map -> odom -> world, both +// identity. The existing tf provider already publishes world -> / with +// the exact FK pose on /tf, so this pair completes the standard +// map -> odom -> base_link tree a Nav2 / MoveIt stack expects, WITHOUT an AMCL: +// with ground truth there is no localisation error, so map, odom and the sim root +// "world" are all coincident. The two static edges are published rather than a +// literal odom -> base_link edge precisely so the base body keeps its single +// parent ("world") in the flat tf tree - a second parent would corrupt the tree. +// odom -> base_link is still exactly recoverable (odom -> world identity composed +// with the dynamic, exact world -> base_link). +class FMjRosOdomFramesProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("rep105_frames"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& /*Snapshot*/) override + { + StaticTfPub = Factory.CreateTf(/*bStatic=*/true); + if (!StaticTfPub.IsValid()) + { + return; + } + const TArray Parents = { TEXT("map"), TEXT("odom") }; + const TArray Children = { TEXT("odom"), TEXT("world") }; + const TArray Translations = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + const TArray RotationsXyzw = { 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; + // Latched (transient-local) static transforms; publish once at Build. + StaticTfPub.PublishTf(Parents, Children, Translations, RotationsXyzw, /*SimTimeNs=*/0); + } + + virtual void Publish(const FMjStateSnapshot& /*Snapshot*/, int64 /*SimTimeNs*/) override + { + // Latched at Build; the identity chain never changes. + } + + virtual int32 GetPublisherCountForTest() const override { return StaticTfPub.IsValid() ? 1 : 0; } + +private: + FMjRosPub StaticTfPub; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("rep105_frames", FMjRosOdomFramesProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosOdometryProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosOdometryProvider.cpp new file mode 100644 index 00000000..c2875672 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosOdometryProvider.cpp @@ -0,0 +1,101 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosStateEstimation.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" + +// nav_msgs/Odometry on //odom, one publisher per free-base articulation. The +// pose is the base link's world pose and the twist is its velocity resolved into +// the base frame (linear rotated world->body, angular native body-frame), both +// from the free joint per the MuJoCo convention. header.frame_id = "odom" +// (REP-105), child_frame_id = "/". Covariance is a ground-truth +// diagonal fixed at create. +class FMjRosOdometryProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("odometry"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + MjRosStateEstimation::FMjFreeBaseState State; + if (!MjRosStateEstimation::ComputeFreeBaseState(Art, State)) + { + continue; // fixed-base art: no odometry + } + const FString ArtName = Art.Name.ToString(); + const FString Topic = FString::Printf(TEXT("/%s/odom"), *ArtName); + const FName BaseName = Art.Bodies.IsValidIndex(State.BaseBodyIndex) + ? Art.Bodies[State.BaseBodyIndex].Name + : FName(TEXT("base_link")); + const FString Child = FMjCanonicalName::Full(Art.Name, BaseName); + FMjRosPub Pub = Factory.CreateOdometry(Topic, TEXT("odom"), Child); + if (Pub.IsValid()) + { + Entries.Add({i, MoveTemp(Pub)}); + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + static constexpr int64 PublishIntervalNs = 20'000'000; // 50 Hz + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + MjRosStateEstimation::FMjFreeBaseState State; + if (MjRosStateEstimation::ComputeFreeBaseState( + Snapshot.Articulations[Entry.ArtIndex], State)) + { + Entry.Pub.PublishOdometry(State.Position, State.OrientationXyzw, + State.LinearBody, State.AngularBody, SimTimeNs); + } + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + FMjRosPub Pub; + }; + TArray Entries; + int64 LastPublishNs = 0; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("odometry", FMjRosOdometryProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosPlanningSceneProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosPlanningSceneProvider.cpp new file mode 100644 index 00000000..eb4f564f --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosPlanningSceneProvider.cpp @@ -0,0 +1,221 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "Ros/UrlabRclCore.h" +#include "Transport/RosContext.h" + +// moveit_msgs/PlanningScene (is_diff) on /planning_scene: the sim's non-robot +// collision geometry, so MoveIt plans around obstacles and can manipulate them. +// The object set is fixed per StructureVersion (Build creates it with shapes); +// Publish streams live world poses for dynamic objects only, throttled to ~10 Hz. +// Static objects (worldbody-fixed) get their initial pose in Build and are not +// republished. +class FMjRosPlanningSceneProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("planning_scene"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Destroy(); + const int32 TotalCount = Snapshot.WorldGeoms.Num(); + if (TotalCount == 0) + { + return; + } + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return; + } + + // Partition dynamic objects to the front so that Publish can send only + // their poses (the C ABI updates collision objects 0..M-1 in order). + TArray Order; + Order.Reserve(TotalCount); + for (int32 i = 0; i < TotalCount; ++i) + { + if (!Snapshot.WorldGeoms[i].bStatic) + { + Order.Add(i); + } + } + DynamicCount = Order.Num(); + for (int32 i = 0; i < TotalCount; ++i) + { + if (Snapshot.WorldGeoms[i].bStatic) + { + Order.Add(i); + } + } + Count = TotalCount; + + // Stable UTF-8 id storage + the pointer array the C ABI takes. + IdBytes.SetNum(Count); + TArray IdPtrs; + IdPtrs.SetNum(Count); + TArray PrimTypes; + PrimTypes.SetNum(Count); + TArray Dims; + Dims.SetNumZeroed(Count * 3); + + // Mesh geometry, flattened across all objects (0 counts for primitives). + TArray MeshVertCounts; + MeshVertCounts.SetNumZeroed(Count); + TArray MeshTriCounts; + MeshTriCounts.SetNumZeroed(Count); + TArray MeshVerts; + TArray MeshTris; + + for (int32 j = 0; j < Count; ++j) + { + const int32 i = Order[j]; + const FMjWorldGeom& G = Snapshot.WorldGeoms[i]; + FTCHARToUTF8 Conv(*G.Name.ToString()); + TArray& Bytes = IdBytes[j]; + Bytes.Append(reinterpret_cast(Conv.Get()), Conv.Length()); + Bytes.Add('\0'); + IdPtrs[j] = Bytes.GetData(); + + // shape_msgs/SolidPrimitive: BOX=1 dims[x,y,z] full; SPHERE=2 dims[r]; + // CYLINDER=3 dims[height, radius]; 4=MESH (geometry in the mesh arrays). + // MuJoCo sizes are half-extents / radius. + switch (G.Shape) + { + case EMjWorldGeomShape::Sphere: + PrimTypes[j] = 2; + Dims[j * 3 + 0] = G.Size[0]; + break; + case EMjWorldGeomShape::Cylinder: + PrimTypes[j] = 3; + Dims[j * 3 + 0] = 2.0 * G.Size[1]; // height + Dims[j * 3 + 1] = G.Size[0]; // radius + break; + case EMjWorldGeomShape::Mesh: + if (G.Mesh.IsValid()) + { + PrimTypes[j] = 4; + const FMjWorldMesh& Me = *G.Mesh; + MeshVertCounts[j] = Me.Verts.Num(); + MeshTriCounts[j] = Me.Tris.Num() / 3; + MeshVerts.Reserve(MeshVerts.Num() + Me.Verts.Num() * 3); + for (const FVector3f& V : Me.Verts) + { + MeshVerts.Add(V.X); + MeshVerts.Add(V.Y); + MeshVerts.Add(V.Z); + } + MeshTris.Append(Me.Tris); + } + else + { + PrimTypes[j] = 1; // degrade to a null box rather than crash + } + break; + case EMjWorldGeomShape::Box: + default: + PrimTypes[j] = 1; + Dims[j * 3 + 0] = 2.0 * G.Size[0]; + Dims[j * 3 + 1] = 2.0 * G.Size[1]; + Dims[j * 3 + 2] = 2.0 * G.Size[2]; + break; + } + } + + Pub = UrlabRcl_CreatePlanningScenePub(Ctx, "/planning_scene", "world", + IdPtrs.GetData(), PrimTypes.GetData(), Dims.GetData(), + MeshVertCounts.GetData(), MeshVerts.GetData(), + MeshTriCounts.GetData(), MeshTris.GetData(), Count); + if (!Pub) + { + Count = 0; + DynamicCount = 0; + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + if (!Pub || DynamicCount == 0) + { + return; + } + // ~10 Hz is plenty for a planning scene and avoids re-adding geometry every + // physics step. Dynamic objects are at the front of the collision-object + // list; static objects at the tail keep their initial (never-changing) pose. + static constexpr int64 PublishIntervalNs = 100'000'000; + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + const int32 N = FMath::Min(DynamicCount, Snapshot.WorldGeoms.Num()); + Poses.SetNumUninitialized(N * 7, EAllowShrinking::No); + for (int32 i = 0; i < N; ++i) + { + const FMjWorldGeom& G = Snapshot.WorldGeoms[i]; + double* P = &Poses[i * 7]; + P[0] = G.Xpos[0]; + P[1] = G.Xpos[1]; + P[2] = G.Xpos[2]; + P[3] = G.Xquat[1]; // x (mj wxyz -> ros xyzw) + P[4] = G.Xquat[2]; // y + P[5] = G.Xquat[3]; // z + P[6] = G.Xquat[0]; // w + } + UrlabRcl_PublishPlanningScene(Pub, Poses.GetData(), N, SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return Pub ? 1 : 0; } + + virtual ~FMjRosPlanningSceneProvider() { Destroy(); } + +private: + void Destroy() + { + if (Pub) + { + UrlabRcl_DestroyPlanningScenePub(Pub); + Pub = nullptr; + } + Count = 0; + DynamicCount = 0; + LastPublishNs = 0; + IdBytes.Reset(); + } + + UrlabRclPlanningScenePub* Pub = nullptr; + int32 Count = 0; + int32 DynamicCount = 0; + int64 LastPublishNs = 0; + TArray> IdBytes; + TArray Poses; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("planning_scene", FMjRosPlanningSceneProvider); + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/Providers/RosPointCloudProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosPointCloudProvider.cpp new file mode 100644 index 00000000..298e0e8f --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosPointCloudProvider.cpp @@ -0,0 +1,363 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "Ros/UrlabRclCore.h" +#include "Transport/RosContext.h" + +namespace +{ +constexpr double GResolution = 0.02; // metres between sample points +constexpr double GTwoPi = 6.283185307179586; + +// Apply quaternion (wxyz) rotation then translation to a point in-place. +void ApplyWorldTransform(const double* Xpos, const double* Xquat, + double& X, double& Y, double& Z) +{ + const double Qw = Xquat[0], Qx = Xquat[1], Qy = Xquat[2], Qz = Xquat[3]; + const double Xx = Qx * Qx, Yy = Qy * Qy, Zz = Qz * Qz; + const double Xy = Qx * Qy, Xz = Qx * Qz, Yz = Qy * Qz; + const double Wx = Qw * Qx, Wy = Qw * Qy, Wz = Qw * Qz; + + const double R00 = 1.0 - 2.0 * (Yy + Zz); + const double R01 = 2.0 * (Xy - Wz); + const double R02 = 2.0 * (Xz + Wy); + const double R10 = 2.0 * (Xy + Wz); + const double R11 = 1.0 - 2.0 * (Xx + Zz); + const double R12 = 2.0 * (Yz - Wx); + const double R20 = 2.0 * (Xz - Wy); + const double R21 = 2.0 * (Yz + Wx); + const double R22 = 1.0 - 2.0 * (Xx + Yy); + + const double Lx = X, Ly = Y, Lz = Z; + X = R00 * Lx + R01 * Ly + R02 * Lz + Xpos[0]; + Y = R10 * Lx + R11 * Ly + R12 * Lz + Xpos[1]; + Z = R20 * Lx + R21 * Ly + R22 * Lz + Xpos[2]; +} + +void EmitPoint(TArray& Out, double X, double Y, double Z) +{ + Out.Add(static_cast(X)); + Out.Add(static_cast(Y)); + Out.Add(static_cast(Z)); +} + +void SampleBox(const FMjWorldGeom& G, TArray& Out) +{ + const double Sx = G.Size[0], Sy = G.Size[1], Sz = G.Size[2]; + const int32 Nx = FMath::Max(2, FMath::CeilToInt32(2.0 * Sx / GResolution) + 1); + const int32 Ny = FMath::Max(2, FMath::CeilToInt32(2.0 * Sy / GResolution) + 1); + const int32 Nz = FMath::Max(2, FMath::CeilToInt32(2.0 * Sz / GResolution) + 1); + + // +X / -X faces (perpendicular to x, grid in y,z) + for (int32 iy = 0; iy < Ny; ++iy) + { + const double Y = -Sy + (2.0 * Sy * iy) / (Ny - 1); + for (int32 iz = 0; iz < Nz; ++iz) + { + const double Z = -Sz + (2.0 * Sz * iz) / (Nz - 1); + double Px = Sx, Py = Y, Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + Px = -Sx; Py = Y; Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + } + // +Y / -Y faces (perpendicular to y, grid in x,z) + for (int32 ix = 0; ix < Nx; ++ix) + { + const double X = -Sx + (2.0 * Sx * ix) / (Nx - 1); + for (int32 iz = 0; iz < Nz; ++iz) + { + const double Z = -Sz + (2.0 * Sz * iz) / (Nz - 1); + double Px = X, Py = Sy, Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + Px = X; Py = -Sy; Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + } + // +Z / -Z faces (perpendicular to z, grid in x,y) + for (int32 ix = 0; ix < Nx; ++ix) + { + const double X = -Sx + (2.0 * Sx * ix) / (Nx - 1); + for (int32 iy = 0; iy < Ny; ++iy) + { + const double Y = -Sy + (2.0 * Sy * iy) / (Ny - 1); + double Px = X, Py = Y, Pz = Sz; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + Px = X; Py = Y; Pz = -Sz; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + } +} + +void SampleSphere(const FMjWorldGeom& G, TArray& Out) +{ + const double R = G.Size[0]; + const int32 N = FMath::Max(20, FMath::CeilToInt32(4.0 * UE_PI * R * R / (GResolution * GResolution))); + + // Fibonacci lattice on the unit sphere. The irrational angle step + // (golden-angle conjugate) distributes points evenly. + constexpr double GGoldenConjugate = 0.6180339887498948482; + for (int32 i = 0; i < N; ++i) + { + const double Y = 1.0 - (2.0 * static_cast(i) / static_cast(N - 1)); + const double RadiusAtY = FMath::Sqrt(1.0 - Y * Y); + const double Theta = GTwoPi * static_cast(i) * GGoldenConjugate; + const double Lx = RadiusAtY * FMath::Cos(Theta) * R; + const double Ly = Y * R; + const double Lz = RadiusAtY * FMath::Sin(Theta) * R; + double Px = Lx, Py = Ly, Pz = Lz; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } +} + +void SampleCylinder(const FMjWorldGeom& G, TArray& Out) +{ + const double R = G.Size[0]; + const double HalfH = G.Size[1]; + const int32 Nh = FMath::Max(2, FMath::CeilToInt32(2.0 * HalfH / GResolution) + 1); + const double AngRes = GResolution / R; + const int32 Na = FMath::Max(8, FMath::CeilToInt32(GTwoPi / AngRes)); + + // Curved surface: grid in height x angle. + for (int32 ih = 0; ih < Nh; ++ih) + { + const double Z = -HalfH + (2.0 * HalfH * ih) / (Nh - 1); + for (int32 ia = 0; ia < Na; ++ia) + { + const double A = (GTwoPi * ia) / Na; + double Px = R * FMath::Cos(A); + double Py = R * FMath::Sin(A); + double Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + } + // Caps: concentric rings at z = +/-HalfH. + const int32 Nr = FMath::Max(2, FMath::CeilToInt32(R / GResolution) + 1); + for (int32 cap = 0; cap < 2; ++cap) + { + const double Z = (cap == 0) ? HalfH : -HalfH; + for (int32 ir = 0; ir < Nr; ++ir) + { + const double Cr = (R * ir) / (Nr - 1); + const int32 RingN = FMath::Max(1, FMath::CeilToInt32(GTwoPi * Cr / GResolution)); + for (int32 ia = 0; ia < RingN; ++ia) + { + const double A = (GTwoPi * ia) / RingN; + double Px = Cr * FMath::Cos(A); + double Py = Cr * FMath::Sin(A); + double Pz = Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + } + } +} + +void SampleMesh(const FMjWorldGeom& G, TArray& Out) +{ + if (!G.Mesh.IsValid()) + { + return; + } + const FMjWorldMesh& Me = *G.Mesh; + const int32 VertCount = Me.Verts.Num(); + const int32 TriCount = Me.Tris.Num() / 3; + if (TriCount == 0) + { + return; + } + + // Emit all vertices. + for (int32 vi = 0; vi < VertCount; ++vi) + { + const FVector3f& V = Me.Verts[vi]; + double Px = V.X, Py = V.Y, Pz = V.Z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + + // Compute per-triangle area and total area. + TArray TriAreas; + TriAreas.SetNum(TriCount); + double TotalArea = 0.0; + for (int32 ti = 0; ti < TriCount; ++ti) + { + const int32 I0 = Me.Tris[ti * 3 + 0]; + const int32 I1 = Me.Tris[ti * 3 + 1]; + const int32 I2 = Me.Tris[ti * 3 + 2]; + const FVector3f& V0 = Me.Verts[I0]; + const FVector3f& V1 = Me.Verts[I1]; + const FVector3f& V2 = Me.Verts[I2]; + const FVector E1(V1.X - V0.X, V1.Y - V0.Y, V1.Z - V0.Z); + const FVector E2(V2.X - V0.X, V2.Y - V0.Y, V2.Z - V0.Z); + const double Area = 0.5 * FVector::CrossProduct(E1, E2).Size(); + TriAreas[ti] = Area; + TotalArea += Area; + } + if (TotalArea <= 0.0) + { + return; + } + + const int32 TotalTarget = FMath::Max(0, FMath::CeilToInt32(TotalArea / (GResolution * GResolution))); + if (TotalTarget <= 0) + { + return; + } + + // Distribute samples across triangles proportional to area. + Out.Reserve(Out.Num() + TotalTarget * 3); + int32 Emitted = 0; + for (int32 ti = 0; ti < TriCount && Emitted < TotalTarget; ++ti) + { + const int32 TargetForTri = FMath::Max(0, + FMath::RoundToInt32(static_cast(TotalTarget - Emitted) * TriAreas[ti] / TotalArea)); + if (TargetForTri <= 0) + { + continue; + } + const int32 I0 = Me.Tris[ti * 3 + 0]; + const int32 I1 = Me.Tris[ti * 3 + 1]; + const int32 I2 = Me.Tris[ti * 3 + 2]; + const FVector3f& V0 = Me.Verts[I0]; + const FVector3f& V1 = Me.Verts[I1]; + const FVector3f& V2 = Me.Verts[I2]; + const double V0x = V0.X, V0y = V0.Y, V0z = V0.Z; + const double E1x = V1.X - V0x, E1y = V1.Y - V0y, E1z = V1.Z - V0z; + const double E2x = V2.X - V0x, E2y = V2.Y - V0y, E2z = V2.Z - V0z; + + for (int32 s = 0; s < TargetForTri && Emitted < TotalTarget; ++s, ++Emitted) + { + double U = FMath::FRand(); + double V = FMath::FRand(); + if (U + V > 1.0) { U = 1.0 - U; V = 1.0 - V; } + double Px = V0x + U * E1x + V * E2x; + double Py = V0y + U * E1y + V * E2y; + double Pz = V0z + U * E1z + V * E2z; + ApplyWorldTransform(G.Xpos, G.Xquat, Px, Py, Pz); + EmitPoint(Out, Px, Py, Pz); + } + TotalArea -= TriAreas[ti]; + } +} + +void SampleWorldGeom(const FMjWorldGeom& G, TArray& Out) +{ + switch (G.Shape) + { + case EMjWorldGeomShape::Box: + SampleBox(G, Out); + break; + case EMjWorldGeomShape::Sphere: + SampleSphere(G, Out); + break; + case EMjWorldGeomShape::Cylinder: + SampleCylinder(G, Out); + break; + case EMjWorldGeomShape::Mesh: + SampleMesh(G, Out); + break; + } +} +} // namespace + +// sensor_msgs/PointCloud2 on /urlab/obstacle_cloud, world frame. Surface-samples +// every non-robot world geom at a fixed resolution and publishes at ~10 Hz. +class FMjRosPointCloudProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("obstacle_cloud"); } + + virtual void Build(FMjRosPublisherFactory& /*Factory*/, const FMjStateSnapshot& /*Snapshot*/) override + { + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return; + } + Pub = UrlabRcl_CreatePointCloud2Pub(Ctx, "/urlab/obstacle_cloud", "world", 0); + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + if (!Pub) + { + return; + } + static constexpr int64 PublishIntervalNs = 100'000'000; + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + if (Snapshot.WorldGeoms.Num() == 0) + { + UrlabRcl_PublishPointCloud2(Pub, nullptr, 0, SimTimeNs); + return; + } + + Points.Reset(); + for (const FMjWorldGeom& G : Snapshot.WorldGeoms) + { + SampleWorldGeom(G, Points); + } + const int32 N = Points.Num() / 3; + UrlabRcl_PublishPointCloud2(Pub, Points.GetData(), N, SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return Pub ? 1 : 0; } + + virtual ~FMjRosPointCloudProvider() { Destroy(); } + +private: + void Destroy() + { + if (Pub) + { + UrlabRcl_DestroyPointCloud2Pub(Pub); + Pub = nullptr; + } + LastPublishNs = 0; + } + + UrlabRclPointCloud2Pub* Pub = nullptr; + int64 LastPublishNs = 0; + TArray Points; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("obstacle_cloud", FMjRosPointCloudProvider); + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/Providers/RosPoseProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosPoseProvider.cpp new file mode 100644 index 00000000..44db8eef --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosPoseProvider.cpp @@ -0,0 +1,85 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosStateEstimation.h" +#include "State/MjStateTypes.h" + +// geometry_msgs/PoseWithCovarianceStamped on //pose, one publisher per +// free-base articulation. This is the ground-truth counterpart of amcl_pose: the +// robot's exact base pose in the map frame (frame_id = "map"), with a ground-truth +// covariance diagonal fixed at create. Fixed-base arts do not localise, so they +// are skipped (matching the odometry provider). +class FMjRosPoseProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("pose"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + MjRosStateEstimation::FMjFreeBaseState State; + if (!MjRosStateEstimation::ComputeFreeBaseState(Art, State)) + { + continue; + } + const FString Topic = FString::Printf(TEXT("/%s/pose"), *Art.Name.ToString()); + FMjRosPub Pub = Factory.CreatePoseWithCovariance(Topic, TEXT("map")); + if (Pub.IsValid()) + { + Entries.Add({i, MoveTemp(Pub)}); + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + MjRosStateEstimation::FMjFreeBaseState State; + if (MjRosStateEstimation::ComputeFreeBaseState( + Snapshot.Articulations[Entry.ArtIndex], State)) + { + Entry.Pub.PublishPoseWithCovariance(State.Position, State.OrientationXyzw, SimTimeNs); + } + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + FMjRosPub Pub; + }; + TArray Entries; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("pose", FMjRosPoseProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosProviderCommon.h b/Source/URLabRos/Private/Transport/Providers/RosProviderCommon.h new file mode 100644 index 00000000..d27f4d6e --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosProviderCommon.h @@ -0,0 +1,52 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "State/MjStateTypes.h" + +// Small helpers shared by the built-in output providers. Header-only inlines; no +// ROS dependency, so they compile in every configuration. +namespace MjRosProvider +{ + /** Find a sensor on an articulation by canonical name; null if absent. */ + inline const FMjSensorState* FindSensor(const FMjArticulationState& Art, FName Name) + { + for (const FMjSensorState& Sensor : Art.Sensors) + { + if (Sensor.Name == Name) + { + return &Sensor; + } + } + return nullptr; + } + + /** Copy up to three doubles out of a value array into a fixed vec3, zero-padded. */ + inline void FirstThree(const TArray& Values, double Out[3]) + { + Out[0] = Values.Num() > 0 ? Values[0] : 0.0; + Out[1] = Values.Num() > 1 ? Values[1] : 0.0; + Out[2] = Values.Num() > 2 ? Values[2] : 0.0; + } +} // namespace MjRosProvider diff --git a/Source/URLabRos/Private/Transport/Providers/RosRobotDescriptionProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosRobotDescriptionProvider.cpp new file mode 100644 index 00000000..8108bef8 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosRobotDescriptionProvider.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" +#include "MuJoCo/Core/AMjManager.h" + +// Latched std_msgs/String on //robot_description carrying the URDF the +// manager exported on compile. Published once at Build (transient-local QoS +// delivers it to late-joining rviz / MoveIt), so Publish is a no-op. +class FMjRosRobotDescriptionProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("robot_description"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Pubs.Reset(); + const AAMjManager* Manager = Factory.GetManager(); + if (!Manager) + { + return; + } + const TMap& Descriptions = Manager->GetRobotDescriptions(); + for (const FMjArticulationState& Art : Snapshot.Articulations) + { + const FString* Urdf = Descriptions.Find(Art.Name); + if (!Urdf) + { + continue; + } + const FString Topic = FString::Printf(TEXT("/%s/robot_description"), *Art.Name.ToString()); + FMjRosPub Pub = Factory.CreateString(Topic); + if (Pub.IsValid()) + { + Pub.PublishString(*Urdf); + Pubs.Add(MoveTemp(Pub)); + } + } + } + + virtual void Publish(const FMjStateSnapshot& /*Snapshot*/, int64 /*SimTimeNs*/) override + { + // Latched at Build; nothing to publish per step. + } + + virtual int32 GetPublisherCountForTest() const override { return Pubs.Num(); } + +private: + TArray Pubs; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("robot_description", FMjRosRobotDescriptionProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosSensorProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosSensorProvider.cpp new file mode 100644 index 00000000..7c765924 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosSensorProvider.cpp @@ -0,0 +1,199 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosSensorRouting.h" +#include "State/MjStateTypes.h" +#include "Transport/Providers/RosProviderCommon.h" + +// Total sensor -> ROS routing. Every sensor on every articulation reaches ROS via +// its RouteForSemantic message: Force+Torque -> WrenchStamped, Rangefinder -> +// Range, Magnetometer -> MagneticField, Velocimeter -> TwistStamped, and anything +// with no standard typed message -> Float64MultiArray on //sensors/. +// Gyro/Accel (the Imu route) are owned by the Imu provider and skipped here. +class FMjRosSensorProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("sensors"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + const FString ArtName = Art.Name.ToString(); + + // Force + Torque are paired into WrenchStamped; build those first, then + // route the remaining sensors one message per sensor. + TArray Pairs; + MjRosSensorRouting::GatherWrenchPairs(Art, Pairs); + for (const FMjWrenchPair& Pair : Pairs) + { + const FString Topic = MjRosSensorRouting::TopicFor(ArtName, + Pair.TopicName.ToString(), ERosSensorRoute::Wrench); + FMjRosPub Pub = Factory.CreateWrench(Topic, ArtName); + if (Pub.IsValid()) + { + FEntry Entry; + Entry.ArtIndex = i; + Entry.Route = ERosSensorRoute::Wrench; + Entry.PrimaryName = Pair.ForceSensor; + Entry.SecondaryName = Pair.TorqueSensor; + Entry.Pub = MoveTemp(Pub); + Entries.Add(MoveTemp(Entry)); + } + } + + for (const FMjSensorState& Sensor : Art.Sensors) + { + const ERosSensorRoute Route = RouteForSemantic(Sensor.Semantic); + // Imu is owned by the Imu provider; Wrench was handled by the pairing + // pass above. + if (Route == ERosSensorRoute::Imu || Route == ERosSensorRoute::Wrench) + { + continue; + } + const FString SensorName = Sensor.Name.ToString(); + const FString Topic = MjRosSensorRouting::TopicFor(ArtName, SensorName, Route); + FMjRosPub Pub = CreateForRoute(Factory, Route, Topic, ArtName); + if (Pub.IsValid()) + { + FEntry Entry; + Entry.ArtIndex = i; + Entry.Route = Route; + Entry.PrimaryName = Sensor.Name; + Entry.Pub = MoveTemp(Pub); + Entries.Add(MoveTemp(Entry)); + } + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + const FMjArticulationState& Art = Snapshot.Articulations[Entry.ArtIndex]; + + switch (Entry.Route) + { + case ERosSensorRoute::Wrench: + { + double Force[3] = {0.0, 0.0, 0.0}; + double Torque[3] = {0.0, 0.0, 0.0}; + if (const FMjSensorState* F = MjRosProvider::FindSensor(Art, Entry.PrimaryName)) + { + MjRosProvider::FirstThree(F->Values, Force); + } + if (const FMjSensorState* T = MjRosProvider::FindSensor(Art, Entry.SecondaryName)) + { + MjRosProvider::FirstThree(T->Values, Torque); + } + Entry.Pub.PublishWrench(Force, Torque, SimTimeNs); + break; + } + case ERosSensorRoute::Range: + { + const FMjSensorState* S = MjRosProvider::FindSensor(Art, Entry.PrimaryName); + const double Reading = (S && S->Values.Num() > 0) ? S->Values[0] : 0.0; + Entry.Pub.PublishRange(Reading, SimTimeNs); + break; + } + case ERosSensorRoute::MagneticField: + { + double Field[3] = {0.0, 0.0, 0.0}; + if (const FMjSensorState* S = MjRosProvider::FindSensor(Art, Entry.PrimaryName)) + { + MjRosProvider::FirstThree(S->Values, Field); + } + Entry.Pub.PublishMagneticField(Field, SimTimeNs); + break; + } + case ERosSensorRoute::Twist: + { + double Linear[3] = {0.0, 0.0, 0.0}; + const double Angular[3] = {0.0, 0.0, 0.0}; + if (const FMjSensorState* S = MjRosProvider::FindSensor(Art, Entry.PrimaryName)) + { + MjRosProvider::FirstThree(S->Values, Linear); + } + Entry.Pub.PublishTwistStamped(Linear, Angular, SimTimeNs); + break; + } + case ERosSensorRoute::MultiArray: + { + if (const FMjSensorState* S = MjRosProvider::FindSensor(Art, Entry.PrimaryName)) + { + Entry.Pub.PublishFloat64MultiArray(S->Values.GetData(), S->Values.Num()); + } + break; + } + case ERosSensorRoute::Imu: + break; // owned by the Imu provider + } + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + ERosSensorRoute Route = ERosSensorRoute::MultiArray; + FName PrimaryName; // the source sensor (Wrench: the force sensor, may be None) + FName SecondaryName; // Wrench only: the torque sensor (may be None) + FMjRosPub Pub; + }; + + static FMjRosPub CreateForRoute(FMjRosPublisherFactory& Factory, ERosSensorRoute Route, + const FString& Topic, const FString& ArtName) + { + switch (Route) + { + case ERosSensorRoute::Range: + // No FOV / range bounds in the IR; publish the reading with neutral + // constants (infrared, unbounded) so consumers still get the distance. + return Factory.CreateRange(Topic, ArtName, /*RadiationType=*/1, + /*FieldOfView=*/0.0f, /*MinRange=*/0.0f, /*MaxRange=*/TNumericLimits::Max()); + case ERosSensorRoute::MagneticField: + return Factory.CreateMagneticField(Topic, ArtName); + case ERosSensorRoute::Twist: + return Factory.CreateTwistStamped(Topic, ArtName); + case ERosSensorRoute::MultiArray: + return Factory.CreateFloat64MultiArray(Topic); + case ERosSensorRoute::Wrench: + case ERosSensorRoute::Imu: + default: + return FMjRosPub(); + } + } + + TArray Entries; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("sensors", FMjRosSensorProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosTfProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosTfProvider.cpp new file mode 100644 index 00000000..7d946865 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosTfProvider.cpp @@ -0,0 +1,68 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" + +// tf2_msgs/TFMessage on /tf, one process-wide publisher carrying every body's +// world pose (parent "world", child "/"). The flatten stays in the +// tested pure UURLabRosPublishTransport::FillTf. +class FMjRosTfProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("tf"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& /*Snapshot*/) override + { + TfPub = Factory.CreateTf(/*bStatic=*/false); + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + static constexpr int64 PublishIntervalNs = 20'000'000; // 50 Hz + if (LastPublishNs != 0 && (SimTimeNs - LastPublishNs) < PublishIntervalNs) + { + return; + } + LastPublishNs = SimTimeNs; + + if (!TfPub.IsValid()) + { + return; + } + TArray Parents; + TArray Children; + TArray Translations; + TArray Rotations; + UURLabRosPublishTransport::FillTf(Snapshot, Parents, Children, Translations, Rotations); + TfPub.PublishTf(Parents, Children, Translations, Rotations, SimTimeNs); + } + + virtual int32 GetPublisherCountForTest() const override { return TfPub.IsValid() ? 1 : 0; } + +private: + FMjRosPub TfPub; + int64 LastPublishNs = 0; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("tf", FMjRosTfProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosTwistProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosTwistProvider.cpp new file mode 100644 index 00000000..8ed04fa5 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosTwistProvider.cpp @@ -0,0 +1,83 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" + +// geometry_msgs/TwistStamped on //cmd_twist, one publisher per articulation +// that carries a twist command. +class FMjRosTwistProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("cmd_twist"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + if (!Art.Twist.IsSet()) + { + continue; + } + const FString ArtName = Art.Name.ToString(); + const FString Topic = FString::Printf(TEXT("/%s/cmd_twist"), *ArtName); + FMjRosPub Pub = Factory.CreateTwistStamped(Topic, ArtName); + if (Pub.IsValid()) + { + Entries.Add({i, MoveTemp(Pub)}); + } + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + for (FEntry& Entry : Entries) + { + if (!Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + continue; + } + const FMjArticulationState& Art = Snapshot.Articulations[Entry.ArtIndex]; + double Lin[3]; + double Ang[3]; + if (UURLabRosPublishTransport::FillTwistStamped(Art, Lin, Ang)) + { + Entry.Pub.PublishTwistStamped(Lin, Ang, SimTimeNs); + } + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = 0; + FMjRosPub Pub; + }; + TArray Entries; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("cmd_twist", FMjRosTwistProvider); diff --git a/Source/URLabRos/Private/Transport/Providers/RosUserChannelProvider.cpp b/Source/URLabRos/Private/Transport/Providers/RosUserChannelProvider.cpp new file mode 100644 index 00000000..fcb27d13 --- /dev/null +++ b/Source/URLabRos/Private/Transport/Providers/RosUserChannelProvider.cpp @@ -0,0 +1,243 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "State/MjStateTypes.h" +#include "Bridge/MsgpackHelpers.h" +#include "Dom/JsonObject.h" +#include "Serialization/JsonSerializer.h" + +// User-channel -> ROS routing, typed per kind. Every FMjUserChannel on every +// articulation (topic //user/) and every scene-scoped channel (topic +// /urlab/user/) reaches ROS as a typed message keyed on its kind: +// Bool -> std_msgs/Bool +// Int / Scalar -> std_msgs/Float64 +// Vec3 -> geometry_msgs/Vector3 +// Quat / Transform -> geometry_msgs/PoseStamped (wxyz reordered to xyzw) +// Array -> std_msgs/Float64MultiArray +// String -> std_msgs/String +// Struct -> std_msgs/String (the packed msgpack map as JSON text) +// The declared channel set is structure, so the publisher set rebuilds only on a +// StructureVersion change, like every other provider. +class FMjRosUserChannelProvider : public IMjRosOutputProvider +{ +public: + virtual FName GetProviderName() const override { return TEXT("user_channels"); } + + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) override + { + Entries.Reset(); + for (int32 i = 0; i < Snapshot.Articulations.Num(); ++i) + { + const FMjArticulationState& Art = Snapshot.Articulations[i]; + const FString ArtName = Art.Name.ToString(); + for (const FMjUserChannel& Channel : Art.UserChannels) + { + const FString Topic = FString::Printf(TEXT("/%s/user/%s"), + *ArtName, *Channel.Name.ToString()); + BuildEntry(Factory, i, Channel, Topic, ArtName); + } + } + for (const FMjUserChannel& Channel : Snapshot.UserChannels) + { + const FString Topic = FString::Printf(TEXT("/urlab/user/%s"), + *Channel.Name.ToString()); + BuildEntry(Factory, /*SceneScope*/ -1, Channel, Topic, TEXT("world")); + } + } + + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) override + { + for (FEntry& Entry : Entries) + { + const TArray* Channels = nullptr; + if (Entry.ArtIndex < 0) + { + Channels = &Snapshot.UserChannels; + } + else if (Snapshot.Articulations.IsValidIndex(Entry.ArtIndex)) + { + Channels = &Snapshot.Articulations[Entry.ArtIndex].UserChannels; + } + if (!Channels) + { + continue; + } + const FMjUserChannel* Channel = FindChannel(*Channels, Entry.Name); + if (!Channel) + { + continue; + } + PublishEntry(Entry, *Channel, SimTimeNs); + } + } + + virtual int32 GetPublisherCountForTest() const override { return Entries.Num(); } + +private: + struct FEntry + { + int32 ArtIndex = -1; // -1 = scene scope + FName Name; + EMjUserChannelKind Kind = EMjUserChannelKind::Scalar; + FMjRosPub Pub; + }; + + static const FMjUserChannel* FindChannel(const TArray& Channels, FName Name) + { + for (const FMjUserChannel& C : Channels) + { + if (C.Name == Name) + { + return &C; + } + } + return nullptr; + } + + void BuildEntry(FMjRosPublisherFactory& Factory, int32 ArtIndex, + const FMjUserChannel& Channel, const FString& Topic, const FString& FrameId) + { + FMjRosPub Pub; + switch (Channel.Kind) + { + case EMjUserChannelKind::Bool: + Pub = Factory.CreateBool(Topic); + break; + case EMjUserChannelKind::Int: + case EMjUserChannelKind::Scalar: + Pub = Factory.CreateFloat64(Topic); + break; + case EMjUserChannelKind::Vec3: + Pub = Factory.CreateVector3(Topic); + break; + case EMjUserChannelKind::Quat: + case EMjUserChannelKind::Transform: + Pub = Factory.CreatePoseStamped(Topic, FrameId); + break; + case EMjUserChannelKind::Array: + Pub = Factory.CreateFloat64MultiArray(Topic); + break; + case EMjUserChannelKind::String: + case EMjUserChannelKind::Struct: + Pub = Factory.CreateString(Topic); + break; + } + if (Pub.IsValid()) + { + FEntry Entry; + Entry.ArtIndex = ArtIndex; + Entry.Name = Channel.Name; + Entry.Kind = Channel.Kind; + Entry.Pub = MoveTemp(Pub); + Entries.Add(MoveTemp(Entry)); + } + } + + static void PublishEntry(FEntry& Entry, const FMjUserChannel& Channel, int64 SimTimeNs) + { + switch (Entry.Kind) + { + case EMjUserChannelKind::Bool: + Entry.Pub.PublishBool(Channel.Values.Num() > 0 && Channel.Values[0] != 0.0); + break; + case EMjUserChannelKind::Int: + case EMjUserChannelKind::Scalar: + Entry.Pub.PublishFloat64(Channel.Values.Num() > 0 ? Channel.Values[0] : 0.0); + break; + case EMjUserChannelKind::Vec3: + { + double Xyz[3] = {0.0, 0.0, 0.0}; + for (int32 i = 0; i < 3 && i < Channel.Values.Num(); ++i) + { + Xyz[i] = Channel.Values[i]; + } + Entry.Pub.PublishVector3(Xyz); + break; + } + case EMjUserChannelKind::Quat: + { + // Values are wxyz; PoseStamped orientation is xyzw, position zero. + const double Pos[3] = {0.0, 0.0, 0.0}; + double Quat[4] = {0.0, 0.0, 0.0, 1.0}; + if (Channel.Values.Num() >= 4) + { + Quat[0] = Channel.Values[1]; + Quat[1] = Channel.Values[2]; + Quat[2] = Channel.Values[3]; + Quat[3] = Channel.Values[0]; + } + Entry.Pub.PublishPoseStamped(Pos, Quat, SimTimeNs); + break; + } + case EMjUserChannelKind::Transform: + { + // Values: [0..2] pos, [3..6] quat wxyz -> xyzw. + double Pos[3] = {0.0, 0.0, 0.0}; + double Quat[4] = {0.0, 0.0, 0.0, 1.0}; + for (int32 i = 0; i < 3 && i < Channel.Values.Num(); ++i) + { + Pos[i] = Channel.Values[i]; + } + if (Channel.Values.Num() >= 7) + { + Quat[0] = Channel.Values[4]; + Quat[1] = Channel.Values[5]; + Quat[2] = Channel.Values[6]; + Quat[3] = Channel.Values[3]; + } + Entry.Pub.PublishPoseStamped(Pos, Quat, SimTimeNs); + break; + } + case EMjUserChannelKind::Array: + Entry.Pub.PublishFloat64MultiArray(Channel.Values.GetData(), Channel.Values.Num()); + break; + case EMjUserChannelKind::String: + Entry.Pub.PublishString(Channel.Text); + break; + case EMjUserChannelKind::Struct: + Entry.Pub.PublishString(StructToJson(Channel.Packed)); + break; + } + } + + /** Inflate the packed msgpack map and re-serialise it as JSON text so a Struct + * channel is human-readable on its std_msgs/String topic. */ + static FString StructToJson(const TArray& Packed) + { + TSharedPtr Parsed; + if (Packed.Num() > 0 + && FURLabMsgpackUtil::UnpackToJsonObject(Packed.GetData(), Packed.Num(), Parsed) + && Parsed.IsValid()) + { + FString Out; + TSharedRef> Writer = TJsonWriterFactory<>::Create(&Out); + FJsonSerializer::Serialize(Parsed.ToSharedRef(), Writer); + return Out; + } + return TEXT("{}"); + } + + TArray Entries; +}; + +REGISTER_MJ_ROS_OUTPUT_PROVIDER("user_channels", FMjRosUserChannelProvider); diff --git a/Source/URLabRos/Private/Transport/RosContext.cpp b/Source/URLabRos/Private/Transport/RosContext.cpp new file mode 100644 index 00000000..c260754b --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosContext.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// The whole body is fenced so that when ROS is not linked (URLAB_WITH_ROS2 +// undefined or 0) this is an empty translation unit and no rcl symbol is +// referenced. Every caller of FURLabRosContext is fenced the same way, so the +// out-of-line definitions below are only needed when the feature is on. +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "Transport/RosContext.h" +#include "URLabRosLog.h" + +#include "Ros/UrlabRclCore.h" + +FURLabRosContext& FURLabRosContext::Get() +{ + static FURLabRosContext Instance; + return Instance; +} + +FURLabRosContext::~FURLabRosContext() +{ + Shutdown(); +} + +bool FURLabRosContext::Initialize() +{ + FScopeLock Lock(&Mutex); + if (bInitAttempted) + { + return Context != nullptr; + } + bInitAttempted = true; + + Context = UrlabRcl_Init("urlab", "", -1); + if (Context == nullptr) + { + UE_LOG(LogURLabRos, Warning, + TEXT("ROS 2 unavailable: rcl context init failed (%hs). ROS publishing is disabled."), + UrlabRcl_LastError()); + return false; + } + + UE_LOG(LogURLabRos, Log, TEXT("ROS 2 context up (distro %hs, node 'urlab')."), + UrlabRcl_DistroName()); + return true; +} + +bool FURLabRosContext::IsAvailable() const +{ + FScopeLock Lock(&Mutex); + return Context != nullptr; +} + +void FURLabRosContext::Shutdown() +{ + FScopeLock Lock(&Mutex); + if (Context != nullptr) + { + UrlabRcl_Shutdown(Context); + Context = nullptr; + } + // Clear the attempt latch so an explicit teardown can be followed by a fresh + // Initialize(). The latch exists only to stop a failed auto-init from being + // retried every step, not to make shutdown terminal. + bInitAttempted = false; +} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/RosOutputProvider.cpp b/Source/URLabRos/Private/Transport/RosOutputProvider.cpp new file mode 100644 index 00000000..23988029 --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosOutputProvider.cpp @@ -0,0 +1,693 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosOutputProvider.h" +#include "URLabRosLog.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 +#include "Ros/UrlabRclCore.h" +#endif + +// --- Registry (ROS-agnostic; populated in every configuration) -------------- + +FMjRosOutputRegistry& FMjRosOutputRegistry::Get() +{ + static FMjRosOutputRegistry Instance; + return Instance; +} + +void FMjRosOutputRegistry::Register(FName Name, FMjRosOutputProviderFactoryFn Factory) +{ + for (TPair& Entry : Entries) + { + if (Entry.Key == Name) + { + UE_LOG(LogURLabRos, Warning, + TEXT("[URLabRos] output provider '%s' re-registered; the later one wins."), + *Name.ToString()); + Entry.Value = MoveTemp(Factory); + return; + } + } + Entries.Emplace(Name, MoveTemp(Factory)); +} + +void FMjRosOutputRegistry::InstantiateAll(TArray>& Out) const +{ + Out.Reset(); + Out.Reserve(Entries.Num()); + for (const TPair& Entry : Entries) + { + if (Entry.Value) + { + if (TUniquePtr Provider = Entry.Value()) + { + Out.Add(MoveTemp(Provider)); + } + } + } +} + +TArray FMjRosOutputRegistry::GetRegisteredNames() const +{ + TArray Names; + Names.Reserve(Entries.Num()); + for (const TPair& Entry : Entries) + { + Names.Add(Entry.Key); + } + return Names; +} + +FMjRosOutputProviderRegistrar::FMjRosOutputProviderRegistrar(FName Name, + FMjRosOutputProviderFactoryFn Factory) +{ + FMjRosOutputRegistry::Get().Register(Name, MoveTemp(Factory)); +} + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +// --- FMjRosPub (owning handle over one rcl publisher) ----------------------- + +void FMjRosPub::Reset() +{ + if (Handle == nullptr) + { + return; + } + switch (Kind) + { + case EKind::JointState: + UrlabRcl_DestroyJointStatePub(static_cast(Handle)); + break; + case EKind::Imu: + UrlabRcl_DestroyImuPub(static_cast(Handle)); + break; + case EKind::Tf: + UrlabRcl_DestroyTfPub(static_cast(Handle)); + break; + case EKind::TwistStamped: + UrlabRcl_DestroyTwistStampedPub(static_cast(Handle)); + break; + case EKind::Clock: + UrlabRcl_DestroyClockPub(static_cast(Handle)); + break; + case EKind::String: + UrlabRcl_DestroyStringPub(static_cast(Handle)); + break; + case EKind::Wrench: + UrlabRcl_DestroyWrenchStampedPub(static_cast(Handle)); + break; + case EKind::Range: + UrlabRcl_DestroyRangePub(static_cast(Handle)); + break; + case EKind::MagneticField: + UrlabRcl_DestroyMagneticFieldPub(static_cast(Handle)); + break; + case EKind::MultiArray: + UrlabRcl_DestroyFloat64MultiArrayPub(static_cast(Handle)); + break; + case EKind::Odometry: + UrlabRcl_DestroyOdometryPub(static_cast(Handle)); + break; + case EKind::PoseWithCovariance: + UrlabRcl_DestroyPoseWithCovariancePub(static_cast(Handle)); + break; + case EKind::CameraInfo: + UrlabRcl_DestroyCameraInfoPub(static_cast(Handle)); + break; + case EKind::Bool: + UrlabRcl_DestroyBoolPub(static_cast(Handle)); + break; + case EKind::Float64: + UrlabRcl_DestroyFloat64Pub(static_cast(Handle)); + break; + case EKind::Vector3: + UrlabRcl_DestroyVector3Pub(static_cast(Handle)); + break; + case EKind::PoseStamped: + UrlabRcl_DestroyPoseStampedPub(static_cast(Handle)); + break; + case EKind::None: + break; + } + Handle = nullptr; + Kind = EKind::None; +} + +void FMjRosPub::PublishJointState(const double* Positions, const double* Velocities, + const double* Efforts, int32 Count, int64 SimTimeNs) +{ + if (Kind == EKind::JointState && Handle) + { + UrlabRcl_PublishJointState(static_cast(Handle), + Positions, Velocities, Efforts, Count, SimTimeNs); + } +} + +void FMjRosPub::PublishImu(const double* AngularVel3, const double* LinearAccel3, + const double* OrientationXyzw4, int64 SimTimeNs) +{ + if (Kind == EKind::Imu && Handle) + { + UrlabRcl_PublishImu(static_cast(Handle), + AngularVel3, LinearAccel3, OrientationXyzw4, SimTimeNs); + } +} + +void FMjRosPub::PublishTf(const TArray& Parents, const TArray& Children, + const TArray& TranslationsXyz, const TArray& RotationsXyzw, int64 SimTimeNs) +{ + if (Kind != EKind::Tf || !Handle || Parents.Num() == 0) + { + return; + } + // The core copies the frame strings at publish time; keep stable UTF-8 buffers + // and pointer arrays alive across the call. + TArray> ParentBytes; + TArray> ChildBytes; + ParentBytes.Reserve(Parents.Num()); + ChildBytes.Reserve(Children.Num()); + TArray ParentPtrs; + TArray ChildPtrs; + ParentPtrs.Reserve(Parents.Num()); + ChildPtrs.Reserve(Children.Num()); + auto AppendUtf8 = [](TArray>& Store, TArray& Ptrs, + const FString& Value) + { + FTCHARToUTF8 Conv(*Value); + TArray& Bytes = Store.AddDefaulted_GetRef(); + Bytes.Append(reinterpret_cast(Conv.Get()), Conv.Length()); + Bytes.Add('\0'); + Ptrs.Add(Bytes.GetData()); + }; + for (const FString& P : Parents) + { + AppendUtf8(ParentBytes, ParentPtrs, P); + } + for (const FString& C : Children) + { + AppendUtf8(ChildBytes, ChildPtrs, C); + } + + UrlabRcl_PublishTf(static_cast(Handle), ParentPtrs.GetData(), + ChildPtrs.GetData(), TranslationsXyz.GetData(), RotationsXyzw.GetData(), + ParentPtrs.Num(), SimTimeNs); +} + +void FMjRosPub::PublishTwistStamped(const double Linear3[3], const double Angular3[3], + int64 SimTimeNs) +{ + if (Kind == EKind::TwistStamped && Handle) + { + UrlabRcl_PublishTwistStamped(static_cast(Handle), + Linear3, Angular3, SimTimeNs); + } +} + +void FMjRosPub::PublishClock(int64 SimTimeNs) +{ + if (Kind == EKind::Clock && Handle) + { + UrlabRcl_PublishClock(static_cast(Handle), SimTimeNs); + } +} + +void FMjRosPub::PublishString(const FString& Text) +{ + if (Kind == EKind::String && Handle) + { + UrlabRcl_PublishString(static_cast(Handle), TCHAR_TO_UTF8(*Text)); + } +} + +void FMjRosPub::PublishWrench(const double Force3[3], const double Torque3[3], int64 SimTimeNs) +{ + if (Kind == EKind::Wrench && Handle) + { + UrlabRcl_PublishWrenchStamped(static_cast(Handle), + Force3, Torque3, SimTimeNs); + } +} + +void FMjRosPub::PublishRange(double Range, int64 SimTimeNs) +{ + if (Kind == EKind::Range && Handle) + { + UrlabRcl_PublishRange(static_cast(Handle), + static_cast(Range), SimTimeNs); + } +} + +void FMjRosPub::PublishMagneticField(const double Field3[3], int64 SimTimeNs) +{ + if (Kind == EKind::MagneticField && Handle) + { + UrlabRcl_PublishMagneticField(static_cast(Handle), + Field3, SimTimeNs); + } +} + +void FMjRosPub::PublishFloat64MultiArray(const double* Values, int32 Count) +{ + if (Kind == EKind::MultiArray && Handle) + { + UrlabRcl_PublishFloat64MultiArray( + static_cast(Handle), Values, Count); + } +} + +void FMjRosPub::PublishOdometry(const double Position3[3], const double OrientationXyzw4[4], + const double LinearBody3[3], const double AngularBody3[3], int64 SimTimeNs) +{ + if (Kind == EKind::Odometry && Handle) + { + UrlabRcl_PublishOdometry(static_cast(Handle), + Position3, OrientationXyzw4, LinearBody3, AngularBody3, SimTimeNs); + } +} + +void FMjRosPub::PublishPoseWithCovariance(const double Position3[3], + const double OrientationXyzw4[4], int64 SimTimeNs) +{ + if (Kind == EKind::PoseWithCovariance && Handle) + { + UrlabRcl_PublishPoseWithCovariance(static_cast(Handle), + Position3, OrientationXyzw4, SimTimeNs); + } +} + +void FMjRosPub::PublishCameraInfo(int64 SimTimeNs) +{ + if (Kind == EKind::CameraInfo && Handle) + { + UrlabRcl_PublishCameraInfo(static_cast(Handle), SimTimeNs); + } +} + +void FMjRosPub::PublishBool(bool bValue) +{ + if (Kind == EKind::Bool && Handle) + { + UrlabRcl_PublishBool(static_cast(Handle), bValue ? 1 : 0); + } +} + +void FMjRosPub::PublishFloat64(double Value) +{ + if (Kind == EKind::Float64 && Handle) + { + UrlabRcl_PublishFloat64(static_cast(Handle), Value); + } +} + +void FMjRosPub::PublishVector3(const double Xyz3[3]) +{ + if (Kind == EKind::Vector3 && Handle) + { + UrlabRcl_PublishVector3(static_cast(Handle), Xyz3); + } +} + +void FMjRosPub::PublishPoseStamped(const double Position3[3], const double OrientationXyzw4[4], + int64 SimTimeNs) +{ + if (Kind == EKind::PoseStamped && Handle) + { + UrlabRcl_PublishPoseStamped(static_cast(Handle), + Position3, OrientationXyzw4, SimTimeNs); + } +} + +// --- FMjRosPublisherFactory ------------------------------------------------- + +FMjRosPub FMjRosPublisherFactory::CreateJointState(const FString& Topic, + const TArray& JointNames) +{ + if (!Context) + { + return FMjRosPub(); + } + // The core copies the name strings at create time; hold one UTF-8 buffer per + // name and hand it a stable pointer array (the outer array is reserved so the + // element pointers do not move). + TArray> NameBytes; + NameBytes.Reserve(JointNames.Num()); + TArray NamePtrs; + NamePtrs.Reserve(JointNames.Num()); + for (const FString& Name : JointNames) + { + FTCHARToUTF8 Conv(*Name); + TArray& Bytes = NameBytes.AddDefaulted_GetRef(); + Bytes.Append(reinterpret_cast(Conv.Get()), Conv.Length()); + Bytes.Add('\0'); + NamePtrs.Add(Bytes.GetData()); + } + + UrlabRclJointStatePub* Pub = UrlabRcl_CreateJointStatePub(Context, + TCHAR_TO_UTF8(*Topic), NamePtrs.GetData(), NamePtrs.Num()); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: JointState publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::JointState); +} + +FMjRosPub FMjRosPublisherFactory::CreateImu(const FString& Topic, const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclImuPub* Pub = UrlabRcl_CreateImuPub(Context, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Imu publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Imu); +} + +FMjRosPub FMjRosPublisherFactory::CreateTf(bool bStatic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclTfPub* Pub = UrlabRcl_CreateTfPub(Context, bStatic ? 1 : 0); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: /tf publisher create failed (%hs)"), + UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Tf); +} + +FMjRosPub FMjRosPublisherFactory::CreateTwistStamped(const FString& Topic, const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclTwistStampedPub* Pub = UrlabRcl_CreateTwistStampedPub(Context, + TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: TwistStamped publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::TwistStamped); +} + +FMjRosPub FMjRosPublisherFactory::CreateClock() +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclClockPub* Pub = UrlabRcl_CreateClockPub(Context); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: /clock publisher create failed (%hs)"), + UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Clock); +} + +FMjRosPub FMjRosPublisherFactory::CreateString(const FString& Topic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclStringPub* Pub = UrlabRcl_CreateStringPub(Context, TCHAR_TO_UTF8(*Topic)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: String publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::String); +} + +FMjRosPub FMjRosPublisherFactory::CreateWrench(const FString& Topic, const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclWrenchStampedPub* Pub = UrlabRcl_CreateWrenchStampedPub(Context, + TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: WrenchStamped publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Wrench); +} + +FMjRosPub FMjRosPublisherFactory::CreateRange(const FString& Topic, const FString& FrameId, + uint8 RadiationType, float FieldOfView, float MinRange, float MaxRange) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclRangePub* Pub = UrlabRcl_CreateRangePub(Context, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*FrameId), RadiationType, FieldOfView, MinRange, MaxRange); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Range publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Range); +} + +FMjRosPub FMjRosPublisherFactory::CreateMagneticField(const FString& Topic, const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclMagneticFieldPub* Pub = UrlabRcl_CreateMagneticFieldPub(Context, + TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: MagneticField publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::MagneticField); +} + +FMjRosPub FMjRosPublisherFactory::CreateFloat64MultiArray(const FString& Topic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclFloat64MultiArrayPub* Pub = UrlabRcl_CreateFloat64MultiArrayPub(Context, + TCHAR_TO_UTF8(*Topic)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Float64MultiArray publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::MultiArray); +} + +FMjRosPub FMjRosPublisherFactory::CreateOdometry(const FString& Topic, const FString& FrameId, + const FString& ChildFrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclOdometryPub* Pub = UrlabRcl_CreateOdometryPub(Context, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*FrameId), TCHAR_TO_UTF8(*ChildFrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Odometry publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Odometry); +} + +FMjRosPub FMjRosPublisherFactory::CreatePoseWithCovariance(const FString& Topic, + const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclPoseWithCovariancePub* Pub = UrlabRcl_CreatePoseWithCovariancePub(Context, + TCHAR_TO_UTF8(*Topic), TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, + TEXT("ROS: PoseWithCovarianceStamped publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::PoseWithCovariance); +} + +FMjRosPub FMjRosPublisherFactory::CreateCameraInfo(const FString& Topic, const FString& FrameId, + int32 Width, int32 Height, const double K9[9]) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclCameraInfoPub* Pub = UrlabRcl_CreateCameraInfoPub(Context, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*FrameId), Width, Height, K9); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: CameraInfo publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::CameraInfo); +} + +FMjRosPub FMjRosPublisherFactory::CreateBool(const FString& Topic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclBoolPub* Pub = UrlabRcl_CreateBoolPub(Context, TCHAR_TO_UTF8(*Topic)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Bool publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Bool); +} + +FMjRosPub FMjRosPublisherFactory::CreateFloat64(const FString& Topic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclFloat64Pub* Pub = UrlabRcl_CreateFloat64Pub(Context, TCHAR_TO_UTF8(*Topic)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Float64 publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Float64); +} + +FMjRosPub FMjRosPublisherFactory::CreateVector3(const FString& Topic) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclVector3Pub* Pub = UrlabRcl_CreateVector3Pub(Context, TCHAR_TO_UTF8(*Topic)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: Vector3 publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::Vector3); +} + +FMjRosPub FMjRosPublisherFactory::CreatePoseStamped(const FString& Topic, const FString& FrameId) +{ + if (!Context) + { + return FMjRosPub(); + } + UrlabRclPoseStampedPub* Pub = UrlabRcl_CreatePoseStampedPub(Context, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*FrameId)); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: PoseStamped publisher create failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + return FMjRosPub(); + } + return FMjRosPub(Pub, FMjRosPub::EKind::PoseStamped); +} + +#else // URLAB_WITH_ROS2 + +// Absent-ROS stubs: handles are never created (Create* return an invalid handle), +// so publishing and release are no-ops. Providers compile and register in every +// configuration; the transport simply never drives them without a live context. + +void FMjRosPub::Reset() {} +void FMjRosPub::PublishJointState(const double*, const double*, const double*, int32, int64) {} +void FMjRosPub::PublishImu(const double*, const double*, const double*, int64) {} +void FMjRosPub::PublishTf(const TArray&, const TArray&, + const TArray&, const TArray&, int64) {} +void FMjRosPub::PublishTwistStamped(const double[3], const double[3], int64) {} +void FMjRosPub::PublishClock(int64) {} +void FMjRosPub::PublishString(const FString&) {} +void FMjRosPub::PublishWrench(const double[3], const double[3], int64) {} +void FMjRosPub::PublishRange(double, int64) {} +void FMjRosPub::PublishMagneticField(const double[3], int64) {} +void FMjRosPub::PublishFloat64MultiArray(const double*, int32) {} +void FMjRosPub::PublishOdometry(const double[3], const double[4], const double[3], + const double[3], int64) {} +void FMjRosPub::PublishPoseWithCovariance(const double[3], const double[4], int64) {} +void FMjRosPub::PublishCameraInfo(int64) {} +void FMjRosPub::PublishBool(bool) {} +void FMjRosPub::PublishFloat64(double) {} +void FMjRosPub::PublishVector3(const double[3]) {} +void FMjRosPub::PublishPoseStamped(const double[3], const double[4], int64) {} + +FMjRosPub FMjRosPublisherFactory::CreateJointState(const FString&, const TArray&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateImu(const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateTf(bool) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateTwistStamped(const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateClock() { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateString(const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateWrench(const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateRange(const FString&, const FString&, uint8, float, float, float) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateMagneticField(const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateFloat64MultiArray(const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateOdometry(const FString&, const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreatePoseWithCovariance(const FString&, const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateCameraInfo(const FString&, const FString&, int32, int32, const double[9]) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateBool(const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateFloat64(const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreateVector3(const FString&) { return FMjRosPub(); } +FMjRosPub FMjRosPublisherFactory::CreatePoseStamped(const FString&, const FString&) { return FMjRosPub(); } + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/RosPublishTransport.cpp b/Source/URLabRos/Private/Transport/RosPublishTransport.cpp new file mode 100644 index 00000000..428ccddf --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosPublishTransport.cpp @@ -0,0 +1,300 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 +#include "Transport/RosContext.h" +#include "Transport/RosOutputProvider.h" +#include "URLabRosLog.h" +#include "MuJoCo/Core/AMjManager.h" +#endif + +// ConsumeState is the IMjStateConsumer entry point the manager fan-out calls; it +// forwards to PublishState (which has an absent-ROS no-op stub), so it links in +// every configuration. +void UURLabRosPublishTransport::ConsumeState(const FMjStateSnapshot& Snapshot) +{ + PublishState(Snapshot); +} + +// The Fill* functions are pure IR -> arrays transforms with no rcl dependency, so +// they compile in every configuration (and are exercised by the fill-correctness +// tests whether or not ROS is linked). + +void UURLabRosPublishTransport::FillJointState(const FMjArticulationState& Art, + TArray& OutNames, TArray& OutPositions, + TArray& OutVelocities, TArray& OutEfforts) +{ + OutNames.Reset(); + OutPositions.Reset(); + OutVelocities.Reset(); + OutEfforts.Reset(); + OutNames.Reserve(Art.Joints.Num()); + OutPositions.Reserve(Art.Joints.Num()); + OutVelocities.Reserve(Art.Joints.Num()); + OutEfforts.Reserve(Art.Joints.Num()); + + // Map joint -> actuator force by the joint each actuator drives (its target + // joint), which is the name JointState reports the joint under. An actuator's + // own name need not match the joint it drives (e.g. 'actuator1' drives 'joint1'), + // so keying by the target joint is what makes effort line up. Joints with no + // joint-transmission actuator report zero. + TMap ForceByJoint; + ForceByJoint.Reserve(Art.Actuators.Num()); + for (const FMjActuatorState& Actuator : Art.Actuators) + { + if (!Actuator.TargetJoint.IsNone()) + { + ForceByJoint.Add(Actuator.TargetJoint, Actuator.Force); + } + } + + bool bAnyEffort = false; + for (const FMjJointState& Joint : Art.Joints) + { + // sensor_msgs/JointState is parallel scalar arrays. Only hinge / slide joints + // are scalar (1 qpos / 1 qvel); free (7/6) and ball (4/3) joints are not URDF + // joints and reach ROS through /tf, so they are not JointState entries. + if (Joint.Type != EMjJointType::Hinge && Joint.Type != EMjJointType::Slide) + { + continue; + } + + OutNames.Add(Joint.Name.ToString()); + // Emit qpos - qpos0 so the ROS zero pose matches the exported URDF (whose + // joint limits are shifted by qpos0). RefPos is filled only for the 1-DOF + // joints the URDF exposes; an empty slice means no shift. + const double Pos = Joint.QPos.Num() > 0 ? Joint.QPos[0] : 0.0; + const double Ref = Joint.RefPos.Num() > 0 ? Joint.RefPos[0] : 0.0; + OutPositions.Add(Pos - Ref); + OutVelocities.Add(Joint.QVel.Num() > 0 ? Joint.QVel[0] : 0.0); + + if (const double* Force = ForceByJoint.Find(Joint.Name)) + { + OutEfforts.Add(*Force); + bAnyEffort = true; + } + else + { + OutEfforts.Add(0.0); + } + } + + // Distinguish "no effort data" (no actuator drives any joint) from a genuine + // all-zero effort by leaving the array empty in the former case. + if (!bAnyEffort) + { + OutEfforts.Reset(); + } +} + +bool UURLabRosPublishTransport::FillImu(const FMjArticulationState& Art, + double OutAngularVel[3], bool& bOutHasAngularVel, + double OutLinearAccel[3], bool& bOutHasLinearAccel) +{ + bOutHasAngularVel = false; + bOutHasLinearAccel = false; + for (int32 i = 0; i < 3; ++i) + { + OutAngularVel[i] = 0.0; + OutLinearAccel[i] = 0.0; + } + + // Pair the first gyro with the first accel found on the art. A gyro without an + // accel still yields an Imu with angular velocity only, and vice versa. + for (const FMjSensorState& Sensor : Art.Sensors) + { + if (!bOutHasAngularVel && Sensor.Semantic == EMjSensorSemantic::Gyro + && Sensor.Values.Num() >= 3) + { + OutAngularVel[0] = Sensor.Values[0]; + OutAngularVel[1] = Sensor.Values[1]; + OutAngularVel[2] = Sensor.Values[2]; + bOutHasAngularVel = true; + } + else if (!bOutHasLinearAccel && Sensor.Semantic == EMjSensorSemantic::Accel + && Sensor.Values.Num() >= 3) + { + OutLinearAccel[0] = Sensor.Values[0]; + OutLinearAccel[1] = Sensor.Values[1]; + OutLinearAccel[2] = Sensor.Values[2]; + bOutHasLinearAccel = true; + } + } + + return bOutHasAngularVel || bOutHasLinearAccel; +} + +bool UURLabRosPublishTransport::FillTwistStamped(const FMjArticulationState& Art, + double OutLinear[3], double OutAngular[3]) +{ + if (!Art.Twist.IsSet()) + { + return false; + } + const FMjTwistState& Twist = Art.Twist.GetValue(); + for (int32 i = 0; i < 3; ++i) + { + OutLinear[i] = Twist.Linear[i]; + OutAngular[i] = Twist.Angular[i]; + } + return true; +} + +void UURLabRosPublishTransport::FillTf(const FMjStateSnapshot& Snapshot, + TArray& OutParents, TArray& OutChildren, + TArray& OutTranslations, TArray& OutRotationsXyzw) +{ + OutParents.Reset(); + OutChildren.Reset(); + OutTranslations.Reset(); + OutRotationsXyzw.Reset(); + + for (const FMjArticulationState& Art : Snapshot.Articulations) + { + for (const FMjBodyState& Body : Art.Bodies) + { + OutParents.Add(TEXT("world")); + OutChildren.Add(FMjCanonicalName::Full(Art.Name, Body.Name)); + OutTranslations.Add(Body.Xpos[0]); + OutTranslations.Add(Body.Xpos[1]); + OutTranslations.Add(Body.Xpos[2]); + // MuJoCo stores quaternions wxyz; the core (and ROS) expect xyzw. + OutRotationsXyzw.Add(Body.Xquat[1]); + OutRotationsXyzw.Add(Body.Xquat[2]); + OutRotationsXyzw.Add(Body.Xquat[3]); + OutRotationsXyzw.Add(Body.Xquat[0]); + } + } +} + +int64 UURLabRosPublishTransport::FillClock(const FMjClock& Clock) +{ + return static_cast(Clock.SimSec) * 1000000000LL + + static_cast(Clock.SimNsec); +} + +// Reports the joint_state provider's per-articulation publisher count (one per +// art), preserving the historical "art publisher count" test seam. Compiles in +// every configuration; Providers is empty when ROS is not linked. +int32 UURLabRosPublishTransport::GetArtPublisherCountForTest() const +{ + for (const TUniquePtr& Provider : Providers) + { + if (Provider && Provider->GetProviderName() == TEXT("joint_state")) + { + return Provider->GetPublisherCountForTest(); + } + } + return 0; +} + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +bool UURLabRosPublishTransport::TransportInit() +{ + if (!FURLabRosContext::Get().Initialize()) + { + return false; + } + // Register with the owning manager as a typed state consumer so the post-step + // fan-out drives ConsumeState. Null-safe: a transport created without a manager + // outer (isolated publish test) simply is not registered and is driven directly. + if (AAMjManager* Manager = GetTypedOuter()) + { + Manager->RegisterStateConsumer(this, this); + } + return true; +} + +void UURLabRosPublishTransport::TransportShutdown() +{ + if (AAMjManager* Manager = GetTypedOuter()) + { + Manager->UnregisterStateConsumer(this); + } + // Destroying each provider releases the publishers it owns. + Providers.Reset(); + bProvidersBuilt = false; +} + +void UURLabRosPublishTransport::RebuildProviders(const FMjStateSnapshot& Snapshot) +{ + // Destroy the previous provider set (releasing its publishers) before building + // a fresh one for the current structure. + Providers.Reset(); + bProvidersBuilt = false; + + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (Ctx == nullptr) + { + return; + } + + FMjRosPublisherFactory Factory(Ctx, GetTypedOuter()); + FMjRosOutputRegistry::Get().InstantiateAll(Providers); + for (const TUniquePtr& Provider : Providers) + { + if (Provider) + { + Provider->Build(Factory, Snapshot); + } + } + + CachedStructureVersion = Snapshot.StructureVersion; + bProvidersBuilt = true; +} + +void UURLabRosPublishTransport::PublishState(const FMjStateSnapshot& Snapshot) +{ + if (!FURLabRosContext::Get().IsAvailable()) + { + return; + } + if (!bProvidersBuilt || Snapshot.StructureVersion != CachedStructureVersion) + { + RebuildProviders(Snapshot); + } + ++PublishStateCount; + + const int64 SimTimeNs = FillClock(Snapshot.Clock); + for (const TUniquePtr& Provider : Providers) + { + if (Provider) + { + Provider->Publish(Snapshot, SimTimeNs); + } + } +} + +#else // URLAB_WITH_ROS2 + +// Absent-ROS stubs so the class links; every real caller is fenced off too. +bool UURLabRosPublishTransport::TransportInit() { return false; } +void UURLabRosPublishTransport::TransportShutdown() {} +void UURLabRosPublishTransport::PublishState(const FMjStateSnapshot& /*Snapshot*/) {} +void UURLabRosPublishTransport::RebuildProviders(const FMjStateSnapshot& /*Snapshot*/) {} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/RosRpcTransport.cpp b/Source/URLabRos/Private/Transport/RosRpcTransport.cpp new file mode 100644 index 00000000..954ffbe1 --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosRpcTransport.cpp @@ -0,0 +1,766 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosRpcTransport.h" + +// The node name FURLabRosContext creates the process ROS node under; the control +// source id every ROS write is tagged with derives from it. ROS-agnostic, so it +// is defined outside the ROS fence. +FString UURLabRosRpcTransport::RosControlSourceId() +{ + return TEXT("ros:urlab"); +} + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 +#include "Transport/RosContext.h" +#include "Ros/UrlabRclCore.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/RpcDispatcher.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Input/MjTwistController.h" +#include "State/MjCanonicalName.h" +#include "State/MjStateTypes.h" +#include "Dom/JsonObject.h" +#include "HAL/RunnableThread.h" +#include "HAL/Runnable.h" +#include "HAL/PlatformProcess.h" +#include "URLabRosLog.h" + +// Poll cadence for the executor loop. The wait set blocks up to this long when +// subscriptions exist; the trailing sleep keeps the thread off a busy loop when +// no manager is live and there is nothing to spin. +namespace +{ +constexpr int64 RosSpinTimeoutNs = 50 * 1000 * 1000; // 50 ms +constexpr float RosIdleSleepSeconds = 0.02f; +} + +// Per-articulation command binding. Holds the identity used to resolve + gate the +// write (Art->GetName(), the GetArticulation + ownership key) and the per-art +// subscription + service handles. The stable heap address is handed to the core as +// the callback / service User pointer. +struct FRosArtCommand +{ + UURLabRosRpcTransport* Transport = nullptr; + FString ArtName; + UrlabRclCtrlSub* CtrlSub = nullptr; + UrlabRclTwistSub* TwistSub = nullptr; + UrlabRclJointStateSub* JointCommandSub = nullptr; + UrlabRclTriggerService* ClaimSrv = nullptr; + UrlabRclTriggerService* ReleaseSrv = nullptr; +}; + +// Per declared user-input-channel binding. Carries the routing identity +// (canonical art segment or None for scene, the channel name, its kind) and its +// Float64MultiArray subscription handle. +struct FRosUserInputSub +{ + UURLabRosRpcTransport* Transport = nullptr; + FName ArtOrNone; + FName Channel; + EMjUserChannelKind Kind = EMjUserChannelKind::Scalar; + UrlabRclCtrlSub* Sub = nullptr; +}; + +namespace +{ +void RosCtrlTrampoline(const double* Values, int32_t Count, void* User) +{ + FRosArtCommand* Cmd = static_cast(User); + if (Cmd && Cmd->Transport) + { + Cmd->Transport->HandleRosCtrl(Cmd->ArtName, Values, static_cast(Count)); + } +} + +void RosTwistTrampoline(const double Linear[3], const double Angular[3], void* User) +{ + FRosArtCommand* Cmd = static_cast(User); + if (Cmd && Cmd->Transport) + { + Cmd->Transport->HandleRosTwist(Cmd->ArtName, Linear, Angular); + } +} + +void RosJointCommandTrampoline(const char** Names, const double* Positions, + int32_t Count, void* User) +{ + FRosArtCommand* Cmd = static_cast(User); + if (Cmd && Cmd->Transport) + { + Cmd->Transport->HandleRosJointCommand(Cmd->ArtName, Names, Positions, + static_cast(Count)); + } +} + +void RosClaimTrampoline(void* User, int32_t* OutSuccess, char* OutMessage, int32_t Cap) +{ + FRosArtCommand* Cmd = static_cast(User); + if (Cmd && Cmd->Transport) + { + Cmd->Transport->HandleRosClaimRelease(Cmd->ArtName, /*bClaim=*/true, + OutSuccess, OutMessage, Cap); + } +} + +void RosReleaseTrampoline(void* User, int32_t* OutSuccess, char* OutMessage, int32_t Cap) +{ + FRosArtCommand* Cmd = static_cast(User); + if (Cmd && Cmd->Transport) + { + Cmd->Transport->HandleRosClaimRelease(Cmd->ArtName, /*bClaim=*/false, + OutSuccess, OutMessage, Cap); + } +} + +void RosUserInputTrampoline(const double* Values, int32_t Count, void* User) +{ + FRosUserInputSub* Sub = static_cast(User); + if (Sub && Sub->Transport) + { + Sub->Transport->HandleRosUserChannel(Sub->ArtOrNone, Sub->Channel, Sub->Kind, + Values, static_cast(Count)); + } +} +} // namespace + +class FRosExecutorRunnable : public FRunnable +{ +public: + explicit FRosExecutorRunnable(UURLabRosRpcTransport* InTransport) + : Transport(InTransport) {} + virtual uint32 Run() override + { + Transport->RunExecutorLoop(); + return 0; + } + virtual void Stop() override { Transport->bStop = true; } + +private: + UURLabRosRpcTransport* Transport; +}; + +bool UURLabRosRpcTransport::TransportInit() +{ + if (bIsInitialized) + { + return true; + } + if (!FURLabRosContext::Get().Initialize()) + { + UE_LOG(LogURLabRos, Warning, + TEXT("UURLabRosRpcTransport: ROS context unavailable; not binding.")); + return false; + } + + bStop = false; + bIsInitialized = true; + WorkerRunnable = new FRosExecutorRunnable(this); + WorkerThread = FRunnableThread::Create(WorkerRunnable, TEXT("URLabRosExecutor")); + + UE_LOG(LogURLabRos, Log, TEXT("UURLabRosRpcTransport initialised.")); + return true; +} + +void UURLabRosRpcTransport::TransportShutdown() +{ + if (!bIsInitialized) + { + return; + } + bStop = true; + if (WorkerThread) + { + WorkerThread->WaitForCompletion(); + delete WorkerThread; + WorkerThread = nullptr; + } + // FRunnableThread never owns the runnable; delete it so the bind/unbind cycle + // does not leak one runnable each time. + delete WorkerRunnable; + WorkerRunnable = nullptr; + // The executor loop tears its subscriptions down on exit; this guards the case + // where the thread never started. + TeardownCommandSubscriptions(); + bIsInitialized = false; +} + +void UURLabRosRpcTransport::RunExecutorLoop() +{ + while (!bStop.load(std::memory_order_acquire)) + { + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (Ctx) + { + SyncCommandSubscriptions(Ctx); + UrlabRcl_SpinSome(Ctx, RosSpinTimeoutNs); + } + FPlatformProcess::Sleep(RosIdleSleepSeconds); + } + // Destroy handles on the thread that created them, per the core contract. + TeardownCommandSubscriptions(); +} + +void UURLabRosRpcTransport::SyncCommandSubscriptions(UrlabRclContext* Ctx) +{ + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + if (bHaveSubscriptions) + { + TeardownCommandSubscriptions(); + } + return; + } + + const uint32 Version = Mgr->GetStateCollector().GetStructureVersion(); + if (bHaveSubscriptions && Mgr == SubscribedManager.Get() + && Version == SubscribedStructureVersion) + { + return; + } + RebuildCommandSubscriptions(Ctx); +} + +void UURLabRosRpcTransport::RebuildCommandSubscriptions(UrlabRclContext* Ctx) +{ + TeardownCommandSubscriptions(); + if (!Ctx) + { + return; + } + + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + return; + } + + for (AMjArticulation* Art : Mgr->GetAllArticulations()) + { + if (!Art) + { + continue; + } + // The topic uses the canonical, ROS-legal segment (matching the publish + // side); the raw actor name resolves the art and keys ownership. + const FString Segment = FMjCanonicalName::ArtSegment(Art).ToString(); + + FRosArtCommand* Cmd = new FRosArtCommand(); + Cmd->Transport = this; + Cmd->ArtName = Art->GetName(); + + const FString CtrlTopic = FString::Printf(TEXT("/%s/cmd_ctrl"), *Segment); + Cmd->CtrlSub = UrlabRcl_CreateCtrlSub(Ctx, TCHAR_TO_UTF8(*CtrlTopic), + &RosCtrlTrampoline, Cmd); + if (Cmd->CtrlSub == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: cmd_ctrl subscription failed for %s (%hs)"), + *CtrlTopic, UrlabRcl_LastError()); + } + + const FString VelTopic = FString::Printf(TEXT("/%s/cmd_vel"), *Segment); + Cmd->TwistSub = UrlabRcl_CreateTwistSub(Ctx, TCHAR_TO_UTF8(*VelTopic), + &RosTwistTrampoline, Cmd); + if (Cmd->TwistSub == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: cmd_vel subscription failed for %s (%hs)"), + *VelTopic, UrlabRcl_LastError()); + } + + // JointState jog input: standard joint_state_publisher_gui publishes here. + const FString JogTopic = FString::Printf(TEXT("/%s/joint_command"), *Segment); + Cmd->JointCommandSub = UrlabRcl_CreateJointStateSub(Ctx, TCHAR_TO_UTF8(*JogTopic), + &RosJointCommandTrampoline, Cmd); + if (Cmd->JointCommandSub == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: joint_command subscription failed for %s (%hs)"), + *JogTopic, UrlabRcl_LastError()); + } + + // claim_control / release_control as std_srvs/Trigger services; the art is + // encoded in the service name. + const FString ClaimName = FString::Printf(TEXT("/%s/claim_control"), *Segment); + Cmd->ClaimSrv = UrlabRcl_CreateTriggerService(Ctx, TCHAR_TO_UTF8(*ClaimName), + &RosClaimTrampoline, Cmd); + if (Cmd->ClaimSrv == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: claim_control service failed for %s (%hs)"), + *ClaimName, UrlabRcl_LastError()); + } + const FString ReleaseName = FString::Printf(TEXT("/%s/release_control"), *Segment); + Cmd->ReleaseSrv = UrlabRcl_CreateTriggerService(Ctx, TCHAR_TO_UTF8(*ReleaseName), + &RosReleaseTrampoline, Cmd); + if (Cmd->ReleaseSrv == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: release_control service failed for %s (%hs)"), + *ReleaseName, UrlabRcl_LastError()); + } + + if (Cmd->CtrlSub == nullptr && Cmd->TwistSub == nullptr + && Cmd->JointCommandSub == nullptr && Cmd->ClaimSrv == nullptr + && Cmd->ReleaseSrv == nullptr) + { + delete Cmd; + continue; + } + ArtCommands.Add(Cmd); + } + + // One subscription per declared user-input channel. Numeric kinds ride the + // Float64MultiArray (cmd_ctrl) sub shape; text-family channels take input over + // the byte transports (set_user_channels), not over ROS. + TArray InputChannels; + Mgr->GetUserInputChannels(InputChannels); + for (const FMjUserInputChannelInfo& Info : InputChannels) + { + if (Info.Kind == EMjUserChannelKind::String || Info.Kind == EMjUserChannelKind::Struct) + { + continue; // text-family channels have no ROS input subscription + } + const FString Topic = Info.ArtSegment.IsEmpty() + ? FString::Printf(TEXT("/urlab/user/%s"), *Info.Channel.ToString()) + : FString::Printf(TEXT("/%s/user/%s"), *Info.ArtSegment, *Info.Channel.ToString()); + + FRosUserInputSub* Binding = new FRosUserInputSub(); + Binding->Transport = this; + Binding->ArtOrNone = Info.ArtSegment.IsEmpty() ? NAME_None : FName(*Info.ArtSegment); + Binding->Channel = Info.Channel; + Binding->Kind = Info.Kind; + Binding->Sub = UrlabRcl_CreateCtrlSub(Ctx, TCHAR_TO_UTF8(*Topic), + &RosUserInputTrampoline, Binding); + if (Binding->Sub == nullptr) + { + UE_LOG(LogURLabRos, Warning, TEXT("ROS: user input subscription failed for %s (%hs)"), + *Topic, UrlabRcl_LastError()); + delete Binding; + continue; + } + UserInputSubs.Add(Binding); + } + + SubscribedManager = Mgr; + SubscribedStructureVersion = Mgr->GetStateCollector().GetStructureVersion(); + bHaveSubscriptions = true; +} + +void UURLabRosRpcTransport::TeardownCommandSubscriptions() +{ + for (FRosArtCommand* Cmd : ArtCommands) + { + if (!Cmd) + { + continue; + } + UrlabRcl_DestroyCtrlSub(Cmd->CtrlSub); + UrlabRcl_DestroyTwistSub(Cmd->TwistSub); + UrlabRcl_DestroyJointStateSub(Cmd->JointCommandSub); + UrlabRcl_DestroyTriggerService(Cmd->ClaimSrv); + UrlabRcl_DestroyTriggerService(Cmd->ReleaseSrv); + delete Cmd; + } + ArtCommands.Reset(); + + for (FRosUserInputSub* Binding : UserInputSubs) + { + if (!Binding) + { + continue; + } + UrlabRcl_DestroyCtrlSub(Binding->Sub); + delete Binding; + } + UserInputSubs.Reset(); + + SubscribedManager = nullptr; + SubscribedStructureVersion = 0; + bHaveSubscriptions = false; +} + +void UURLabRosRpcTransport::HandleRosCtrl(const FString& ArtName, const double* Values, int32 Count) +{ + ++RosCtrlCallbackCount; + + FURLabRpcDispatcher* Disp = ResolveDispatcher(); + if (!Disp) + { + return; + } + + // Ownership gate first: a ROS write to an art this source does not own is + // dropped. Ok also heartbeats the claim. + const FName ArtKey(*ArtName); + FString CurrentOwner; + if (Disp->GetControlOwnership().CheckWrite(ArtKey, RosControlSourceId(), CurrentOwner) + != FMjControlOwnership::EWriteCheck::Ok) + { + return; + } + + // ROS control is a Live-mode surface; direct / puppet bundle control into + // their step / push calls, so drop the write outside Live. + if (Disp->GetActiveStepMode() != EStepMode::Live) + { + return; + } + + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + return; + } + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + { + return; + } + + // Float64MultiArray values are in the art's actuator-list order; stage each on + // its actuator's NetworkValue, the same path ApplyStepCtrl writes to. + TArray Acts = Art->GetActuators(); + const int32 N = FMath::Min(Count, Acts.Num()); + for (int32 i = 0; i < N; ++i) + { + if (Acts[i]) + { + Acts[i]->SetNetworkControl(static_cast(Values[i])); + } + } +} + +void UURLabRosRpcTransport::HandleRosTwist(const FString& ArtName, const double Linear[3], + const double Angular[3]) +{ + ++RosTwistCallbackCount; + + FURLabRpcDispatcher* Disp = ResolveDispatcher(); + if (!Disp) + { + return; + } + + const FName ArtKey(*ArtName); + FString CurrentOwner; + if (Disp->GetControlOwnership().CheckWrite(ArtKey, RosControlSourceId(), CurrentOwner) + != FMjControlOwnership::EWriteCheck::Ok) + { + return; + } + + if (Disp->GetActiveStepMode() != EStepMode::Live) + { + return; + } + + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + return; + } + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art) + { + return; + } + UMjTwistController* TC = Art->FindComponentByClass(); + if (!TC) + { + return; + } + + // geometry_msgs/Twist maps as the set_twist RPC does: linear (vx, vy, _), + // angular (_, _, yaw_rate). + TC->SetTwist(static_cast(Linear[0]), static_cast(Linear[1]), + static_cast(Angular[2])); +} + +void UURLabRosRpcTransport::HandleRosJointCommand(const FString& ArtName, + const char** Names, const double* Positions, int32 Count) +{ + ++RosJointCommandCallbackCount; + + FURLabRpcDispatcher* Disp = ResolveDispatcher(); + if (!Disp) + { + return; + } + + // Same gating as cmd_ctrl: ownership first (also heartbeats the claim), then + // Live mode only. + const FName ArtKey(*ArtName); + FString CurrentOwner; + if (Disp->GetControlOwnership().CheckWrite(ArtKey, RosControlSourceId(), CurrentOwner) + != FMjControlOwnership::EWriteCheck::Ok) + { + return; + } + if (Disp->GetActiveStepMode() != EStepMode::Live) + { + return; + } + + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + return; + } + AMjArticulation* Art = Mgr->GetArticulation(ArtName); + if (!Art || !Names || !Positions) + { + return; + } + + // Resolve a commanded name to an actuator two ways: by the joint a + // joint-transmission actuator drives (the JointState / URDF joint name a jog + // GUI echoes back), and by the actuator's own name. The latter reaches + // actuators with no 1-DoF joint target -- e.g. a tendon-driven gripper + // actuator -- so a controller can command the gripper as "". + TMap ByName; + TArray Acts = Art->GetActuators(); + ByName.Reserve(Acts.Num() * 2); + for (UMjActuator* Act : Acts) + { + if (!Act) + { + continue; + } + if (Act->TransmissionType == EMjActuatorTrnType::Joint && !Act->TargetName.IsEmpty()) + { + ByName.Add(FMjCanonicalName::PartSegment(Art, Act->TargetName).ToString(), Act); + } + ByName.Add(FMjCanonicalName::PartSegment(Art, Act->GetMjName()).ToString(), Act); + } + + for (int32 i = 0; i < Count; ++i) + { + if (!Names[i]) + { + continue; + } + const FString JointName = UTF8_TO_TCHAR(Names[i]); + if (UMjActuator** Found = ByName.Find(JointName)) + { + if (*Found) + { + (*Found)->SetNetworkControl(static_cast(Positions[i])); + } + } + } +} + +void UURLabRosRpcTransport::HandleRosUserChannel(FName ArtOrNone, FName Channel, + EMjUserChannelKind Kind, const double* Values, int32 Count) +{ + ++RosUserChannelCallbackCount; + + UURLabBridgeServer* Bridge = GetOwningBridge(); + AAMjManager* Mgr = Bridge ? Bridge->GetActiveManager() : nullptr; + if (!Mgr) + { + return; + } + + // User-channel input is app-level data owned by user logic: no ownership gate + // and no Live-mode gate (unlike control writes, which fight the physics + // authority). The declaring component validates the value against its declared + // kind. + FMjUserChannel Value; + Value.Name = Channel; + Value.Kind = Kind; + if (Values && Count > 0) + { + Value.Values.Append(Values, Count); + } + Mgr->ApplyUserChannelInput(ArtOrNone, Channel, Value); +} + +void UURLabRosRpcTransport::HandleRosClaimRelease(const FString& ArtName, bool bClaim, + int32* OutSuccess, char* OutMessage, int32 OutMessageCap) +{ + auto WriteMessage = [OutMessage, OutMessageCap](const FString& Msg) { + if (OutMessage && OutMessageCap > 0) + { + FCStringAnsi::Strncpy(OutMessage, TCHAR_TO_UTF8(*Msg), OutMessageCap); + } + }; + if (OutSuccess) + { + *OutSuccess = 0; + } + + FURLabRpcDispatcher* Disp = ResolveDispatcher(); + if (!Disp) + { + WriteMessage(TEXT("no active dispatcher")); + return; + } + + // Build the request the ZMQ/SHM path builds: source preset to the ROS node id, + // session preset to the active session so Dispatch's session gate passes. TTL is + // not settable over the Trigger service, so the default TTL applies. + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), bClaim ? TEXT("claim_control") : TEXT("release_control")); + Req->SetStringField(TEXT("articulation"), ArtName); + Req->SetStringField(TEXT("source"), RosControlSourceId()); + Req->SetStringField(TEXT("session_id"), Disp->GetActiveSessionId()); + + const TSharedPtr Reply = Disp->Dispatch(Req); + FString ReplyOp; + if (Reply.IsValid()) + { + Reply->TryGetStringField(TEXT("op"), ReplyOp); + } + + const bool bOk = ReplyOp.Equals(bClaim ? TEXT("claim_control_ok") : TEXT("release_control_ok")); + if (OutSuccess) + { + *OutSuccess = bOk ? 1 : 0; + } + if (bOk) + { + FString Owner; + if (Reply.IsValid()) + { + Reply->TryGetStringField(TEXT("owner"), Owner); + } + WriteMessage(bClaim + ? FString::Printf(TEXT("%s claimed by %s"), *ArtName, + Owner.IsEmpty() ? *RosControlSourceId() : *Owner) + : FString::Printf(TEXT("%s released"), *ArtName)); + } + else + { + FString Code, Message; + if (Reply.IsValid()) + { + Reply->TryGetStringField(TEXT("code"), Code); + Reply->TryGetStringField(TEXT("message"), Message); + } + WriteMessage(Message.IsEmpty() ? Code : Message); + } +} + +void UURLabRosRpcTransport::ApplyRosCtrlForTest(const FString& ArtName, + const TArray& Values) +{ + HandleRosCtrl(ArtName, Values.GetData(), Values.Num()); +} + +void UURLabRosRpcTransport::ApplyRosJointCommandForTest(const FString& ArtName, + const TArray& Names, const TArray& Positions) +{ + // Build the stable UTF-8 pointer array the wire callback would hand in. + TArray> NameBytes; + NameBytes.Reserve(Names.Num()); + TArray NamePtrs; + NamePtrs.Reserve(Names.Num()); + for (const FString& Name : Names) + { + FTCHARToUTF8 Conv(*Name); + TArray& Bytes = NameBytes.AddDefaulted_GetRef(); + Bytes.Append(reinterpret_cast(Conv.Get()), Conv.Length()); + Bytes.Add('\0'); + NamePtrs.Add(Bytes.GetData()); + } + const int32 N = FMath::Min(Names.Num(), Positions.Num()); + HandleRosJointCommand(ArtName, N > 0 ? NamePtrs.GetData() : nullptr, + N > 0 ? Positions.GetData() : nullptr, N); +} + +bool UURLabRosRpcTransport::ApplyRosClaimReleaseForTest(const FString& ArtName, bool bClaim) +{ + int32 Success = 0; + char Message[256] = {0}; + HandleRosClaimRelease(ArtName, bClaim, &Success, Message, sizeof(Message)); + return Success != 0; +} + +bool UURLabRosRpcTransport::PublishAndPumpCtrlForTest(const FString& Topic, + const TArray& Values) +{ + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return false; + } + + // Build the subscriptions on THIS thread; the executor thread must not be + // running so no rcl handle is touched from two threads. + RebuildCommandSubscriptions(Ctx); + + UrlabRclCtrlPub* Pub = UrlabRcl_CreateCtrlPub(Ctx, TCHAR_TO_UTF8(*Topic)); + if (Pub == nullptr) + { + TeardownCommandSubscriptions(); + return false; + } + + const int64 Before = RosCtrlCallbackCount.load(); + bool bFired = false; + // Re-publish each round so a message sent before intra-process discovery + // completes is not the only one; bounded so a wire failure still returns. + for (int32 Attempt = 0; Attempt < 200 && !bFired; ++Attempt) + { + UrlabRcl_PublishCtrl(Pub, Values.GetData(), Values.Num()); + UrlabRcl_SpinSome(Ctx, 20 * 1000 * 1000); // 20 ms + bFired = RosCtrlCallbackCount.load() > Before; + if (!bFired) + { + FPlatformProcess::Sleep(0.01f); + } + } + + UrlabRcl_DestroyCtrlPub(Pub); + TeardownCommandSubscriptions(); + return bFired; +} + +#else // URLAB_WITH_ROS2 + +// Absent-ROS stubs so the class links; every real caller is fenced off too. +bool UURLabRosRpcTransport::TransportInit() { return false; } +void UURLabRosRpcTransport::TransportShutdown() {} +void UURLabRosRpcTransport::RunExecutorLoop() {} +void UURLabRosRpcTransport::SyncCommandSubscriptions(UrlabRclContext*) {} +void UURLabRosRpcTransport::RebuildCommandSubscriptions(UrlabRclContext*) {} +void UURLabRosRpcTransport::TeardownCommandSubscriptions() {} +void UURLabRosRpcTransport::HandleRosCtrl(const FString&, const double*, int32) {} +void UURLabRosRpcTransport::HandleRosTwist(const FString&, const double[3], const double[3]) {} +void UURLabRosRpcTransport::HandleRosJointCommand(const FString&, const char**, const double*, int32) {} +void UURLabRosRpcTransport::HandleRosUserChannel(FName, FName, EMjUserChannelKind, const double*, int32) {} +void UURLabRosRpcTransport::HandleRosClaimRelease(const FString&, bool, int32*, char*, int32) {} +void UURLabRosRpcTransport::ApplyRosCtrlForTest(const FString&, const TArray&) {} +void UURLabRosRpcTransport::ApplyRosJointCommandForTest(const FString&, const TArray&, const TArray&) {} +bool UURLabRosRpcTransport::ApplyRosClaimReleaseForTest(const FString&, bool) { return false; } +bool UURLabRosRpcTransport::PublishAndPumpCtrlForTest(const FString&, const TArray&) +{ + return false; +} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabRos/Private/Transport/RosSensorRouting.cpp b/Source/URLabRos/Private/Transport/RosSensorRouting.cpp new file mode 100644 index 00000000..65e722c0 --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosSensorRouting.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosSensorRouting.h" + +ERosSensorRoute RouteForSemantic(EMjSensorSemantic Semantic) +{ + // Total switch, no default: a new EMjSensorSemantic value breaks the build here + // until it is routed, keeping ROS sensor coverage total by construction. + switch (Semantic) + { + case EMjSensorSemantic::Gyro: + case EMjSensorSemantic::Accel: + return ERosSensorRoute::Imu; + + case EMjSensorSemantic::Force: + case EMjSensorSemantic::Torque: + return ERosSensorRoute::Wrench; + + case EMjSensorSemantic::Rangefinder: + return ERosSensorRoute::Range; + + case EMjSensorSemantic::Magnetometer: + return ERosSensorRoute::MagneticField; + + case EMjSensorSemantic::Velocity: + return ERosSensorRoute::Twist; + + case EMjSensorSemantic::Generic: + case EMjSensorSemantic::Touch: + case EMjSensorSemantic::JointPos: + case EMjSensorSemantic::JointVel: + case EMjSensorSemantic::ActuatorPos: + case EMjSensorSemantic::ActuatorVel: + case EMjSensorSemantic::ActuatorFrc: + case EMjSensorSemantic::FramePos: + case EMjSensorSemantic::FrameQuat: + case EMjSensorSemantic::FrameAxis: + case EMjSensorSemantic::FrameLinVel: + case EMjSensorSemantic::FrameAngVel: + case EMjSensorSemantic::FrameLinAcc: + case EMjSensorSemantic::FrameAngAcc: + case EMjSensorSemantic::SubtreeCom: + case EMjSensorSemantic::SubtreeLinVel: + case EMjSensorSemantic::SubtreeAngMom: + case EMjSensorSemantic::Clock: + return ERosSensorRoute::MultiArray; + } + + // Unreachable: the switch above is total. Present only so a corrupt cast does + // not fall through to undefined behaviour. + return ERosSensorRoute::MultiArray; +} + +namespace MjRosSensorRouting +{ +FString TopicFor(const FString& ArtSegment, const FString& SensorName, ERosSensorRoute Route) +{ + switch (Route) + { + case ERosSensorRoute::Imu: + return FString::Printf(TEXT("/%s/imu"), *ArtSegment); + case ERosSensorRoute::Wrench: + return FString::Printf(TEXT("/%s/%s/wrench"), *ArtSegment, *SensorName); + case ERosSensorRoute::Range: + return FString::Printf(TEXT("/%s/%s/range"), *ArtSegment, *SensorName); + case ERosSensorRoute::MagneticField: + return FString::Printf(TEXT("/%s/%s/magnetic_field"), *ArtSegment, *SensorName); + case ERosSensorRoute::Twist: + return FString::Printf(TEXT("/%s/%s/velocity"), *ArtSegment, *SensorName); + case ERosSensorRoute::MultiArray: + default: + return FString::Printf(TEXT("/%s/sensors/%s"), *ArtSegment, *SensorName); + } +} + +void GatherWrenchPairs(const FMjArticulationState& Art, TArray& OutPairs) +{ + OutPairs.Reset(); + + TArray Forces; + TArray Torques; + for (const FMjSensorState& Sensor : Art.Sensors) + { + if (RouteForSemantic(Sensor.Semantic) != ERosSensorRoute::Wrench) + { + continue; + } + if (Sensor.Semantic == EMjSensorSemantic::Force) + { + Forces.Add(Sensor.Name); + } + else if (Sensor.Semantic == EMjSensorSemantic::Torque) + { + Torques.Add(Sensor.Name); + } + } + + const int32 N = FMath::Max(Forces.Num(), Torques.Num()); + OutPairs.Reserve(N); + for (int32 i = 0; i < N; ++i) + { + FMjWrenchPair Pair; + Pair.ForceSensor = Forces.IsValidIndex(i) ? Forces[i] : NAME_None; + Pair.TorqueSensor = Torques.IsValidIndex(i) ? Torques[i] : NAME_None; + Pair.TopicName = Pair.ForceSensor.IsNone() ? Pair.TorqueSensor : Pair.ForceSensor; + OutPairs.Add(Pair); + } +} +} // namespace MjRosSensorRouting diff --git a/Source/URLabRos/Private/Transport/RosStateEstimation.cpp b/Source/URLabRos/Private/Transport/RosStateEstimation.cpp new file mode 100644 index 00000000..efb80f74 --- /dev/null +++ b/Source/URLabRos/Private/Transport/RosStateEstimation.cpp @@ -0,0 +1,129 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "Transport/RosStateEstimation.h" +#include "State/MjStateTypes.h" + +namespace MjRosStateEstimation +{ +namespace +{ +// Rotate V by the quaternion (w, x, y, z) using the standard vector form +// v' = v + 2w(u x v) + 2u x (u x v), u = (x, y, z). +void RotateByQuat(double w, double x, double y, double z, const double V[3], double O[3]) +{ + const double tx = 2.0 * (y * V[2] - z * V[1]); + const double ty = 2.0 * (z * V[0] - x * V[2]); + const double tz = 2.0 * (x * V[1] - y * V[0]); + O[0] = V[0] + w * tx + (y * tz - z * ty); + O[1] = V[1] + w * ty + (z * tx - x * tz); + O[2] = V[2] + w * tz + (x * ty - y * tx); +} +} // namespace + +void RotateWorldToBody(const double QuatWxyz[4], const double VWorld[3], double OutVBody[3]) +{ + // world->body is rotation by the conjugate (negate the vector part). + RotateByQuat(QuatWxyz[0], -QuatWxyz[1], -QuatWxyz[2], -QuatWxyz[3], VWorld, OutVBody); +} + +bool ComputeFreeBaseState(const FMjArticulationState& Art, FMjFreeBaseState& Out) +{ + Out = FMjFreeBaseState(); + + const FMjJointState* Free = nullptr; + for (const FMjJointState& Joint : Art.Joints) + { + if (Joint.Type == EMjJointType::Free && Joint.QPos.Num() >= 7 && Joint.QVel.Num() >= 6) + { + Free = &Joint; + break; + } + } + if (!Free) + { + return false; + } + + // Pose straight from the free-joint qpos: [0..2] world position, [3..6] world + // orientation wxyz. ROS carries xyzw, so reorder the quaternion. + for (int32 i = 0; i < 3; ++i) + { + Out.Position[i] = Free->QPos[i]; + } + const double QuatWxyz[4] = { Free->QPos[3], Free->QPos[4], Free->QPos[5], Free->QPos[6] }; + Out.OrientationXyzw[0] = QuatWxyz[1]; + Out.OrientationXyzw[1] = QuatWxyz[2]; + Out.OrientationXyzw[2] = QuatWxyz[3]; + Out.OrientationXyzw[3] = QuatWxyz[0]; + + // Twist: qvel[0..2] is WORLD linear (rotate into the base frame); qvel[3..5] is + // already BODY angular (pass through). + const double LinearWorld[3] = { Free->QVel[0], Free->QVel[1], Free->QVel[2] }; + RotateWorldToBody(QuatWxyz, LinearWorld, Out.LinearBody); + Out.AngularBody[0] = Free->QVel[3]; + Out.AngularBody[1] = Free->QVel[4]; + Out.AngularBody[2] = Free->QVel[5]; + + // The base body is the free-jointed root, whose world position equals the free + // joint qpos exactly (same mjData source). Match on that. + int32 BestIndex = INDEX_NONE; + double BestSq = 1.0e-12; // tight: an exact copy, not a nearest-neighbour search + for (int32 i = 0; i < Art.Bodies.Num(); ++i) + { + const FMjBodyState& Body = Art.Bodies[i]; + const double dx = Body.Xpos[0] - Out.Position[0]; + const double dy = Body.Xpos[1] - Out.Position[1]; + const double dz = Body.Xpos[2] - Out.Position[2]; + const double Sq = dx * dx + dy * dy + dz * dz; + if (Sq <= BestSq) + { + BestSq = Sq; + BestIndex = i; + } + } + Out.BaseBodyIndex = BestIndex; + + Out.bValid = true; + return true; +} + +void PinholeKFromFovy(double FovyDegrees, int32 Width, int32 Height, double OutK9[9]) +{ + for (int32 i = 0; i < 9; ++i) + { + OutK9[i] = 0.0; + } + const double W = Width > 0 ? static_cast(Width) : 1.0; + const double H = Height > 0 ? static_cast(Height) : 1.0; + // MuJoCo's default camera fovy is 45 degrees; an unset / non-positive fovy + // would otherwise collapse the focal length to infinity. + const double FovyRad = FMath::DegreesToRadians(FovyDegrees > 0.0 ? FovyDegrees : 45.0); + const double Fy = (H * 0.5) / FMath::Tan(FovyRad * 0.5); + const double Fx = Fy; // square pixels; horizontal FOV emerges from the width + OutK9[0] = Fx; + OutK9[2] = W * 0.5; // cx + OutK9[4] = Fy; + OutK9[5] = H * 0.5; // cy + OutK9[8] = 1.0; +} +} // namespace MjRosStateEstimation diff --git a/Source/URLabRos/Private/URLabRos.cpp b/Source/URLabRos/Private/URLabRos.cpp new file mode 100644 index 00000000..5342ff41 --- /dev/null +++ b/Source/URLabRos/Private/URLabRos.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "URLabRos.h" + +#include "Transport/MjExternalTransportProvider.h" +#include "Transport/RpcTransport.h" +#include "Transport/PublishTransport.h" +#include "Transport/RosRpcTransport.h" +#include "Transport/RosPublishTransport.h" +#include "Bridge/BridgeServer.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Components/Sensors/MjCameraFrameBus.h" +#include "URLabRosLog.h" + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 +#include "Transport/RosContext.h" +#include "Ros/UrlabRclCore.h" +#endif + +DEFINE_LOG_CATEGORY(LogURLabRos); + +// Opaque rcl image publisher handle. Forward-declared unconditionally so the +// per-camera pointer map compiles in an ROS-off build too; the handle is only +// created / used inside the URLAB_WITH_ROS2 fence. +struct UrlabRclImagePub; + +namespace +{ +/** Factory for the ROS control RPC transport, installed as the core's external + * control-RPC hook. Creates the transport with the bridge as outer and wires + * ownership; the core calls TransportInit and stores the result. */ +UURLabRpcTransport* MakeRosControlRpcTransport(UURLabBridgeServer* Bridge) +{ + UURLabRosRpcTransport* Ros = NewObject(Bridge, NAME_None); + Ros->SetOwningBridge(Bridge); + return Ros; +} + +/** Factory for the ROS state publish transport, installed as the core's external + * state-publish hook. Creates the transport with the manager as outer; the + * transport registers itself as an IMjStateConsumer in TransportInit. */ +UURLabPublishTransport* MakeRosStatePublishTransport(AAMjManager* Manager) +{ + return NewObject(Manager, NAME_None); +} + +/** + * @class FRosCameraImageSink + * @brief Publishes camera frames as ROS `sensor_msgs/Image`, subscribing to the + * core's transport-neutral FMjCameraFrameBus. + * + * One rcl Image publisher per camera, keyed by canonical name, created lazily on + * the first frame (topic `///image`) and released when the camera + * stops streaming. Runs on the game thread (the bus fires there), matching the + * former in-camera image sink. In an ROS-off build the handlers are no-ops. + */ +class FRosCameraImageSink +{ +public: + void Install() + { + FrameHandle = FMjCameraFrameBus::Get().OnFrameReady.AddRaw( + this, &FRosCameraImageSink::OnFrameReady); + StopHandle = FMjCameraFrameBus::Get().OnStreamStopped.AddRaw( + this, &FRosCameraImageSink::OnStreamStopped); + } + + void Uninstall() + { + FMjCameraFrameBus::Get().OnFrameReady.Remove(FrameHandle); + FMjCameraFrameBus::Get().OnStreamStopped.Remove(StopHandle); + FrameHandle.Reset(); + StopHandle.Reset(); +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + for (TPair& Pair : Pubs) + { + UrlabRcl_DestroyImagePub(Pair.Value); + } +#endif + Pubs.Reset(); + } + +private: + void OnFrameReady(const FMjCameraFramePayload& Frame) + { +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + if (!FURLabRosContext::Get().IsAvailable() || Frame.Data == nullptr) + { + return; + } + UrlabRclImagePub* Pub = FindOrCreatePub(Frame); + if (!Pub) + { + return; + } + const int64 SimTimeNs = static_cast(Frame.SimTime * 1.0e9); + UrlabRcl_PublishImage(Pub, Frame.Data, Frame.RowStrideBytes, SimTimeNs); +#endif + } + + void OnStreamStopped(const FString& CanonicalName) + { +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + if (UrlabRclImagePub** Found = Pubs.Find(CanonicalName)) + { + UrlabRcl_DestroyImagePub(*Found); + Pubs.Remove(CanonicalName); + } +#endif + } + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + UrlabRclImagePub* FindOrCreatePub(const FMjCameraFramePayload& Frame) + { + if (UrlabRclImagePub** Found = Pubs.Find(Frame.CanonicalName)) + { + return *Found; + } + UrlabRclContext* Ctx = FURLabRosContext::Get().GetHandle(); + if (!Ctx) + { + return nullptr; + } + const FString Topic = FString::Printf(TEXT("/%s/image"), *Frame.CanonicalName); + // BGRA8 is the native Unreal FColor format. Downstream OpenCV consumers + // that need RGB8 should convert with cv::cvtColor(img, img, cv::COLOR_BGRA2RGBA). + // Depth frames are single-channel float32. + const char* Encoding = Frame.bDepth ? "32FC1" : "bgra8"; + UrlabRclImagePub* Pub = UrlabRcl_CreateImagePub(Ctx, TCHAR_TO_UTF8(*Topic), + TCHAR_TO_UTF8(*Frame.CanonicalName), Frame.Width, Frame.Height, Encoding); + if (!Pub) + { + UE_LOG(LogURLabRos, Warning, + TEXT("[URLabRos] '%s' ROS image publisher create failed (%hs)"), + *Frame.CanonicalName, UrlabRcl_LastError()); + return nullptr; + } + UE_LOG(LogURLabRos, Log, TEXT("[URLabRos] '%s' ROS image broadcast at %s"), + *Frame.CanonicalName, *Topic); + Pubs.Add(Frame.CanonicalName, Pub); + return Pub; + } +#endif + + TMap Pubs; + FDelegateHandle FrameHandle; + FDelegateHandle StopHandle; +}; + +FRosCameraImageSink GCameraImageSink; +} // namespace + +void FURLabRosModule::StartupModule() +{ + FMjExternalTransportProvider::MakeControlRpcTransport.BindStatic(&MakeRosControlRpcTransport); + FMjExternalTransportProvider::MakeStatePublishTransport.BindStatic(&MakeRosStatePublishTransport); + GCameraImageSink.Install(); +} + +void FURLabRosModule::ShutdownModule() +{ + GCameraImageSink.Uninstall(); + FMjExternalTransportProvider::MakeControlRpcTransport.Unbind(); + FMjExternalTransportProvider::MakeStatePublishTransport.Unbind(); +} + +IMPLEMENT_MODULE(FURLabRosModule, URLabRos) diff --git a/Source/URLabRos/Private/URLabRosLog.h b/Source/URLabRos/Private/URLabRosLog.h new file mode 100644 index 00000000..1ede3013 --- /dev/null +++ b/Source/URLabRos/Private/URLabRosLog.h @@ -0,0 +1,30 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +// URLabRos-owned log category. The core URLab log categories are not exported +// across the module boundary, and an optional module logging under its own +// category is idiomatic; every ROS message in this module uses LogURLabRos. +DECLARE_LOG_CATEGORY_EXTERN(LogURLabRos, Log, All); diff --git a/Source/URLabRos/Public/Transport/RosContext.h b/Source/URLabRos/Public/Transport/RosContext.h new file mode 100644 index 00000000..2066e31e --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosContext.h @@ -0,0 +1,85 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +// Opaque handle from the UE-agnostic rcl seam; its definition lives only in +// UrlabRclCore.cpp. A Public header must not include the Private UrlabRclCore.h, +// so it forward-declares the handle and holds a pointer. +struct UrlabRclContext; + +/** + * @class FURLabRosContext + * @brief Process-wide owner of the single rcl context + node the plugin + * publishes and subscribes through. + * + * ROS 2 has one context and, here, one node per process; every publisher and + * subscription the plugin creates binds to them. This class owns their + * lifetime and hands the underlying core handle to the transports that fill and + * publish messages. + * + * Absent-ROS boot: `IsAvailable()` is the single gate every ROS caller checks. + * When ROS is not linked (`URLAB_WITH_ROS2=0`) the whole class compiles out with + * its callers; when it is linked but the runtime environment cannot bring up a + * context (no DDS, misconfigured domain), `Initialize()` fails gracefully and + * `IsAvailable()` stays false, so ROS work degrades to a no-op instead of + * crashing the editor. + * + * Threading: `Get()`/`Initialize()`/`Shutdown()` are serialized by an internal + * lock. The returned handle is created and destroyed here; per-handle rcl calls + * (publish, spin) are serialized by their owning transport, per the core's + * threading contract. + */ +class URLABROS_API FURLabRosContext +{ +public: + /** The process-wide instance. */ + static FURLabRosContext& Get(); + + /** Bring up the context + node if not already up. Idempotent: repeated calls + * are no-ops once a context exists, and a failed first attempt is not + * retried. Returns IsAvailable(). */ + bool Initialize(); + + /** True when a usable rcl context + node is live. All ROS work is gated on + * this. */ + bool IsAvailable() const; + + /** Fini the node then the context, in reverse creation order. Idempotent. */ + void Shutdown(); + + /** The core context handle, or null when unavailable. Only translation units + * that include UrlabRclCore.h use it. */ + UrlabRclContext* GetHandle() const { return Context; } + +private: + FURLabRosContext() = default; + ~FURLabRosContext(); + FURLabRosContext(const FURLabRosContext&) = delete; + FURLabRosContext& operator=(const FURLabRosContext&) = delete; + + UrlabRclContext* Context = nullptr; + bool bInitAttempted = false; + mutable FCriticalSection Mutex; +}; diff --git a/Source/URLabRos/Public/Transport/RosOutputProvider.h b/Source/URLabRos/Public/Transport/RosOutputProvider.h new file mode 100644 index 00000000..7a37a7d4 --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosOutputProvider.h @@ -0,0 +1,250 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +struct FMjStateSnapshot; +struct UrlabRclContext; +class AAMjManager; + +/** + * @class FMjRosPub + * @brief An owning handle to one rcl publisher of a fixed message family. + * + * Created only by FMjRosPublisherFactory, which hides the C-ABI seam entirely, so + * a provider (built-in or user, in any module) never touches rcl. Move-only; the + * destructor releases the underlying publisher, so a provider that holds FMjRosPub + * members cannot leak one. Each handle carries a message-family tag: the Publish* + * method matching the create call fills and sends, and any mismatched Publish* is + * a safe no-op. Every method is a no-op on an invalid handle and when ROS is not + * linked, so provider code is written once and runs in every configuration. + */ +class URLABROS_API FMjRosPub +{ +public: + FMjRosPub() = default; + ~FMjRosPub() { Reset(); } + + FMjRosPub(FMjRosPub&& Other) noexcept + : Handle(Other.Handle), Kind(Other.Kind) + { + Other.Handle = nullptr; + Other.Kind = EKind::None; + } + FMjRosPub& operator=(FMjRosPub&& Other) noexcept + { + if (this != &Other) + { + Reset(); + Handle = Other.Handle; + Kind = Other.Kind; + Other.Handle = nullptr; + Other.Kind = EKind::None; + } + return *this; + } + FMjRosPub(const FMjRosPub&) = delete; + FMjRosPub& operator=(const FMjRosPub&) = delete; + + bool IsValid() const { return Handle != nullptr; } + + /** Release the underlying rcl publisher, if any. */ + void Reset(); + + // Typed publish operations. Exactly one matches this handle's family; the rest + // are no-ops. SimTimeNs is sim time in nanoseconds. + void PublishJointState(const double* Positions, const double* Velocities, + const double* Efforts, int32 Count, int64 SimTimeNs); + void PublishImu(const double* AngularVel3, const double* LinearAccel3, + const double* OrientationXyzw4, int64 SimTimeNs); + void PublishTf(const TArray& Parents, const TArray& Children, + const TArray& TranslationsXyz, const TArray& RotationsXyzw, + int64 SimTimeNs); + void PublishTwistStamped(const double Linear3[3], const double Angular3[3], int64 SimTimeNs); + void PublishClock(int64 SimTimeNs); + void PublishString(const FString& Text); + void PublishWrench(const double Force3[3], const double Torque3[3], int64 SimTimeNs); + void PublishRange(double Range, int64 SimTimeNs); + void PublishMagneticField(const double Field3[3], int64 SimTimeNs); + void PublishFloat64MultiArray(const double* Values, int32 Count); + void PublishOdometry(const double Position3[3], const double OrientationXyzw4[4], + const double LinearBody3[3], const double AngularBody3[3], int64 SimTimeNs); + void PublishPoseWithCovariance(const double Position3[3], + const double OrientationXyzw4[4], int64 SimTimeNs); + void PublishCameraInfo(int64 SimTimeNs); + void PublishBool(bool bValue); + void PublishFloat64(double Value); + void PublishVector3(const double Xyz3[3]); + void PublishPoseStamped(const double Position3[3], const double OrientationXyzw4[4], + int64 SimTimeNs); + +private: + enum class EKind : uint8 + { + None, JointState, Imu, Tf, TwistStamped, Clock, String, + Wrench, Range, MagneticField, MultiArray, + Odometry, PoseWithCovariance, CameraInfo, + Bool, Float64, Vector3, PoseStamped + }; + + FMjRosPub(void* InHandle, EKind InKind) : Handle(InHandle), Kind(InKind) {} + + void* Handle = nullptr; + EKind Kind = EKind::None; + + friend class FMjRosPublisherFactory; +}; + +/** + * @class FMjRosPublisherFactory + * @brief The single place providers turn a topic into an owning FMjRosPub. + * + * One is built per publisher rebuild by the transport and handed to every + * provider's Build. Each Create* wraps one UrlabRclCore triple; providers hold the + * returned handles and publish through them, never seeing rcl. Also exposes the + * owning manager for providers that read model-structure state (e.g. the exported + * URDF for //robot_description). + */ +class URLABROS_API FMjRosPublisherFactory +{ +public: + FMjRosPublisherFactory(UrlabRclContext* InContext, const AAMjManager* InManager) + : Context(InContext), Manager(InManager) + { + } + + bool IsValid() const { return Context != nullptr; } + const AAMjManager* GetManager() const { return Manager; } + + FMjRosPub CreateJointState(const FString& Topic, const TArray& JointNames); + FMjRosPub CreateImu(const FString& Topic, const FString& FrameId); + FMjRosPub CreateTf(bool bStatic); + FMjRosPub CreateTwistStamped(const FString& Topic, const FString& FrameId); + FMjRosPub CreateClock(); + FMjRosPub CreateString(const FString& Topic); + FMjRosPub CreateWrench(const FString& Topic, const FString& FrameId); + FMjRosPub CreateRange(const FString& Topic, const FString& FrameId, + uint8 RadiationType, float FieldOfView, float MinRange, float MaxRange); + FMjRosPub CreateMagneticField(const FString& Topic, const FString& FrameId); + FMjRosPub CreateFloat64MultiArray(const FString& Topic); + FMjRosPub CreateOdometry(const FString& Topic, const FString& FrameId, + const FString& ChildFrameId); + FMjRosPub CreatePoseWithCovariance(const FString& Topic, const FString& FrameId); + FMjRosPub CreateCameraInfo(const FString& Topic, const FString& FrameId, + int32 Width, int32 Height, const double K9[9]); + FMjRosPub CreateBool(const FString& Topic); + FMjRosPub CreateFloat64(const FString& Topic); + FMjRosPub CreateVector3(const FString& Topic); + FMjRosPub CreatePoseStamped(const FString& Topic, const FString& FrameId); + +private: + UrlabRclContext* Context = nullptr; + const AAMjManager* Manager = nullptr; +}; + +/** + * @class IMjRosOutputProvider + * @brief One ROS output (a message type or a per-articulation family of them), + * self-registered into FMjRosOutputRegistry. + * + * The built-in outputs (JointState, Imu, tf2, TwistStamped, Clock, sensor + * routing, robot_description) are each just an instance of this interface, + * registered through the same registry a user's out-of-plugin provider uses. + * Adding an output is dropping a self-registering file; there is no central switch + * to edit. The transport instantiates one provider per registered entry on a + * structure change, calls Build to create publishers for the current model, and + * calls Publish each step. Destroying the provider releases its publishers. + * + * Build and Publish run on the physics thread; Publish must be fast and + * non-blocking. + */ +class URLABROS_API IMjRosOutputProvider +{ +public: + virtual ~IMjRosOutputProvider() = default; + + /** A stable, unique name (also the key used to register). */ + virtual FName GetProviderName() const = 0; + + /** Create publishers for the current model shape. Called on every structure + * change (a fresh provider instance each time). */ + virtual void Build(FMjRosPublisherFactory& Factory, const FMjStateSnapshot& Snapshot) = 0; + + /** Fill and publish this output from the per-step snapshot. */ + virtual void Publish(const FMjStateSnapshot& Snapshot, int64 SimTimeNs) = 0; + + /** Number of publishers this provider currently owns; a test seam, default 0. */ + virtual int32 GetPublisherCountForTest() const { return 0; } +}; + +/** Factory function that produces a fresh provider instance. Each transport owns + * its own provider set, so providers are instantiated per transport, not shared. */ +using FMjRosOutputProviderFactoryFn = TFunction()>; + +/** + * @class FMjRosOutputRegistry + * @brief The process-wide table of registered output providers. + * + * Populated by self-registering statics at module load (see + * REGISTER_MJ_ROS_OUTPUT_PROVIDER), independent of whether ROS is linked. The + * transport asks it to instantiate the full set on each publisher rebuild. + */ +class URLABROS_API FMjRosOutputRegistry +{ +public: + static FMjRosOutputRegistry& Get(); + + /** Register a named provider factory. A later duplicate name replaces the + * earlier entry (with a warning), so a user can override a built-in by name. */ + void Register(FName Name, FMjRosOutputProviderFactoryFn Factory); + + /** Instantiate one provider per registered entry, in registration order. */ + void InstantiateAll(TArray>& Out) const; + + /** The registered names, in registration order. Test / introspection seam. */ + TArray GetRegisteredNames() const; + + int32 Num() const { return Entries.Num(); } + +private: + TArray> Entries; +}; + +/** Self-registration helper: a file-scope instance registers its factory at + * module load. Use REGISTER_MJ_ROS_OUTPUT_PROVIDER rather than constructing + * directly. */ +struct URLABROS_API FMjRosOutputProviderRegistrar +{ + FMjRosOutputProviderRegistrar(FName Name, FMjRosOutputProviderFactoryFn Factory); +}; + +/** + * Register a provider type under a topic-family name. Drop this at file scope in a + * provider .cpp; no other file needs to change. Example: + * REGISTER_MJ_ROS_OUTPUT_PROVIDER("joint_state", FMjRosJointStateProvider); + */ +#define REGISTER_MJ_ROS_OUTPUT_PROVIDER(NameLiteral, Type) \ + static const FMjRosOutputProviderRegistrar GMjRosProviderRegistrar_##Type( \ + FName(TEXT(NameLiteral)), \ + []() -> TUniquePtr { return MakeUnique(); }) diff --git a/Source/URLabRos/Public/Transport/RosPublishTransport.h b/Source/URLabRos/Public/Transport/RosPublishTransport.h new file mode 100644 index 00000000..c68efc95 --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosPublishTransport.h @@ -0,0 +1,155 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "Transport/PublishTransport.h" +#include "Transport/RosOutputProvider.h" +#include "State/MjStateConsumer.h" +#include "RosPublishTransport.generated.h" + +struct FMjStateSnapshot; +struct FMjArticulationState; +struct FMjClock; + +/** + * @class UURLabRosPublishTransport + * @brief Publishes the per-step state IR as typed ROS 2 messages. + * + * Unlike the ZMQ / SHM transports, this one does not move opaque bytes: it IS + * the encoder. It does not, however, hard-code the message set. It drives a + * self-registering provider library (`FMjRosOutputRegistry`): on a + * `StructureVersion` change it instantiates one provider per registered output, + * calls `Build` to create that output's publishers for the current model, then + * calls `Publish` on every provider each step. Adding an output is dropping a + * self-registering provider file; there is no central switch here to edit. The + * byte `Publish(topic, payload)` path is a no-op; state flows in via + * `PublishState`, from the manager's post-step fan-out in every mode. + * + * The built-in providers cover, per articulation on `//...`: + * - `sensor_msgs/JointState` on `joint_states`, + * - `sensor_msgs/Imu` on `imu` (gyro and/or accel), + * - `geometry_msgs/TwistStamped` on `cmd_twist` (twist command), + * - total sensor routing (Force+Torque -> `WrenchStamped`, Rangefinder -> + * `Range`, Magnetometer -> `MagneticField`, Velocimeter -> `TwistStamped`, + * everything else -> `std_msgs/Float64MultiArray` on `sensors/`), + * - latched `robot_description` (exported URDF), + * - `nav_msgs/Odometry` on `odom` and ground-truth + * `geometry_msgs/PoseWithCovarianceStamped` on `pose` (free-base arts), + * - `sensor_msgs/CameraInfo` on `/camera_info` (one per camera), + * and process-wide: `tf2_msgs/TFMessage` on `/tf`, `rosgraph_msgs/Clock` on + * `/clock`, and the REP-105 `map -> odom -> world` ground-truth static chain on + * `/tf_static`. + * + * The pure IR -> array fill helpers (`Fill*`) stay static here so they are + * tested with no rcl dependency; the providers call them. + */ +UCLASS() +class URLABROS_API UURLabRosPublishTransport : public UURLabPublishTransport, public IMjStateConsumer +{ + GENERATED_BODY() + +public: + // UURLabPublishTransport contract. + virtual bool TransportInit() override; + virtual void TransportShutdown() override; + virtual FString GetTransportName() const override { return TEXT("ros2-pub"); } + + /** The byte path is unused: this transport encodes the IR itself. */ + virtual void Publish(const FString& /*Topic*/, const TArray& /*Payload*/) override {} + + /** IMjStateConsumer: the manager's post-step fan-out entry point. Forwards to + * PublishState so ROS receives the typed IR every step in all modes. */ + virtual void ConsumeState(const FMjStateSnapshot& Snapshot) override; + + /** Encode the snapshot to typed ROS messages and publish. Safe to call when + * ROS is unavailable (no-op). Rebuilds the publisher set first if the + * structure version changed. */ + void PublishState(const FMjStateSnapshot& Snapshot); + + /** Flatten one articulation's 1-DOF joints into the parallel arrays a + * `sensor_msgs/JointState` carries: one entry per hinge / slide joint (the + * canonical part segment), each with a scalar position (`qpos - qpos0`), + * velocity, and effort. Free and ball joints are multi-slot and not scalar + * URDF joints, so they are skipped here (their pose reaches ROS via `/tf`); + * including them would misalign names against values and truncate the tail. + * Effort is the driving actuator's force, matched to the joint by shared + * canonical name (the 1:1 transmission case); `OutEfforts` is left empty when + * no actuator drives any of the joints. Pure function, exposed for the + * fill-correctness test. */ + static void FillJointState(const FMjArticulationState& Art, + TArray& OutNames, TArray& OutPositions, + TArray& OutVelocities, TArray& OutEfforts); + + /** Collapse an articulation's gyro + accel sensors into the components a + * `sensor_msgs/Imu` carries: the first gyro's angular velocity and the first + * accel's linear acceleration. Either may be absent (the flags say which are + * present); an unpaired gyro still yields angular velocity only. Returns true + * when at least one component is present, i.e. an Imu is worth publishing. + * Pure function, exposed for the pairing test. */ + static bool FillImu(const FMjArticulationState& Art, + double OutAngularVel[3], bool& bOutHasAngularVel, + double OutLinearAccel[3], bool& bOutHasLinearAccel); + + /** Copy an articulation's twist command into the linear + angular vectors a + * `geometry_msgs/TwistStamped` carries. Returns false when the art has no + * twist. Pure function. */ + static bool FillTwistStamped(const FMjArticulationState& Art, + double OutLinear[3], double OutAngular[3]); + + /** Flatten every body across the snapshot into the parallel arrays a + * `tf2_msgs/TFMessage` carries: parent `world`, child `/`, + * translation from the body's world position, rotation reordered from MuJoCo + * wxyz to the xyzw the core expects. Pure function. */ + static void FillTf(const FMjStateSnapshot& Snapshot, + TArray& OutParents, TArray& OutChildren, + TArray& OutTranslations, TArray& OutRotationsXyzw); + + /** Project the IR clock's sim time to nanoseconds, matching the sec/nsec + * split `FURLabRpcDispatcher::AppendClockFields` uses for the msgpack wire. + * Pure function, exposed for the clock-parity test. */ + static int64 FillClock(const FMjClock& Clock); + + /** Test seams: inspect the rebuilt provider set and the per-step publish count + * without a ROS runtime dependency. */ + int32 GetArtPublisherCountForTest() const; + int32 GetProviderCountForTest() const { return Providers.Num(); } + uint32 GetCachedStructureVersionForTest() const { return CachedStructureVersion; } + int64 GetPublishStateCountForTest() const { return PublishStateCount; } + +private: + /** The registered output providers, instantiated on each structure change. + * Destroying an entry releases its publishers. */ + TArray> Providers; + uint32 CachedStructureVersion = 0; + bool bProvidersBuilt = false; + + /** Counts PublishState invocations so a test can assert the fan-out drove ROS + * in a given mode; incremented before the availability short-circuit is not + * useful, so it counts only calls that reached the publish body. */ + int64 PublishStateCount = 0; + + /** Instantiate the registered providers and Build them against the snapshot's + * current structure, releasing the previous set first. */ + void RebuildProviders(const FMjStateSnapshot& Snapshot); +}; diff --git a/Source/URLabRos/Public/Transport/RosRpcTransport.h b/Source/URLabRos/Public/Transport/RosRpcTransport.h new file mode 100644 index 00000000..c96cae67 --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosRpcTransport.h @@ -0,0 +1,193 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "Transport/RpcTransport.h" +#include +#include "RosRpcTransport.generated.h" + +class FRunnableThread; +class FRunnable; +class AAMjManager; + +// Opaque core handle, defined only in UrlabRclCore.cpp; held by pointer so this +// Public header never includes the Private core header. +struct UrlabRclContext; + +// Per-articulation command binding (the cmd_ctrl / cmd_vel / joint_command +// subscriptions, the claim / release services, and the identity used to resolve + +// gate writes). Defined in the .cpp; held by pointer. +struct FRosArtCommand; + +// Per declared user-input-channel subscription binding. Defined in the .cpp. +struct FRosUserInputSub; + +// IR user-channel kind; forward-declared so this public header stays free of the +// core state-types include. +enum class EMjUserChannelKind : uint8; + +/** + * @class UURLabRosRpcTransport + * @brief Request/reply + control-in surface for the in-process ROS 2 node. + * + * Sibling of the ZMQ / SHM RPC transports: owned by `UURLabBridgeServer`, bound + * through `EnsureExternalTransportsBound`. It owns a ROS executor thread (an `FRunnable`) that + * drives the core wait set through `UrlabRcl_SpinSome`, pumping the per-art + * command subscriptions and marshalling them into the sim's control write paths. + * + * Control-in surface (Live mode only; direct / puppet bundle control into their + * step / push calls, so writes are dropped outside Live): + * - `//cmd_ctrl` (`std_msgs/Float64MultiArray`), values in the art's + * actuator-list order, staged on each actuator's NetworkValue exactly as + * `ApplyStepCtrl` does; + * - `//cmd_vel` (`geometry_msgs/Twist`), routed to the art's + * `UMjTwistController::SetTwist`. + * Every write is tagged source id `RosControlSourceId()` and must pass + * `FMjControlOwnership::CheckWrite` first; a non-owning write is dropped. + * + * The subscription set is rebuilt when the articulation registry changes (a + * `StructureVersion` bump) or the live manager swaps, mirroring the publish + * transport's per-art rebuild rule. All rcl handles are created, spun, and + * destroyed on the executor thread, per the core's single-thread-per-handle + * contract; the write targets they reach (`SetNetworkControl`, `SetTwist`, + * `CheckWrite`) are each already thread-safe. + */ +UCLASS() +class URLABROS_API UURLabRosRpcTransport : public UURLabRpcTransport +{ + GENERATED_BODY() + +public: + // --- UURLabRpcTransport contract --- + virtual bool TransportInit() override; + virtual void TransportShutdown() override; + virtual FString GetTransportName() const override { return TEXT("ros2-rpc"); } + /** ROS is a full peer surface, so it accepts editor ops like ZMQ does. */ + virtual bool AcceptsEditorOps() const override { return true; } + + /** Source id every ROS control write carries, mirroring the node name + * `FURLabRosContext` creates. */ + static FString RosControlSourceId(); + + /** Marshal a received `cmd_ctrl` message for `ArtName` into the staging + * `ApplyStepCtrl` writes to: gate on ownership (`CheckWrite`) then Live mode, + * and on success stage each value on the art's actuators in list order. + * Public so the C subscription trampoline can reach it. */ + void HandleRosCtrl(const FString& ArtName, const double* Values, int32 Count); + + /** Marshal a received `cmd_vel` message for `ArtName` into the art's twist + * controller, gated identically to HandleRosCtrl. */ + void HandleRosTwist(const FString& ArtName, const double Linear[3], + const double Angular[3]); + + /** Marshal a received `sensor_msgs/JointState` on `//joint_command`: map + * each named joint to the actuator driving it (shared canonical name) and stage + * its position target, gated identically to HandleRosCtrl (ownership + Live + * mode). This is what lets the standard joint_state_publisher_gui jog the art. + * Public so the C subscription trampoline can reach it. */ + void HandleRosJointCommand(const FString& ArtName, const char** Names, + const double* Positions, int32 Count); + + /** Marshal a user-channel input value for `ArtOrNone` (canonical art segment, or + * None for scene scope) into the declaring component via the core + * `ApplyUserChannelInput`. Unlike control writes, user-channel input is NOT + * ownership- or Live-mode-gated: it is app-level data owned by user logic, not + * a control write that fights the physics authority. Public for the C + * subscription trampoline. */ + void HandleRosUserChannel(FName ArtOrNone, FName Channel, EMjUserChannelKind Kind, + const double* Values, int32 Count); + + /** Serve a `std_srvs/Trigger` claim_control / release_control request for + * `ArtName`: build the request the ZMQ/SHM path builds (source preset to the + * ROS node id, session preset to the active session), route it through + * `Dispatch`, and report ok + the resulting owner in the response. The art is + * encoded in the service NAME, so the request carries no art field of its own. + * TTL is not settable over the ROS service (the default TTL applies). Public + * for the C service trampolines. */ + void HandleRosClaimRelease(const FString& ArtName, bool bClaim, int32* OutSuccess, + char* OutMessage, int32 OutMessageCap); + + // --- Test seams --- + /** Drive HandleRosCtrl directly (no wire), for the mode / ownership gating + * tests. */ + void ApplyRosCtrlForTest(const FString& ArtName, const TArray& Values); + /** Drive HandleRosJointCommand directly (no wire), for the jog input test. */ + void ApplyRosJointCommandForTest(const FString& ArtName, const TArray& Names, + const TArray& Positions); + /** Drive HandleRosClaimRelease directly (no wire), for the service ownership + * test. Returns the service success flag. */ + bool ApplyRosClaimReleaseForTest(const FString& ArtName, bool bClaim); + /** Build the active manager's command subscriptions on the CALLING thread, + * publish `Values` on `Topic` via rcl, and pump the wait set until the ctrl + * callback fires or the spin budget is exhausted, then tear the subs down. + * Single-threaded, so the executor thread must not be running. Returns true + * if the subscription callback fired. */ + bool PublishAndPumpCtrlForTest(const FString& Topic, const TArray& Values); + int64 GetRosCtrlCallbackCountForTest() const { return RosCtrlCallbackCount.load(); } + int64 GetRosTwistCallbackCountForTest() const { return RosTwistCallbackCount.load(); } + int64 GetRosJointCommandCallbackCountForTest() const { return RosJointCommandCallbackCount.load(); } + int64 GetRosUserChannelCallbackCountForTest() const { return RosUserChannelCallbackCount.load(); } + int32 GetCommandSubCountForTest() const { return ArtCommands.Num(); } + int32 GetUserInputSubCountForTest() const { return UserInputSubs.Num(); } + +private: + FRunnableThread* WorkerThread = nullptr; + /** Runnable driving WorkerThread. FRunnableThread does not own it, so the + * transport keeps the pointer and deletes it at shutdown. */ + FRunnable* WorkerRunnable = nullptr; + std::atomic bStop{false}; + bool bIsInitialized = false; + + /** Executor loop: keeps the per-art command subscriptions in sync with the + * live registry, then spins the core wait set to fire their callbacks. */ + void RunExecutorLoop(); + + /** Rebuild the command subscriptions if the live manager or its structure + * version changed since the set was last built; a cheap per-tick check. */ + void SyncCommandSubscriptions(UrlabRclContext* Ctx); + /** Tear down and recreate cmd_ctrl / cmd_vel subscriptions for every + * articulation of the active manager. */ + void RebuildCommandSubscriptions(UrlabRclContext* Ctx); + /** Destroy every command subscription. Runs on the same thread that built + * them (executor thread, or the calling thread of a test seam). */ + void TeardownCommandSubscriptions(); + + /** Per-art command subscriptions; owned here, created / destroyed on one + * thread. Raw pointers with manual lifetime (as the runnable is), so the + * binding type stays confined to the .cpp. */ + TArray ArtCommands; + /** Per declared user-input-channel subscriptions; owned here, created / destroyed + * on the one executor / test thread alongside ArtCommands. */ + TArray UserInputSubs; + TWeakObjectPtr SubscribedManager; + uint32 SubscribedStructureVersion = 0; + bool bHaveSubscriptions = false; + + std::atomic RosCtrlCallbackCount{0}; + std::atomic RosTwistCallbackCount{0}; + std::atomic RosJointCommandCallbackCount{0}; + std::atomic RosUserChannelCallbackCount{0}; + + friend class FRosExecutorRunnable; +}; diff --git a/Source/URLabRos/Public/Transport/RosSensorRouting.h b/Source/URLabRos/Public/Transport/RosSensorRouting.h new file mode 100644 index 00000000..9e32f9b5 --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosSensorRouting.h @@ -0,0 +1,84 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "State/MjStateTypes.h" // EMjSensorSemantic, FMjArticulationState + +/** + * The ROS message family a MuJoCo sensor semantic maps to. The mapping is total + * over EMjSensorSemantic (see RouteForSemantic), so every sensor in a model + * reaches ROS: a typed message where one exists, else the Float64MultiArray + * fallback. Nothing is silently dropped. + */ +enum class ERosSensorRoute : uint8 +{ + /** Gyro / Accel: pooled into the articulation's sensor_msgs/Imu. Owned by the + * Imu provider, so the generic sensor provider skips this route. */ + Imu, + /** Force / Torque: paired into a geometry_msgs/WrenchStamped. */ + Wrench, + /** Rangefinder: sensor_msgs/Range. */ + Range, + /** Magnetometer: sensor_msgs/MagneticField. */ + MagneticField, + /** Velocimeter: geometry_msgs/TwistStamped (linear only). */ + Twist, + /** Everything with no standard typed message (touch, subtree, frame, + * actuator, joint, generic, ...): std_msgs/Float64MultiArray on + * //sensors/, so coverage is total by construction. */ + MultiArray +}; + +/** + * The total sensor-semantic -> ROS route table. Implemented as a switch with no + * default, so adding an EMjSensorSemantic value fails to compile here until it is + * routed. That is what keeps ROS sensor coverage total by construction. + */ +URLABROS_API ERosSensorRoute RouteForSemantic(EMjSensorSemantic Semantic); + +/** One Force+Torque pairing on an articulation. A pair with both names set is a + * fully populated wrench; a half-pair (extra force or torque) leaves the missing + * side None and publishes that half with the other zero. TopicName is the sensor + * the topic is named after (the force sensor, or the torque sensor if unpaired). */ +struct FMjWrenchPair +{ + FName ForceSensor; + FName TorqueSensor; + FName TopicName; +}; + +namespace MjRosSensorRouting +{ + /** The topic a sensor publishes on for its route. The MultiArray fallback uses + * the self-describing //sensors/; typed routes use + * ///. Pure, exposed for tests. */ + URLABROS_API FString TopicFor(const FString& ArtSegment, const FString& SensorName, + ERosSensorRoute Route); + + /** Pair an articulation's Force and Torque sensors into wrench rows by array + * order: the i-th force with the i-th torque. One force + one torque yields a + * single fully-populated pair. Pure, exposed for tests. */ + URLABROS_API void GatherWrenchPairs(const FMjArticulationState& Art, + TArray& OutPairs); +} diff --git a/Source/URLabRos/Public/Transport/RosStateEstimation.h b/Source/URLabRos/Public/Transport/RosStateEstimation.h new file mode 100644 index 00000000..8a8d4ecc --- /dev/null +++ b/Source/URLabRos/Public/Transport/RosStateEstimation.h @@ -0,0 +1,75 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" + +struct FMjArticulationState; + +// Pure IR -> state-estimation math shared by the Odometry and Pose providers and +// the CameraInfo intrinsic derivation. No rcl dependency, so it compiles and is +// unit-tested in every configuration (the providers call it; the tests assert its +// correctness without a ROS runtime). +namespace MjRosStateEstimation +{ + /** + * The ground-truth pose and body-frame twist of a free-base articulation, + * derived from its free joint. MuJoCo stores a free joint as + * qpos = [world position(3), world orientation wxyz(4)] and + * qvel = [WORLD linear velocity(3), BODY angular velocity(3)] - the linear and + * angular halves live in DIFFERENT frames. nav_msgs/Odometry reports the twist + * in the child (base) frame, so the linear half is rotated world->body here and + * the angular half is passed through unchanged. + */ + struct FMjFreeBaseState + { + bool bValid = false; + int32 BaseBodyIndex = INDEX_NONE; // index into Art.Bodies, or INDEX_NONE + double Position[3] = {0.0, 0.0, 0.0}; // world position (m) + double OrientationXyzw[4] = {0.0, 0.0, 0.0, 1.0}; + double LinearBody[3] = {0.0, 0.0, 0.0}; // linear velocity in the base frame + double AngularBody[3] = {0.0, 0.0, 0.0}; // angular velocity in the base frame + }; + + /** Rotate a vector from the world frame into a body frame given the body + * orientation quaternion in MuJoCo wxyz order (i.e. apply the conjugate). + * Pure quaternion algebra, independent of coordinate handedness. */ + URLABROS_API void RotateWorldToBody(const double QuatWxyz[4], const double VWorld[3], + double OutVBody[3]); + + /** Fill Out from the articulation's free joint (first joint of type Free with a + * full 7-DOF qpos / 6-DOF qvel). Returns false when the art has no free base + * (a fixed-base arm), in which case no odometry / base pose is published. + * The base body is the one whose world position matches the free-joint qpos; + * BaseBodyIndex is INDEX_NONE when no body matches (caller falls back to a + * conventional base link name). */ + URLABROS_API bool ComputeFreeBaseState(const FMjArticulationState& Art, + FMjFreeBaseState& Out); + + /** Standard pinhole intrinsics K (row-major 3x3) from a vertical field of view + * and image size: fy = (H/2) / tan(fovy/2), fx = fy (square pixels; the + * horizontal FOV follows from the width), principal point at the image centre + * (cx = W/2, cy = H/2). K[0]=fx, K[2]=cx, K[4]=fy, K[5]=cy, K[8]=1, rest 0. */ + URLABROS_API void PinholeKFromFovy(double FovyDegrees, int32 Width, int32 Height, + double OutK9[9]); +} // namespace MjRosStateEstimation diff --git a/Source/URLabRos/Public/URLabRos.h b/Source/URLabRos/Public/URLabRos.h new file mode 100644 index 00000000..1e9912d1 --- /dev/null +++ b/Source/URLabRos/Public/URLabRos.h @@ -0,0 +1,43 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#pragma once + +#include "CoreMinimal.h" +#include "Modules/ModuleInterface.h" + +/** + * @class FURLabRosModule + * @brief The optional ROS 2 integration module. + * + * On startup it installs the core's FMjExternalTransportProvider factory hooks + * (so the bridge can create the ROS control RPC + state publish transports + * without naming their types) and subscribes a camera image sink to + * FMjCameraFrameBus. All ROS-specific code lives in this module; the core URLab + * module has no dependency on it. + */ +class FURLabRosModule : public IModuleInterface +{ +public: + virtual void StartupModule() override; + virtual void ShutdownModule() override; +}; diff --git a/Source/URLabRos/URLabRos.Build.cs b/Source/URLabRos/URLabRos.Build.cs new file mode 100644 index 00000000..c193ae56 --- /dev/null +++ b/Source/URLabRos/URLabRos.Build.cs @@ -0,0 +1,246 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +using System; +using System.Collections.Generic; +using UnrealBuildTool; +using System.IO; + +// URLabRos is the optional ROS 2 integration module. It depends on the core +// URLab module and contains every ROS-specific piece (the rcl C-ABI seam, the +// state publish transport, the control RPC transport, the ROS context, and the +// camera image sink). Core URLab has no dependency on it and no ROS references; +// non-ROS users simply do not enable this module. When ROS is not installed +// (URLAB_ROS2_ROOT unset) AddRos2 compiles the feature out and this module still +// builds green as a set of no-op stubs. +public class URLabRos : ModuleRules +{ + public URLabRos(ReadOnlyTargetRules Target) : base(Target) + { + PCHUsage = ModuleRules.PCHUsageMode.UseExplicitOrSharedPCHs; + + // Mirror URLab: the ROS transports pull in AMjManager.h and the bridge + // headers, which transitively reach msgpack (exceptions) and windows.h + // (whose GetObject macro leaks across a unity TU into Chaos-using + // neighbours). Disabling unity keeps every .cpp in its own TU. + bEnableExceptions = true; + bUseUnity = false; + + PublicDependencyModuleNames.AddRange(new string[] + { + "Core", + "CoreUObject", + "Engine", + "URLab" + }); + + // The ROS RPC transport includes MjTwistController.h (cmd_vel routing), + // whose public header pulls in EnhancedInput's InputActionValue.h. URLab + // depends on EnhancedInput privately, so it does not propagate; declare it + // here for this module's own translation units. + PrivateDependencyModuleNames.AddRange(new string[] + { + "EnhancedInput", + // The user-channel provider inflates a Struct channel's packed msgpack + // map (via URLab's FURLabMsgpackUtil) and re-serialises it as JSON text + // for its std_msgs/String topic. + "Json" + }); + + if (Target.Platform == UnrealTargetPlatform.Linux) + { + PublicSystemLibraries.AddRange(new string[] { "pthread", "dl", "rt" }); + } + + AddRos2(Target); + } + + private string ThirdPartyPath + { + get { return Path.Combine(PluginDirectory, "third_party", "install"); } + } + + // Links the ROS 2 C API (rcl / rmw / rosidl_runtime_c + the message-package + // typesupport/generator libs) so the in-process ROS publisher can fill rosidl + // C structs and call rcl_publish. UBT compiles only the C ABI, so the large + // version-coupled ROS set is NOT globbed: an explicit pinned link list plus a + // pattern-scoped runtime DLL/so staging. The exact basenames and include + // layout are recorded in docs/ros2_link_facts.md from an actual install; the + // standalone harness (ros/urlab_ros_ws) validates the same C code against real + // DDS. + protected void AddRos2(ReadOnlyTargetRules Target) + { + // Resolve the install root: an explicit override, else the in-repo install + // convention used for the other deps. On a Pixi/Conda (RoboStack) install + // URLAB_ROS2_ROOT points at the environment's "Library" dir; a from-source + // or apt install points at the prefix itself. Both hold include/lib/bin. + string RosRoot = Environment.GetEnvironmentVariable("URLAB_ROS2_ROOT"); + if (string.IsNullOrEmpty(RosRoot)) + { + RosRoot = Path.Combine(ThirdPartyPath, "ros2"); + } + + // Absent ROS: compile the feature out. Every ROS reference in the module + // is fenced with #if URLAB_WITH_ROS2, so the build stays green with no ROS. + if (string.IsNullOrEmpty(RosRoot) || !Directory.Exists(RosRoot)) + { + PublicDefinitions.Add("URLAB_WITH_ROS2=0"); + Console.WriteLine("URLabRos: ROS 2 not found (set URLAB_ROS2_ROOT to enable) - building without ROS."); + return; + } + + string IncludeRoot = Path.Combine(RosRoot, "include"); + string LibDir = Path.Combine(RosRoot, "lib"); + string BinDir = Path.Combine(RosRoot, "bin"); + + // ROS packages use a double-nested layout: include///... , so + // each package directory is added (that is what lets #include + // resolve to include///x.h). rcl transitively pulls in packages + // beyond the ones the module names directly (rcl_yaml_param_parser, + // rcl_interfaces, ...), so the set is discovered rather than hand-listed: + // a directory is a ROS package when it holds a same-named child. The flat + // include root is deliberately NOT added - it also contains unrelated + // dependency headers (hwloc, openssl, ...) that would shadow system + // headers and break other translation units. These go on the system + // include path so ROS headers' warnings (C4668 on __STDC_VERSION__) do + // not trip UE's warnings-as-errors. + if (Directory.Exists(IncludeRoot)) + { + foreach (string PkgDir in Directory.GetDirectories(IncludeRoot)) + { + string Pkg = Path.GetFileName(PkgDir); + if (Directory.Exists(Path.Combine(PkgDir, Pkg))) + { + PublicSystemIncludePaths.Add(PkgDir); + } + } + } + + // Link only the C ABI the module references directly. The rest of the ROS + // graph is loaded by the DDS/rmw runtime, so it is staged, not linked. + string[] LinkNames = + { + "rcl", "rcutils", "rmw", "rosidl_runtime_c", + "builtin_interfaces__rosidl_generator_c", "builtin_interfaces__rosidl_typesupport_c", + "std_msgs__rosidl_generator_c", "std_msgs__rosidl_typesupport_c", + "std_srvs__rosidl_generator_c", "std_srvs__rosidl_typesupport_c", + "geometry_msgs__rosidl_generator_c", "geometry_msgs__rosidl_typesupport_c", + "sensor_msgs__rosidl_generator_c", "sensor_msgs__rosidl_typesupport_c", + "tf2_msgs__rosidl_generator_c", "tf2_msgs__rosidl_typesupport_c", + "nav_msgs__rosidl_generator_c", "nav_msgs__rosidl_typesupport_c", + "rosgraph_msgs__rosidl_generator_c", "rosgraph_msgs__rosidl_typesupport_c", + "shape_msgs__rosidl_generator_c", "shape_msgs__rosidl_typesupport_c", + "moveit_msgs__rosidl_generator_c", "moveit_msgs__rosidl_typesupport_c", + "object_recognition_msgs__rosidl_generator_c", "object_recognition_msgs__rosidl_typesupport_c", + "octomap_msgs__rosidl_generator_c", "octomap_msgs__rosidl_typesupport_c" + }; + + if (Target.Platform == UnrealTargetPlatform.Win64) + { + foreach (string Name in LinkNames) + { + string LibFile = Path.Combine(LibDir, Name + ".lib"); + if (File.Exists(LibFile)) + { + PublicAdditionalLibraries.Add(LibFile); + } + else + { + Console.WriteLine("URLabRos: ROS 2 lib not found (skipped): {0}", LibFile); + } + } + + // Stage the ROS/DDS runtime DLL cluster from the ROS bin dir. Scoped by + // pattern to the ROS + DDS families (not the whole environment), with + // prefixes so the version-suffixed DDS names (fastdds-3.6, fastcdr-2.3, + // foonathan_memory-0.7.4) resolve without hard-coding the suffix. + string[] DllPatterns = + { + "rcl*.dll", "rmw*.dll", "rcutils.dll", "rcpputils.dll", + "ament_index_cpp.dll", "rosidl_*.dll", "*__rosidl_*.dll", + "fastdds*.dll", "fastcdr*.dll", "foonathan_memory*.dll", + "tinyxml2.dll", "spdlog.dll", "dds_security*.dll", + "libssl*.dll", "libcrypto*.dll" + }; + StageRosRuntime(BinDir, DllPatterns, true); + } + else if (Target.Platform == UnrealTargetPlatform.Linux) + { + foreach (string Name in LinkNames) + { + // Link the unversioned .so symlink so the SONAME is recorded, not + // an absolute versioned path. + string SoFile = Path.Combine(LibDir, "lib" + Name + ".so"); + if (File.Exists(SoFile)) + { + PublicAdditionalLibraries.Add(SoFile); + } + else + { + Console.WriteLine("URLabRos: ROS 2 lib not found (skipped): {0}", SoFile); + } + } + + // Stage the shared-object cluster under $ORIGIN. + string[] SoPatterns = + { + "librcl*.so*", "librmw*.so*", "librcutils.so*", "librcpputils.so*", + "libament_index_cpp.so*", "librosidl_*.so*", "*__rosidl_*.so*", + "libfastdds*.so*", "libfastcdr*.so*", "libfoonathan_memory*.so*", + "libtinyxml2.so*", "libspdlog.so*" + }; + StageRosRuntime(LibDir, SoPatterns, false); + } + + PublicDefinitions.Add("URLAB_WITH_ROS2=1"); + } + + // Stages every file under Dir matching any of Patterns next to the plugin + // binary. On Win64 the staged DLLs are also delay-loaded, mirroring + // AddThirdPartyLibrary in URLab.Build.cs. De-duplicates so overlapping + // patterns stage once. + private void StageRosRuntime(string Dir, string[] Patterns, bool bDelayLoad) + { + if (!Directory.Exists(Dir)) + { + Console.WriteLine("URLabRos: ROS 2 runtime dir not found (no staging): {0}", Dir); + return; + } + HashSet Seen = new HashSet(StringComparer.OrdinalIgnoreCase); + foreach (string Pattern in Patterns) + { + foreach (string FilePath in Directory.GetFiles(Dir, Pattern, SearchOption.TopDirectoryOnly)) + { + string Name = Path.GetFileName(FilePath); + if (!Seen.Add(Name)) + { + continue; + } + RuntimeDependencies.Add("$(BinaryOutputDir)/" + Name, FilePath, StagedFileType.NonUFS); + if (bDelayLoad) + { + PublicDelayLoadDLLs.Add(Name); + } + } + } + } +} diff --git a/UnrealRoboticsLab.uplugin b/UnrealRoboticsLab.uplugin index 1c04e955..44cda71d 100644 --- a/UnrealRoboticsLab.uplugin +++ b/UnrealRoboticsLab.uplugin @@ -20,6 +20,11 @@ "Type": "Runtime", "LoadingPhase": "Default" }, + { + "Name": "URLabRos", + "Type": "Runtime", + "LoadingPhase": "Default" + }, { "Name": "URLabEditor", "Type": "Editor", diff --git a/ros/urlab_jog/.gitignore b/ros/urlab_jog/.gitignore new file mode 100644 index 00000000..7a60b85e --- /dev/null +++ b/ros/urlab_jog/.gitignore @@ -0,0 +1,2 @@ +__pycache__/ +*.pyc diff --git a/ros/urlab_jog/README.md b/ros/urlab_jog/README.md new file mode 100644 index 00000000..87f50f19 --- /dev/null +++ b/ros/urlab_jog/README.md @@ -0,0 +1,109 @@ +# urlab_jog + +Jog a URLab articulation from ROS 2 using only standard tooling. The slider +surface is the stock `joint_state_publisher_gui`; there is no custom node and no +relay. This directory ships only launch and rviz configuration plus these docs. + +## How it works + +``` +joint_state_publisher_gui -> //joint_command -> URLab (applies control) +URLab -> /tf , //joint_states -> rviz (RobotModel + TF, real motion) +``` + +- `joint_state_publisher_gui` reads the URDF (`/robot_description`) and builds one + slider per non-fixed joint, with limits taken from the URDF. It publishes + `sensor_msgs/JointState`. The launch file remaps that output off the default + `/joint_states` onto `//joint_command`, which URLab subscribes to and + applies as control in Live mode. No relay node is needed: URLab accepts the + standard `JointState` command directly. +- `robot_state_publisher` serves the URDF on `/robot_description` for rviz. URLab + publishes the authoritative `/tf` (parent `world`, child `/`) from + the live sim, so this node's own `/tf` output is remapped to a dead topic to + avoid a two-parent TF tree. +- `rviz2` shows the robot with the standard RobotModel display (from + `/robot_description` + `/tf`), so you see the actual sim motion, not the slider + echo. The bundled `rviz/franka.rviz` presets RobotModel + TF with fixed frame + `world`. + +## Prerequisites + +1. `joint_state_publisher_gui` is not in the base ROS env. Add it once + (`robot_state_publisher` and `rviz2` are already present from the desktop + install): + + ```powershell + pixi add --manifest-path C:\dev\urlab_ros2_env\pixi.toml ros-lyrical-joint-state-publisher-gui + ``` + +2. A URDF exported for the articulation. `joint_state_publisher_gui` and rviz's + RobotModel both require it, and its names must line up with what URLab + publishes: + + - URDF **joint names** must match URLab's joint names (the `name[]` URLab + publishes on `//joint_states`, e.g. `joint1` .. `joint7`). The command + `JointState` is matched to the sim by joint name, so a mismatch silently + drives the wrong joints or nothing. + - URDF **link names** must match URLab's TF frames `/` (e.g. + `franka/panda_link0`), because rviz's RobotModel places each link by looking + up its TF frame by name. The root/fixed frame is `world`. + + URLab is the naming authority (`FMjCanonicalName`); export the URDF to match. + `ros2 topic echo --once //joint_states` and `ros2 topic echo --once /tf` + against a running URLab show the exact joint and frame names to target. + +3. Grant ROS control ownership of the art. URLab gates control writes per + articulation; a ROS command is only applied when the art is owned by source + `ros:urlab`, otherwise it is silently ignored. Claim it once from the bridge + environment (`Plugins/URLab_Bridge`): + + ```bash + uv run python -c "from urlab_client import URLabClient; c=URLabClient(); c.connect(); print(c.runtime.claim_control(articulation='franka', source='ros:urlab'))" + ``` + + Release later with + `c.runtime.release_control(articulation='franka', source='ros:urlab')`. + +## Launch (against a running URLab + Franka in Live mode) + +`ros2 launch` accepts a direct file path, so no colcon build is needed: + +```powershell +# Windows (from this directory) +pixi run --manifest-path C:\dev\urlab_ros2_env\pixi.toml ` + ros2 launch launch\franka_jog.launch.py art:=franka urdf:=C:\path\to\franka.urdf +``` + +```bash +# Linux (with the ROS env active, from this directory) +ros2 launch launch/franka_jog.launch.py art:=franka urdf:=/path/to/franka.urdf +``` + +Move a slider and the Franka tracks it in both the sim and rviz. + +### Arguments + +- `art` articulation name / topic namespace (default `franka`); commands go + to `//joint_command`. +- `urdf` path to the articulation's URDF (required). +- `rviz` rviz2 config path (default: the bundled `rviz/franka.rviz`). +- `use_rviz` set `false` to launch the sliders without rviz. + +## Verify without moving anything + +```bash +ros2 topic echo --once /franka/joint_command # sliders publish here +ros2 topic echo --once /franka/joint_states # URLab real state +ros2 topic echo --once /tf # URLab TF tree (frame names) +``` + +## Notes + +- Only stock nodes are used: `joint_state_publisher_gui`, + `robot_state_publisher`, `rviz2`. No relay, no custom GUI. +- The `//joint_command` `JointState` command input is served by URLab's ROS + transport. If commands do not move the robot, check in order: the art is + claimed for `ros:urlab` (step 3), URLab is in Live mode, and the URDF joint + names match `//joint_states`. +- For a mobile base, `cmd_vel` (`geometry_msgs/Twist`) is a separate surface + covered by `teleop_twist_keyboard`; it is out of scope for this joint-jog tool. diff --git a/ros/urlab_jog/launch/franka_jog.launch.py b/ros/urlab_jog/launch/franka_jog.launch.py new file mode 100644 index 00000000..a92b6ba2 --- /dev/null +++ b/ros/urlab_jog/launch/franka_jog.launch.py @@ -0,0 +1,161 @@ +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Jog a URLab articulation with the standard joint_state_publisher_gui sliders. + +This launch file wires only stock ROS 2 nodes. No custom node, no relay: + + joint_state_publisher_gui builds one slider per non-fixed URDF joint (limits + from the URDF) and publishes sensor_msgs/JointState. + Its output is remapped off /joint_states onto the + command topic //joint_command, which URLab + subscribes to and applies as control in Live mode. + + robot_state_publisher serves the URDF on /robot_description so rviz's + RobotModel display can load it. Its /tf outputs are + remapped to dead topics because URLab publishes the + authoritative /tf (parent 'world', child + '/') from the live sim. + + rviz2 RobotModel + TF, using the bundled franka.rviz. + +The URDF must name its joints to match URLab's canonical joint names (the +JointState.name[] URLab publishes on //joint_states, e.g. actuator/joint +short names) and its links to match URLab's TF frames '/', with the +fixed frame 'world'. Otherwise the sliders command the wrong joints and rviz +cannot place the links. +""" + +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +_THIS_DIR = os.path.dirname(os.path.abspath(__file__)) +_DEFAULT_RVIZ = os.path.normpath(os.path.join(_THIS_DIR, "..", "rviz", "franka.rviz")) + + +def _setup(context, *args, **kwargs): + art = LaunchConfiguration("art").perform(context) + urdf_path = LaunchConfiguration("urdf").perform(context) + rviz_config = LaunchConfiguration("rviz").perform(context) + use_rviz = LaunchConfiguration("use_rviz").perform(context).lower() in ("1", "true", "yes") + + if not urdf_path: + raise RuntimeError( + "franka_jog.launch.py requires 'urdf:=' (the URDF exported for " + "the articulation; its joint/link names must match URLab's)." + ) + with open(urdf_path, "r", encoding="utf-8") as handle: + robot_description = handle.read() + + command_topic = f"/{art}/joint_command" + + # Claim control for the ROS source so URLab accepts the jog. URLab gates + # control writes on ownership; the jog is dropped until '/claim_control' + # is called (the claim never expires, so once is enough). Requires the sim to + # be in Live mode with the control source set to the network slot, which the + # Python bring-up does before this launch. + claim = ExecuteProcess( + cmd=["ros2", "service", "call", f"/{art}/claim_control", "std_srvs/srv/Trigger"], + output="screen", + ) + + nodes = [ + claim, + # Sliders. Publishes JointState; remapped onto the URLab command topic so + # it does not collide with URLab's own //joint_states (real state). + Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + name="joint_state_publisher_gui", + parameters=[{"robot_description": robot_description, "use_sim_time": True}], + remappings=[("joint_states", command_topic)], + output="screen", + ), + # Poses the URDF for rviz: subscribe URLab's real //joint_states and + # publish /tf for the (bare-named) URDF links so RobotModel can place them. + # (URLab also publishes /tf under '/'; the bare frames here are + # distinct and are what the bare-link URDF matches.) + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + parameters=[{"robot_description": robot_description, "use_sim_time": True}], + remappings=[("joint_states", f"/{art}/joint_states")], + output="screen", + ), + # rsp roots the tree at the URDF root link; anchor it under 'world' so the + # bundled rviz fixed frame resolves. + Node( + package="tf2_ros", + executable="static_transform_publisher", + name="world_to_root", + # Flag form, not the deprecated positional "x y z ... frame child": + # the positional path crashes (access violation) on the Windows build, + # which severs world->link0 and leaves the RobotModel unable to place + # any link against the 'world' fixed frame. + arguments=[ + "--frame-id", "world", "--child-frame-id", "link0", + "--x", "0", "--y", "0", "--z", "0", + "--roll", "0", "--pitch", "0", "--yaw", "0", + ], + parameters=[{"use_sim_time": True}], + output="screen", + ), + ] + + if use_rviz: + nodes.append( + Node( + package="rviz2", + executable="rviz2", + name="rviz2", + arguments=["-d", rviz_config], + parameters=[{"use_sim_time": True}], + output="screen", + ) + ) + + return nodes + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + DeclareLaunchArgument( + "art", + default_value="franka", + description="Articulation name (topic namespace): commands go to //joint_command.", + ), + DeclareLaunchArgument( + "urdf", + default_value="", + description="Path to the URDF exported for the articulation (required).", + ), + DeclareLaunchArgument( + "rviz", + default_value=_DEFAULT_RVIZ, + description="Path to the rviz2 config.", + ), + DeclareLaunchArgument( + "use_rviz", + default_value="true", + description="Start rviz2 alongside the sliders.", + ), + OpaqueFunction(function=_setup), + ] + ) diff --git a/ros/urlab_jog/rviz/franka.rviz b/ros/urlab_jog/rviz/franka.rviz new file mode 100644 index 00000000..999c3e6f --- /dev/null +++ b/ros/urlab_jog/rviz/franka.rviz @@ -0,0 +1,86 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /RobotModel1 + - /TF1 + Splitter Ratio: 0.5 + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Cell Size: 1 + Plane Cell Count: 10 + Color: 160; 160; 164 + Reference Frame: + - Class: rviz_default_plugins/RobotModel + Name: RobotModel + Enabled: true + Visual Enabled: true + Collision Enabled: false + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Description File: "" + Alpha: 1 + TF Prefix: "" + - Class: rviz_default_plugins/TF + Name: TF + Enabled: true + Show Names: true + Show Axes: true + Show Arrows: false + Marker Scale: 0.3 + Frame Timeout: 15 + Update Interval: 0 + - Class: rviz_default_plugins/Image + Name: WristCam + Enabled: true + Topic: + Value: /franka/wrist_cam/image + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Max Value: 1 + Min Value: 0 + Normalize Range: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Views: + Current: + Class: rviz_default_plugins/Orbit + Name: Current View + Distance: 3.0 + Focal Point: + X: 0 + Y: 0 + Z: 0.3 + Pitch: 0.4 + Yaw: 0.8 + Target Frame: + Saved: ~ +Window Geometry: + Height: 800 + Width: 1200 + Displays: + collapsed: false + Views: + collapsed: false diff --git a/ros/urlab_ros_ws/.gitignore b/ros/urlab_ros_ws/.gitignore new file mode 100644 index 00000000..d4ac50e8 --- /dev/null +++ b/ros/urlab_ros_ws/.gitignore @@ -0,0 +1,4 @@ +build/ +install/ +log/ +.pixi/ diff --git a/ros/urlab_ros_ws/CMakeLists.txt b/ros/urlab_ros_ws/CMakeLists.txt new file mode 100644 index 00000000..16a4dd80 --- /dev/null +++ b/ros/urlab_ros_ws/CMakeLists.txt @@ -0,0 +1,99 @@ +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Standalone (non-colcon) CMake project that builds the shared UrlabRclCore +# against a user-installed ROS 2, together with a self-verifying harness. This +# exists so the rcl-facing C++ can be built and tested OUTSIDE Unreal, and so the +# unproven Windows in-process leg can be validated before any UE wiring starts. +# +# It consumes the ROS 2 installation the user has sourced; it does not build any +# ROS package. The same UrlabRclCore.cpp is later compiled into the URLab module +# by UBT with UE's own toolchain. + +# URLAB_LIBCXX switches to clang + libc++ for the Linux toolchain smoke build. +# It must be handled before project() so the compiler selection takes effect. +# It is a Linux-only concern: on Windows both UE and ROS 2 use MSVC, so there is +# no libc++/libstdc++ ABI mix to pre-validate. +if(URLAB_LIBCXX) + set(CMAKE_C_COMPILER clang) + set(CMAKE_CXX_COMPILER clang++) +endif() + +cmake_minimum_required(VERSION 3.16) +project(urlab_rcl_test CXX) + +if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(URLAB_LIBCXX) + add_compile_options($<$:-stdlib=libc++>) + add_link_options(-stdlib=libc++) +endif() + +# Consume the sourced ROS 2 install via modern imported targets. Each package's +# find_package defines namespaced CMake targets (rcl::rcl, +# ::__rosidl_typesupport_c, ...) whose INTERFACE include dirs and link +# libraries propagate transitively, so we link with plain target_link_libraries +# rather than ament_target_dependencies (that ament_cmake macro is not available +# in this ROS 2 build). This also mirrors how the UE Build.cs will link these +# libraries explicitly, without ament. +find_package(rcl REQUIRED) +find_package(rcutils REQUIRED) +find_package(rmw REQUIRED) +find_package(rosidl_runtime_c REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(tf2_msgs REQUIRED) +find_package(rosgraph_msgs REQUIRED) + +# The shared core, reached by relative path up into the plugin module tree. +set(URLAB_RCL_CORE + "${CMAKE_CURRENT_SOURCE_DIR}/../../Source/URLab/Private/Ros/UrlabRclCore.cpp") + +add_executable(urlab_rcl_test + src/test_main.cpp + "${URLAB_RCL_CORE}") + +target_include_directories(urlab_rcl_test PRIVATE + "${CMAKE_CURRENT_SOURCE_DIR}/../../Source/URLab/Private/Ros") + +# The standalone build is the one place URLAB_WITH_ROS2 is defined outside UBT, +# so the fenced core body compiles here for fast iteration and testing. The +# distro name is the compile-time pin reported by UrlabRcl_DistroName(); it is +# set here (rather than in the core source) so the core stays distro-agnostic. +target_compile_definitions(urlab_rcl_test PRIVATE + URLAB_WITH_ROS2=1 + URLAB_ROS_DISTRO_NAME="lyrical") + +target_link_libraries(urlab_rcl_test PRIVATE + rcl::rcl + rosidl_runtime_c::rosidl_runtime_c + builtin_interfaces::builtin_interfaces__rosidl_typesupport_c + std_msgs::std_msgs__rosidl_typesupport_c + geometry_msgs::geometry_msgs__rosidl_typesupport_c + sensor_msgs::sensor_msgs__rosidl_typesupport_c + tf2_msgs::tf2_msgs__rosidl_typesupport_c + rosgraph_msgs::rosgraph_msgs__rosidl_typesupport_c) + +# winmm provides timeBeginPeriod/timeEndPeriod, used by the harness publish loop +# to get 1 ms timer resolution so the 50 Hz cadence is accurate on Windows. +if(WIN32) + target_link_libraries(urlab_rcl_test PRIVATE winmm) +endif() diff --git a/ros/urlab_ros_ws/build_and_test.ps1 b/ros/urlab_ros_ws/build_and_test.ps1 new file mode 100644 index 00000000..85fd0475 --- /dev/null +++ b/ros/urlab_ros_ws/build_and_test.ps1 @@ -0,0 +1,161 @@ +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +<# +.SYNOPSIS + Configure + build the standalone urlab_rcl_test harness against a + user-installed ROS 2 Lyrical Luth and run its selftest plus a cross-process + `ros2 topic echo` check. This is the primary Windows validation of the + in-process rcl publish/subscribe path. + +.DESCRIPTION + Assumes ROS 2 Lyrical is already installed by the user via Pixi/Conda + (prefix.dev + RoboStack), the default Windows install path. The normal flow + is to activate that environment with `pixi shell` and then run this script; + the script detects the active ROS environment and proceeds. Alternatively an + explicit activation script can be supplied via -RosSetup or URLAB_ROS2_SETUP. + It installs nothing and writes nothing outside build\. + +.PARAMETER RosSetup + Full path to a ROS/conda activation .bat script to import before building. + Overrides URLAB_ROS2_SETUP. Omit it when the ROS env is already active + (e.g. inside `pixi shell`). + +.NOTES + Exit codes: 0 ok, 1 build/env failed, 2 tests failed, 3 bad args. +#> + +[CmdletBinding()] +param( + [string] $RosSetup = '' +) + +$ErrorActionPreference = 'Stop' +$ScriptDir = Split-Path -Parent $MyInvocation.MyCommand.Path +Set-Location $ScriptDir + +# --- Resolve / detect the ROS 2 environment -------------------------------- +if ([string]::IsNullOrWhiteSpace($RosSetup) -and + -not [string]::IsNullOrWhiteSpace($env:URLAB_ROS2_SETUP)) { + $RosSetup = $env:URLAB_ROS2_SETUP +} + +if (-not [string]::IsNullOrWhiteSpace($RosSetup)) { + if (-not (Test-Path $RosSetup)) { + Write-Error "ROS activation script not found: $RosSetup" + exit 3 + } + Write-Host ">>> Activating ROS 2 environment: $RosSetup" + # Run the activation script in a child cmd and import the resulting + # environment into this session so cmake/ros2 see the ROS toolchain + libs. + $envDump = cmd /c "call `"$RosSetup`" >nul 2>&1 && set" + foreach ($line in $envDump) { + if ($line -match '^([^=]+)=(.*)$') { + Set-Item -Path ("env:" + $matches[1]) -Value $matches[2] + } + } +} + +if ([string]::IsNullOrWhiteSpace($env:AMENT_PREFIX_PATH)) { + Write-Error @" +No active ROS 2 environment (AMENT_PREFIX_PATH is empty). + +The default Windows install is Pixi/Conda (prefix.dev + RoboStack). Activate it +and re-run, e.g.: + + cd + pixi shell # activates ROS 2 Lyrical for this shell + cd \ros\urlab_ros_ws + .\build_and_test.ps1 + +Or point -RosSetup / `$env:URLAB_ROS2_SETUP at a ROS/conda activation .bat. +See docs/ros_workspace_setup.md. +"@ + exit 3 +} +Write-Host ">>> ROS 2 environment active (AMENT_PREFIX_PATH set)." + +# --- Configure + build ----------------------------------------------------- +Write-Host ">>> Configuring (cmake)..." +cmake -B build -DCMAKE_BUILD_TYPE=Release +if ($LASTEXITCODE -ne 0) { Write-Error "cmake configure failed."; exit 1 } + +Write-Host ">>> Building (Release)..." +cmake --build build --config Release +if ($LASTEXITCODE -ne 0) { Write-Error "cmake build failed."; exit 1 } + +# Locate the executable (multi-config generators nest it under Release\). +$exe = Get-ChildItem -Path build -Recurse -Filter 'urlab_rcl_test.exe' | + Select-Object -First 1 -ExpandProperty FullName +if (-not $exe) { + Write-Error "urlab_rcl_test.exe not found under build\ after the build." + exit 1 +} +Write-Host ">>> Built: $exe" + +# --- Selftest -------------------------------------------------------------- +Write-Host ">>> Running selftest..." +& $exe --selftest +if ($LASTEXITCODE -ne 0) { Write-Error "selftest failed."; exit 2 } + +# --- Cross-process echo verify --------------------------------------------- +# Publish continuously in the background and confirm a second process sees the +# expected JointState over real DDS. This is the in-process Windows leg the +# whole ROS design must prove. +Write-Host ">>> Cross-process verify (ros2 topic echo)..." +$topic = '/urlab_test/joint_states' +$echoOut = Join-Path ([System.IO.Path]::GetTempPath()) "urlab_echo_$PID.out" +$echoErr = Join-Path ([System.IO.Path]::GetTempPath()) "urlab_echo_$PID.err" +# Publish long enough to outlast `ros2 topic echo` discovery on a cold ROS +# daemon; the publisher is force-killed as soon as echo returns, so this is an +# upper bound, not a fixed wait. `ros2 topic echo --once` has no built-in +# timeout and blocks forever if it misses the publisher's window (or the topic +# name is wrong), so it runs as a child process bounded by WaitForExit and is +# force-killed on timeout -- the verify can never hang the script. +$pub = Start-Process -FilePath $exe -ArgumentList '--publish', '3000' -PassThru -NoNewWindow +$echoProc = $null +$echo = '' +try { + Start-Sleep -Seconds 3 + $echoProc = Start-Process -FilePath 'ros2' ` + -ArgumentList 'topic', 'echo', '--once', $topic ` + -NoNewWindow -PassThru ` + -RedirectStandardOutput $echoOut -RedirectStandardError $echoErr + if (-not $echoProc.WaitForExit(30000)) { + Write-Warning "ros2 topic echo did not return within 30s; killing it." + Stop-Process -Id $echoProc.Id -Force -ErrorAction SilentlyContinue + } + $echo = ((Get-Content $echoOut -Raw -ErrorAction SilentlyContinue) + "`n" + + (Get-Content $echoErr -Raw -ErrorAction SilentlyContinue)) + Write-Host $echo +} finally { + if ($echoProc -and -not $echoProc.HasExited) { + Stop-Process -Id $echoProc.Id -Force -ErrorAction SilentlyContinue + } + if ($pub -and -not $pub.HasExited) { + Stop-Process -Id $pub.Id -Force -ErrorAction SilentlyContinue + } + Remove-Item $echoOut, $echoErr -ErrorAction SilentlyContinue +} + +if ($echo -match 'joint_a' -and $echo -match 'joint_b' -and $echo -match 'joint_c') { + Write-Host ">>> Cross-process verify OK: JointState names received." +} else { + Write-Error "Cross-process verify failed: expected joint names not seen in `ros2 topic echo` output." + exit 2 +} + +Write-Host "" +Write-Host "=== urlab_rcl_test: ALL CHECKS PASSED (Windows) ===" +exit 0 diff --git a/ros/urlab_ros_ws/build_and_test.sh b/ros/urlab_ros_ws/build_and_test.sh new file mode 100644 index 00000000..6e015525 --- /dev/null +++ b/ros/urlab_ros_ws/build_and_test.sh @@ -0,0 +1,123 @@ +#!/usr/bin/env bash +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# build_and_test.sh — configure + build the standalone urlab_rcl_test harness +# against a user-installed ROS 2 Lyrical and run its selftest plus a cross-process +# `ros2 topic echo` check. Linux parity for build_and_test.ps1 (the primary +# Windows validation), plus the --libcxx clang toolchain smoke for gate A3. +# +# Assumes ROS 2 Lyrical is already installed by the user (apt into +# /opt/ros/lyrical, or a Pixi/Conda env). Sources the ROS environment +# (URLAB_ROS2_SETUP override, the apt default, or an already-active env such as +# `pixi shell`), then builds and tests. Installs nothing and writes nothing +# outside build/. +# +# Usage: +# ./build_and_test.sh [--libcxx] +# +# --libcxx : build UrlabRclCore.cpp with clang + libc++ and link it against the +# libstdc++-built rcl cluster (pre-validates the UE-Linux stdlib mix). +# +# Exit codes: 0 ok, 1 build/env failed, 2 tests failed, 3 bad args. + +set -eu + +SCRIPT_DIR=$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd) +cd "$SCRIPT_DIR" + +LIBCXX=0 +while [[ $# -gt 0 ]]; do + case "$1" in + --libcxx) LIBCXX=1; shift ;; + -h|--help) + echo "Usage: ./build_and_test.sh [--libcxx]" >&2 + exit 3 ;; + *) echo "Unknown arg: $1" >&2; exit 3 ;; + esac +done + +DEFAULT_SETUP="/opt/ros/lyrical/setup.bash" +ROS_SETUP="${URLAB_ROS2_SETUP:-$DEFAULT_SETUP}" + +if [[ -f "$ROS_SETUP" ]]; then + echo ">>> Sourcing ROS 2 environment: $ROS_SETUP" + # shellcheck disable=SC1090 + source "$ROS_SETUP" +elif [[ -n "${AMENT_PREFIX_PATH:-}" ]]; then + # Already-active environment (e.g. inside `pixi shell`). + echo ">>> Using already-active ROS 2 environment." +fi + +if [[ -z "${AMENT_PREFIX_PATH:-}" ]]; then + cat >&2 <>> Toolchain smoke: clang + libc++ (gate A3)" + CMAKE_ARGS+=(-DURLAB_LIBCXX=ON) +fi + +echo ">>> Configuring (cmake)..." +cmake "${CMAKE_ARGS[@]}" + +echo ">>> Building (Release)..." +cmake --build build --config Release + +EXE="build/urlab_rcl_test" +if [[ ! -x "$EXE" ]]; then + echo "urlab_rcl_test not found at $EXE after the build." >&2 + exit 1 +fi +echo ">>> Built: $EXE" + +echo ">>> Running selftest..." +"$EXE" --selftest || { echo "selftest failed." >&2; exit 2; } + +# Cross-process echo verify over real DDS. Publish long enough to outlast echo +# discovery on a cold ROS daemon (the publisher is killed as soon as echo +# returns). `ros2 topic echo --once` has no built-in timeout and blocks forever +# if it misses the publisher's window (or the topic name is wrong), so bound it +# with `timeout` -- the verify can never hang the script. +echo ">>> Cross-process verify (ros2 topic echo)..." +"$EXE" --publish 3000 & +PUB_PID=$! +trap 'kill "$PUB_PID" 2>/dev/null || true' EXIT +sleep 3 +ECHO_OUT=$(timeout 30 ros2 topic echo --once /urlab_test/joint_states 2>&1 || true) +echo "$ECHO_OUT" +kill "$PUB_PID" 2>/dev/null || true +trap - EXIT + +if echo "$ECHO_OUT" | grep -q 'joint_a' \ + && echo "$ECHO_OUT" | grep -q 'joint_b' \ + && echo "$ECHO_OUT" | grep -q 'joint_c'; then + echo ">>> Cross-process verify OK: JointState names received." +else + echo "Cross-process verify failed: expected joint names not seen in echo output." >&2 + exit 2 +fi + +echo "" +echo "=== urlab_rcl_test: ALL CHECKS PASSED (Linux) ===" +exit 0 diff --git a/ros/urlab_ros_ws/scripts/test_maps.py b/ros/urlab_ros_ws/scripts/test_maps.py new file mode 100644 index 00000000..1e1e6c93 --- /dev/null +++ b/ros/urlab_ros_ws/scripts/test_maps.py @@ -0,0 +1,180 @@ +""" +End-to-end integration test for the three ROS map providers. + +Boots a test scene via the URLab RPC, starts PIE, and verifies that +/urlab/obstacle_cloud, /map, and /octomap_binary are publishing. + +Usage: + uv run ros/urlab_ros_ws/scripts/test_maps.py [--ue-address tcp://localhost] + +Requires: + - UE editor running with the URLab plugin loaded (no PIE yet — the + script calls begin_pie) + - ROS 2 sourced (ros2 CLI on PATH) + - uv environment with urlab_client installed +""" + +from __future__ import annotations + +import argparse +import os +import subprocess +import sys +import time +from pathlib import Path +from typing import Optional + +# --- helpers ----------------------------------------------------------- + +def ros2(*args: str, timeout_s: float = 10.0) -> subprocess.CompletedProcess: + """Run ``ros2 `` and return the completed process.""" + return subprocess.run( + ["ros2", *args], + capture_output=True, text=True, timeout=timeout_s, + ) + + +def require_ros2() -> None: + """Fail fast when ros2 isn't on PATH.""" + r = subprocess.run(["ros2", "topic", "list"], capture_output=True, text=True, timeout=5.0) + if r.returncode != 0: + sys.exit(f"ros2 not available; source ROS 2 first.\nstderr: {r.stderr}") + + +def topic_exists(topic: str) -> bool: + return topic in ros2("topic", "list").stdout.splitlines() + + +def echo_once(topic: str, timeout_s: float = 15.0) -> str: + """Return one message body from ``ros2 topic echo --once``.""" + r = ros2("topic", "echo", "--once", "--full-length", topic, timeout_s=timeout_s) + if r.returncode != 0: + raise RuntimeError(f"echo {topic} failed: {r.stderr}") + return r.stdout + + +# --- main -------------------------------------------------------------- + +def main() -> None: + parser = argparse.ArgumentParser(description="End-to-end ROS maps integration test") + parser.add_argument("--ue-address", default="tcp://localhost", + help="URLab RPC address (default: tcp://localhost)") + parser.add_argument("--scene", default=None, + help="Path to test MJCF; defaults to the bundled ros_maps_test.xml") + parser.add_argument("--timeout", type=float, default=30.0, + help="Seconds to wait for each topic to appear") + parser.add_argument("--keep-pie", action="store_true", + help="Leave PIE running after the test (default: stop PIE)") + args = parser.parse_args() + + # Resolve the test scene. + if args.scene: + scene_path = Path(args.scene).resolve() + else: + scene_path = Path(__file__).resolve().parents[3] / "Content" / "TestData" / "ros_maps_test.xml" + if not scene_path.exists(): + sys.exit(f"Test scene not found: {scene_path}") + + require_ros2() + print(f"ROS 2 OK | topics before test: {len(ros2('topic', 'list').stdout.splitlines())}") + + # --- Import the scene into UE --------------------------------------- + from urlab_client import URLabClient + + client = URLabClient(args.ue_address, step_mode="direct") + print("Connecting to UE...") + client.connect(observations="standard") + + # Before PIE the map topics shouldn't exist (no manager = no providers). + assert not topic_exists("/urlab/obstacle_cloud"), \ + "/urlab/obstacle_cloud already present before PIE — stale publishers?" + + print(f"Importing test scene: {scene_path}") + bp = client.scene.import_xml(str(scene_path)) + print(f"import_xml → {bp.class_path}") + + # Spawn it into the level so the model is in the world at PIE start. + client.scene.spawn_actor(bp, "ros_maps_test_actor") + print("spawn_actor OK") + + # Start PIE. + print("Starting PIE...") + client.sim.start() + assert client.manager_present, "PIE did not start (manager_present still False)" + + # Give providers a moment to build and publish. + time.sleep(2.0) + + # --- Verify topics --------------------------------------------------- + + topics = { + "/urlab/obstacle_cloud": "PointCloud2", + "/map": "OccupancyGrid", + "/octomap_binary": "Octomap", + } + + for topic, msg_type in topics.items(): + deadline = time.monotonic() + args.timeout + while not topic_exists(topic): + if time.monotonic() > deadline: + sys.exit(f"FAIL: {topic} ({msg_type}) did not appear within {args.timeout}s") + print(f" waiting for {topic}...") + time.sleep(1.0) + + # Check topic type. + info = ros2("topic", "info", topic) + if msg_type not in info.stdout: + sys.exit(f"FAIL: {topic} type mismatch — expected {msg_type}, got\n{info.stdout}") + print(f" {topic} ({msg_type}) — OK") + + # --- Content checks -------------------------------------------------- + + # Point cloud: should have points. + pc_echo = echo_once("/urlab/obstacle_cloud", timeout_s=15.0) + if "height: 1" not in pc_echo and "is_dense: true" not in pc_echo: + sys.exit(f"FAIL: /urlab/obstacle_cloud does not look like PointCloud2:\n{pc_echo[:500]}") + # Count actual data lines — a non-empty cloud has 'data:' followed by hex or length. + if "width:" in pc_echo: + for line in pc_echo.splitlines(): + if "width:" in line.strip(): + width = int(line.strip().split()[-1]) + if width == 0: + sys.exit(f"FAIL: /urlab/obstacle_cloud has zero points: {line.strip()}") + print(f" /urlab/obstacle_cloud: {width} points — OK") + break + + # Occupancy grid: latched, should have non-empty data. + occ_echo = echo_once("/map", timeout_s=15.0) + if "resolution:" not in occ_echo: + sys.exit(f"FAIL: /map does not look like OccupancyGrid:\n{occ_echo[:500]}") + has_occupied = False + for line in occ_echo.splitlines(): + stripped = line.strip() + if stripped.startswith("data:"): + # The data array is printed as a list of int8 values. + vals = stripped.removeprefix("data:").strip().strip("[]") + if vals and any(v.strip() not in ("-1", "0") for v in vals.split(",")): + has_occupied = True + if has_occupied: + print(" /map: occupied cells present — OK") + else: + print(" /map: no occupied cells found (may be outside grid bounds) — WARN") + + # Octomap: binary, check header. + octo_echo = echo_once("/octomap_binary", timeout_s=15.0) + if "resolution:" not in octo_echo and "binary: true" not in octo_echo: + sys.exit(f"FAIL: /octomap_binary does not look like Octomap:\n{octo_echo[:500]}") + print(" /octomap_binary: publishing — OK") + + # --- Cleanup --------------------------------------------------------- + if not args.keep_pie: + client.sim.stop() + print("PIE stopped.") + client.close() + + print("\n=== All ROS map providers verified ===") + sys.exit(0) + + +if __name__ == "__main__": + main() diff --git a/ros/urlab_ros_ws/src/test_main.cpp b/ros/urlab_ros_ws/src/test_main.cpp new file mode 100644 index 00000000..1ccd73c4 --- /dev/null +++ b/ros/urlab_ros_ws/src/test_main.cpp @@ -0,0 +1,464 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Standalone harness that drives the whole UrlabRclCore API and self-verifies. +// It lives inside the ROS workspace, so it is allowed to use rcl directly for +// the loopback probe that exercises the subscription leg; the core-side sub path +// stays the code under test. +// +// Modes: +// (default) / --selftest : create every publisher and subscription, then run +// an in-process loopback that publishes a deterministic Float64MultiArray +// and Twist to the core's own subscriptions and asserts the callbacks fire +// with the exact values. +// --publish N : publish N rounds of deterministic values on every +// publisher at 50 Hz for an external `ros2 topic echo` cross-check. +// +// Exit 0 = pass, non-zero = failure. + +#include "UrlabRclCore.h" + +#include +#include +#include +#include +#include + +#if defined(_WIN32) +#include +#include // timeBeginPeriod / timeEndPeriod (link winmm) +#endif + +#include +#include +#include +#include +#include + +namespace +{ +constexpr const char* kCtrlTopic = "/urlab_test/cmd_ctrl"; +constexpr const char* kTwistTopic = "/urlab_test/cmd_vel"; +constexpr int64_t kSpinTimeoutNs = 10 * 1000 * 1000; // 10 ms + +// Callback capture targets, populated by the core's subscription callbacks. +struct FCtrlCapture +{ + bool bReceived = false; + int32_t Count = 0; + double Values[16] = {0.0}; +}; + +struct FTwistCapture +{ + bool bReceived = false; + double Linear[3] = {0.0, 0.0, 0.0}; + double Angular[3] = {0.0, 0.0, 0.0}; +}; + +void OnCtrl(const double* Values, int32_t Count, void* User) +{ + FCtrlCapture* Cap = static_cast(User); + Cap->bReceived = true; + Cap->Count = Count; + const int32_t N = Count < 16 ? Count : 16; + for (int32_t i = 0; i < N; ++i) + { + Cap->Values[i] = Values[i]; + } +} + +void OnTwist(const double Linear[3], const double Angular[3], void* User) +{ + FTwistCapture* Cap = static_cast(User); + Cap->bReceived = true; + for (int i = 0; i < 3; ++i) + { + Cap->Linear[i] = Linear[i]; + Cap->Angular[i] = Angular[i]; + } +} + +bool NearlyEqual(double A, double B) +{ + const double D = A - B; + return (D < 1e-9) && (D > -1e-9); +} + +// Every core handle the harness creates, so both modes share one setup/teardown. +struct FHarness +{ + UrlabRclContext* Ctx = nullptr; + UrlabRclJointStatePub* JointState = nullptr; + UrlabRclImuPub* Imu = nullptr; + UrlabRclTfPub* Tf = nullptr; + UrlabRclTfPub* TfStatic = nullptr; + UrlabRclTwistStampedPub* TwistStamped = nullptr; + UrlabRclClockPub* Clock = nullptr; + UrlabRclImagePub* Image = nullptr; + UrlabRclCtrlSub* CtrlSub = nullptr; + UrlabRclTwistSub* TwistSub = nullptr; + FCtrlCapture CtrlCapture; + FTwistCapture TwistCapture; +}; + +bool CreateHarness(FHarness& H) +{ + H.Ctx = UrlabRcl_Init("urlab_test", "", -1); + if (!H.Ctx) + { + std::fprintf(stderr, "UrlabRcl_Init failed: %s\n", UrlabRcl_LastError()); + return false; + } + std::printf("distro: %s\n", UrlabRcl_DistroName()); + + const char* JointNames[3] = {"joint_a", "joint_b", "joint_c"}; + H.JointState = UrlabRcl_CreateJointStatePub(H.Ctx, + "/urlab_test/joint_states", JointNames, 3); + H.Imu = UrlabRcl_CreateImuPub(H.Ctx, "/urlab_test/imu", "urlab_test/imu"); + H.Tf = UrlabRcl_CreateTfPub(H.Ctx, 0); + H.TfStatic = UrlabRcl_CreateTfPub(H.Ctx, 1); + H.TwistStamped = UrlabRcl_CreateTwistStampedPub(H.Ctx, + "/urlab_test/twist", "urlab_test/base"); + H.Clock = UrlabRcl_CreateClockPub(H.Ctx); + H.Image = UrlabRcl_CreateImagePub(H.Ctx, "/urlab_test/image", + "urlab_test/camera", 4, 4, "rgb8"); + H.CtrlSub = UrlabRcl_CreateCtrlSub(H.Ctx, kCtrlTopic, &OnCtrl, &H.CtrlCapture); + H.TwistSub = UrlabRcl_CreateTwistSub(H.Ctx, kTwistTopic, &OnTwist, &H.TwistCapture); + + if (!H.JointState || !H.Imu || !H.Tf || !H.TfStatic || !H.TwistStamped || + !H.Clock || !H.Image || !H.CtrlSub || !H.TwistSub) + { + std::fprintf(stderr, "handle creation failed: %s\n", UrlabRcl_LastError()); + return false; + } + return true; +} + +void DestroyHarness(FHarness& H) +{ + UrlabRcl_DestroyCtrlSub(H.CtrlSub); + UrlabRcl_DestroyTwistSub(H.TwistSub); + UrlabRcl_DestroyImagePub(H.Image); + UrlabRcl_DestroyClockPub(H.Clock); + UrlabRcl_DestroyTwistStampedPub(H.TwistStamped); + UrlabRcl_DestroyTfPub(H.TfStatic); + UrlabRcl_DestroyTfPub(H.Tf); + UrlabRcl_DestroyImuPub(H.Imu); + UrlabRcl_DestroyJointStatePub(H.JointState); + UrlabRcl_Shutdown(H.Ctx); +} + +int PublishRound(FHarness& H, int32_t Round, int64_t SimTimeNs) +{ + const double Positions[3] = {1.0, 2.0, 3.0}; + const double Velocities[3] = {0.1, 0.2, 0.3}; + const double Efforts[3] = {10.0, 20.0, 30.0}; + if (UrlabRcl_PublishJointState(H.JointState, Positions, Velocities, Efforts, 3, SimTimeNs) != 0) + { + std::fprintf(stderr, "PublishJointState failed: %s\n", UrlabRcl_LastError()); + return 1; + } + + const double AngularVel[3] = {0.01, 0.02, 0.03}; + const double LinearAccel[3] = {0.0, 0.0, 9.81}; + const double OrientationXyzw[4] = {0.0, 0.0, 0.0, 1.0}; + UrlabRcl_PublishImu(H.Imu, AngularVel, LinearAccel, OrientationXyzw, SimTimeNs); + + const char* Parents[1] = {"world"}; + const char* Children[1] = {"urlab_test/base"}; + const double Translation[3] = {static_cast(Round) * 0.001, 0.0, 0.5}; + const double Rotation[4] = {0.0, 0.0, 0.0, 1.0}; + UrlabRcl_PublishTf(H.Tf, Parents, Children, Translation, Rotation, 1, SimTimeNs); + + const char* StaticParents[1] = {"urlab_test/base"}; + const char* StaticChildren[1] = {"urlab_test/imu"}; + const double StaticTranslation[3] = {0.0, 0.0, 0.1}; + const double StaticRotation[4] = {0.0, 0.0, 0.0, 1.0}; + UrlabRcl_PublishTf(H.TfStatic, StaticParents, StaticChildren, + StaticTranslation, StaticRotation, 1, SimTimeNs); + + const double Linear[3] = {0.5, 0.0, 0.0}; + const double Angular[3] = {0.0, 0.0, 0.2}; + UrlabRcl_PublishTwistStamped(H.TwistStamped, Linear, Angular, SimTimeNs); + + UrlabRcl_PublishClock(H.Clock, SimTimeNs); + + uint8_t Pixels[4 * 4 * 3]; + for (int i = 0; i < 4 * 4 * 3; ++i) + { + Pixels[i] = static_cast(i); + } + UrlabRcl_PublishImage(H.Image, Pixels, 4 * 3, SimTimeNs); + + return 0; +} + +// A minimal rcl publisher pair used only by the selftest to feed the core's own +// subscriptions from within this process (loopback over DDS). +struct FProbe +{ + rcl_context_t Context = rcl_get_zero_initialized_context(); + rcl_init_options_t InitOptions = rcl_get_zero_initialized_init_options(); + rcl_node_t Node = rcl_get_zero_initialized_node(); + rcl_publisher_t CtrlPub = rcl_get_zero_initialized_publisher(); + rcl_publisher_t TwistPub = rcl_get_zero_initialized_publisher(); + bool bValid = false; +}; + +bool ProbeInit(FProbe& P) +{ + rcl_allocator_t Allocator = rcl_get_default_allocator(); + if (rcl_init_options_init(&P.InitOptions, Allocator) != RCL_RET_OK) + { + return false; + } + if (rcl_init(0, nullptr, &P.InitOptions, &P.Context) != RCL_RET_OK) + { + return false; + } + rcl_node_options_t NodeOptions = rcl_node_get_default_options(); + if (rcl_node_init(&P.Node, "urlab_test_probe", "", &P.Context, &NodeOptions) != RCL_RET_OK) + { + return false; + } + + rcl_publisher_options_t PubOptions = rcl_publisher_get_default_options(); + const rosidl_message_type_support_t* CtrlTs = + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float64MultiArray); + if (rcl_publisher_init(&P.CtrlPub, &P.Node, CtrlTs, kCtrlTopic, &PubOptions) != RCL_RET_OK) + { + return false; + } + const rosidl_message_type_support_t* TwistTs = + ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist); + if (rcl_publisher_init(&P.TwistPub, &P.Node, TwistTs, kTwistTopic, &PubOptions) != RCL_RET_OK) + { + return false; + } + P.bValid = true; + return true; +} + +void ProbeShutdown(FProbe& P) +{ + rcl_publisher_fini(&P.TwistPub, &P.Node); + rcl_publisher_fini(&P.CtrlPub, &P.Node); + rcl_node_fini(&P.Node); + rcl_shutdown(&P.Context); + rcl_context_fini(&P.Context); + rcl_init_options_fini(&P.InitOptions); +} + +int ProbePublishCtrl(FProbe& P, const double* Values, int32_t Count) +{ + std_msgs__msg__Float64MultiArray Msg; + std_msgs__msg__Float64MultiArray__init(&Msg); + rosidl_runtime_c__double__Sequence__init(&Msg.data, Count); + for (int32_t i = 0; i < Count; ++i) + { + Msg.data.data[i] = Values[i]; + } + const rcl_ret_t Ret = rcl_publish(&P.CtrlPub, &Msg, nullptr); + std_msgs__msg__Float64MultiArray__fini(&Msg); + return Ret == RCL_RET_OK ? 0 : 1; +} + +int ProbePublishTwist(FProbe& P, const double Linear[3], const double Angular[3]) +{ + geometry_msgs__msg__Twist Msg; + geometry_msgs__msg__Twist__init(&Msg); + Msg.linear.x = Linear[0]; + Msg.linear.y = Linear[1]; + Msg.linear.z = Linear[2]; + Msg.angular.x = Angular[0]; + Msg.angular.y = Angular[1]; + Msg.angular.z = Angular[2]; + const rcl_ret_t Ret = rcl_publish(&P.TwistPub, &Msg, nullptr); + geometry_msgs__msg__Twist__fini(&Msg); + return Ret == RCL_RET_OK ? 0 : 1; +} + +int RunSelftest() +{ + FHarness H; + if (!CreateHarness(H)) + { + return 1; + } + + FProbe Probe; + if (!ProbeInit(Probe)) + { + std::fprintf(stderr, "probe init failed\n"); + DestroyHarness(H); + return 1; + } + + const double CtrlValues[4] = {1.5, 2.5, 3.5, 4.5}; + const double ProbeLinear[3] = {0.11, 0.22, 0.33}; + const double ProbeAngular[3] = {0.44, 0.55, 0.66}; + + // Publish and spin repeatedly; DDS discovery between the probe and the core + // subscriptions can take a moment on the first sample. + int Result = 1; + for (int Attempt = 0; Attempt < 300; ++Attempt) + { + ProbePublishCtrl(Probe, CtrlValues, 4); + ProbePublishTwist(Probe, ProbeLinear, ProbeAngular); + UrlabRcl_SpinSome(H.Ctx, kSpinTimeoutNs); + if (H.CtrlCapture.bReceived && H.TwistCapture.bReceived) + { + Result = 0; + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (Result != 0) + { + std::fprintf(stderr, "selftest: subscription callbacks did not fire " + "(ctrl=%d twist=%d)\n", H.CtrlCapture.bReceived ? 1 : 0, + H.TwistCapture.bReceived ? 1 : 0); + } + else + { + if (H.CtrlCapture.Count != 4) + { + std::fprintf(stderr, "selftest: ctrl count %d != 4\n", H.CtrlCapture.Count); + Result = 1; + } + for (int i = 0; i < 4 && Result == 0; ++i) + { + if (!NearlyEqual(H.CtrlCapture.Values[i], CtrlValues[i])) + { + std::fprintf(stderr, "selftest: ctrl[%d] %f != %f\n", + i, H.CtrlCapture.Values[i], CtrlValues[i]); + Result = 1; + } + } + for (int i = 0; i < 3 && Result == 0; ++i) + { + if (!NearlyEqual(H.TwistCapture.Linear[i], ProbeLinear[i]) || + !NearlyEqual(H.TwistCapture.Angular[i], ProbeAngular[i])) + { + std::fprintf(stderr, "selftest: twist component %d mismatch\n", i); + Result = 1; + } + } + } + + if (Result == 0) + { + std::printf("selftest: OK (ctrl + twist loopback verified)\n"); + } + + ProbeShutdown(Probe); + DestroyHarness(H); + return Result; +} + +int RunPublish(int32_t Rounds) +{ + FHarness H; + if (!CreateHarness(H)) + { + return 1; + } + + const int64_t StepNs = 20 * 1000 * 1000; // 50 Hz + int64_t SimTimeNs = 0; + int Result = 0; +#if defined(_WIN32) + // Windows' default ~15.6 ms scheduler tick rounds a 20 ms sleep up to ~31 ms + // (~32 Hz). Request 1 ms timer resolution so the 50 Hz cadence is accurate. + timeBeginPeriod(1); +#endif + const auto Start = std::chrono::steady_clock::now(); + for (int32_t r = 0; r < Rounds; ++r) + { + if (PublishRound(H, r, SimTimeNs) != 0) + { + Result = 1; + break; + } + UrlabRcl_SpinSome(H.Ctx, 0); + SimTimeNs += StepNs; + // Pace on a fixed cadence relative to Start so per-round publish cost + // does not accumulate into drift. + std::this_thread::sleep_until(Start + std::chrono::milliseconds(20) * (r + 1)); + } +#if defined(_WIN32) + timeEndPeriod(1); +#endif + + if (Result == 0) + { + std::printf("publish: OK (%d rounds)\n", Rounds); + } + DestroyHarness(H); + return Result; +} + +int RunReinitRoundTrip() +{ + UrlabRclContext* Ctx = UrlabRcl_Init("urlab_test_reinit", "", -1); + if (!Ctx) + { + std::fprintf(stderr, "re-init round trip failed: %s\n", UrlabRcl_LastError()); + return 1; + } + UrlabRcl_Shutdown(Ctx); + std::printf("reinit: OK\n"); + return 0; +} +} // namespace + +int main(int argc, char** argv) +{ + int32_t PublishRounds = -1; + for (int i = 1; i < argc; ++i) + { + if (std::strcmp(argv[i], "--publish") == 0 && i + 1 < argc) + { + PublishRounds = std::atoi(argv[i + 1]); + ++i; + } + } + + int Result; + if (PublishRounds >= 0) + { + Result = RunPublish(PublishRounds); + } + else + { + Result = RunSelftest(); + } + + if (Result == 0) + { + Result = RunReinitRoundTrip(); + } + + if (Result == 0) + { + std::printf("urlab_rcl_test: PASS\n"); + } + else + { + std::printf("urlab_rcl_test: FAIL\n"); + } + return Result; +} From ffe691b03e2071d3baa8096853ec9f52404aa43b Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:39 +0100 Subject: [PATCH 07/32] Plan and execute for the Franka through MoveIt URDF export with an auto-generated SRDF, the simulated world published as a planning scene, a trajectory execution bridge, gripper control and a pick pipeline. --- ros/urlab_moveit/README.md | 76 ++++++ ros/urlab_moveit/config/franka.srdf | 64 +++++ ros/urlab_moveit/config/joint_limits.yaml | 43 +++ ros/urlab_moveit/config/kinematics.yaml | 4 + .../config/moveit_controllers.yaml | 31 +++ ros/urlab_moveit/config/ompl_planning.yaml | 27 ++ .../launch/franka_moveit.launch.py | 160 ++++++++++++ ros/urlab_moveit/rviz/moveit.rviz | 66 +++++ ros/urlab_moveit/scripts/generate_srdf.py | 247 ++++++++++++++++++ ros/urlab_moveit/scripts/gripper_bridge.py | 114 ++++++++ ros/urlab_moveit/scripts/pick_bottle.py | 158 +++++++++++ ros/urlab_moveit/scripts/trajectory_bridge.py | 127 +++++++++ 12 files changed, 1117 insertions(+) create mode 100644 ros/urlab_moveit/README.md create mode 100644 ros/urlab_moveit/config/franka.srdf create mode 100644 ros/urlab_moveit/config/joint_limits.yaml create mode 100644 ros/urlab_moveit/config/kinematics.yaml create mode 100644 ros/urlab_moveit/config/moveit_controllers.yaml create mode 100644 ros/urlab_moveit/config/ompl_planning.yaml create mode 100644 ros/urlab_moveit/launch/franka_moveit.launch.py create mode 100644 ros/urlab_moveit/rviz/moveit.rviz create mode 100644 ros/urlab_moveit/scripts/generate_srdf.py create mode 100644 ros/urlab_moveit/scripts/gripper_bridge.py create mode 100644 ros/urlab_moveit/scripts/pick_bottle.py create mode 100644 ros/urlab_moveit/scripts/trajectory_bridge.py diff --git a/ros/urlab_moveit/README.md b/ros/urlab_moveit/README.md new file mode 100644 index 00000000..3da616df --- /dev/null +++ b/ros/urlab_moveit/README.md @@ -0,0 +1,76 @@ +# urlab_moveit — MoveIt planning for the URLab Franka + +Motion-plan the arm in rviz and execute the trajectory on the running URLab sim. +Uses stock MoveIt; the only custom piece is a small FollowJointTrajectory bridge +that streams planned points onto `//joint_command` (so no ros2_control stack +is needed on the sim side). + +## Layout + +``` +config/ + franka.srdf auto-generated (MuJoCo-sampled disable-collisions + keyframe states) + kinematics.yaml KDL IK for the panda_arm group + joint_limits.yaml velocity (URDF) + acceleration limits for time-parameterization + ompl_planning.yaml OMPL planners (RRTConnect default) + moveit_controllers.yaml maps panda_arm -> FollowJointTrajectory +scripts/ + generate_srdf.py regenerate the SRDF from a MuJoCo model + trajectory_bridge.py FollowJointTrajectory -> //joint_command +launch/ + franka_moveit.launch.py move_group + rsp + static tf + bridge + rviz +rviz/ + moveit.rviz MotionPlanning display +``` + +## Prerequisite (one-time, user runs) + +MoveIt is not in the env yet. Add it to the pixi ROS env: + +``` +pixi add --manifest-path C:\dev\urlab_ros2_env\pixi.toml ros-lyrical-moveit +``` + +(That metapackage pulls `move_group`, the OMPL planner, KDL kinematics, and the +rviz MotionPlanning plugin.) + +## Regenerate the SRDF (optional; already committed) + +Run under the bridge env (`uv run`), pointing at the MuJoCo model whose link +names match the exported URDF: + +``` +uv run python scripts/generate_srdf.py \ + --xml C:\dev\menagerie\franka_emika_panda\panda_ros_demo.xml \ + --out config\franka.srdf --samples 20000 +``` + +Disabled pairs come from real MuJoCo collision sampling (Adjacent / Default / +Always / Never), so they match the physics rather than an approximate mesh check. + +## Bring-up + +1. Start the sim in Live mode (imports the Franka, unpauses, streams). From the + bridge repo: + ``` + uv run python /m1_sensors_setup.py + ``` +2. Launch MoveIt (path args must be Windows 8.3 short paths because the project + path contains a space): + ``` + pixi run --manifest-path C:\dev\urlab_ros2_env\pixi.toml ros2 launch \ + \launch\franka_moveit.launch.py urdf:=\franka\model.urdf + ``` +3. In rviz's **MotionPlanning** panel: drag the goal marker (or pick the `home` + named state), **Plan**, then **Plan & Execute** — the arm follows the + trajectory in the UE sim. + +## Notes + +- URDF link names are bare (`link0..link7`, `hand`); the launch anchors them + under `world` with a static transform and feeds joint state from + `//joint_states`. +- Everything runs on **sim time** (`/clock`), so trajectory timing matches the + (possibly non-real-time) sim. +- The gripper (`finger_joint1/2`) is tendon-driven with no direct joint actuator, + so it is not a MoveIt execution group yet — arm planning only for now. diff --git a/ros/urlab_moveit/config/franka.srdf b/ros/urlab_moveit/config/franka.srdf new file mode 100644 index 00000000..14e2dc5b --- /dev/null +++ b/ros/urlab_moveit/config/franka.srdf @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros/urlab_moveit/config/joint_limits.yaml b/ros/urlab_moveit/config/joint_limits.yaml new file mode 100644 index 00000000..b541f439 --- /dev/null +++ b/ros/urlab_moveit/config/joint_limits.yaml @@ -0,0 +1,43 @@ +# Velocity limits mirror the exported URDF (3.14 rad/s on every joint); +# acceleration limits are added because MoveIt's time-parameterization needs +# them and the URDF has none. Conservative values keep executed trajectories +# smooth on the sim. +default_velocity_scaling_factor: 0.3 +default_acceleration_scaling_factor: 0.3 + +joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 3.75 + joint2: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.875 + joint3: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 2.5 + joint4: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 3.125 + joint5: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 3.75 + joint6: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 + joint7: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 5.0 diff --git a/ros/urlab_moveit/config/kinematics.yaml b/ros/urlab_moveit/config/kinematics.yaml new file mode 100644 index 00000000..e721385b --- /dev/null +++ b/ros/urlab_moveit/config/kinematics.yaml @@ -0,0 +1,4 @@ +panda_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.05 diff --git a/ros/urlab_moveit/config/moveit_controllers.yaml b/ros/urlab_moveit/config/moveit_controllers.yaml new file mode 100644 index 00000000..229beb1f --- /dev/null +++ b/ros/urlab_moveit/config/moveit_controllers.yaml @@ -0,0 +1,31 @@ +# MoveIt executes planned trajectories through a FollowJointTrajectory action. +# The urlab trajectory_bridge node serves that action and streams each point's +# joint positions onto //joint_command (which URLab applies as control), +# so no ros2_control stack is needed on the sim side. +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - panda_arm_controller + - hand_controller + panda_arm_controller: + type: FollowJointTrajectory + action_ns: follow_joint_trajectory + default: true + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 + # The gripper is one tendon actuator; the urlab gripper bridge serves this + # GripperCommand action and maps the commanded finger opening onto that actuator. + hand_controller: + type: GripperCommand + action_ns: gripper_cmd + default: true + joints: + - finger_joint1 + command_joint: finger_joint1 diff --git a/ros/urlab_moveit/config/ompl_planning.yaml b/ros/urlab_moveit/config/ompl_planning.yaml new file mode 100644 index 00000000..9c21b0c7 --- /dev/null +++ b/ros/urlab_moveit/config/ompl_planning.yaml @@ -0,0 +1,27 @@ +planning_plugins: + - ompl_interface/OMPLPlanner +request_adapters: + - default_planning_request_adapters/ResolveConstraintFrames + - default_planning_request_adapters/ValidateWorkspaceBounds + - default_planning_request_adapters/CheckStartStateBounds + - default_planning_request_adapters/CheckStartStateCollision +response_adapters: + - default_planning_response_adapters/AddTimeOptimalParameterization + - default_planning_response_adapters/ValidateSolution + - default_planning_response_adapters/DisplayMotionPath + +planner_configs: + RRTConnect: + type: geometric::RRTConnect + range: 0.0 + RRTstar: + type: geometric::RRTstar + range: 0.0 + goal_bias: 0.05 + +panda_arm: + planner_configs: + - RRTConnect + - RRTstar + projection_evaluator: joints(joint1,joint2) + longest_valid_segment_fraction: 0.005 diff --git a/ros/urlab_moveit/launch/franka_moveit.launch.py b/ros/urlab_moveit/launch/franka_moveit.launch.py new file mode 100644 index 00000000..600fbfed --- /dev/null +++ b/ros/urlab_moveit/launch/franka_moveit.launch.py @@ -0,0 +1,160 @@ +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# Licensed under the Apache License, Version 2.0. +"""Bring up MoveIt move_group + rviz MotionPlanning for the URLab Franka, with +trajectory execution streamed to the sim via the urlab trajectory bridge. + +Nodes: + move_group plans; loads URDF (urdf:=), SRDF, kinematics, + joint limits, OMPL, and the FollowJointTrajectory + controller mapping. Reads current state from + //joint_states (remapped). + robot_state_publisher URDF -> /tf from //joint_states. + static world->link0 anchors the (bare-link) URDF under the 'world' frame. + trajectory_bridge serves /panda_arm_controller/follow_joint_trajectory + and streams points to //joint_command. + rviz2 MotionPlanning display (plan + execute interactively). + +Requires MoveIt for the active ROS distro (e.g. `pixi add ros--moveit`). +The sim must be up in Live mode (the urlab Python bring-up) before executing. +""" +import os + +import yaml +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +_THIS = os.path.dirname(os.path.abspath(__file__)) +_CFG = os.path.normpath(os.path.join(_THIS, "..", "config")) +_RVIZ = os.path.normpath(os.path.join(_THIS, "..", "rviz", "moveit.rviz")) + + +def _load(name): + with open(os.path.join(_CFG, name), "r", encoding="utf-8") as f: + return yaml.safe_load(f) + + +def _read(path): + with open(path, "r", encoding="utf-8") as f: + return f.read() + + +def _setup(context, *args, **kwargs): + art = LaunchConfiguration("art").perform(context) + urdf_path = LaunchConfiguration("urdf").perform(context) + if not urdf_path: + raise RuntimeError("franka_moveit.launch.py requires 'urdf:='") + + robot_description = {"robot_description": _read(urdf_path)} + robot_description_semantic = { + "robot_description_semantic": _read(os.path.join(_CFG, "franka.srdf")) + } + kinematics = _load("kinematics.yaml") + joint_limits = {"robot_description_planning": _load("joint_limits.yaml")} + ompl = _load("ompl_planning.yaml") + controllers = _load("moveit_controllers.yaml") + + planning_pipeline = { + "planning_pipelines": ["ompl"], + "default_planning_pipeline": "ompl", + "ompl": ompl, + } + trajectory_execution = { + "moveit_manage_controllers": True, + "trajectory_execution.allowed_execution_duration_scaling": 2.0, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.05, + } + # MuJoCo's soft joint limits let joints transiently overshoot the URDF hard + # limits (e.g. the Franka's razor-thin joint4 upper bound), which MoveIt's + # CheckStartStateBounds rejects. Clamp marginal start-state violations instead + # of failing the plan; the tight URDF limits still bound the planned motion. + start_state = {"start_state_max_bounds_error": 0.1} + planning_scene_monitor = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + use_sim_time = {"use_sim_time": True} + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + {"robot_description_kinematics": kinematics}, + joint_limits, + planning_pipeline, + trajectory_execution, + start_state, + controllers, + planning_scene_monitor, + use_sim_time, + ], + # move_group reads current joint state from 'joint_states'; the sim + # publishes it namespaced. + remappings=[("joint_states", f"/{art}/joint_states")], + ) + + rsp = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description, use_sim_time], + remappings=[("joint_states", f"/{art}/joint_states")], + ) + + world_to_root = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="world_to_root", + arguments=["--frame-id", "world", "--child-frame-id", "link0", + "--x", "0", "--y", "0", "--z", "0"], + parameters=[use_sim_time], + output="screen", + ) + + bridge = _bridge_process(art) + gripper = _gripper_process(art) + + rviz = Node( + package="rviz2", + executable="rviz2", + output="screen", + arguments=["-d", _RVIZ], + parameters=[ + robot_description, + robot_description_semantic, + {"robot_description_kinematics": kinematics}, + planning_pipeline, + use_sim_time, + ], + ) + + return [move_group, rsp, world_to_root, bridge, gripper, rviz] + + +def _bridge_process(art): + # Run the trajectory bridge directly by path (urlab_moveit is a source tree, + # not an installed package), so `python trajectory_bridge.py` works anywhere. + from launch.actions import ExecuteProcess + script = os.path.normpath(os.path.join(_THIS, "..", "scripts", "trajectory_bridge.py")) + return ExecuteProcess(cmd=["python", script, f"--art={art}"], output="screen") + + +def _gripper_process(art): + from launch.actions import ExecuteProcess + script = os.path.normpath(os.path.join(_THIS, "..", "scripts", "gripper_bridge.py")) + return ExecuteProcess(cmd=["python", script, f"--art={art}"], output="screen") + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription([ + DeclareLaunchArgument("art", default_value="franka"), + DeclareLaunchArgument("urdf", default_value=""), + OpaqueFunction(function=_setup), + ]) diff --git a/ros/urlab_moveit/rviz/moveit.rviz b/ros/urlab_moveit/rviz/moveit.rviz new file mode 100644 index 00000000..aa4f10a5 --- /dev/null +++ b/ros/urlab_moveit/rviz/moveit.rviz @@ -0,0 +1,66 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Enabled: true + Cell Size: 1 + Plane Cell Count: 10 + Color: 160; 160; 164 + Reference Frame: + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Enabled: true + Move Group Namespace: "" + Robot Description: robot_description + Planning Scene Topic: /monitored_planning_scene + Planning Request: + Planning Group: panda_arm + Query Start State: false + Query Goal State: true + Interactive Marker Size: 0 + Planned Path: + Trajectory Topic: /display_planned_path + State Display Time: 0.05 s + Loop Animation: false + Show Robot Visual: true + Show Trail: false + Scene Geometry: + Scene Alpha: 0.9 + Scene Robot: + Show Robot Visual: true + - Class: rviz_default_plugins/TF + Name: TF + Enabled: false + Marker Scale: 0.3 + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Tools: + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/Measure + Views: + Current: + Class: rviz_default_plugins/Orbit + Name: Current View + Distance: 2.5 + Focal Point: + X: 0 + Y: 0 + Z: 0.4 + Pitch: 0.5 + Yaw: 0.8 + Target Frame: + Saved: ~ +Window Geometry: + Height: 900 + Width: 1400 + Displays: + collapsed: false diff --git a/ros/urlab_moveit/scripts/generate_srdf.py b/ros/urlab_moveit/scripts/generate_srdf.py new file mode 100644 index 00000000..7166c8ce --- /dev/null +++ b/ros/urlab_moveit/scripts/generate_srdf.py @@ -0,0 +1,247 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# Licensed under the Apache License, Version 2.0. +"""Auto-generate a MoveIt SRDF for a MuJoCo arm by *sampling actual collisions*. + +This is the MoveIt Setup Assistant's disable-collisions algorithm, but the +collision truth comes from MuJoCo's own broadphase/narrowphase on the compiled +model (the same geometry the sim steps), so the disabled pairs match the physics +exactly instead of an approximate URDF mesh check. + +For every ordered link pair we decide whether self-collision checking can be +disabled, and record the reason MoveIt uses: + - Adjacent : linked by a joint (never a meaningful self-collision). + - Default : in collision in the model's default/home pose. + - Always : in collision in every sampled configuration (overlapping). + - Never : in collision in none of the sampled configurations. +Pairs that collide in *some* configurations keep collision checking on. + +Groups and named states are derived from the model: the arm is the chain of +1-DoF joints from the base to the flange; named states come from frames. + +Usage: + python generate_srdf.py --xml --out \ + [--samples 20000] [--arm-group panda_arm] [--seed 0] +""" +import argparse +import itertools +import sys +import xml.etree.ElementTree as ET + +import numpy as np + +try: + import mujoco +except ImportError: + sys.exit("mujoco is required (run under the bridge env: `uv run python ...`)") + + +def link_of_geom(m, gid): + """URDF link (MuJoCo body) name a geom belongs to, or None for worldbody.""" + bid = m.geom_bodyid[gid] + if bid == 0: + return None + return mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, bid) + + +def body_link_names(m): + """All non-world body names, in id order (these are the URDF link names).""" + return [ + mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, b) + for b in range(1, m.nbody) + ] + + +def adjacent_pairs(m): + """Link pairs connected directly by a joint (parent/child body).""" + pairs = set() + for b in range(1, m.nbody): + p = m.body_parentid[b] + if p == 0: + continue + a = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, b) + c = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_BODY, p) + pairs.add(frozenset((a, c))) + return pairs + + +def colliding_link_pairs(m, d): + """Set of link pairs currently in contact (maps active contacts to links).""" + out = set() + for i in range(d.ncon): + c = d.contact[i] + la = link_of_geom(m, c.geom1) + lb = link_of_geom(m, c.geom2) + if la and lb and la != lb: + out.add(frozenset((la, lb))) + return out + + +def sample_qpos(m, rng): + """Random qpos within joint ranges (limited joints) / [-pi, pi] (free hinges).""" + q = m.qpos0.copy() + for j in range(m.njnt): + jtype = m.jnt_type[j] + if jtype not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE): + continue # free/ball: leave at qpos0 (base stays put) + adr = m.jnt_qposadr[j] + if m.jnt_limited[j]: + lo, hi = m.jnt_range[j] + else: + lo, hi = -np.pi, np.pi + q[adr] = rng.uniform(lo, hi) + return q + + +def keyframe_states(m): + """{name: {joint_name: qpos}} for each , 1-DoF joints only.""" + states = {} + for k in range(m.nkey): + name = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_KEY, k) or f"key{k}" + qpos = m.key_qpos[k] + joints = {} + for j in range(m.njnt): + if m.jnt_type[j] not in (mujoco.mjtJoint.mjJNT_HINGE, mujoco.mjtJoint.mjJNT_SLIDE): + continue + jn = mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, j) + joints[jn] = float(qpos[m.jnt_qposadr[j]]) + states[name] = joints + return states + + +def arm_joint_chain(m): + """1-DoF joint names from base to flange, in kinematic order (the arm).""" + names = [] + for j in range(m.njnt): + if m.jnt_type[j] != mujoco.mjtJoint.mjJNT_HINGE: + continue + names.append(mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, j)) + return names + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--xml", required=True) + ap.add_argument("--out", required=True) + ap.add_argument("--robot-name", default="franka") + ap.add_argument("--arm-group", default="panda_arm") + ap.add_argument("--hand-group", default="hand") + ap.add_argument("--hand-joints", default="finger_joint1,finger_joint2") + ap.add_argument("--finger-open", type=float, default=0.04) + ap.add_argument("--base-link", default="link0") + ap.add_argument("--flange-link", default="hand") + ap.add_argument("--samples", type=int, default=20000) + ap.add_argument("--passive-joints", default="", + help="comma-separated joint names to exclude from arm group") + ap.add_argument("--seed", type=int, default=42) + args = ap.parse_args() + + m = mujoco.MjModel.from_xml_path(args.xml) + d = mujoco.MjData(m) + rng = np.random.default_rng(args.seed) + + links = body_link_names(m) + all_pairs = {frozenset(p) for p in itertools.combinations(links, 2)} + adj = adjacent_pairs(m) + + # Default pose collisions. + mujoco.mj_forward(m, d) + default_col = colliding_link_pairs(m, d) + + # Sample: count how often each pair collides. + ever = set() + count = {p: 0 for p in all_pairs} + for _ in range(args.samples): + d.qpos[:] = sample_qpos(m, rng) + mujoco.mj_forward(m, d) + for p in colliding_link_pairs(m, d): + if p in count: + count[p] += 1 + ever.add(p) + + # Classify each pair -> (disabled?, reason). + disabled = [] # (linkA, linkB, reason) + for p in sorted(all_pairs, key=lambda s: sorted(s)): + a, b = sorted(p) + if p in adj: + disabled.append((a, b, "Adjacent")) + elif p in default_col: + disabled.append((a, b, "Default")) + elif count[p] >= args.samples: + disabled.append((a, b, "Always")) + elif count[p] == 0: + disabled.append((a, b, "Never")) + # else: sometimes collides -> keep checking + + # Auto-detect virtual joint type from the base link's joint. + vtype = "fixed" + bid = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, args.base_link) + if bid >= 0 and m.body_jntnum[bid] > 0: + if m.jnt_type[m.body_jntadr[bid]] == mujoco.mjtJoint.mjJNT_FREE: + vtype = "floating" + + # Build active arm joint list (exclude passive joints). + passive = set(args.passive_joints.split(",")) if args.passive_joints else set() + arm_joints_all = arm_joint_chain(m) + arm_joints_active = [j for j in arm_joints_all if j not in passive] + + # --- Emit SRDF --- + robot = ET.Element("robot", name=args.robot_name) + ET.SubElement(robot, "virtual_joint", name="virtual_joint", type=vtype, + parent_frame="world", child_link=args.base_link) + + arm = ET.SubElement(robot, "group", name=args.arm_group) + if passive: + for j in arm_joints_active: + ET.SubElement(arm, "joint", name=j) + else: + ET.SubElement(arm, "chain", base_link=args.base_link, tip_link=args.flange_link) + + # Gripper group + end-effector. The finger joints are tendon-coupled in the + # sim (one actuator drives both); MoveIt treats them as the hand group and the + # gripper bridge maps the commanded opening onto that actuator. + hand_joints = [j for j in args.hand_joints.split(",") if j] + if hand_joints: + hand = ET.SubElement(robot, "group", name=args.hand_group) + for j in hand_joints: + ET.SubElement(hand, "joint", name=j) + ET.SubElement(robot, "end_effector", name="hand_ee", + parent_link=args.flange_link, group=args.hand_group, + parent_group=args.arm_group) + for state, val in (("open", args.finger_open), ("closed", 0.0)): + gs = ET.SubElement(robot, "group_state", name=state, group=args.hand_group) + for j in hand_joints: + ET.SubElement(gs, "joint", name=j, value=f"{val:.6g}") + + # Named states from keyframes (active arm joints only). + arm_joints = set(arm_joints_active) + for kname, joints in keyframe_states(m).items(): + gs = ET.SubElement(robot, "group_state", name=kname, group=args.arm_group) + for jn, val in joints.items(): + if jn in arm_joints: + ET.SubElement(gs, "joint", name=jn, value=f"{val:.6g}") + + for a, b, reason in disabled: + ET.SubElement(robot, "disable_collisions", link1=a, link2=b, reason=reason) + + ET.indent(robot, space=" ") + xml = '\n' + ET.tostring(robot, encoding="unicode") + with open(args.out, "w", encoding="utf-8") as f: + f.write(xml + "\n") + + kept = len(all_pairs) - len(disabled) + by_reason = {} + for _, _, r in disabled: + by_reason[r] = by_reason.get(r, 0) + 1 + print(f"Collision pairs summary:") + print(f" Total pairs checked: {len(all_pairs)}") + print(f" Pairs kept (self-collision checked): {kept}") + print(f" Pairs disabled: {len(disabled)}") + for reason in ("Adjacent", "Default", "Always", "Never"): + if reason in by_reason: + print(f" {reason}: {by_reason[reason]}") + print(f"wrote {args.out}") + + +if __name__ == "__main__": + main() diff --git a/ros/urlab_moveit/scripts/gripper_bridge.py b/ros/urlab_moveit/scripts/gripper_bridge.py new file mode 100644 index 00000000..43a52013 --- /dev/null +++ b/ros/urlab_moveit/scripts/gripper_bridge.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# Licensed under the Apache License, Version 2.0. +"""GripperCommand -> URLab gripper bridge. + +The Franka gripper is a single tendon actuator (ctrl 0..255) that drives both +fingers symmetrically; there is no per-finger joint actuator. MoveIt controls the +'hand' group through a GripperCommand action (target finger opening, metres). +This node serves that action and maps the opening onto the tendon actuator's +ctrl, published on //joint_command by the actuator's own name (URLab now +resolves a joint_command entry to an actuator by name, not only by driven joint). + + ctrl = clamp(position, 0, finger_max) / finger_max * ctrl_max + +Runs on a MultiThreadedExecutor and claims control (ros:urlab) so writes land. +""" +import sys +import time + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + +from control_msgs.action import GripperCommand +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger + + +class GripperBridge(Node): + def __init__(self, art, actuator, finger_max, ctrl_max): + super().__init__("urlab_gripper_bridge", + parameter_overrides=[rclpy.parameter.Parameter("use_sim_time", value=True)]) + self.art = art + self.actuator = actuator + self.finger_max = finger_max + self.ctrl_max = ctrl_max + self._cb = ReentrantCallbackGroup() + self._cmd = self.create_publisher(JointState, f"/{art}/joint_command", 10) + self._claim_cli = self.create_client( + Trigger, f"/{art}/claim_control", callback_group=self._cb) + self._server = ActionServer( + self, GripperCommand, "/hand_controller/gripper_cmd", + execute_callback=self._execute, + goal_callback=lambda _g: GoalResponse.ACCEPT, + cancel_callback=lambda _g: CancelResponse.ACCEPT, + callback_group=self._cb) + self.get_logger().info( + f"gripper bridge up: GripperCommand -> /{art}/joint_command '{actuator}'") + + def claim(self, timeout=2.0): + if not self._claim_cli.service_is_ready(): + if not self._claim_cli.wait_for_service(timeout_sec=timeout): + return + fut = self._claim_cli.call_async(Trigger.Request()) + t0 = time.time() + while not fut.done() and time.time() - t0 < timeout: + time.sleep(0.02) + + def _publish_ctrl(self, ctrl): + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = [self.actuator] + msg.position = [float(ctrl)] + self._cmd.publish(msg) + + def _execute(self, goal_handle): + self.claim() + pos = float(goal_handle.request.command.position) + pos = max(0.0, min(self.finger_max, pos)) + ctrl = pos / self.finger_max * self.ctrl_max if self.finger_max > 0 else 0.0 + self.get_logger().info(f"gripper: finger={pos:.4f} -> ctrl={ctrl:.1f}") + + # Hold the command briefly so the tendon servo settles. + for _ in range(30): + self._publish_ctrl(ctrl) + time.sleep(0.02) + + goal_handle.succeed() + result = GripperCommand.Result() + result.position = pos + result.reached_goal = True + result.stalled = False + return result + + +def main(): + art, actuator, finger_max, ctrl_max = "franka", "actuator8", 0.04, 255.0 + for a in sys.argv[1:]: + if a.startswith("--art="): + art = a.split("=", 1)[1] + elif a.startswith("--actuator="): + actuator = a.split("=", 1)[1] + elif a.startswith("--finger-max="): + finger_max = float(a.split("=", 1)[1]) + elif a.startswith("--ctrl-max="): + ctrl_max = float(a.split("=", 1)[1]) + rclpy.init() + node = GripperBridge(art, actuator, finger_max, ctrl_max) + node.claim(timeout=15.0) + ex = MultiThreadedExecutor(num_threads=3) + ex.add_node(node) + try: + ex.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros/urlab_moveit/scripts/pick_bottle.py b/ros/urlab_moveit/scripts/pick_bottle.py new file mode 100644 index 00000000..f9e74f2c --- /dev/null +++ b/ros/urlab_moveit/scripts/pick_bottle.py @@ -0,0 +1,158 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# Licensed under the Apache License, Version 2.0. +"""Scripted MoveIt pick of a scene object (the coke bottle) on the URLab Franka. + +Sequence: open gripper -> plan arm to a pre-grasp pose above the object -> +approach to the grasp pose -> close gripper -> attach the object to the hand +(so the planner carries it) -> lift. The arm goes through move_group (obstacle +aware, using the live planning scene); the gripper goes through the GripperCommand +bridge. The grasp pose is parametric so it can be tuned against the actual object. + +Run under the ROS env. The sim must be Live + the MoveIt stack (move_group, +trajectory + gripper bridges) up. Uses the object id the scene provider assigns. +""" +import sys +import time + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSProfile + +from control_msgs.action import GripperCommand +from geometry_msgs.msg import Point, Pose, Quaternion +from moveit_msgs.action import MoveGroup +from moveit_msgs.msg import (AttachedCollisionObject, CollisionObject, + Constraints, OrientationConstraint, PlanningScene, + PositionConstraint, PlanningOptions) +from shape_msgs.msg import SolidPrimitive + +ARM = "panda_arm" +HAND_LINK = "hand" +FRAME = "world" + +# Object + grasp geometry (world frame). Defaults target the coke bottle; tune +# via CLI. quat is xyzw for a top-down approach (hand z-axis pointing down). +OBJECT_ID = "Geom_0_0_1" +GRASP_XYZ = (0.70, -0.10, 0.12) +PREGRASP_DZ = 0.12 # pre-grasp / lift height above the grasp +GRASP_QUAT = (1.0, 0.0, 0.0, 0.0) # 180deg about X -> hand points down +OPEN, CLOSED = 0.04, 0.0 + + +class Pick(Node): + def __init__(self): + super().__init__("pick_bottle") + self.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=True)]) + self.move = ActionClient(self, MoveGroup, "/move_action") + self.grip = ActionClient(self, GripperCommand, "/hand_controller/gripper_cmd") + self.scene_pub = self.create_publisher(PlanningScene, "/planning_scene", QoSProfile(depth=1)) + + # --- arm --- + def pose_goal(self, xyz, quat, pos_tol=0.02, ori_tol=0.1): + c = Constraints() + pc = PositionConstraint() + pc.header.frame_id = FRAME + pc.link_name = HAND_LINK + pc.constraint_region.primitives.append( + SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[pos_tol])) + p = Pose(); p.position = Point(x=xyz[0], y=xyz[1], z=xyz[2]); p.orientation.w = 1.0 + pc.constraint_region.primitive_poses.append(p) + pc.weight = 1.0 + c.position_constraints.append(pc) + oc = OrientationConstraint() + oc.header.frame_id = FRAME + oc.link_name = HAND_LINK + oc.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + oc.absolute_x_axis_tolerance = ori_tol + oc.absolute_y_axis_tolerance = ori_tol + oc.absolute_z_axis_tolerance = ori_tol + oc.weight = 1.0 + c.orientation_constraints.append(oc) + return c + + def move_to(self, xyz, quat, label): + if not self.move.wait_for_server(timeout_sec=15.0): + print("move_action unavailable"); return False + req = MoveGroup.Goal() + req.request.group_name = ARM + req.request.num_planning_attempts = 10 + req.request.allowed_planning_time = 8.0 + req.request.max_velocity_scaling_factor = 0.2 + req.request.max_acceleration_scaling_factor = 0.2 + req.request.goal_constraints.append(self.pose_goal(xyz, quat)) + req.planning_options = PlanningOptions() + req.planning_options.plan_only = False + fut = self.move.send_goal_async(req) + rclpy.spin_until_future_complete(self, fut, timeout_sec=15.0) + gh = fut.result() + if not gh or not gh.accepted: + print(f"{label}: goal rejected"); return False + res = gh.get_result_async() + rclpy.spin_until_future_complete(self, res, timeout_sec=40.0) + code = res.result().result.error_code.val if res.result() else None + print(f"{label}: error_code={code} ({'OK' if code == 1 else 'FAIL'})") + return code == 1 + + # --- gripper --- + def gripper(self, position, label): + if not self.grip.wait_for_server(timeout_sec=10.0): + print("gripper action unavailable"); return False + g = GripperCommand.Goal() + g.command.position = float(position) + g.command.max_effort = 40.0 + fut = self.grip.send_goal_async(g) + rclpy.spin_until_future_complete(self, fut, timeout_sec=10.0) + gh = fut.result() + if not gh or not gh.accepted: + print(f"{label}: rejected"); return False + res = gh.get_result_async() + rclpy.spin_until_future_complete(self, res, timeout_sec=10.0) + print(f"{label}: done") + return True + + # --- attach object to the hand so the planner carries it --- + def attach(self, obj_id): + ps = PlanningScene(); ps.is_diff = True + aco = AttachedCollisionObject() + aco.link_name = HAND_LINK + aco.object.id = obj_id + aco.object.operation = CollisionObject.ADD + aco.touch_links = ["hand", "left_finger", "right_finger"] + ps.robot_state.attached_collision_objects.append(aco) + ps.robot_state.is_diff = True + # Remove the world copy so it is not double-counted. + rem = CollisionObject(); rem.id = obj_id; rem.operation = CollisionObject.REMOVE + ps.world.collision_objects.append(rem) + for _ in range(5): + self.scene_pub.publish(ps); time.sleep(0.1); rclpy.spin_once(self, timeout_sec=0.05) + print(f"attached '{obj_id}' to {HAND_LINK}") + + def run(self): + gx, gy, gz = GRASP_XYZ + pre = (gx, gy, gz + PREGRASP_DZ) + print("== PICK ==") + self.gripper(OPEN, "open") + if not self.move_to(pre, GRASP_QUAT, "pre-grasp"): + return + if not self.move_to((gx, gy, gz), GRASP_QUAT, "approach"): + return + self.gripper(CLOSED, "close") + self.attach(OBJECT_ID) + self.move_to(pre, GRASP_QUAT, "lift") + print("== PICK sequence complete ==") + + +def main(): + rclpy.init() + node = Pick() + try: + node.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/ros/urlab_moveit/scripts/trajectory_bridge.py b/ros/urlab_moveit/scripts/trajectory_bridge.py new file mode 100644 index 00000000..a33523ba --- /dev/null +++ b/ros/urlab_moveit/scripts/trajectory_bridge.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 +# Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +# Licensed under the Apache License, Version 2.0. +"""FollowJointTrajectory -> URLab bridge. + +MoveIt executes a planned trajectory by sending a FollowJointTrajectory action +to the controller named in moveit_controllers.yaml. This node serves that action +and streams each trajectory point's joint positions onto //joint_command +(the same topic the jog GUI uses), which URLab applies as position control in +Live mode. URLab maps the JointState.name[] to the actuators that drive those +joints, so no ros2_control stack is needed on the sim side. + +Runs on a MultiThreadedExecutor: the execute callback paces the trajectory in +its own thread (using sim time via /clock) while the executor keeps servicing +the clock and action interfaces. It claims control (ros:urlab) up front and +re-asserts the claim per goal so writes are always accepted. +""" +import sys +import time + +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.duration import Duration +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node + +from control_msgs.action import FollowJointTrajectory +from sensor_msgs.msg import JointState +from std_srvs.srv import Trigger + + +class TrajectoryBridge(Node): + def __init__(self, art: str): + super().__init__("urlab_trajectory_bridge", + parameter_overrides=[rclpy.parameter.Parameter("use_sim_time", value=True)]) + self.art = art + self._cb = ReentrantCallbackGroup() + self._cmd = self.create_publisher(JointState, f"/{art}/joint_command", 10) + self._claim_cli = self.create_client( + Trigger, f"/{art}/claim_control", callback_group=self._cb) + self._server = ActionServer( + self, + FollowJointTrajectory, + "/panda_arm_controller/follow_joint_trajectory", + execute_callback=self._execute, + goal_callback=lambda _g: GoalResponse.ACCEPT, + cancel_callback=lambda _g: CancelResponse.ACCEPT, + callback_group=self._cb, + ) + self.get_logger().info( + f"trajectory bridge up: FollowJointTrajectory -> /{art}/joint_command") + + def claim(self, timeout=2.0): + """Claim control (ros:urlab). Safe to call repeatedly; the claim never + expires but re-asserting is cheap and covers a lost startup race.""" + if not self._claim_cli.service_is_ready(): + if not self._claim_cli.wait_for_service(timeout_sec=timeout): + self.get_logger().warn("claim_control service not ready") + return + fut = self._claim_cli.call_async(Trigger.Request()) + t0 = time.time() + while not fut.done() and time.time() - t0 < timeout: + time.sleep(0.02) + if fut.done() and fut.result(): + self.get_logger().info(f"claim_control: {fut.result().message}") + + def _publish(self, names, positions): + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = list(names) + msg.position = [float(p) for p in positions] + self._cmd.publish(msg) + + def _execute(self, goal_handle): + self.claim() # re-assert ownership before every trajectory + traj = goal_handle.request.trajectory + names = list(traj.joint_names) + points = list(traj.points) + self.get_logger().info(f"executing trajectory: {len(points)} points") + + start = self.get_clock().now() + for pt in points: + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return FollowJointTrajectory.Result( + error_code=FollowJointTrajectory.Result.SUCCESSFUL) + target = start + Duration(seconds=pt.time_from_start.sec, + nanoseconds=pt.time_from_start.nanosec) + # The executor (other threads) advances the clock; just sleep here. + while rclpy.ok() and self.get_clock().now() < target: + time.sleep(0.004) + self._publish(names, pt.positions) + + # Hold the final target briefly so the position servo settles on it. + if points: + for _ in range(25): + self._publish(names, points[-1].positions) + time.sleep(0.02) + + goal_handle.succeed() + self.get_logger().info("trajectory complete") + return FollowJointTrajectory.Result( + error_code=FollowJointTrajectory.Result.SUCCESSFUL) + + +def main(): + art = "franka" + for a in sys.argv[1:]: + if a.startswith("--art="): + art = a.split("=", 1)[1] + rclpy.init() + node = TrajectoryBridge(art) + node.claim(timeout=15.0) # robust startup claim (service may be slow to appear) + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(node) + try: + executor.spin() + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 1a8891269f2aba67dc4bf189565e072de4119f25 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:39 +0100 Subject: [PATCH 08/32] Fix import and export across the element tree Class-inherited actuator gains survived neither import nor export. Geom type and size did not resolve through defaults. MJCF fragments were not resolved at all, and exported meshes carried no normals. --- Scripts/codegen/codegen_rules.json | 73 ++-- Scripts/codegen/generate_ue_components.py | 312 +++++++++++++----- .../codegen/tests/test_rule_shape_contract.py | 3 + .../tests/test_sensor_per_type_extractor.py | 108 +++--- .../Components/Actuators/MjActuator.cpp | 19 ++ .../MuJoCo/Components/Bodies/MjBody.cpp | 34 +- .../MuJoCo/Components/Joints/MjFreeJoint.cpp | 43 --- .../MuJoCo/Components/Joints/MjJoint.cpp | 68 +++- .../QuickConvert/AMjHeightfieldActor.cpp | 1 + .../Generated/MjOptionGeneratedExtras.cpp | 14 +- .../MuJoCo/Generated/MjSensorTypeInfo.cpp | 176 ++++++++++ .../MuJoCo/Input/MjTwistController.cpp | 14 + .../MuJoCo/Components/Actuators/MjActuator.h | 2 + .../Public/MuJoCo/Components/Bodies/MjBody.h | 6 + .../MuJoCo/Components/Joints/MjFreeJoint.h | 6 - .../Public/MuJoCo/Components/Joints/MjJoint.h | 3 +- .../Public/MuJoCo/Components/MjComponent.h | 11 +- .../MuJoCo/Generated/MjSensorTypeInfo.h | 79 +++++ .../Public/MuJoCo/Input/MjTwistController.h | 7 + 19 files changed, 720 insertions(+), 259 deletions(-) create mode 100644 Source/URLab/Private/MuJoCo/Generated/MjSensorTypeInfo.cpp create mode 100644 Source/URLab/Public/MuJoCo/Generated/MjSensorTypeInfo.h diff --git a/Scripts/codegen/codegen_rules.json b/Scripts/codegen/codegen_rules.json index ee017f23..f6962f5f 100644 --- a/Scripts/codegen/codegen_rules.json +++ b/Scripts/codegen/codegen_rules.json @@ -799,22 +799,21 @@ "schema_common_block": "sensor_common.attrs", "schema_subtypes_block": "sensor_types", "type_enum_name": "EMjSensorType", + "_note_type_info": "Each subtype row carries the URLab-side sensor metadata that feeds the generated FMjSensorTypeInfo table (MuJoCo/Generated/MjSensorTypeInfo.h). 'semantic' defaults to Generic, 'value_kind' to Scalar, and 'fixed_dim' to the value-kind's default dim (Scalar=1, Position/Direction/Vector3=3, Quaternion=4, GeomFromTo=6); mj_type + the objtype/reftype policy are derived from sensor_per_type unless obj_source/ref_source override them.", "subtypes": [ - {"key": "touch", "enum_value": "Touch", "class_name": "UMjTouchSensor", "header": "MjTouchSensor.h", "fully_emitted": true}, - {"key": "accelerometer", "enum_value": "Accelerometer", "class_name": "UMjAccelerometer", "header": "MjAccelerometer.h", "fully_emitted": true}, - {"key": "velocimeter", "enum_value": "Velocimeter", "class_name": "UMjVelocimeter", "header": "MjVelocimeter.h", "fully_emitted": true}, - {"key": "gyro", "enum_value": "Gyro", "class_name": "UMjGyro", "header": "MjGyro.h", "fully_emitted": true}, - {"key": "force", "enum_value": "Force", "class_name": "UMjForceSensor", "header": "MjForceSensor.h", "fully_emitted": true}, - {"key": "torque", "enum_value": "Torque", "class_name": "UMjTorqueSensor", "header": "MjTorqueSensor.h", "fully_emitted": true}, - {"key": "magnetometer", "enum_value": "Magnetometer", "class_name": "UMjMagnetometer", "header": "MjMagnetometer.h", "fully_emitted": true}, - {"key": "camprojection", "enum_value": "CamProjection", "class_name": "UMjCamProjectionSensor", "header": "MjCamProjectionSensor.h", "fully_emitted": true}, - {"key": "rangefinder", "enum_value": "RangeFinder", "class_name": "UMjRangeFinderSensor", "header": "MjRangeFinderSensor.h", - "case_body_override": "Element->type = mjSENS_RANGEFINDER; Element->objtype = (ObjType == EMjObjType::Camera) ? mjOBJ_CAMERA : mjOBJ_SITE;", - "fully_emitted": true}, - {"key": "jointpos", "enum_value": "JointPos", "class_name": "UMjJointPosSensor", "header": "MjJointPosSensor.h"}, - {"key": "jointvel", "enum_value": "JointVel", "class_name": "UMjJointVelSensor", "header": "MjJointVelSensor.h", "fully_emitted": true}, - {"key": "ballquat", "enum_value": "BallQuat", "class_name": "UMjBallQuatSensor", "header": "MjBallQuatSensor.h", "fully_emitted": true}, - {"key": "ballangvel", "enum_value": "BallAngVel", "class_name": "UMjBallAngVelSensor", "header": "MjBallAngVelSensor.h", "fully_emitted": true}, + {"key": "touch", "enum_value": "Touch", "class_name": "UMjTouchSensor", "header": "MjTouchSensor.h", "fully_emitted": true, "semantic": "Touch"}, + {"key": "accelerometer", "enum_value": "Accelerometer", "class_name": "UMjAccelerometer", "header": "MjAccelerometer.h", "fully_emitted": true, "semantic": "Accel", "value_kind": "Vector3"}, + {"key": "velocimeter", "enum_value": "Velocimeter", "class_name": "UMjVelocimeter", "header": "MjVelocimeter.h", "fully_emitted": true, "semantic": "Velocity", "value_kind": "Vector3"}, + {"key": "gyro", "enum_value": "Gyro", "class_name": "UMjGyro", "header": "MjGyro.h", "fully_emitted": true, "semantic": "Gyro", "value_kind": "Vector3"}, + {"key": "force", "enum_value": "Force", "class_name": "UMjForceSensor", "header": "MjForceSensor.h", "fully_emitted": true, "semantic": "Force", "value_kind": "Vector3"}, + {"key": "torque", "enum_value": "Torque", "class_name": "UMjTorqueSensor", "header": "MjTorqueSensor.h", "fully_emitted": true, "semantic": "Torque", "value_kind": "Vector3"}, + {"key": "magnetometer", "enum_value": "Magnetometer", "class_name": "UMjMagnetometer", "header": "MjMagnetometer.h", "fully_emitted": true, "semantic": "Magnetometer", "value_kind": "Vector3"}, + {"key": "camprojection", "enum_value": "CamProjection", "class_name": "UMjCamProjectionSensor", "header": "MjCamProjectionSensor.h", "fully_emitted": true, "fixed_dim": 2}, + {"key": "rangefinder", "enum_value": "RangeFinder", "class_name": "UMjRangeFinderSensor", "header": "MjRangeFinderSensor.h", "fully_emitted": true, "semantic": "Rangefinder", "obj_source": "Computed", "ref_source": "None", "fixed_dim": -1}, + {"key": "jointpos", "enum_value": "JointPos", "class_name": "UMjJointPosSensor", "header": "MjJointPosSensor.h", "semantic": "JointPos"}, + {"key": "jointvel", "enum_value": "JointVel", "class_name": "UMjJointVelSensor", "header": "MjJointVelSensor.h", "fully_emitted": true, "semantic": "JointVel"}, + {"key": "ballquat", "enum_value": "BallQuat", "class_name": "UMjBallQuatSensor", "header": "MjBallQuatSensor.h", "fully_emitted": true, "value_kind": "Quaternion"}, + {"key": "ballangvel", "enum_value": "BallAngVel", "class_name": "UMjBallAngVelSensor", "header": "MjBallAngVelSensor.h", "fully_emitted": true, "value_kind": "Vector3"}, {"key": "jointlimitpos", "enum_value": "JointLimitPos", "class_name": "UMjJointLimitPosSensor", "header": "MjJointLimitPosSensor.h", "fully_emitted": true}, {"key": "jointlimitvel", "enum_value": "JointLimitVel", "class_name": "UMjJointLimitVelSensor", "header": "MjJointLimitVelSensor.h", "fully_emitted": true}, {"key": "jointlimitfrc", "enum_value": "JointLimitFrc", "class_name": "UMjJointLimitFrcSensor", "header": "MjJointLimitFrcSensor.h", "fully_emitted": true}, @@ -823,34 +822,34 @@ {"key": "tendonlimitpos", "enum_value": "TendonLimitPos", "class_name": "UMjTendonLimitPosSensor", "header": "MjTendonLimitPosSensor.h", "fully_emitted": true}, {"key": "tendonlimitvel", "enum_value": "TendonLimitVel", "class_name": "UMjTendonLimitVelSensor", "header": "MjTendonLimitVelSensor.h", "fully_emitted": true}, {"key": "tendonlimitfrc", "enum_value": "TendonLimitFrc", "class_name": "UMjTendonLimitFrcSensor", "header": "MjTendonLimitFrcSensor.h", "fully_emitted": true}, - {"key": "actuatorpos", "enum_value": "ActuatorPos", "class_name": "UMjActuatorPosSensor", "header": "MjActuatorPosSensor.h", "fully_emitted": true}, - {"key": "actuatorvel", "enum_value": "ActuatorVel", "class_name": "UMjActuatorVelSensor", "header": "MjActuatorVelSensor.h", "fully_emitted": true}, - {"key": "actuatorfrc", "enum_value": "ActuatorFrc", "class_name": "UMjActuatorFrcSensor", "header": "MjActuatorFrcSensor.h", "fully_emitted": true}, + {"key": "actuatorpos", "enum_value": "ActuatorPos", "class_name": "UMjActuatorPosSensor", "header": "MjActuatorPosSensor.h", "fully_emitted": true, "semantic": "ActuatorPos"}, + {"key": "actuatorvel", "enum_value": "ActuatorVel", "class_name": "UMjActuatorVelSensor", "header": "MjActuatorVelSensor.h", "fully_emitted": true, "semantic": "ActuatorVel"}, + {"key": "actuatorfrc", "enum_value": "ActuatorFrc", "class_name": "UMjActuatorFrcSensor", "header": "MjActuatorFrcSensor.h", "fully_emitted": true, "semantic": "ActuatorFrc"}, {"key": "jointactuatorfrc", "enum_value": "JointActFrc", "class_name": "UMjJointActFrcSensor", "header": "MjJointActFrcSensor.h", "fully_emitted": true}, {"key": "tendonactuatorfrc","enum_value": "TendonActFrc", "class_name": "UMjTendonActFrcSensor", "header": "MjTendonActFrcSensor.h", "fully_emitted": true}, - {"key": "framepos", "enum_value": "FramePos", "class_name": "UMjFramePosSensor", "header": "MjFramePosSensor.h", "fully_emitted": true}, - {"key": "framequat", "enum_value": "FrameQuat", "class_name": "UMjFrameQuatSensor", "header": "MjFrameQuatSensor.h", "fully_emitted": true}, - {"key": "framexaxis", "enum_value": "FrameXAxis", "class_name": "UMjFrameXAxisSensor", "header": "MjFrameXAxisSensor.h", "fully_emitted": true}, - {"key": "frameyaxis", "enum_value": "FrameYAxis", "class_name": "UMjFrameYAxisSensor", "header": "MjFrameYAxisSensor.h", "fully_emitted": true}, - {"key": "framezaxis", "enum_value": "FrameZAxis", "class_name": "UMjFrameZAxisSensor", "header": "MjFrameZAxisSensor.h", "fully_emitted": true}, - {"key": "framelinvel", "enum_value": "FrameLinVel", "class_name": "UMjFrameLinVelSensor", "header": "MjFrameLinVelSensor.h", "fully_emitted": true}, - {"key": "frameangvel", "enum_value": "FrameAngVel", "class_name": "UMjFrameAngVelSensor", "header": "MjFrameAngVelSensor.h", "fully_emitted": true}, - {"key": "framelinacc", "enum_value": "FrameLinAcc", "class_name": "UMjFrameLinAccSensor", "header": "MjFrameLinAccSensor.h", "fully_emitted": true}, - {"key": "frameangacc", "enum_value": "FrameAngAcc", "class_name": "UMjFrameAngAccSensor", "header": "MjFrameAngAccSensor.h", "fully_emitted": true}, - {"key": "subtreecom", "enum_value": "SubtreeCom", "class_name": "UMjSubtreeComSensor", "header": "MjSubtreeComSensor.h", "fully_emitted": true}, - {"key": "subtreelinvel", "enum_value": "SubtreeLinVel", "class_name": "UMjSubtreeLinVelSensor", "header": "MjSubtreeLinVelSensor.h", "fully_emitted": true}, - {"key": "subtreeangmom", "enum_value": "SubtreeAngMom", "class_name": "UMjSubtreeAngMomSensor", "header": "MjSubtreeAngMomSensor.h", "fully_emitted": true}, + {"key": "framepos", "enum_value": "FramePos", "class_name": "UMjFramePosSensor", "header": "MjFramePosSensor.h", "fully_emitted": true, "semantic": "FramePos", "value_kind": "Position"}, + {"key": "framequat", "enum_value": "FrameQuat", "class_name": "UMjFrameQuatSensor", "header": "MjFrameQuatSensor.h", "fully_emitted": true, "semantic": "FrameQuat", "value_kind": "Quaternion"}, + {"key": "framexaxis", "enum_value": "FrameXAxis", "class_name": "UMjFrameXAxisSensor", "header": "MjFrameXAxisSensor.h", "fully_emitted": true, "semantic": "FrameAxis", "value_kind": "Direction"}, + {"key": "frameyaxis", "enum_value": "FrameYAxis", "class_name": "UMjFrameYAxisSensor", "header": "MjFrameYAxisSensor.h", "fully_emitted": true, "semantic": "FrameAxis", "value_kind": "Direction"}, + {"key": "framezaxis", "enum_value": "FrameZAxis", "class_name": "UMjFrameZAxisSensor", "header": "MjFrameZAxisSensor.h", "fully_emitted": true, "semantic": "FrameAxis", "value_kind": "Direction"}, + {"key": "framelinvel", "enum_value": "FrameLinVel", "class_name": "UMjFrameLinVelSensor", "header": "MjFrameLinVelSensor.h", "fully_emitted": true, "semantic": "FrameLinVel", "value_kind": "Vector3"}, + {"key": "frameangvel", "enum_value": "FrameAngVel", "class_name": "UMjFrameAngVelSensor", "header": "MjFrameAngVelSensor.h", "fully_emitted": true, "semantic": "FrameAngVel", "value_kind": "Vector3"}, + {"key": "framelinacc", "enum_value": "FrameLinAcc", "class_name": "UMjFrameLinAccSensor", "header": "MjFrameLinAccSensor.h", "fully_emitted": true, "semantic": "FrameLinAcc", "value_kind": "Vector3"}, + {"key": "frameangacc", "enum_value": "FrameAngAcc", "class_name": "UMjFrameAngAccSensor", "header": "MjFrameAngAccSensor.h", "fully_emitted": true, "semantic": "FrameAngAcc", "value_kind": "Vector3"}, + {"key": "subtreecom", "enum_value": "SubtreeCom", "class_name": "UMjSubtreeComSensor", "header": "MjSubtreeComSensor.h", "fully_emitted": true, "semantic": "SubtreeCom", "value_kind": "Position"}, + {"key": "subtreelinvel", "enum_value": "SubtreeLinVel", "class_name": "UMjSubtreeLinVelSensor", "header": "MjSubtreeLinVelSensor.h", "fully_emitted": true, "semantic": "SubtreeLinVel", "value_kind": "Vector3"}, + {"key": "subtreeangmom", "enum_value": "SubtreeAngMom", "class_name": "UMjSubtreeAngMomSensor", "header": "MjSubtreeAngMomSensor.h", "fully_emitted": true, "semantic": "SubtreeAngMom", "value_kind": "Vector3"}, {"key": "insidesite", "enum_value": "InsideSite", "class_name": "UMjInsideSiteSensor", "header": "MjInsideSiteSensor.h", "fully_emitted": true}, {"key": "distance", "enum_value": "GeomDist", "class_name": "UMjGeomDistSensor", "header": "MjGeomDistSensor.h", "fully_emitted": true}, - {"key": "normal", "enum_value": "GeomNormal", "class_name": "UMjGeomNormalSensor", "header": "MjGeomNormalSensor.h", "fully_emitted": true}, - {"key": "fromto", "enum_value": "GeomFromTo", "class_name": "UMjGeomFromToSensor", "header": "MjGeomFromToSensor.h", "fully_emitted": true}, - {"key": "contact", "enum_value": "Contact", "class_name": "UMjContactSensor", "header": "MjContactSensor.h", "fully_emitted": true}, + {"key": "normal", "enum_value": "GeomNormal", "class_name": "UMjGeomNormalSensor", "header": "MjGeomNormalSensor.h", "fully_emitted": true, "value_kind": "Direction"}, + {"key": "fromto", "enum_value": "GeomFromTo", "class_name": "UMjGeomFromToSensor", "header": "MjGeomFromToSensor.h", "fully_emitted": true, "value_kind": "GeomFromTo"}, + {"key": "contact", "enum_value": "Contact", "class_name": "UMjContactSensor", "header": "MjContactSensor.h", "fully_emitted": true, "fixed_dim": -1}, {"key": "e_potential", "enum_value": "EPotential", "class_name": "UMjEPotentialSensor", "header": "MjEPotentialSensor.h", "fully_emitted": true}, {"key": "e_kinetic", "enum_value": "EKinetic", "class_name": "UMjEKineticSensor", "header": "MjEKineticSensor.h", "fully_emitted": true}, - {"key": "clock", "enum_value": "Clock", "class_name": "UMjClockSensor", "header": "MjClockSensor.h", "fully_emitted": true}, - {"key": "tactile", "enum_value": "Tactile", "class_name": "UMjTactileSensor", "header": "MjTactileSensor.h"}, - {"key": "user", "enum_value": "User", "class_name": "UMjUserSensor", "header": "MjUserSensor.h", "fully_emitted": true}, - {"key": "plugin", "enum_value": "Plugin", "class_name": "UMjPluginSensor", "header": "MjPluginSensor.h", "fully_emitted": true} + {"key": "clock", "enum_value": "Clock", "class_name": "UMjClockSensor", "header": "MjClockSensor.h", "fully_emitted": true, "semantic": "Clock"}, + {"key": "tactile", "enum_value": "Tactile", "class_name": "UMjTactileSensor", "header": "MjTactileSensor.h", "fixed_dim": -1}, + {"key": "user", "enum_value": "User", "class_name": "UMjUserSensor", "header": "MjUserSensor.h", "fully_emitted": true, "ref_source": "None", "fixed_dim": -1}, + {"key": "plugin", "enum_value": "Plugin", "class_name": "UMjPluginSensor", "header": "MjPluginSensor.h", "fully_emitted": true, "fixed_dim": -1} ] }, diff --git a/Scripts/codegen/generate_ue_components.py b/Scripts/codegen/generate_ue_components.py index b09d7b74..8e63f656 100644 --- a/Scripts/codegen/generate_ue_components.py +++ b/Scripts/codegen/generate_ue_components.py @@ -3261,91 +3261,241 @@ def _phase_synthetic_categories(ctx: PhaseContext) -> None: # --------------------------------------------------------------------------- -# Sensor switch + TagToType codegen +# Sensor type-info descriptor table # --------------------------------------------------------------------------- # -# Replaces the hand-written switch + map in MjSensor.cpp with codegen -# output driven by: -# - codegen_rules.json[categories.sensor.subtypes] for XML key + enum -# value + (optional) case_body_override -# - sensor_per_type (from build_mjcf_schema_snapshot.py's Sensor() -# scrape) for mj_type + static objtype/reftype literals +# Emits FMjSensorTypeInfo, one row per sensor type, into a generated header + +# source pair. The row carries everything the six formerly type-keyed switches +# each recomputed: the mjSENS_* enum, MJCF tag, objtype/reftype export policy, +# ROS semantic, coordinate-transform value kind, MuJoCo output dimension, and +# the concrete UMj*Sensor UClass. Consumers (ExportTo, the ImportFromXml tag +# lookup, TransformSensorReading, DescribeState, and the editor XML parser's +# tag -> UClass chain) all read this one table. # -# Variable-objtype/reftype branches (frame*, geomdist, contact, plugin, -# user) stay hand-written in the post-switch block — that block reads -# UE-side ObjType / RefType properties and applies them after the -# codegen case fires. - -def _emit_sensor_switch_block(cat_rules: Dict[str, Any], - sensor_per_type: Dict[str, Any]) -> str: - """One ``case EMjSensorType::X: ...`` per subtype + the default fallback. - Lives between ``CODEGEN_SENSOR_TYPE_SWITCH_*`` markers in MjSensor.cpp. +# Data sources: +# - codegen_rules.json[categories.sensor.subtypes] for XML key, enum value, +# class name, header, and the URLab-side policy (semantic, value_kind, +# fixed_dim, and any obj_source/ref_source overrides) +# - sensor_per_type (from build_mjcf_schema_snapshot.py's Sensor() scrape) +# for mj_type + the objtype/reftype literals + +# Value kind -> MuJoCo output dimension. Used as the default FixedDim when a +# subtype rule doesn't pin one explicitly. Mirrors mjs_sensorDim in MuJoCo's +# user_api.cc; camprojection (2) and the variable-dim sensors (-1) override it. +_SENSOR_VALUE_KIND_DIM = { + "Scalar": 1, + "Position": 3, + "Direction": 3, + "Vector3": 3, + "Quaternion": 4, + "GeomFromTo": 6, +} + + +def _sensor_obj_ref_policy(per: Dict[str, Any], + subtype: Dict[str, Any]) -> Tuple[str, str, str, str]: + """Resolve ``(obj_source, obj_literal, ref_source, ref_literal)`` for one + sensor subtype. + + The base policy is derived from the scraped ``sensor_per_type`` objtype / + reftype: + - ``mjOBJ_*`` -> Static, carrying that literal + - ``from_xml`` -> FromXml (UE translates its ObjType / RefType property) + - ``computed`` -> FromXml (geom / contact / plugin read the properties) + - reftype null while objtype is from_xml -> FromXml (frame sensors carry + an optional relative reference frame the scraper records as null) + - otherwise -> None (leave the mjs field at its zero default) + + A subtype rule overrides either side via ``obj_source`` / ``ref_source`` + (plus an ``obj_type`` / ``ref_type`` literal when Static): rangefinder + computes its objtype from the attachment, and user / rangefinder never + write a reftype. """ - lines: List[str] = [] + def resolve(source_key: str, type_key: str, + snapshot_val: Any, allow_frame_ref: bool) -> Tuple[str, str]: + override = subtype.get(source_key) + if override: + return override, subtype.get(type_key, "mjOBJ_UNKNOWN") + if isinstance(snapshot_val, str) and snapshot_val.startswith("mjOBJ_"): + return "Static", snapshot_val + if snapshot_val in ("from_xml", "computed"): + return "FromXml", "mjOBJ_UNKNOWN" + if snapshot_val is None and allow_frame_ref: + return "FromXml", "mjOBJ_UNKNOWN" + return "None", "mjOBJ_UNKNOWN" + + objtype = per.get("objtype") + reftype = per.get("reftype") + obj_source, obj_literal = resolve("obj_source", "obj_type", objtype, False) + ref_source, ref_literal = resolve( + "ref_source", "ref_type", reftype, objtype == "from_xml") + return obj_source, obj_literal, ref_source, ref_literal + + +_SENSOR_TYPE_INFO_HEADER = ( + f"{COPYRIGHT_BLOCK}\n" + "#pragma once\n\n" + '#include "CoreMinimal.h"\n' + '#include "MuJoCo/Components/Sensors/MjSensor.h"\n' + '#include "State/MjStateTypes.h"\n\n' + "// How a sensor's MuJoCo objtype / reftype is resolved during ExportTo.\n" + "enum class EMjSensorObjSource : uint8\n" + "{\n" + " None, // leave the mjs field at its zero default (global sensors)\n" + " Static, // write the fixed mjOBJ_* literal carried in the descriptor\n" + " FromXml, // translate the UE ObjType / RefType property\n" + " Computed, // derive from the attachment (rangefinder: camera or site)\n" + "};\n\n" + "// Coordinate / unit family for the MuJoCo -> UE reading transform.\n" + "enum class EMjSensorValueKind : uint8\n" + "{\n" + " Scalar, // no transform\n" + " Position, // metres -> centimetres, negate Y\n" + " Direction, // unit vector, negate Y\n" + " Vector3, // velocity / acceleration / force / torque / angular, negate Y\n" + " Quaternion, // (w,x,y,z) -> UE (x,y,z,w) with handedness fix\n" + " GeomFromTo, // two concatenated positions\n" + "};\n\n" + "// One row of sensor-type metadata. This table collapses the six\n" + "// type-keyed switches that used to live across MjSensor.cpp and the\n" + "// editor XML parser into a single source of truth.\n" + "struct FMjSensorTypeInfo\n" + "{\n" + " EMjSensorType Type; // URLab sensor enum\n" + " int32 MjType; // mjSENS_* value\n" + " const TCHAR* Tag; // MJCF element tag (lowercase)\n" + " EMjSensorObjSource ObjSource; // how objtype is resolved\n" + " int32 ObjType; // mjOBJ_* literal when ObjSource == Static\n" + " EMjSensorObjSource RefSource; // how reftype is resolved\n" + " int32 RefType; // mjOBJ_* literal when RefSource == Static\n" + " EMjSensorSemantic Semantic; // ROS-facing grouping\n" + " EMjSensorValueKind ValueKind; // coordinate-transform family\n" + " int32 FixedDim; // MuJoCo output dim; -1 if variable\n" + " UClass* SensorClass; // concrete UMj*Sensor component class\n" + "};\n\n" + "// Descriptor for a sensor type. Falls back to the accelerometer row for\n" + "// unmapped values; never returns null.\n" + "URLAB_API const FMjSensorTypeInfo& MjSensorTypeInfoFor(EMjSensorType Type);\n\n" + "// Descriptor for a MJCF sensor tag (case-insensitive), or null if the\n" + "// tag is not a recognised sensor element.\n" + "URLAB_API const FMjSensorTypeInfo* MjSensorTypeInfoForTag(const FString& Tag);\n\n" + "// The full descriptor table, one row per sensor type.\n" + "URLAB_API TArrayView MjSensorTypeInfoTable();\n" +) + + +_SENSOR_TYPE_INFO_CPP_BODY = ( + "namespace\n" + "{\n" + "const TArray& GetSensorTypeInfoTable()\n" + "{\n" + " static const TArray Table = {\n" + "{ROWS}" + " };\n" + " return Table;\n" + "}\n" + "} // namespace\n\n" + "TArrayView MjSensorTypeInfoTable()\n" + "{\n" + " return GetSensorTypeInfoTable();\n" + "}\n\n" + "const FMjSensorTypeInfo& MjSensorTypeInfoFor(EMjSensorType Type)\n" + "{\n" + " static const TMap ByType = []\n" + " {\n" + " TMap Map;\n" + " for (const FMjSensorTypeInfo& Info : GetSensorTypeInfoTable())\n" + " {\n" + " Map.Add(Info.Type, &Info);\n" + " }\n" + " return Map;\n" + " }();\n" + " if (const FMjSensorTypeInfo* const* Found = ByType.Find(Type))\n" + " {\n" + " return **Found;\n" + " }\n" + " return *ByType.FindChecked(EMjSensorType::Accelerometer);\n" + "}\n\n" + "const FMjSensorTypeInfo* MjSensorTypeInfoForTag(const FString& Tag)\n" + "{\n" + " static const TMap ByTag = []\n" + " {\n" + " TMap Map;\n" + " for (const FMjSensorTypeInfo& Info : GetSensorTypeInfoTable())\n" + " {\n" + " Map.Add(FString(Info.Tag).ToLower(), &Info);\n" + " }\n" + " return Map;\n" + " }();\n" + " if (const FMjSensorTypeInfo* const* Found = ByTag.Find(Tag.ToLower()))\n" + " {\n" + " return *Found;\n" + " }\n" + " return nullptr;\n" + "}\n" +) + + +def _emit_sensor_type_info_files(cat_rules: Dict[str, Any], + sensor_per_type: Dict[str, Any], + public_root: str, + private_root: str) -> List["FileWrite"]: + """Build the FMjSensorTypeInfo header + source FileWrites.""" type_enum = cat_rules.get("type_enum_name", "EMjSensorType") + rows: List[str] = [] + includes: List[str] = [] for subtype in cat_rules.get("subtypes", []): key = subtype["key"] enum_value = subtype["enum_value"] - override = subtype.get("case_body_override") - if override: - lines.append(f" case {type_enum}::{enum_value}:") - lines.append(f" {override} break;") - continue + class_name = subtype["class_name"] + header = subtype.get("header") per = sensor_per_type.get(key, {}) mj_type = per.get("mj_type") - if not mj_type: - # The sensor switch will fall through to ``default:`` for this - # subtype at runtime — i.e. all framejerk/whatever sensors get - # the default mjSENS_ACCELEROMETER + mjOBJ_SITE substitution. + if not mj_type or not header: + # A missing scrape entry (or header) drops this row from the + # table; the runtime lookup then falls back to accelerometer. # Loud diagnostic so a regression in build_mjcf_schema_snapshot's # _extract_sensor_per_type regex doesn't silently miscompile. _diag_add( - f"[diagnostic] sensor subtype '{key}' has no entry in " - f"sensor_per_type snapshot — runtime will fall through to " - f"the default mjSENS_ACCELEROMETER + mjOBJ_SITE branch. " - f"Check the _extract_sensor_per_type regex against " - f"mujoco/src/user/user_objects.cc.", - source="sensor_scrape_miss", + f"[diagnostic] sensor subtype '{key}' has no " + f"{'mj_type in sensor_per_type' if not mj_type else 'header'} " + f"entry — it is omitted from the FMjSensorTypeInfo table and " + f"will fall back to the accelerometer descriptor at runtime.", + source="sensor_type_info", ) - lines.append(f" // (skipped: no mj_type for '{key}' in sensor_per_type)") continue - stmts = [f"Element->type = {mj_type};"] - # Only emit static objtype/reftype when sensor_per_type carries a - # literal mjOBJ_X. "from_xml" and "computed" entries are handled - # by the post-switch block reading UE-side ObjType/RefType. - objtype = per.get("objtype") - if isinstance(objtype, str) and objtype.startswith("mjOBJ_"): - stmts.append(f"Element->objtype = {objtype};") - reftype = per.get("reftype") - if isinstance(reftype, str) and reftype.startswith("mjOBJ_"): - stmts.append(f"Element->reftype = {reftype};") - lines.append(f" case {type_enum}::{enum_value}: " - f"{' '.join(stmts)} break;") - default_per = sensor_per_type.get("accelerometer", {}) - default_type = default_per.get("mj_type", "mjSENS_ACCELEROMETER") - default_objtype = default_per.get("objtype") or "mjOBJ_SITE" - if not (isinstance(default_objtype, str) and default_objtype.startswith("mjOBJ_")): - default_objtype = "mjOBJ_SITE" - lines.append(f" default: Element->type = {default_type}; " - f"Element->objtype = {default_objtype}; break;") - return "\n".join(lines) + "\n" - + obj_source, obj_literal, ref_source, ref_literal = _sensor_obj_ref_policy( + per, subtype) + semantic = subtype.get("semantic", "Generic") + value_kind = subtype.get("value_kind", "Scalar") + fixed_dim = subtype.get("fixed_dim") + if fixed_dim is None: + fixed_dim = _SENSOR_VALUE_KIND_DIM.get(value_kind, 1) + includes.append(f'#include "MuJoCo/Components/Sensors/{header}"') + rows.append( + f" {{ {type_enum}::{enum_value}, {mj_type}, " + f'TEXT("{key}"), ' + f"EMjSensorObjSource::{obj_source}, {obj_literal}, " + f"EMjSensorObjSource::{ref_source}, {ref_literal}, " + f"EMjSensorSemantic::{semantic}, EMjSensorValueKind::{value_kind}, " + f"{fixed_dim}, {class_name}::StaticClass() }},\n" + ) -def _emit_sensor_tag_to_type_block(cat_rules: Dict[str, Any]) -> str: - """One ``{TEXT(""), EMjSensorType::X}`` per subtype. Lives between - ``CODEGEN_SENSOR_TAG_TO_TYPE_*`` markers in MjSensor.cpp. - """ - lines: List[str] = [] - type_enum = cat_rules.get("type_enum_name", "EMjSensorType") - for subtype in cat_rules.get("subtypes", []): - # Some subtype XML keys collide with C++ enum members that diverge - # (e.g. `key=tendonactuatorfrc`, `enum_value=TendonActFrc`, - # mjSENS_TENDONACTFRC). The tag-to-type map uses the XML key - # verbatim so MJCF lookups match. - key = subtype["key"] - enum_value = subtype["enum_value"] - lines.append(f' {{TEXT("{key}"), {type_enum}::{enum_value}}},') - return "\n".join(lines) + "\n" + cpp_content = ( + f"{COPYRIGHT_BLOCK}\n" + '#include "MuJoCo/Generated/MjSensorTypeInfo.h"\n\n' + '#include "mujoco/mujoco.h"\n\n' + + "\n".join(includes) + "\n\n" + + _SENSOR_TYPE_INFO_CPP_BODY.replace("{ROWS}", "".join(rows)) + ) + pub_path = os.path.join( + public_root, "MuJoCo", "Generated", "MjSensorTypeInfo.h") + priv_path = os.path.join( + private_root, "MuJoCo", "Generated", "MjSensorTypeInfo.cpp") + return [ + FileWrite(path=pub_path, content=_SENSOR_TYPE_INFO_HEADER), + FileWrite(path=priv_path, content=cpp_content), + ] # --------------------------------------------------------------------------- @@ -3859,8 +4009,8 @@ def emit_fn(cat_rules): ) -def _phase_sensor_codegen(ctx: PhaseContext) -> None: - """Inject codegen-emitted sensor switch + TagToType into MjSensor.cpp.""" +def _phase_sensor_type_info(ctx: PhaseContext) -> None: + """Emit the FMjSensorTypeInfo descriptor table (header + source).""" sensor_rules = ctx.rules.get("categories", {}).get("sensor") if not sensor_rules: return @@ -3868,23 +4018,13 @@ def _phase_sensor_codegen(ctx: PhaseContext) -> None: if not sensor_per_type: _diag_add( "[diagnostic] sensor_per_type missing from schema snapshot; " - "skipping sensor switch codegen (run " + "skipping FMjSensorTypeInfo table (run " "build_mjcf_schema_snapshot.py).", - source="sensor_codegen", + source="sensor_type_info", ) return - cpp_path = os.path.join( - ctx.private_root, "MuJoCo", "Components", "Sensors", "MjSensor.cpp", - ) - _inject_tags_into_cpp( - cpp_path, - [ - ("SENSOR_TYPE_SWITCH", _emit_sensor_switch_block(sensor_rules, sensor_per_type)), - ("SENSOR_TAG_TO_TYPE", _emit_sensor_tag_to_type_block(sensor_rules)), - ], - ctx.writes, - diag_source="sensor_codegen", - ) + ctx.writes.extend(_emit_sensor_type_info_files( + sensor_rules, sensor_per_type, ctx.public_root, ctx.private_root)) _KNOWN_LAYOUTS = {"single_uclass_per_file", "multi_uclass", "no_subclasses"} @@ -3954,7 +4094,7 @@ def _phase_diagnostics(ctx: PhaseContext) -> None: EmissionPhase(name="generated_enums", fn=_phase_generated_enums), EmissionPhase(name="editor_option_helpers", fn=_phase_editor_option_helpers), EmissionPhase(name="articulation_registry", fn=_phase_articulation_registry), - EmissionPhase(name="sensor_codegen", fn=_phase_sensor_codegen), + EmissionPhase(name="sensor_type_info", fn=_phase_sensor_type_info), EmissionPhase(name="objtype_dispatch", fn=_phase_objtype_dispatch), EmissionPhase(name="geom_final_type", fn=_phase_geom_final_type), EmissionPhase(name="bind_h", fn=_phase_bind_h), diff --git a/Scripts/codegen/tests/test_rule_shape_contract.py b/Scripts/codegen/tests/test_rule_shape_contract.py index fc1cd6f9..0b1ee1ea 100644 --- a/Scripts/codegen/tests/test_rule_shape_contract.py +++ b/Scripts/codegen/tests/test_rule_shape_contract.py @@ -139,6 +139,9 @@ def test_synthetic_categories_carry_block_or_skip_flag(real_rules): _SUBTYPE_REQUIRED = {"key", "enum_value", "class_name"} _SUBTYPE_OPTIONAL = { "header", "fully_emitted", "case_body_override", "extra_constructor", + # FMjSensorTypeInfo descriptor-table policy (sensor category). + "semantic", "value_kind", "fixed_dim", + "obj_source", "obj_type", "ref_source", "ref_type", } _ELEMENT_RULE_OPTIONAL = { "exclude_attrs", "applies_canonicalizations", "xml_enum_attrs", diff --git a/Scripts/codegen/tests/test_sensor_per_type_extractor.py b/Scripts/codegen/tests/test_sensor_per_type_extractor.py index 3197e0cf..fa4804ff 100644 --- a/Scripts/codegen/tests/test_sensor_per_type_extractor.py +++ b/Scripts/codegen/tests/test_sensor_per_type_extractor.py @@ -124,63 +124,71 @@ def test_missing_sensor_method_returns_empty(): assert _extract_sensor_per_type("// no Sensor method here") == {} -# ---------- sensor switch + TagToType codegen ---------------------------- - -def test_sensor_switch_body_emits_static_objtype_when_present(): - from generate_ue_components import _emit_sensor_switch_block # noqa +# ---------- FMjSensorTypeInfo descriptor table --------------------------- + +def test_obj_ref_policy_derives_static_from_snapshot_literal(): + from generate_ue_components import _sensor_obj_ref_policy + per = {"objtype": "mjOBJ_SITE", "reftype": None} + obj_source, obj_literal, ref_source, ref_literal = _sensor_obj_ref_policy(per, {}) + assert (obj_source, obj_literal) == ("Static", "mjOBJ_SITE") + assert (ref_source, ref_literal) == ("None", "mjOBJ_UNKNOWN") + + +def test_obj_ref_policy_frame_null_reftype_becomes_from_xml(): + from generate_ue_components import _sensor_obj_ref_policy + # Frame sensors: objtype from_xml, reftype null -> both FromXml so the + # optional relative reference frame round-trips. + per = {"objtype": "from_xml", "reftype": None} + obj_source, _, ref_source, _ = _sensor_obj_ref_policy(per, {}) + assert obj_source == "FromXml" + assert ref_source == "FromXml" + + +def test_obj_ref_policy_honours_subtype_overrides(): + from generate_ue_components import _sensor_obj_ref_policy + # Rangefinder: computed objtype in the snapshot, but the rule pins the + # camera-or-site Computed policy and suppresses the reftype. + per = {"objtype": "computed", "reftype": "computed"} + subtype = {"obj_source": "Computed", "ref_source": "None"} + obj_source, _, ref_source, _ = _sensor_obj_ref_policy(per, subtype) + assert obj_source == "Computed" + assert ref_source == "None" + + +def test_type_info_files_emit_header_and_source_rows(): + from generate_ue_components import _emit_sensor_type_info_files cat_rules = { "type_enum_name": "EMjSensorType", "subtypes": [ - {"key": "touch", "enum_value": "Touch"}, - {"key": "framepos", "enum_value": "FramePos"}, + {"key": "touch", "enum_value": "Touch", "class_name": "UMjTouchSensor", + "header": "MjTouchSensor.h", "semantic": "Touch"}, + {"key": "framequat", "enum_value": "FrameQuat", "class_name": "UMjFrameQuatSensor", + "header": "MjFrameQuatSensor.h", "semantic": "FrameQuat", "value_kind": "Quaternion"}, ], } sensor_per_type = { - "touch": {"mj_type": "mjSENS_TOUCH", "objtype": "mjOBJ_SITE", "reftype": None}, - "framepos": {"mj_type": "mjSENS_FRAMEPOS", "objtype": "from_xml", "reftype": None}, - "accelerometer": {"mj_type": "mjSENS_ACCELEROMETER", "objtype": "mjOBJ_SITE", "reftype": None}, - } - out = _emit_sensor_switch_block(cat_rules, sensor_per_type) - # Touch gets static objtype literal. - assert "case EMjSensorType::Touch: Element->type = mjSENS_TOUCH; Element->objtype = mjOBJ_SITE; break;" in out - # framepos has objtype=from_xml -> NO static objtype emitted (handled in post-switch block). - assert "case EMjSensorType::FramePos: Element->type = mjSENS_FRAMEPOS; break;" in out - # Default fallback is hard-wired to accelerometer. - assert "default: Element->type = mjSENS_ACCELEROMETER; Element->objtype = mjOBJ_SITE; break;" in out - - -def test_sensor_switch_body_honours_case_override(): - from generate_ue_components import _emit_sensor_switch_block - cat_rules = { - "type_enum_name": "EMjSensorType", - "subtypes": [ - {"key": "rangefinder", "enum_value": "RangeFinder", - "case_body_override": "Element->type = mjSENS_RANGEFINDER; Element->objtype = X;"}, - ], - } - out = _emit_sensor_switch_block(cat_rules, {"rangefinder": {"mj_type": "mjSENS_RANGEFINDER"}}) - # Override appears verbatim; the emitter's automatic mj_type / objtype - # lines are SKIPPED for this case. - assert "Element->objtype = X;" in out - assert out.count("RangeFinder") >= 1 - - -def test_sensor_tag_to_type_map_uses_xml_key_verbatim(): - from generate_ue_components import _emit_sensor_tag_to_type_block - cat_rules = { - "type_enum_name": "EMjSensorType", - "subtypes": [ - {"key": "touch", "enum_value": "Touch"}, - {"key": "e_potential", "enum_value": "EPotential"}, - {"key": "jointactuatorfrc", "enum_value": "JointActFrc"}, - ], + "touch": {"mj_type": "mjSENS_TOUCH", "objtype": "mjOBJ_SITE", "reftype": None}, + "framequat": {"mj_type": "mjSENS_FRAMEQUAT", "objtype": "from_xml", "reftype": None}, } - out = _emit_sensor_tag_to_type_block(cat_rules) - assert '{TEXT("touch"), EMjSensorType::Touch},' in out - # Underscore-containing XML keys preserved literally. - assert '{TEXT("e_potential"), EMjSensorType::EPotential},' in out - # XML key and UE enum_value can diverge; map uses XML key. - assert '{TEXT("jointactuatorfrc"), EMjSensorType::JointActFrc},' in out + writes = _emit_sensor_type_info_files(cat_rules, sensor_per_type, "/pub", "/priv") + paths = {w.path.replace("\\", "/") for w in writes} + assert any(p.endswith("MuJoCo/Generated/MjSensorTypeInfo.h") for p in paths) + assert any(p.endswith("MuJoCo/Generated/MjSensorTypeInfo.cpp") for p in paths) + cpp = next(w.content for w in writes if w.path.endswith(".cpp")) + # touch: static site objtype, no reftype, scalar dim 1. + assert ("EMjSensorType::Touch, mjSENS_TOUCH, TEXT(\"touch\"), " + "EMjSensorObjSource::Static, mjOBJ_SITE, " + "EMjSensorObjSource::None, mjOBJ_UNKNOWN, " + "EMjSensorSemantic::Touch, EMjSensorValueKind::Scalar, 1, " + "UMjTouchSensor::StaticClass()") in cpp + # framequat: from_xml obj + ref, quaternion dim defaults to 4. + assert ("EMjSensorType::FrameQuat, mjSENS_FRAMEQUAT, TEXT(\"framequat\"), " + "EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, " + "EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, " + "EMjSensorSemantic::FrameQuat, EMjSensorValueKind::Quaternion, 4, " + "UMjFrameQuatSensor::StaticClass()") in cpp + # Source pulls in the concrete subclass headers. + assert '#include "MuJoCo/Components/Sensors/MjTouchSensor.h"' in cpp def test_real_snapshot_covers_every_schema_sensor(): diff --git a/Source/URLab/Private/MuJoCo/Components/Actuators/MjActuator.cpp b/Source/URLab/Private/MuJoCo/Components/Actuators/MjActuator.cpp index cbbe72aa..0d8c205a 100644 --- a/Source/URLab/Private/MuJoCo/Components/Actuators/MjActuator.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Actuators/MjActuator.cpp @@ -33,6 +33,8 @@ #include "MuJoCo/Components/Bodies/MjBody.h" #include "MuJoCo/Components/Tendons/MjTendon.h" #include "Utils/URLabLogging.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" UMjActuator::UMjActuator() { @@ -385,6 +387,23 @@ void UMjActuator::Bind(mjModel* Model, mjData* Data, const FString& Prefix) BindAndCacheView(m_ActuatorView, Prefix); } +void UMjActuator::DescribeState(FMjArticulationState& Out) const +{ + const ActuatorView& V = m_ActuatorView; + if (V.id < 0) + return; + FMjActuatorState& A = Out.Actuators.AddDefaulted_GetRef(); + const AMjArticulation* Art = Cast(GetOwner()); + A.Name = FMjCanonicalName::PartSegment(Art, GetMjName()); + if (TransmissionType == EMjActuatorTrnType::Joint && !TargetName.IsEmpty()) + { + A.TargetJoint = FMjCanonicalName::PartSegment(Art, TargetName); + } + A.Ctrl = V.ctrl ? V.ctrl[0] : 0.0; + A.Act = V.act ? V.act[0] : 0.0; // null for stateless actuators + A.Force = V.actuator_force ? V.actuator_force[0] : 0.0; +} + // ---------------------------------------------------------------------------------- // Blueprint Runtime API — Setup & Control // ---------------------------------------------------------------------------------- diff --git a/Source/URLab/Private/MuJoCo/Components/Bodies/MjBody.cpp b/Source/URLab/Private/MuJoCo/Components/Bodies/MjBody.cpp index bcf92183..9830332a 100644 --- a/Source/URLab/Private/MuJoCo/Components/Bodies/MjBody.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Bodies/MjBody.cpp @@ -31,6 +31,8 @@ #include "MuJoCo/Core/MjPhysicsEngine.h" #include "MuJoCo/Core/Spec/MjSpecWrapper.h" #include "MuJoCo/Core/MjRenderSnapshot.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" #include "MuJoCo/Utils/MjXmlUtils.h" #include "MuJoCo/Utils/MjUtils.h" #include "MuJoCo/Utils/MjOrientationUtils.h" @@ -110,11 +112,20 @@ void UMjBody::ApplyRenderState(const FMjRenderSnapshot& Snap) const int32 QuatIdx = Id * 4; if (Snap.XPos.Num() <= PosIdx + 2 || Snap.XQuat.Num() <= QuatIdx + 3) { - UE_LOG(LogURLabBind, Warning, - TEXT("MjBody::ApplyRenderState - Body '%s' (id=%d) out of range " - "of snapshot (XPos=%d, XQuat=%d). Disabling updates."), - *GetName(), Id, Snap.XPos.Num(), Snap.XQuat.Num()); - m_IsSetup = false; + // An empty snapshot means the physics worker has not published one yet + // (the first ticks after BeginPlay, before the consumer-rate publish + // fills it). Skip this frame and retry next tick -- do NOT disable, or + // the body freezes permanently once the snapshot does fill. Warn only + // when the snapshot is populated yet still too small for this id (a real + // model/index mismatch), and only once. + if (Snap.XPos.Num() > 0 && !m_bWarnedSnapshotRange) + { + UE_LOG(LogURLabBind, Warning, + TEXT("MjBody::ApplyRenderState - Body '%s' (id=%d) out of range " + "of a populated snapshot (XPos=%d, XQuat=%d)."), + *GetName(), Id, Snap.XPos.Num(), Snap.XQuat.Num()); + m_bWarnedSnapshotRange = true; + } return; } @@ -376,6 +387,19 @@ BodyView UMjBody::GetBodyView() const return m_BodyView; } +void UMjBody::DescribeState(FMjArticulationState& Out) const +{ + const BodyView& V = m_BodyView; + if (V.id < 0 || !V.xpos || !V.xquat) + return; + FMjBodyState& B = Out.Bodies.AddDefaulted_GetRef(); + B.Name = FMjCanonicalName::PartSegment(Cast(GetOwner()), GetMjName()); + for (int32 i = 0; i < 3; ++i) + B.Xpos[i] = V.xpos[i]; + for (int32 i = 0; i < 4; ++i) + B.Xquat[i] = V.xquat[i]; +} + FVector UMjBody::GetWorldPosition() const { if (m_BodyView.id < 0 || !m_BodyView.xpos) diff --git a/Source/URLab/Private/MuJoCo/Components/Joints/MjFreeJoint.cpp b/Source/URLab/Private/MuJoCo/Components/Joints/MjFreeJoint.cpp index 139ffee3..42bb5add 100644 --- a/Source/URLab/Private/MuJoCo/Components/Joints/MjFreeJoint.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Joints/MjFreeJoint.cpp @@ -21,7 +21,6 @@ // CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. #include "MuJoCo/Components/Joints/MjFreeJoint.h" -#include "Serialization/BufferArchive.h" #include "MuJoCo/Utils/MjOrientationUtils.h" UMjFreeJoint::UMjFreeJoint() @@ -30,48 +29,6 @@ UMjFreeJoint::UMjFreeJoint() Type = EMjJointType::Free; } -void UMjFreeJoint::BuildBinaryPayload(FBufferArchive& OutBuffer) const -{ - // Free joint: qpos has 7 values (pos[3] + quat[4]), qvel has 6 values (linvel[3] + angvel[3]) - if (!m_JointView._d || !m_JointView.qpos || !m_JointView.qvel) - { - return; - } - - // pos[3] - for (int i = 0; i < 3; ++i) - { - float Val = (float)m_JointView.qpos[i]; - OutBuffer << Val; - } - // quat[4] — MuJoCo stores as (w,x,y,z), send as (x,y,z,w) - float qx = (float)m_JointView.qpos[4]; - float qy = (float)m_JointView.qpos[5]; - float qz = (float)m_JointView.qpos[6]; - float qw = (float)m_JointView.qpos[3]; - OutBuffer << qx; - OutBuffer << qy; - OutBuffer << qz; - OutBuffer << qw; - // linvel[3] - for (int i = 0; i < 3; ++i) - { - float Val = (float)m_JointView.qvel[i]; - OutBuffer << Val; - } - // angvel[3] - for (int i = 3; i < 6; ++i) - { - float Val = (float)m_JointView.qvel[i]; - OutBuffer << Val; - } -} - -FString UMjFreeJoint::GetTelemetryTopicName() const -{ - return FString::Printf(TEXT("base_state/%s"), *GetName()); -} - void UMjFreeJoint::ExportTo(mjsJoint* Element, mjsDefault* Default) { if (!Element) diff --git a/Source/URLab/Private/MuJoCo/Components/Joints/MjJoint.cpp b/Source/URLab/Private/MuJoCo/Components/Joints/MjJoint.cpp index 10684920..c3e72381 100644 --- a/Source/URLab/Private/MuJoCo/Components/Joints/MjJoint.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Joints/MjJoint.cpp @@ -28,6 +28,9 @@ #include "Utils/URLabLogging.h" #include "MuJoCo/Core/Spec/MjSpecWrapper.h" #include "MuJoCo/Components/Defaults/MjDefault.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" UMjJoint::UMjJoint() { @@ -589,23 +592,58 @@ void UMjJoint::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) ExportTo(Jnt, effectiveDefault); } -void UMjJoint::BuildBinaryPayload(FBufferArchive& OutBuffer) const +void UMjJoint::DescribeState(FMjArticulationState& Out) const { - int32 JointID = m_ID; - OutBuffer << JointID; - - float JointPos = GetPosition(); - float JointVel = GetVelocity(); - float JointAcc = GetAcceleration(); - - OutBuffer << JointPos; - OutBuffer << JointVel; - OutBuffer << JointAcc; -} + const JointView& V = m_JointView; + if (V.id < 0 || !V.qpos || !V.qvel) + return; -FString UMjJoint::GetTelemetryTopicName() const -{ - return FString::Printf(TEXT("joint/%s"), *GetName()); + // Per-joint slot widths follow the joint type (free 7/6, ball 4/3, + // hinge/slide 1/1). V.qpos / V.qvel already point at this joint's first slot. + int32 QSize = 1; + int32 VSize = 1; + EMjJointType JType = EMjJointType::Hinge; + switch (V.jnt_type) + { + case mjJNT_FREE: + QSize = 7; + VSize = 6; + JType = EMjJointType::Free; + break; + case mjJNT_BALL: + QSize = 4; + VSize = 3; + JType = EMjJointType::Ball; + break; + case mjJNT_SLIDE: + JType = EMjJointType::Slide; + break; + case mjJNT_HINGE: + default: + JType = EMjJointType::Hinge; + break; + } + + FMjJointState& J = Out.Joints.AddDefaulted_GetRef(); + J.Name = FMjCanonicalName::PartSegment(Cast(GetOwner()), GetMjName()); + J.Type = JType; + J.QPos.SetNumUninitialized(QSize); + J.QVel.SetNumUninitialized(VSize); + for (int32 i = 0; i < QSize; ++i) + J.QPos[i] = V.qpos[i]; + for (int32 i = 0; i < VSize; ++i) + J.QVel[i] = V.qvel[i]; + + // Reference (qpos0) slice for the 1-DOF joints the URDF exposes, so the ROS + // /joint_states shift can emit qpos - qpos0 (URDF q=0 == MuJoCo qpos0). Free + // and ball joints are not URDF joints, so no shift is recorded for them. + if ((JType == EMjJointType::Hinge || JType == EMjJointType::Slide) + && V._m && V.jnt_qposadr >= 0) + { + J.RefPos.SetNumUninitialized(QSize); + for (int32 i = 0; i < QSize; ++i) + J.RefPos[i] = V._m->qpos0[V.jnt_qposadr + i]; + } } #if WITH_EDITOR diff --git a/Source/URLab/Private/MuJoCo/Components/QuickConvert/AMjHeightfieldActor.cpp b/Source/URLab/Private/MuJoCo/Components/QuickConvert/AMjHeightfieldActor.cpp index 510575ee..1f41c717 100644 --- a/Source/URLab/Private/MuJoCo/Components/QuickConvert/AMjHeightfieldActor.cpp +++ b/Source/URLab/Private/MuJoCo/Components/QuickConvert/AMjHeightfieldActor.cpp @@ -26,6 +26,7 @@ #include "DrawDebugHelpers.h" #include "MuJoCo/Components/QuickConvert/MjQuickConvertComponent.h" #include "MuJoCo/Core/MjArticulation.h" +#include "Serialization/BufferArchive.h" #include "Utils/URLabLogging.h" AMjHeightfieldActor::AMjHeightfieldActor() diff --git a/Source/URLab/Private/MuJoCo/Generated/MjOptionGeneratedExtras.cpp b/Source/URLab/Private/MuJoCo/Generated/MjOptionGeneratedExtras.cpp index ae2e326c..70ca3368 100644 --- a/Source/URLab/Private/MuJoCo/Generated/MjOptionGeneratedExtras.cpp +++ b/Source/URLab/Private/MuJoCo/Generated/MjOptionGeneratedExtras.cpp @@ -16,11 +16,6 @@ #include "MuJoCo/Generated/MjOptionGenerated.h" #include -// Bit positions in mjOption.enableflags. Match mjENBL_MULTICCD / mjENBL_SLEEP -// from mjmodel.h; hardcoded to avoid pulling mjtEnableBit into the header. -static constexpr int MJ_ENBL_MULTICCD = 1 << 4; -static constexpr int MJ_ENBL_SLEEP = 1 << 5; - void ApplyMjOptionExtras(mjOption* Opt, const FMjOptionGenerated& Self, mjSpec* Spec, mjModel* /*Model*/) { @@ -32,18 +27,19 @@ void ApplyMjOptionExtras(mjOption* Opt, const FMjOptionGenerated& Self, Spec->memory = static_cast(Self.MemoryMB) * 1024 * 1024; } + // MULTICCD moved from enableflags to disableflags in MuJoCo 3.9.0. if (Self.bEnableMultiCCD) - Opt->enableflags |= MJ_ENBL_MULTICCD; + Opt->disableflags &= ~mjDSBL_MULTICCD; else - Opt->enableflags &= ~MJ_ENBL_MULTICCD; + Opt->disableflags |= mjDSBL_MULTICCD; if (Self.bEnableSleep) { - Opt->enableflags |= MJ_ENBL_SLEEP; + Opt->enableflags |= mjENBL_SLEEP; Opt->sleep_tolerance = Self.SleepTolerance; } else { - Opt->enableflags &= ~MJ_ENBL_SLEEP; + Opt->enableflags &= ~mjENBL_SLEEP; } } diff --git a/Source/URLab/Private/MuJoCo/Generated/MjSensorTypeInfo.cpp b/Source/URLab/Private/MuJoCo/Generated/MjSensorTypeInfo.cpp new file mode 100644 index 00000000..4e3b6271 --- /dev/null +++ b/Source/URLab/Private/MuJoCo/Generated/MjSensorTypeInfo.cpp @@ -0,0 +1,176 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. +// +// AUTOGENERATED by Scripts/codegen/generate_ue_components.py +// Do not edit by hand. Re-run the generator to regenerate. + +#include "MuJoCo/Generated/MjSensorTypeInfo.h" + +#include "mujoco/mujoco.h" + +#include "MuJoCo/Components/Sensors/MjTouchSensor.h" +#include "MuJoCo/Components/Sensors/MjAccelerometer.h" +#include "MuJoCo/Components/Sensors/MjVelocimeter.h" +#include "MuJoCo/Components/Sensors/MjGyro.h" +#include "MuJoCo/Components/Sensors/MjForceSensor.h" +#include "MuJoCo/Components/Sensors/MjTorqueSensor.h" +#include "MuJoCo/Components/Sensors/MjMagnetometer.h" +#include "MuJoCo/Components/Sensors/MjCamProjectionSensor.h" +#include "MuJoCo/Components/Sensors/MjRangeFinderSensor.h" +#include "MuJoCo/Components/Sensors/MjJointPosSensor.h" +#include "MuJoCo/Components/Sensors/MjJointVelSensor.h" +#include "MuJoCo/Components/Sensors/MjBallQuatSensor.h" +#include "MuJoCo/Components/Sensors/MjBallAngVelSensor.h" +#include "MuJoCo/Components/Sensors/MjJointLimitPosSensor.h" +#include "MuJoCo/Components/Sensors/MjJointLimitVelSensor.h" +#include "MuJoCo/Components/Sensors/MjJointLimitFrcSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonPosSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonVelSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonLimitPosSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonLimitVelSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonLimitFrcSensor.h" +#include "MuJoCo/Components/Sensors/MjActuatorPosSensor.h" +#include "MuJoCo/Components/Sensors/MjActuatorVelSensor.h" +#include "MuJoCo/Components/Sensors/MjActuatorFrcSensor.h" +#include "MuJoCo/Components/Sensors/MjJointActFrcSensor.h" +#include "MuJoCo/Components/Sensors/MjTendonActFrcSensor.h" +#include "MuJoCo/Components/Sensors/MjFramePosSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameQuatSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameXAxisSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameYAxisSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameZAxisSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameLinVelSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameAngVelSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameLinAccSensor.h" +#include "MuJoCo/Components/Sensors/MjFrameAngAccSensor.h" +#include "MuJoCo/Components/Sensors/MjSubtreeComSensor.h" +#include "MuJoCo/Components/Sensors/MjSubtreeLinVelSensor.h" +#include "MuJoCo/Components/Sensors/MjSubtreeAngMomSensor.h" +#include "MuJoCo/Components/Sensors/MjInsideSiteSensor.h" +#include "MuJoCo/Components/Sensors/MjGeomDistSensor.h" +#include "MuJoCo/Components/Sensors/MjGeomNormalSensor.h" +#include "MuJoCo/Components/Sensors/MjGeomFromToSensor.h" +#include "MuJoCo/Components/Sensors/MjContactSensor.h" +#include "MuJoCo/Components/Sensors/MjEPotentialSensor.h" +#include "MuJoCo/Components/Sensors/MjEKineticSensor.h" +#include "MuJoCo/Components/Sensors/MjClockSensor.h" +#include "MuJoCo/Components/Sensors/MjTactileSensor.h" +#include "MuJoCo/Components/Sensors/MjUserSensor.h" +#include "MuJoCo/Components/Sensors/MjPluginSensor.h" + +namespace +{ +const TArray& GetSensorTypeInfoTable() +{ + static const TArray Table = { + { EMjSensorType::Touch, mjSENS_TOUCH, TEXT("touch"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Touch, EMjSensorValueKind::Scalar, 1, UMjTouchSensor::StaticClass()}, + { EMjSensorType::Accelerometer, mjSENS_ACCELEROMETER, TEXT("accelerometer"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Accel, EMjSensorValueKind::Vector3, 3, UMjAccelerometer::StaticClass()}, + { EMjSensorType::Velocimeter, mjSENS_VELOCIMETER, TEXT("velocimeter"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Velocity, EMjSensorValueKind::Vector3, 3, UMjVelocimeter::StaticClass()}, + { EMjSensorType::Gyro, mjSENS_GYRO, TEXT("gyro"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Gyro, EMjSensorValueKind::Vector3, 3, UMjGyro::StaticClass()}, + { EMjSensorType::Force, mjSENS_FORCE, TEXT("force"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Force, EMjSensorValueKind::Vector3, 3, UMjForceSensor::StaticClass()}, + { EMjSensorType::Torque, mjSENS_TORQUE, TEXT("torque"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Torque, EMjSensorValueKind::Vector3, 3, UMjTorqueSensor::StaticClass()}, + { EMjSensorType::Magnetometer, mjSENS_MAGNETOMETER, TEXT("magnetometer"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Magnetometer, EMjSensorValueKind::Vector3, 3, UMjMagnetometer::StaticClass()}, + { EMjSensorType::CamProjection, mjSENS_CAMPROJECTION, TEXT("camprojection"), EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorObjSource::Static, mjOBJ_CAMERA, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 2, UMjCamProjectionSensor::StaticClass()}, + { EMjSensorType::RangeFinder, mjSENS_RANGEFINDER, TEXT("rangefinder"), EMjSensorObjSource::Computed, mjOBJ_UNKNOWN, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Rangefinder, EMjSensorValueKind::Scalar, -1, UMjRangeFinderSensor::StaticClass()}, + { EMjSensorType::JointPos, mjSENS_JOINTPOS, TEXT("jointpos"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::JointPos, EMjSensorValueKind::Scalar, 1, UMjJointPosSensor::StaticClass()}, + { EMjSensorType::JointVel, mjSENS_JOINTVEL, TEXT("jointvel"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::JointVel, EMjSensorValueKind::Scalar, 1, UMjJointVelSensor::StaticClass()}, + { EMjSensorType::BallQuat, mjSENS_BALLQUAT, TEXT("ballquat"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Quaternion, 4, UMjBallQuatSensor::StaticClass()}, + { EMjSensorType::BallAngVel, mjSENS_BALLANGVEL, TEXT("ballangvel"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Vector3, 3, UMjBallAngVelSensor::StaticClass()}, + { EMjSensorType::JointLimitPos, mjSENS_JOINTLIMITPOS, TEXT("jointlimitpos"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjJointLimitPosSensor::StaticClass()}, + { EMjSensorType::JointLimitVel, mjSENS_JOINTLIMITVEL, TEXT("jointlimitvel"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjJointLimitVelSensor::StaticClass()}, + { EMjSensorType::JointLimitFrc, mjSENS_JOINTLIMITFRC, TEXT("jointlimitfrc"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjJointLimitFrcSensor::StaticClass()}, + { EMjSensorType::TendonPos, mjSENS_TENDONPOS, TEXT("tendonpos"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonPosSensor::StaticClass()}, + { EMjSensorType::TendonVel, mjSENS_TENDONVEL, TEXT("tendonvel"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonVelSensor::StaticClass()}, + {EMjSensorType::TendonLimitPos, mjSENS_TENDONLIMITPOS, TEXT("tendonlimitpos"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonLimitPosSensor::StaticClass()}, + {EMjSensorType::TendonLimitVel, mjSENS_TENDONLIMITVEL, TEXT("tendonlimitvel"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonLimitVelSensor::StaticClass()}, + {EMjSensorType::TendonLimitFrc, mjSENS_TENDONLIMITFRC, TEXT("tendonlimitfrc"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonLimitFrcSensor::StaticClass()}, + { EMjSensorType::ActuatorPos, mjSENS_ACTUATORPOS, TEXT("actuatorpos"), EMjSensorObjSource::Static, mjOBJ_ACTUATOR, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::ActuatorPos, EMjSensorValueKind::Scalar, 1, UMjActuatorPosSensor::StaticClass()}, + { EMjSensorType::ActuatorVel, mjSENS_ACTUATORVEL, TEXT("actuatorvel"), EMjSensorObjSource::Static, mjOBJ_ACTUATOR, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::ActuatorVel, EMjSensorValueKind::Scalar, 1, UMjActuatorVelSensor::StaticClass()}, + { EMjSensorType::ActuatorFrc, mjSENS_ACTUATORFRC, TEXT("actuatorfrc"), EMjSensorObjSource::Static, mjOBJ_ACTUATOR, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::ActuatorFrc, EMjSensorValueKind::Scalar, 1, UMjActuatorFrcSensor::StaticClass()}, + { EMjSensorType::JointActFrc, mjSENS_JOINTACTFRC, TEXT("jointactuatorfrc"), EMjSensorObjSource::Static, mjOBJ_JOINT, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjJointActFrcSensor::StaticClass()}, + { EMjSensorType::TendonActFrc, mjSENS_TENDONACTFRC, TEXT("tendonactuatorfrc"), EMjSensorObjSource::Static, mjOBJ_TENDON, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjTendonActFrcSensor::StaticClass()}, + { EMjSensorType::FramePos, mjSENS_FRAMEPOS, TEXT("framepos"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FramePos, EMjSensorValueKind::Position, 3, UMjFramePosSensor::StaticClass()}, + { EMjSensorType::FrameQuat, mjSENS_FRAMEQUAT, TEXT("framequat"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameQuat, EMjSensorValueKind::Quaternion, 4, UMjFrameQuatSensor::StaticClass()}, + { EMjSensorType::FrameXAxis, mjSENS_FRAMEXAXIS, TEXT("framexaxis"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameAxis, EMjSensorValueKind::Direction, 3, UMjFrameXAxisSensor::StaticClass()}, + { EMjSensorType::FrameYAxis, mjSENS_FRAMEYAXIS, TEXT("frameyaxis"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameAxis, EMjSensorValueKind::Direction, 3, UMjFrameYAxisSensor::StaticClass()}, + { EMjSensorType::FrameZAxis, mjSENS_FRAMEZAXIS, TEXT("framezaxis"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameAxis, EMjSensorValueKind::Direction, 3, UMjFrameZAxisSensor::StaticClass()}, + { EMjSensorType::FrameLinVel, mjSENS_FRAMELINVEL, TEXT("framelinvel"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameLinVel, EMjSensorValueKind::Vector3, 3, UMjFrameLinVelSensor::StaticClass()}, + { EMjSensorType::FrameAngVel, mjSENS_FRAMEANGVEL, TEXT("frameangvel"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameAngVel, EMjSensorValueKind::Vector3, 3, UMjFrameAngVelSensor::StaticClass()}, + { EMjSensorType::FrameLinAcc, mjSENS_FRAMELINACC, TEXT("framelinacc"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameLinAcc, EMjSensorValueKind::Vector3, 3, UMjFrameLinAccSensor::StaticClass()}, + { EMjSensorType::FrameAngAcc, mjSENS_FRAMEANGACC, TEXT("frameangacc"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::FrameAngAcc, EMjSensorValueKind::Vector3, 3, UMjFrameAngAccSensor::StaticClass()}, + { EMjSensorType::SubtreeCom, mjSENS_SUBTREECOM, TEXT("subtreecom"), EMjSensorObjSource::Static, mjOBJ_BODY, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::SubtreeCom, EMjSensorValueKind::Position, 3, UMjSubtreeComSensor::StaticClass()}, + { EMjSensorType::SubtreeLinVel, mjSENS_SUBTREELINVEL, TEXT("subtreelinvel"), EMjSensorObjSource::Static, mjOBJ_BODY, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::SubtreeLinVel, EMjSensorValueKind::Vector3, 3, UMjSubtreeLinVelSensor::StaticClass()}, + { EMjSensorType::SubtreeAngMom, mjSENS_SUBTREEANGMOM, TEXT("subtreeangmom"), EMjSensorObjSource::Static, mjOBJ_BODY, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::SubtreeAngMom, EMjSensorValueKind::Vector3, 3, UMjSubtreeAngMomSensor::StaticClass()}, + { EMjSensorType::InsideSite, mjSENS_INSIDESITE, TEXT("insidesite"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::Static, mjOBJ_SITE, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjInsideSiteSensor::StaticClass()}, + { EMjSensorType::GeomDist, mjSENS_GEOMDIST, TEXT("distance"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjGeomDistSensor::StaticClass()}, + { EMjSensorType::GeomNormal, mjSENS_GEOMNORMAL, TEXT("normal"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Direction, 3, UMjGeomNormalSensor::StaticClass()}, + { EMjSensorType::GeomFromTo, mjSENS_GEOMFROMTO, TEXT("fromto"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::GeomFromTo, 6, UMjGeomFromToSensor::StaticClass()}, + { EMjSensorType::Contact, mjSENS_CONTACT, TEXT("contact"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, -1, UMjContactSensor::StaticClass()}, + { EMjSensorType::EPotential, mjSENS_E_POTENTIAL, TEXT("e_potential"), EMjSensorObjSource::Static, mjOBJ_UNKNOWN, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjEPotentialSensor::StaticClass()}, + { EMjSensorType::EKinetic, mjSENS_E_KINETIC, TEXT("e_kinetic"), EMjSensorObjSource::Static, mjOBJ_UNKNOWN, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, 1, UMjEKineticSensor::StaticClass()}, + { EMjSensorType::Clock, mjSENS_CLOCK, TEXT("clock"), EMjSensorObjSource::Static, mjOBJ_UNKNOWN, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Clock, EMjSensorValueKind::Scalar, 1, UMjClockSensor::StaticClass()}, + { EMjSensorType::Tactile, mjSENS_TACTILE, TEXT("tactile"), EMjSensorObjSource::Static, mjOBJ_MESH, EMjSensorObjSource::Static, mjOBJ_GEOM, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, -1, UMjTactileSensor::StaticClass()}, + { EMjSensorType::User, mjSENS_USER, TEXT("user"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::None, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, -1, UMjUserSensor::StaticClass()}, + { EMjSensorType::Plugin, mjSENS_PLUGIN, TEXT("plugin"), EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorObjSource::FromXml, mjOBJ_UNKNOWN, EMjSensorSemantic::Generic, EMjSensorValueKind::Scalar, -1, UMjPluginSensor::StaticClass()}, + }; + return Table; +} +} // namespace + +TArrayView MjSensorTypeInfoTable() +{ + return GetSensorTypeInfoTable(); +} + +const FMjSensorTypeInfo& MjSensorTypeInfoFor(EMjSensorType Type) +{ + static const TMap ByType = [] { + TMap Map; + for (const FMjSensorTypeInfo& Info : GetSensorTypeInfoTable()) + { + Map.Add(Info.Type, &Info); + } + return Map; + }(); + if (const FMjSensorTypeInfo* const* Found = ByType.Find(Type)) + { + return **Found; + } + return *ByType.FindChecked(EMjSensorType::Accelerometer); +} + +const FMjSensorTypeInfo* MjSensorTypeInfoForTag(const FString& Tag) +{ + static const TMap ByTag = [] { + TMap Map; + for (const FMjSensorTypeInfo& Info : GetSensorTypeInfoTable()) + { + Map.Add(FString(Info.Tag).ToLower(), &Info); + } + return Map; + }(); + if (const FMjSensorTypeInfo* const* Found = ByTag.Find(Tag.ToLower())) + { + return *Found; + } + return nullptr; +} diff --git a/Source/URLab/Private/MuJoCo/Input/MjTwistController.cpp b/Source/URLab/Private/MuJoCo/Input/MjTwistController.cpp index b67ad779..768ea715 100644 --- a/Source/URLab/Private/MuJoCo/Input/MjTwistController.cpp +++ b/Source/URLab/Private/MuJoCo/Input/MjTwistController.cpp @@ -23,6 +23,7 @@ #include "MuJoCo/Input/MjTwistController.h" #include "EnhancedInputComponent.h" #include "InputAction.h" +#include "State/MjStateTypes.h" UMjTwistController::UMjTwistController() { @@ -42,6 +43,19 @@ int32 UMjTwistController::GetActiveActions() const return ActionBitmask; } +void UMjTwistController::DescribeState(FMjArticulationState& Out) const +{ + // geometry_msgs/Twist layout: (linear.x, linear.y, angular.z) filled from + // (Vx, Vy, YawRate); the rest stays zero. + const FVector Twist = GetTwist(); + FMjTwistState State; + State.Linear[0] = Twist.X; + State.Linear[1] = Twist.Y; + State.Angular[2] = Twist.Z; + State.Actions = GetActiveActions(); + Out.Twist = State; +} + void UMjTwistController::ResetTwist() { FScopeLock Lock(&TwistMutex); diff --git a/Source/URLab/Public/MuJoCo/Components/Actuators/MjActuator.h b/Source/URLab/Public/MuJoCo/Components/Actuators/MjActuator.h index 8e055045..ee9db22d 100644 --- a/Source/URLab/Public/MuJoCo/Components/Actuators/MjActuator.h +++ b/Source/URLab/Public/MuJoCo/Components/Actuators/MjActuator.h @@ -281,6 +281,8 @@ class URLAB_API UMjActuator : public UMjComponent virtual FString GetMjName() const override; + virtual void DescribeState(FMjArticulationState& Out) const override; + UFUNCTION(BlueprintCallable, Category = "MuJoCo|Runtime") void SetGear(const TArray& NewGear); diff --git a/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h b/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h index b8269b32..c331ba1d 100644 --- a/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h +++ b/Source/URLab/Public/MuJoCo/Components/Bodies/MjBody.h @@ -154,6 +154,8 @@ class URLAB_API UMjBody : public UMjComponent void Bind(mjModel* Model, mjData* Data, const FString& Prefix = TEXT("")); + virtual void DescribeState(FMjArticulationState& Out) const override; + BodyView GetBodyView() const; /** @brief Semantic accessor for raw MuJoCo data and helper methods. */ @@ -225,6 +227,10 @@ class URLAB_API UMjBody : public UMjComponent // instead of every frame. bool m_bWarnedDegenerateXform = false; + // One-shot guard so a genuine snapshot/index mismatch warns once instead of + // every frame. + bool m_bWarnedSnapshotRange = false; + FVector m_MeshPivotOffset = FVector::ZeroVector; UPROPERTY() diff --git a/Source/URLab/Public/MuJoCo/Components/Joints/MjFreeJoint.h b/Source/URLab/Public/MuJoCo/Components/Joints/MjFreeJoint.h index cbab1450..3bf695dd 100644 --- a/Source/URLab/Public/MuJoCo/Components/Joints/MjFreeJoint.h +++ b/Source/URLab/Public/MuJoCo/Components/Joints/MjFreeJoint.h @@ -40,12 +40,6 @@ class URLAB_API UMjFreeJoint : public UMjJoint UMjFreeJoint(); - /** Broadcasts full free joint state: pos[3], quat[4], linvel[3], angvel[3] = 13 floats. */ - virtual void BuildBinaryPayload(FBufferArchive& OutBuffer) const override; - - /** Returns base_state/ to distinguish from scalar hinge joints. */ - virtual FString GetTelemetryTopicName() const override; - virtual void ImportFromXml(const class FXmlNode* Node, const struct FMjCompilerSettings& CompilerSettings = FMjCompilerSettings{}) override; virtual void ExportTo(mjsJoint* Element, mjsDefault* Default = nullptr) override; diff --git a/Source/URLab/Public/MuJoCo/Components/Joints/MjJoint.h b/Source/URLab/Public/MuJoCo/Components/Joints/MjJoint.h index 0fda7bef..a80ded3b 100644 --- a/Source/URLab/Public/MuJoCo/Components/Joints/MjJoint.h +++ b/Source/URLab/Public/MuJoCo/Components/Joints/MjJoint.h @@ -237,8 +237,7 @@ class URLAB_API UMjJoint : public UMjComponent UFUNCTION(BlueprintCallable, Category = "MuJoCo|Runtime") FVector2D GetJointRange() const; - virtual void BuildBinaryPayload(FBufferArchive& OutBuffer) const override; - virtual FString GetTelemetryTopicName() const override; + virtual void DescribeState(FMjArticulationState& Out) const override; /** @brief Gets the complete runtime state (Pos, Vel, Accel) for this joint. */ UFUNCTION(BlueprintCallable, Category = "MuJoCo|Runtime") diff --git a/Source/URLab/Public/MuJoCo/Components/MjComponent.h b/Source/URLab/Public/MuJoCo/Components/MjComponent.h index 26fb70b1..928ed51a 100644 --- a/Source/URLab/Public/MuJoCo/Components/MjComponent.h +++ b/Source/URLab/Public/MuJoCo/Components/MjComponent.h @@ -29,8 +29,8 @@ #include "MuJoCo/Core/Spec/MjSpecElement.h" #include "MuJoCo/Utils/MjBind.h" #include "Utils/URLabLogging.h" -#include "Serialization/BufferArchive.h" class UMjDefault; +struct FMjArticulationState; #include "MjComponent.generated.h" @@ -71,11 +71,10 @@ class URLAB_API UMjComponent : public USceneComponent */ virtual void Bind(mjModel* model, mjData* data, const FString& Prefix = TEXT("")) override; - /** @brief Serializes the component's runtime state into a binary buffer for network transmission. */ - virtual void BuildBinaryPayload(FBufferArchive& OutBuffer) const {} - - /** @brief Gets the topic name used for broadcasting this component's data. */ - virtual FString GetTelemetryTopicName() const { return FString(); } + /** @brief Declares this component's per-step state into the articulation IR. + * Overridden by joints, sensors, actuators, and bodies; the base is a no-op + * so the collector can call it on every component without a type switch. */ + virtual void DescribeState(FMjArticulationState& Out) const {} /** @brief Returns the number of objects of a given type in the compiled model. */ static int GetMjObjectCount(const mjModel* M, mjtObj ObjType) diff --git a/Source/URLab/Public/MuJoCo/Generated/MjSensorTypeInfo.h b/Source/URLab/Public/MuJoCo/Generated/MjSensorTypeInfo.h new file mode 100644 index 00000000..5207c3cf --- /dev/null +++ b/Source/URLab/Public/MuJoCo/Generated/MjSensorTypeInfo.h @@ -0,0 +1,79 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. +// +// AUTOGENERATED by Scripts/codegen/generate_ue_components.py +// Do not edit by hand. Re-run the generator to regenerate. + +#pragma once + +#include "CoreMinimal.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "State/MjStateTypes.h" + +// How a sensor's MuJoCo objtype / reftype is resolved during ExportTo. +enum class EMjSensorObjSource : uint8 +{ + None, // leave the mjs field at its zero default (global sensors) + Static, // write the fixed mjOBJ_* literal carried in the descriptor + FromXml, // translate the UE ObjType / RefType property + Computed, // derive from the attachment (rangefinder: camera or site) +}; + +// Coordinate / unit family for the MuJoCo -> UE reading transform. +enum class EMjSensorValueKind : uint8 +{ + Scalar, // no transform + Position, // metres -> centimetres, negate Y + Direction, // unit vector, negate Y + Vector3, // velocity / acceleration / force / torque / angular, negate Y + Quaternion, // (w,x,y,z) -> UE (x,y,z,w) with handedness fix + GeomFromTo, // two concatenated positions +}; + +// One row of sensor-type metadata. This table collapses the six +// type-keyed switches that used to live across MjSensor.cpp and the +// editor XML parser into a single source of truth. +struct FMjSensorTypeInfo +{ + EMjSensorType Type; // URLab sensor enum + int32 MjType; // mjSENS_* value + const TCHAR* Tag; // MJCF element tag (lowercase) + EMjSensorObjSource ObjSource; // how objtype is resolved + int32 ObjType; // mjOBJ_* literal when ObjSource == Static + EMjSensorObjSource RefSource; // how reftype is resolved + int32 RefType; // mjOBJ_* literal when RefSource == Static + EMjSensorSemantic Semantic; // ROS-facing grouping + EMjSensorValueKind ValueKind; // coordinate-transform family + int32 FixedDim; // MuJoCo output dim; -1 if variable + UClass* SensorClass; // concrete UMj*Sensor component class +}; + +// Descriptor for a sensor type. Falls back to the accelerometer row for +// unmapped values; never returns null. +URLAB_API const FMjSensorTypeInfo& MjSensorTypeInfoFor(EMjSensorType Type); + +// Descriptor for a MJCF sensor tag (case-insensitive), or null if the +// tag is not a recognised sensor element. +URLAB_API const FMjSensorTypeInfo* MjSensorTypeInfoForTag(const FString& Tag); + +// The full descriptor table, one row per sensor type. +URLAB_API TArrayView MjSensorTypeInfoTable(); diff --git a/Source/URLab/Public/MuJoCo/Input/MjTwistController.h b/Source/URLab/Public/MuJoCo/Input/MjTwistController.h index 120d9026..553fd32a 100644 --- a/Source/URLab/Public/MuJoCo/Input/MjTwistController.h +++ b/Source/URLab/Public/MuJoCo/Input/MjTwistController.h @@ -27,6 +27,7 @@ #include "InputActionValue.h" #include "MjTwistController.generated.h" +struct FMjArticulationState; class UInputAction; class UInputMappingContext; class UEnhancedInputComponent; @@ -82,6 +83,12 @@ class URLAB_API UMjTwistController : public UActorComponent /** Returns bitmask of currently pressed action keys (bits 0-9). Thread-safe. */ int32 GetActiveActions() const; + /** Declares this controller's twist + active actions into the articulation IR. + * UMjTwistController derives from UActorComponent, not UMjComponent, so the + * collector calls this through a cached weak ptr rather than the DescribeState + * virtual. */ + void DescribeState(FMjArticulationState& Out) const; + /** Bind input actions to an EnhancedInputComponent. Called from AMjArticulation::SetupPlayerInputComponent. */ void BindInput(UEnhancedInputComponent* EIC); From bb0e9f88ae01d27fb87a05ee47bc9259b9f0fe70 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:39 +0100 Subject: [PATCH 09/32] Follow the overhaul through the editor and its tests --- .../Private/MjBridgeServerSubsystem.cpp | 45 +- .../Private/MjEditorOpHandlers.cpp | 200 +++- .../URLabEditor/Private/MujocoXmlParser.cpp | 152 +-- .../Tests/MjBridgeServerConfigTests.cpp | 192 +++ .../Tests/MjBridgeServerSubsystemTests.cpp | 161 +++ .../Private/Tests/MjCameraHistoryTests.cpp | 313 +++++ .../Tests/MjCameraReadbackQueueTests.cpp | 181 +++ .../Private/Tests/MjCameraTests.cpp | 117 +- .../Private/Tests/MjControlOwnershipTests.cpp | 405 +++++++ .../Private/Tests/MjImportTests.cpp | 34 + .../Private/Tests/MjJointStateTests.cpp | 195 ++++ .../Private/Tests/MjLevelOpsTests.cpp | 48 +- .../Private/Tests/MjModelUploadTests.cpp | 302 +++++ .../Private/Tests/MjPhysicsTests.cpp | 6 +- .../Private/Tests/MjRosLinkTests.cpp | 1029 +++++++++++++++++ .../Private/Tests/MjRosProviderTests.cpp | 323 ++++++ .../Private/Tests/MjSensorTypeInfoTests.cpp | 239 ++++ .../Private/Tests/MjStateCollectorTests.cpp | 741 ++++++++++++ .../Private/Tests/MjStepServerTests.cpp | 259 +++-- .../Private/Tests/MjThreadTests.cpp | 114 +- .../Private/Tests/MjUrdfExportTests.cpp | 537 +++++++++ .../Private/Tests/MjUserChannelTests.cpp | 335 ++++++ .../Public/MjBridgeServerSubsystem.h | 10 + Source/URLabEditor/URLabEditor.Build.cs | 1 + 24 files changed, 5650 insertions(+), 289 deletions(-) create mode 100644 Source/URLabEditor/Private/Tests/MjCameraHistoryTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjCameraReadbackQueueTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjControlOwnershipTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjJointStateTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjModelUploadTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjRosLinkTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjRosProviderTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjSensorTypeInfoTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjStateCollectorTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjUrdfExportTests.cpp create mode 100644 Source/URLabEditor/Private/Tests/MjUserChannelTests.cpp diff --git a/Source/URLabEditor/Private/MjBridgeServerSubsystem.cpp b/Source/URLabEditor/Private/MjBridgeServerSubsystem.cpp index e93e151b..055f79e5 100644 --- a/Source/URLabEditor/Private/MjBridgeServerSubsystem.cpp +++ b/Source/URLabEditor/Private/MjBridgeServerSubsystem.cpp @@ -6,6 +6,7 @@ #include "MjBridgeServerSubsystem.h" #include "Bridge/BridgeServerConfigUtils.h" +#include "Bridge/InstanceRegistry.h" #include "URLabEditorLogging.h" void UURLabBridgeServerSubsystem::Initialize(FSubsystemCollectionBase& Collection) @@ -40,23 +41,58 @@ void UURLabBridgeServerSubsystem::StartServer() { Server = NewObject(this, TEXT("EditorBridgeServer")); } - const FString Endpoint = FString::Printf(TEXT("tcp://0.0.0.0:%d"), Config.StepPort); + Server->SetInstanceConfig(Config); + const FString Endpoint = FString::Printf(TEXT("tcp://%s:%d"), *Config.BindAddress, Config.StepPort); Server->Start(Endpoint); - Server->EnsureShmBound(); // open req.shm/rep.shm under "live" + Server->EnsureShmBound(Config.InstanceId); // empty id -> "live" (single editor) + + CachedUrlabVersion.Reset(); + if (const FURLabRpcDispatcher* Dispatcher = Server->GetDispatcher()) + CachedUrlabVersion = Dispatcher->URLabVersion; + FURLabInstanceRegistry::WriteEntry(Config, CachedUrlabVersion, + /*bManagerPresent=*/false, /*bBusy=*/false); + + // Refresh the registry entry on a ticker so its mtime stays fresh (discovery + // treats a too-old entry as dead) and its `busy` tracks the live lease. + if (!HeartbeatHandle.IsValid()) + { + HeartbeatHandle = FTSTicker::GetCoreTicker().AddTicker( + FTickerDelegate::CreateUObject(this, &UURLabBridgeServerSubsystem::RefreshRegistryHeartbeat), + /*DelaySeconds=*/10.0f); + } + UE_LOG(LogURLabEditor, Log, - TEXT("[BridgeServer] started on %s (rpc_transports=%d)"), - *Endpoint, Server->GetRpcTransports().Num()); + TEXT("[BridgeServer] started instance='%s' index=%d bind=%s step=%d state=%d cam_base=%d " + "(rpc_transports=%d)"), + Config.InstanceId.IsEmpty() ? TEXT("live") : *Config.InstanceId, + Config.InstanceIndex, *Config.BindAddress, Config.StepPort, Config.StatePort, + Config.CamBasePort, Server->GetRpcTransports().Num()); } void UURLabBridgeServerSubsystem::StopServer() { if (!Server) return; + if (HeartbeatHandle.IsValid()) + { + FTSTicker::GetCoreTicker().RemoveTicker(HeartbeatHandle); + HeartbeatHandle.Reset(); + } Server->Stop(); Server = nullptr; + FURLabInstanceRegistry::RemoveEntry(Config); UE_LOG(LogURLabEditor, Log, TEXT("[BridgeServer] stopped")); } +bool UURLabBridgeServerSubsystem::RefreshRegistryHeartbeat(float /*DeltaTime*/) +{ + if (!Server) + return false; // server gone: stop ticking + FURLabInstanceRegistry::RefreshEntry(Config, CachedUrlabVersion, + /*bManagerPresent=*/false, /*bBusy=*/Server->IsLeaseHeld()); + return true; // keep ticking +} + bool UURLabBridgeServerSubsystem::IsRunning() const { return Server && Server->IsRunning(); @@ -66,4 +102,5 @@ void UURLabBridgeServerSubsystem::ReloadConfig() { Config = FURLabBridgeServerConfig{}; // reset to defaults URLabBridgeServerConfigUtils::LoadFromIni(Config); + URLabBridgeServerConfigUtils::ApplyEnvAndCommandLineOverrides(Config); } diff --git a/Source/URLabEditor/Private/MjEditorOpHandlers.cpp b/Source/URLabEditor/Private/MjEditorOpHandlers.cpp index 5303bec1..b97df3fe 100644 --- a/Source/URLabEditor/Private/MjEditorOpHandlers.cpp +++ b/Source/URLabEditor/Private/MjEditorOpHandlers.cpp @@ -34,6 +34,7 @@ #include "Bridge/OpRegistry.h" #include "Bridge/OpHelpers.h" #include "Bridge/RpcDispatcher.h" +#include "Bridge/RpcErrorCodes.h" namespace { @@ -104,7 +105,7 @@ TSharedPtr RunOnGameThreadSync(Lambda&& Body) { if (Dispatcher && Dispatcher->IsDraining()) { - return URLabOpHelpers::MakeError(TEXT("shutting_down"), + return URLabOpHelpers::MakeError(URLabError::ShuttingDown, TEXT("editor op aborted: dispatcher draining")); } FPlatformProcess::Sleep(0.05f); @@ -112,13 +113,103 @@ TSharedPtr RunOnGameThreadSync(Lambda&& Body) return State->Result; } +// ---- async editor jobs ------------------------------------------------- +// Long editor ops (import_xml, spawn, create/load/save level) block the game +// thread for seconds. Running them synchronously also blocks the single RPC +// worker thread (no other RPC is served until they finish). Instead we kick +// the work onto a game-thread ticker, return a `op_started` + job_id +// immediately, and let the client poll `op_status` (a fast worker-thread read) +// until the job is done -- freeing the worker thread and giving the client a +// liveness signal. The python client tolerates BOTH this async form and a +// direct synchronous reply. +struct FEditorJob +{ + FString State = TEXT("running"); // running | done | failed + FString Progress; + TSharedPtr Result; // the original handler reply once done +}; +static FCriticalSection GEditorJobLock; +static TMap GEditorJobs; +static std::atomic GEditorJobCounter{0}; + +template +TSharedPtr RunEditorJobAsync(Lambda&& Body) +{ + const FString JobId = FString::Printf( + TEXT("job_%llu"), GEditorJobCounter.fetch_add(1, std::memory_order_relaxed) + 1); + { + FScopeLock Lock(&GEditorJobLock); + GEditorJobs.Add(JobId, FEditorJob{}); + } + // Run the body on the game thread next tick; store the result in the job + // registry. The ticker (game thread) is the only writer of Result/State. + FTSTicker::GetCoreTicker().AddTicker( + FTickerDelegate::CreateLambda( + [JobId, BodyCopy = Forward(Body)](float /*Dt*/) mutable -> bool { + TSharedPtr Res = BodyCopy(); + FString ReplyOp; + if (Res.IsValid()) + Res->TryGetStringField(TEXT("op"), ReplyOp); + FScopeLock Lock(&GEditorJobLock); + if (FEditorJob* Job = GEditorJobs.Find(JobId)) + { + Job->Result = Res; + Job->State = (ReplyOp == TEXT("error")) ? TEXT("failed") : TEXT("done"); + } + return false; // one-shot + })); + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("op_started")); + Reply->SetStringField(TEXT("job_id"), JobId); + Reply->SetStringField(TEXT("state"), TEXT("running")); + return Reply; +} + +// Fast, worker-thread read of a job's state. Pops the job once it is reported +// terminal (done/failed) so the registry doesn't grow unbounded. +TSharedPtr HandleOpStatus(const TSharedPtr& Req) +{ + FString JobId; + if (!Req->TryGetStringField(TEXT("job_id"), JobId) || JobId.IsEmpty()) + return URLabOpHelpers::MakeError(URLabError::BadRequest, + TEXT("op_status requires a non-empty 'job_id'")); + + FString State, Progress; + TSharedPtr Result; + bool bTerminal = false; + { + FScopeLock Lock(&GEditorJobLock); + FEditorJob* Job = GEditorJobs.Find(JobId); + if (!Job) + return URLabOpHelpers::MakeError(URLabError::UnknownJob, + FString::Printf(TEXT("no job %s (already collected or never existed)"), *JobId)); + State = Job->State; + Progress = Job->Progress; + Result = Job->Result; + bTerminal = (State != TEXT("running")); + if (bTerminal) + GEditorJobs.Remove(JobId); + } + + TSharedPtr Reply = MakeShared(); + Reply->SetStringField(TEXT("op"), TEXT("op_status_ok")); + Reply->SetStringField(TEXT("job_id"), JobId); + Reply->SetStringField(TEXT("state"), State); + if (!Progress.IsEmpty()) + Reply->SetStringField(TEXT("progress"), Progress); + if (bTerminal && Result.IsValid()) + Reply->SetObjectField(TEXT("result"), Result); + return Reply; +} + // ---- import_xml -------------------------------------------------------- TSharedPtr HandleImportXml(const TSharedPtr& Req) { FString Path; if (!Req->TryGetStringField(TEXT("path"), Path) || Path.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("import_xml requires a non-empty 'path'")); } bool bForceReimport = false; @@ -148,7 +239,7 @@ TSharedPtr HandleCreateLevel(const TSharedPtr& Req) FString Name; if (!Req->TryGetStringField(TEXT("name"), Name) || Name.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("create_level requires non-empty 'name'")); } bool bForceOverwrite = false; @@ -167,7 +258,7 @@ TSharedPtr HandleCreateLevel(const TSharedPtr& Req) TSharedPtr HandleCurrentLevel(const TSharedPtr& /*Req*/) { if (!GEditor) - return MakeJsonError(TEXT("not_in_editor"), TEXT("GEditor null")); + return MakeJsonError(URLabError::NotInEditor, TEXT("GEditor null")); UWorld* World = GEditor->GetEditorWorldContext().World(); if (!World) return MakeJsonError(TEXT("no_world"), TEXT("editor world unavailable")); @@ -183,7 +274,7 @@ TSharedPtr HandleDestroyAsset(const TSharedPtr& Req) FString AssetPath; if (!Req->TryGetStringField(TEXT("asset_path"), AssetPath) || AssetPath.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("destroy_asset requires non-empty 'asset_path'")); } bool bWasFound = false; @@ -202,7 +293,7 @@ TSharedPtr HandleDestroyAsset(const TSharedPtr& Req) TSharedPtr HandleEnsureManager(const TSharedPtr& /*Req*/) { if (!GEditor) - return MakeJsonError(TEXT("not_in_editor"), TEXT("GEditor null")); + return MakeJsonError(URLabError::NotInEditor, TEXT("GEditor null")); UWorld* World = GEditor->GetEditorWorldContext().World(); if (!World) return MakeJsonError(TEXT("no_world"), TEXT("editor world unavailable")); @@ -245,7 +336,7 @@ TSharedPtr HandleLoadLevel(const TSharedPtr& Req) Req->TryGetStringField(TEXT("name"), Path); if (Path.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("load_level requires 'level_path' or 'name'")); } FString OutPath, Err; @@ -272,14 +363,14 @@ TSharedPtr HandleSaveLevel(const TSharedPtr& /*Req*/) return Reply; } -// ---- spawn_actor / destroy_actor --------------------------------------- +// ---- spawn_actor / remove_actor --------------------------------------- TSharedPtr HandleSpawnActor(const TSharedPtr& Req) { FString Blueprint; if (!Req->TryGetStringField(TEXT("blueprint"), Blueprint) || Blueprint.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("spawn_actor requires non-empty 'blueprint'")); } FString ActorId; @@ -331,12 +422,12 @@ TSharedPtr HandleSpawnGrid(const TSharedPtr& Req) { FString Blueprint; if (!Req->TryGetStringField(TEXT("blueprint"), Blueprint) || Blueprint.IsEmpty()) - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("spawn_grid requires non-empty 'blueprint'")); FString BaseId; if (!Req->TryGetStringField(TEXT("base_actor_id"), BaseId) || BaseId.IsEmpty()) - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("spawn_grid requires non-empty 'base_actor_id'")); int32 CountX = 0, CountY = 0; @@ -627,7 +718,7 @@ TSharedPtr HandleBeginPie(const TSharedPtr& Req) } if (bDraining) { - return MakeJsonError(TEXT("shutting_down"), + return MakeJsonError(URLabError::ShuttingDown, TEXT("Bridge stopping; begin_pie abandoned")); } @@ -702,7 +793,7 @@ TSharedPtr HandleStopPie(const TSharedPtr& /*Req*/) FPlatformProcess::ReturnSynchEventToPool(DoneEvent); if (!bTriggered) { - return MakeJsonError(TEXT("timeout"), + return MakeJsonError(URLabError::Timeout, TEXT("game thread blocked (modal dialog?); stop_pie did not complete")); } @@ -766,7 +857,7 @@ TSharedPtr HandleSetActorTransform(const TSharedPtr& R bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("set_actor_transform: %s"), *Err)); } @@ -826,7 +917,7 @@ TSharedPtr HandleGetActorBounds(const TSharedPtr& Req) bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("get_actor_bounds: %s"), *Err)); } bool bComponentsOnly = false; @@ -881,13 +972,13 @@ TSharedPtr HandleDuplicateActor(const TSharedPtr& Req) bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("duplicate_actor: %s"), *Err)); } FString NewActorId; if (!Req->TryGetStringField(TEXT("new_actor_id"), NewActorId) || NewActorId.IsEmpty()) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("duplicate_actor requires non-empty 'new_actor_id'")); } @@ -916,7 +1007,7 @@ TSharedPtr HandleActorHierarchy(const TSharedPtr& Req) bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("actor_hierarchy: %s"), *Err)); } TSharedPtr Root; @@ -952,7 +1043,7 @@ TSharedPtr HandleSelectActor(const TSharedPtr& Req) bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("select_actor: %s"), *Err)); } FString ActorName; @@ -972,7 +1063,7 @@ TSharedPtr HandleAddQuickConvert(const TSharedPtr& Req bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("add_quick_convert: %s"), *Err)); } bool bStatic = false, bComplexMesh = false, bDrivenByUnreal = false; @@ -1025,7 +1116,7 @@ TSharedPtr HandleRemoveQuickConvert(const TSharedPtr& bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("remove_quick_convert: %s"), *Err)); } FString ActorName; @@ -1041,21 +1132,21 @@ TSharedPtr HandleRemoveQuickConvert(const TSharedPtr& return Reply; } -TSharedPtr HandleDestroyActor(const TSharedPtr& Req) +TSharedPtr HandleRemoveActor(const TSharedPtr& Req) { FString Id, Err; bool bByName = false; if (!ResolveActorKey(Req, Id, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), - FString::Printf(TEXT("destroy_actor: %s"), *Err)); + return MakeJsonError(URLabError::MissingField, + FString::Printf(TEXT("remove_actor: %s"), *Err)); } if (!URLabLevelOps::DestroyActorSync(Id, Err)) { return MakeJsonError(TEXT("destroy_failed"), Err); } TSharedPtr Reply = MakeShared(); - Reply->SetStringField(TEXT("op"), TEXT("destroy_actor_ok")); + Reply->SetStringField(TEXT("op"), TEXT("remove_actor_ok")); Reply->SetBoolField(TEXT("requires_pie_restart"), GEditor ? GEditor->IsPlayingSessionInEditor() : false); return Reply; @@ -1118,7 +1209,7 @@ TSharedPtr HandleDrawMarker(const TSharedPtr& Req) FVector MjLoc; if (!ReadVec3(Req, TEXT("location"), MjLoc, FVector::ZeroVector)) - return MakeJsonError(TEXT("missing_field"), TEXT("draw_marker requires 'location'")); + return MakeJsonError(URLabError::MissingField, TEXT("draw_marker requires 'location'")); const double MjPos[3] = {MjLoc.X, MjLoc.Y, MjLoc.Z}; const FVector UELoc = MjUtils::MjToUEPosition(MjPos); @@ -1148,7 +1239,7 @@ TSharedPtr HandleDrawLine(const TSharedPtr& Req) FVector MjFrom, MjTo; if (!ReadVec3(Req, TEXT("from"), MjFrom, FVector::ZeroVector) || !ReadVec3(Req, TEXT("to"), MjTo, FVector::ZeroVector)) - return MakeJsonError(TEXT("missing_field"), TEXT("draw_line requires 'from' + 'to'")); + return MakeJsonError(URLabError::MissingField, TEXT("draw_line requires 'from' + 'to'")); const double MjF[3] = {MjFrom.X, MjFrom.Y, MjFrom.Z}; const double MjT[3] = {MjTo.X, MjTo.Y, MjTo.Z}; const FVector UEFrom = MjUtils::MjToUEPosition(MjF); @@ -1178,7 +1269,7 @@ TSharedPtr HandleDrawBox(const TSharedPtr& Req) FVector MjCenter, MjHalf; if (!ReadVec3(Req, TEXT("center"), MjCenter, FVector::ZeroVector) || !ReadVec3(Req, TEXT("half_extents"), MjHalf, FVector(0.1, 0.1, 0.1))) - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("draw_box requires 'center' + 'half_extents'")); const double MjC[3] = {MjCenter.X, MjCenter.Y, MjCenter.Z}; @@ -1215,7 +1306,7 @@ TSharedPtr HandleDrawArrow(const TSharedPtr& Req) FVector MjFrom, MjTo; if (!ReadVec3(Req, TEXT("from"), MjFrom, FVector::ZeroVector) || !ReadVec3(Req, TEXT("to"), MjTo, FVector::ZeroVector)) - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, TEXT("draw_arrow requires 'from' + 'to'")); const double MjF[3] = {MjFrom.X, MjFrom.Y, MjFrom.Z}; const double MjT[3] = {MjTo.X, MjTo.Y, MjTo.Z}; @@ -1253,7 +1344,7 @@ TSharedPtr HandleDrawAxes(const TSharedPtr& Req) FVector MjLoc; if (!ReadVec3(Req, TEXT("location"), MjLoc, FVector::ZeroVector)) - return MakeJsonError(TEXT("missing_field"), TEXT("draw_axes requires 'location'")); + return MakeJsonError(URLabError::MissingField, TEXT("draw_axes requires 'location'")); const double MjPos[3] = {MjLoc.X, MjLoc.Y, MjLoc.Z}; const FVector UEOrigin = MjUtils::MjToUEPosition(MjPos); @@ -1420,7 +1511,7 @@ TSharedPtr HandleViewportSetCamera(const TSharedPtr& R FVector MjLoc; if (!ReadVec3(Req, TEXT("location"), MjLoc, FVector::ZeroVector)) - return MakeJsonError(TEXT("missing_field"), TEXT("set_camera requires 'location'")); + return MakeJsonError(URLabError::MissingField, TEXT("set_camera requires 'location'")); const double MjPos[3] = {MjLoc.X, MjLoc.Y, MjLoc.Z}; Client->SetViewLocation(MjUtils::MjToUEPosition(MjPos)); @@ -1461,7 +1552,7 @@ TSharedPtr HandleViewportFrameActor(const TSharedPtr& bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("frame_actor: %s"), *Err)); } AActor* Actor = FindActorInEditorWorld(Key, bByName); @@ -1490,7 +1581,7 @@ TSharedPtr HandleViewportSetMode(const TSharedPtr& Req FString ModeStr; Req->TryGetStringField(TEXT("mode"), ModeStr); if (ModeStr.IsEmpty()) - return MakeJsonError(TEXT("missing_field"), TEXT("set_mode requires 'mode'")); + return MakeJsonError(URLabError::MissingField, TEXT("set_mode requires 'mode'")); EViewModeIndex Mode = VMI_Lit; if (ModeStr.Equals(TEXT("lit"), ESearchCase::IgnoreCase)) @@ -1555,7 +1646,7 @@ TSharedPtr HandleViewportTrackActor(const TSharedPtr& bool bByName = false; if (!ResolveActorKey(Req, Key, bByName, Err)) { - return MakeJsonError(TEXT("missing_field"), + return MakeJsonError(URLabError::MissingField, FString::Printf(TEXT("track_actor: %s"), *Err)); } AActor* Actor = FindActorInEditorWorld(Key, bByName); @@ -1639,6 +1730,17 @@ static URLabOpRegistry::FHandler GameThreadHandler( }; } +// Async variant for long-running editor ops: returns op_started + job_id +// immediately and runs the body on a game-thread ticker; the client polls +// op_status. Req is copied (TSharedPtr) so the deferred body owns it safely. +static URLabOpRegistry::FHandler GameThreadHandlerAsync( + TSharedPtr (*Inner)(const TSharedPtr&)) +{ + return [Inner](const TSharedPtr& Req) { + return RunEditorJobAsync([Inner, Req]() { return Inner(Req); }); + }; +} + /** Each editor op carries its category + namespace metadata so the * bridge can synthesise method bindings from the `meta` payload. */ void RegEditor(const TCHAR* Name, const TCHAR* Ns, @@ -1662,11 +1764,11 @@ void RegisterAll() { // scene namespace: level / asset import + spawn ops. RegEditor(TEXT("import_xml"), TEXT("scene"), - GameThreadHandler(&HandleImportXml), + GameThreadHandlerAsync(&HandleImportXml), /*Reply=*/{TEXT("op:string"), TEXT("blueprint_class_path:string"), TEXT("blueprint_short_name:string"), TEXT("imported_now:bool")}, /*Required=*/{TEXT("path")}); RegEditor(TEXT("create_level"), TEXT("scene"), - GameThreadHandler(&HandleCreateLevel), + GameThreadHandlerAsync(&HandleCreateLevel), /*Reply=*/{TEXT("op:string"), TEXT("level_path:string")}, /*Required=*/{TEXT("name")}); RegEditor(TEXT("ensure_manager"), TEXT("scene"), @@ -1685,7 +1787,7 @@ void RegisterAll() {TEXT("op:string"), TEXT("actors:array"), TEXT("in_pie:bool"), TEXT("level_path:string")}); RegEditor(TEXT("duplicate_actor"), TEXT("scene"), - GameThreadHandler(&HandleDuplicateActor), + GameThreadHandlerAsync(&HandleDuplicateActor), /*Reply=*/{TEXT("op:string"), TEXT("actor_id:string"), TEXT("actor_name:string"), TEXT("actor_path:string"), TEXT("blueprint_class_path:string")}, /*Required=*/{TEXT("target"), TEXT("new_actor_id")}); RegEditor(TEXT("actor_hierarchy"), TEXT("scene"), @@ -1693,34 +1795,41 @@ void RegisterAll() /*Reply=*/{TEXT("op:string"), TEXT("root:object")}, /*Required=*/{TEXT("target")}); RegEditor(TEXT("load_level"), TEXT("scene"), - GameThreadHandler(&HandleLoadLevel), + GameThreadHandlerAsync(&HandleLoadLevel), /*Reply=*/{TEXT("op:string"), TEXT("level_path:string")}, /*Required=*/{TEXT("level_path")}); RegEditor(TEXT("save_level"), TEXT("scene"), - GameThreadHandler(&HandleSaveLevel), + GameThreadHandlerAsync(&HandleSaveLevel), {TEXT("op:string"), TEXT("level_path:string")}); RegEditor(TEXT("spawn_actor"), TEXT("scene"), - GameThreadHandler(&HandleSpawnActor), + GameThreadHandlerAsync(&HandleSpawnActor), /*Reply=*/{TEXT("op:string"), TEXT("actor_id:string"), TEXT("actor_name:string"), TEXT("actor_path:string"), TEXT("blueprint_class_path:string"), TEXT("location:array"), TEXT("rotation_quat:array"), TEXT("was_existing:bool"), TEXT("requires_pie_restart:bool")}, /*Required=*/{TEXT("blueprint")}); RegEditor(TEXT("spawn_grid"), TEXT("scene"), - GameThreadHandler(&HandleSpawnGrid), + GameThreadHandlerAsync(&HandleSpawnGrid), /*Reply=*/{TEXT("op:string"), TEXT("count:int"), TEXT("blueprint_class_path:string"), TEXT("actors:array"), TEXT("requires_pie_restart:bool")}, /*Required=*/{TEXT("blueprint"), TEXT("base_actor_id"), TEXT("count_x"), TEXT("count_y")}); RegEditor(TEXT("spawn_light"), TEXT("scene"), - GameThreadHandler(&HandleSpawnLight), + GameThreadHandlerAsync(&HandleSpawnLight), {TEXT("op:string"), TEXT("actor_id:string"), TEXT("actor_name:string"), TEXT("actor_path:string"), TEXT("kind:string"), TEXT("intensity:float"), TEXT("color:array"), TEXT("location:array"), TEXT("requires_pie_restart:bool")}); - RegEditor(TEXT("destroy_actor"), TEXT("scene"), - GameThreadHandler(&HandleDestroyActor), + RegEditor(TEXT("remove_actor"), TEXT("scene"), + GameThreadHandlerAsync(&HandleRemoveActor), /*Reply=*/{TEXT("op:string"), TEXT("requires_pie_restart:bool")}, /*Required=*/{TEXT("target")}); RegEditor(TEXT("set_actor_transform"), TEXT("scene"), GameThreadHandler(&HandleSetActorTransform), /*Reply=*/{TEXT("op:string"), TEXT("target:string"), TEXT("actor_name:string"), TEXT("requires_pie_restart:bool")}, /*Required=*/{TEXT("target")}); + // Poll an async editor job (import_xml / spawn / create / save). NOT + // game-thread-wrapped: it's a fast worker-thread read of the job registry, + // so it stays responsive while the game thread runs the actual op. + RegEditor(TEXT("op_status"), TEXT("scene"), + [](const TSharedPtr& Req) { return HandleOpStatus(Req); }, + /*Reply=*/{TEXT("op:string"), TEXT("job_id:string"), TEXT("state:string"), TEXT("progress:string?"), TEXT("result:object?")}, + /*Required=*/{TEXT("job_id")}); // sim namespace: PIE lifecycle. Per the plan §5.1 the Python // wrappers expose these as client.sim.start / sim.stop / @@ -1839,7 +1948,8 @@ void UnregisterAll() URLabOpRegistry::UnregisterHandler(TEXT("spawn_actor")); URLabOpRegistry::UnregisterHandler(TEXT("spawn_grid")); URLabOpRegistry::UnregisterHandler(TEXT("spawn_light")); - URLabOpRegistry::UnregisterHandler(TEXT("destroy_actor")); + URLabOpRegistry::UnregisterHandler(TEXT("remove_actor")); + URLabOpRegistry::UnregisterHandler(TEXT("op_status")); URLabOpRegistry::UnregisterHandler(TEXT("set_actor_transform")); URLabOpRegistry::UnregisterHandler(TEXT("begin_pie")); URLabOpRegistry::UnregisterHandler(TEXT("stop_pie")); diff --git a/Source/URLabEditor/Private/MujocoXmlParser.cpp b/Source/URLabEditor/Private/MujocoXmlParser.cpp index f86f74a2..4cd05964 100644 --- a/Source/URLabEditor/Private/MujocoXmlParser.cpp +++ b/Source/URLabEditor/Private/MujocoXmlParser.cpp @@ -37,53 +37,7 @@ #include "MuJoCo/Components/Joints/MjFreeJoint.h" #include "MuJoCo/Components/Sensors/MjSensor.h" -#include "MuJoCo/Components/Sensors/MjTouchSensor.h" -#include "MuJoCo/Components/Sensors/MjAccelerometer.h" -#include "MuJoCo/Components/Sensors/MjVelocimeter.h" -#include "MuJoCo/Components/Sensors/MjGyro.h" -#include "MuJoCo/Components/Sensors/MjForceSensor.h" -#include "MuJoCo/Components/Sensors/MjTorqueSensor.h" -#include "MuJoCo/Components/Sensors/MjMagnetometer.h" -#include "MuJoCo/Components/Sensors/MjCamProjectionSensor.h" -#include "MuJoCo/Components/Sensors/MjRangeFinderSensor.h" -#include "MuJoCo/Components/Sensors/MjJointPosSensor.h" -#include "MuJoCo/Components/Sensors/MjJointVelSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonPosSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonVelSensor.h" -#include "MuJoCo/Components/Sensors/MjActuatorPosSensor.h" -#include "MuJoCo/Components/Sensors/MjActuatorVelSensor.h" -#include "MuJoCo/Components/Sensors/MjActuatorFrcSensor.h" -#include "MuJoCo/Components/Sensors/MjJointActFrcSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonActFrcSensor.h" -#include "MuJoCo/Components/Sensors/MjBallQuatSensor.h" -#include "MuJoCo/Components/Sensors/MjBallAngVelSensor.h" -#include "MuJoCo/Components/Sensors/MjJointLimitPosSensor.h" -#include "MuJoCo/Components/Sensors/MjJointLimitVelSensor.h" -#include "MuJoCo/Components/Sensors/MjJointLimitFrcSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonLimitPosSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonLimitVelSensor.h" -#include "MuJoCo/Components/Sensors/MjTendonLimitFrcSensor.h" -#include "MuJoCo/Components/Sensors/MjFramePosSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameQuatSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameXAxisSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameYAxisSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameZAxisSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameLinVelSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameAngVelSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameLinAccSensor.h" -#include "MuJoCo/Components/Sensors/MjFrameAngAccSensor.h" -#include "MuJoCo/Components/Sensors/MjSubtreeComSensor.h" -#include "MuJoCo/Components/Sensors/MjSubtreeLinVelSensor.h" -#include "MuJoCo/Components/Sensors/MjSubtreeAngMomSensor.h" -#include "MuJoCo/Components/Sensors/MjInsideSiteSensor.h" -#include "MuJoCo/Components/Sensors/MjGeomDistSensor.h" -#include "MuJoCo/Components/Sensors/MjGeomNormalSensor.h" -#include "MuJoCo/Components/Sensors/MjGeomFromToSensor.h" -#include "MuJoCo/Components/Sensors/MjContactSensor.h" -#include "MuJoCo/Components/Sensors/MjEPotentialSensor.h" -#include "MuJoCo/Components/Sensors/MjEKineticSensor.h" -#include "MuJoCo/Components/Sensors/MjClockSensor.h" -#include "MuJoCo/Components/Sensors/MjTactileSensor.h" +#include "MuJoCo/Generated/MjSensorTypeInfo.h" #include "MuJoCo/Components/Actuators/MjActuator.h" #include "MuJoCo/Components/Actuators/MjMotorActuator.h" @@ -1028,7 +982,7 @@ void UMujocoGenerationAction::ImportNodeRecursive(const FXmlNode* Node, USCS_Nod } } // --- SENSOR --- - else if (Tag.Equals(TEXT("sensor")) || Tag.EndsWith(TEXT("sensor")) || Tag == "touch" || Tag == "accelerometer" || Tag == "velocimeter" || Tag == "gyro" || Tag == "force" || Tag == "torque" || Tag == "magnetometer" || Tag == "camprojection" || Tag == "rangefinder" || Tag == "jointpos" || Tag == "jointvel" || Tag == "tendonpos" || Tag == "tendonvel" || Tag == "actuatorpos" || Tag == "actuatorvel" || Tag == "actuatorfrc" || Tag == "jointactuatorfrc" || Tag == "tendonactuatorfrc" || Tag == "ballquat" || Tag == "ballangvel" || Tag == "jointlimitpos" || Tag == "jointlimitvel" || Tag == "jointlimitfrc" || Tag == "tendonlimitpos" || Tag == "tendonlimitvel" || Tag == "tendonlimitfrc" || Tag == "framepos" || Tag == "framequat" || Tag == "framexaxis" || Tag == "frameyaxis" || Tag == "framezaxis" || Tag == "framelinvel" || Tag == "frameangvel" || Tag == "framelinacc" || Tag == "frameangacc" || Tag == "insidesite" || Tag == "subtreecom" || Tag == "subtreelinvel" || Tag == "subtreeangmom" || Tag == "distance" || Tag == "normal" || Tag == "fromto" || Tag == "contact" || Tag == "e_potential" || Tag == "e_kinetic" || Tag == "clock" || Tag == "tactile" || Tag == "user" || Tag == "plugin") + else if (Tag.Equals(TEXT("sensor")) || Tag.EndsWith(TEXT("sensor")) || MjSensorTypeInfoForTag(Tag)) { FString Name = Node->GetAttribute(TEXT("name")); if (Name.IsEmpty()) @@ -1038,101 +992,15 @@ void UMujocoGenerationAction::ImportNodeRecursive(const FXmlNode* Node, USCS_Nod Name = SensorTag + TEXT("Sensor"); } + // Map the MJCF tag to its concrete UMj*Sensor subclass via the + // codegen-emitted descriptor table. Tags with no descriptor (the + // bare container) fall back to the base UMjSensor. UClass* Class = UMjSensor::StaticClass(); - if (Tag == "touch") - Class = UMjTouchSensor::StaticClass(); - else if (Tag == "accelerometer") - Class = UMjAccelerometer::StaticClass(); - else if (Tag == "velocimeter") - Class = UMjVelocimeter::StaticClass(); - else if (Tag == "gyro") - Class = UMjGyro::StaticClass(); - else if (Tag == "force") - Class = UMjForceSensor::StaticClass(); - else if (Tag == "torque") - Class = UMjTorqueSensor::StaticClass(); - else if (Tag == "magnetometer") - Class = UMjMagnetometer::StaticClass(); - else if (Tag == "camprojection") - Class = UMjCamProjectionSensor::StaticClass(); - else if (Tag == "rangefinder") - Class = UMjRangeFinderSensor::StaticClass(); - else if (Tag == "jointpos") - Class = UMjJointPosSensor::StaticClass(); - else if (Tag == "jointvel") - Class = UMjJointVelSensor::StaticClass(); - else if (Tag == "tendonpos") - Class = UMjTendonPosSensor::StaticClass(); - else if (Tag == "tendonvel") - Class = UMjTendonVelSensor::StaticClass(); - else if (Tag == "actuatorpos") - Class = UMjActuatorPosSensor::StaticClass(); - else if (Tag == "actuatorvel") - Class = UMjActuatorVelSensor::StaticClass(); - else if (Tag == "actuatorfrc") - Class = UMjActuatorFrcSensor::StaticClass(); - else if (Tag == "jointactuatorfrc") - Class = UMjJointActFrcSensor::StaticClass(); - else if (Tag == "tendonactuatorfrc") - Class = UMjTendonActFrcSensor::StaticClass(); - else if (Tag == "ballquat") - Class = UMjBallQuatSensor::StaticClass(); - else if (Tag == "ballangvel") - Class = UMjBallAngVelSensor::StaticClass(); - else if (Tag == "jointlimitpos") - Class = UMjJointLimitPosSensor::StaticClass(); - else if (Tag == "jointlimitvel") - Class = UMjJointLimitVelSensor::StaticClass(); - else if (Tag == "jointlimitfrc") - Class = UMjJointLimitFrcSensor::StaticClass(); - else if (Tag == "tendonlimitpos") - Class = UMjTendonLimitPosSensor::StaticClass(); - else if (Tag == "tendonlimitvel") - Class = UMjTendonLimitVelSensor::StaticClass(); - else if (Tag == "tendonlimitfrc") - Class = UMjTendonLimitFrcSensor::StaticClass(); - else if (Tag == "framepos") - Class = UMjFramePosSensor::StaticClass(); - else if (Tag == "framequat") - Class = UMjFrameQuatSensor::StaticClass(); - else if (Tag == "framexaxis") - Class = UMjFrameXAxisSensor::StaticClass(); - else if (Tag == "frameyaxis") - Class = UMjFrameYAxisSensor::StaticClass(); - else if (Tag == "framezaxis") - Class = UMjFrameZAxisSensor::StaticClass(); - else if (Tag == "framelinvel") - Class = UMjFrameLinVelSensor::StaticClass(); - else if (Tag == "frameangvel") - Class = UMjFrameAngVelSensor::StaticClass(); - else if (Tag == "framelinacc") - Class = UMjFrameLinAccSensor::StaticClass(); - else if (Tag == "frameangacc") - Class = UMjFrameAngAccSensor::StaticClass(); - else if (Tag == "insidesite") - Class = UMjInsideSiteSensor::StaticClass(); - else if (Tag == "subtreecom") - Class = UMjSubtreeComSensor::StaticClass(); - else if (Tag == "subtreelinvel") - Class = UMjSubtreeLinVelSensor::StaticClass(); - else if (Tag == "subtreeangmom") - Class = UMjSubtreeAngMomSensor::StaticClass(); - else if (Tag == "distance") - Class = UMjGeomDistSensor::StaticClass(); - else if (Tag == "normal") - Class = UMjGeomNormalSensor::StaticClass(); - else if (Tag == "fromto") - Class = UMjGeomFromToSensor::StaticClass(); - else if (Tag == "contact") - Class = UMjContactSensor::StaticClass(); - else if (Tag == "e_potential") - Class = UMjEPotentialSensor::StaticClass(); - else if (Tag == "e_kinetic") - Class = UMjEKineticSensor::StaticClass(); - else if (Tag == "clock") - Class = UMjClockSensor::StaticClass(); - else if (Tag == "tactile") - Class = UMjTactileSensor::StaticClass(); + if (const FMjSensorTypeInfo* Info = MjSensorTypeInfoForTag(Tag)) + { + if (Info->SensorClass) + Class = Info->SensorClass; + } CreatedNode = BP->SimpleConstructionScript->CreateNode(Class, *Name); UMjSensor* SensComp = Cast(CreatedNode->ComponentTemplate); diff --git a/Source/URLabEditor/Private/Tests/MjBridgeServerConfigTests.cpp b/Source/URLabEditor/Private/Tests/MjBridgeServerConfigTests.cpp index 7cf82a2f..f5930609 100644 --- a/Source/URLabEditor/Private/Tests/MjBridgeServerConfigTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjBridgeServerConfigTests.cpp @@ -40,6 +40,19 @@ void LoadFrom(const FString& Path, FURLabBridgeServerConfig& Out) Out.StepPort = TmpInt; if (File.GetInt(S, TEXT("StatePort"), TmpInt)) Out.StatePort = TmpInt; + FString TmpStr; + if (File.GetString(S, TEXT("InstanceId"), TmpStr)) + Out.InstanceId = TmpStr; + if (File.GetInt(S, TEXT("InstanceIndex"), TmpInt)) + Out.InstanceIndex = TmpInt; + if (File.GetInt(S, TEXT("PortBase"), TmpInt)) + Out.PortBase = TmpInt; + if (File.GetInt(S, TEXT("PortStride"), TmpInt)) + Out.PortStride = TmpInt; + if (File.GetInt(S, TEXT("CamBasePort"), TmpInt)) + Out.CamBasePort = TmpInt; + if (File.GetString(S, TEXT("BindAddress"), TmpStr)) + Out.BindAddress = TmpStr; if (File.GetBool(S, TEXT("StopOnPIEEnd"), TmpBool)) Out.bStopOnPIEEnd = TmpBool; } @@ -53,6 +66,12 @@ void SaveTo(const FString& Path, const FURLabBridgeServerConfig& In) File.SetString(S, TEXT("AutoStart"), In.bAutoStart ? TEXT("True") : TEXT("False")); File.SetInt64(S, TEXT("StepPort"), In.StepPort); File.SetInt64(S, TEXT("StatePort"), In.StatePort); + File.SetString(S, TEXT("InstanceId"), *In.InstanceId); + File.SetInt64(S, TEXT("InstanceIndex"), In.InstanceIndex); + File.SetInt64(S, TEXT("PortBase"), In.PortBase); + File.SetInt64(S, TEXT("PortStride"), In.PortStride); + File.SetInt64(S, TEXT("CamBasePort"), In.CamBasePort); + File.SetString(S, TEXT("BindAddress"), *In.BindAddress); File.SetString(S, TEXT("StopOnPIEEnd"), In.bStopOnPIEEnd ? TEXT("True") : TEXT("False")); File.Dirty = true; File.Write(Path); @@ -153,3 +172,176 @@ bool FMjBridgeServerConfigPartial::RunTest(const FString& Parameters) TestFalse(TEXT("StopOnPIEEnd default kept"), Cfg.bStopOnPIEEnd); return true; } + +// --------------------------------------------------------------------------- +// 4. New per-instance fields survive a Save / Load round-trip. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerConfigInstanceRoundTrip, + "URLab.BridgeServerConfig.InstanceRoundTrip", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerConfigInstanceRoundTrip::RunTest(const FString& Parameters) +{ + FURLabBridgeServerConfig Out; + Out.InstanceId = TEXT("instance_7"); + Out.InstanceIndex = 7; + Out.PortBase = 6000; + Out.PortStride = 20; + Out.CamBasePort = 6142; + Out.BindAddress = TEXT("127.0.0.1"); + + const FString Path = MakeScratchIniPath(TEXT("instance_roundtrip")); + IFileManager::Get().Delete(*Path, /*RequireExists=*/false, /*EvenReadOnly=*/true); + + SaveTo(Path, Out); + FURLabBridgeServerConfig In; + LoadFrom(Path, In); + + TestEqual(TEXT("InstanceId roundtrip"), In.InstanceId, Out.InstanceId); + TestEqual(TEXT("InstanceIndex roundtrip"), In.InstanceIndex, Out.InstanceIndex); + TestEqual(TEXT("PortBase roundtrip"), In.PortBase, Out.PortBase); + TestEqual(TEXT("PortStride roundtrip"), In.PortStride, Out.PortStride); + TestEqual(TEXT("CamBasePort roundtrip"), In.CamBasePort, Out.CamBasePort); + TestEqual(TEXT("BindAddress roundtrip"), In.BindAddress, Out.BindAddress); + return true; +} + +// --------------------------------------------------------------------------- +// 5. DerivePorts: farm index derives the strided port block; explicit ports +// win; index < 0 preserves single-editor defaults. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerConfigDerivePorts, + "URLab.BridgeServerConfig.DerivePorts", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerConfigDerivePorts::RunTest(const FString& Parameters) +{ + using namespace URLabBridgeServerConfigUtils; + + // Index 0, defaults (base 5559, stride 10): 5559 / 5560 / 5561. + { + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 0; + DerivePorts(Cfg, /*Step=*/false, /*State=*/false, /*Cam=*/false); + TestEqual(TEXT("index0 StepPort"), Cfg.StepPort, 5559); + TestEqual(TEXT("index0 StatePort"), Cfg.StatePort, 5560); + TestEqual(TEXT("index0 CamBasePort"), Cfg.CamBasePort, 5561); + TestEqual(TEXT("index0 InstanceId derived"), Cfg.InstanceId, FString(TEXT("instance_0"))); + } + + // Index 3, base 5559, stride 10: 5589 / 5590 / 5591. + { + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 3; + DerivePorts(Cfg, false, false, false); + TestEqual(TEXT("index3 StepPort"), Cfg.StepPort, 5589); + TestEqual(TEXT("index3 StatePort"), Cfg.StatePort, 5590); + TestEqual(TEXT("index3 CamBasePort"), Cfg.CamBasePort, 5591); + } + + // Explicit StepPort survives derivation; the rest still derive. + { + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 3; + Cfg.StepPort = 7000; + DerivePorts(Cfg, /*Step=*/true, /*State=*/false, /*Cam=*/false); + TestEqual(TEXT("explicit StepPort kept"), Cfg.StepPort, 7000); + TestEqual(TEXT("derived StatePort alongside explicit step"), Cfg.StatePort, 5590); + TestEqual(TEXT("derived CamBasePort alongside explicit step"), Cfg.CamBasePort, 5591); + } + + // Index < 0: single-editor defaults untouched; CamBasePort falls back to + // StepPort + 2. + { + FURLabBridgeServerConfig Cfg; // InstanceIndex == -1 by default + DerivePorts(Cfg, false, false, false); + TestEqual(TEXT("no-index StepPort default"), Cfg.StepPort, 5559); + TestEqual(TEXT("no-index StatePort default"), Cfg.StatePort, 5555); + TestEqual(TEXT("no-index CamBasePort = StepPort+2"), Cfg.CamBasePort, 5561); + TestTrue(TEXT("no-index InstanceId stays empty"), Cfg.InstanceId.IsEmpty()); + } + + // Explicit CamBasePort is not overwritten by the StepPort+2 fallback. + { + FURLabBridgeServerConfig Cfg; + Cfg.CamBasePort = 5900; + DerivePorts(Cfg, false, false, /*Cam=*/true); + TestEqual(TEXT("explicit CamBasePort kept"), Cfg.CamBasePort, 5900); + } + + return true; +} + +// --------------------------------------------------------------------------- +// 6. ApplyEnvAndCommandLineOverrides derives ports from a farm index when no +// env / command-line override is present in the test process. The explicit- +// override precedence (env / -URLab* beating derivation) and BindAddress +// override are validated in the live-launch integration test, since process +// env / command line can't be set portably from a unit test. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerConfigApplyOverrides, + "URLab.BridgeServerConfig.ApplyOverrides", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerConfigApplyOverrides::RunTest(const FString& Parameters) +{ + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 2; // as if -URLabInstanceIndex=2 / INI had set it + URLabBridgeServerConfigUtils::ApplyEnvAndCommandLineOverrides(Cfg); + + // base 5559 + 2*10 = 5579 block. + TestEqual(TEXT("apply index2 StepPort"), Cfg.StepPort, 5579); + TestEqual(TEXT("apply index2 StatePort"), Cfg.StatePort, 5580); + TestEqual(TEXT("apply index2 CamBasePort"), Cfg.CamBasePort, 5581); + TestEqual(TEXT("apply index2 InstanceId"), Cfg.InstanceId, FString(TEXT("instance_2"))); + return true; +} + +// --------------------------------------------------------------------------- +// 7. BuildCameraEndpoint: cameras allocate one port each, upward from +// CamBasePort, on the configured BindAddress. Distinct instances (distinct +// CamBasePort blocks) never overlap, which is what lets N editors stream +// cameras concurrently. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerConfigCameraEndpoint, + "URLab.BridgeServerConfig.CameraEndpoint", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerConfigCameraEndpoint::RunTest(const FString& Parameters) +{ + using namespace URLabBridgeServerConfigUtils; + + // Instance 0 (CamBasePort 5561): cameras land on 5561, 5562, 5563 ... + { + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 0; + DerivePorts(Cfg, false, false, false); // CamBasePort -> 5561 + TestEqual(TEXT("inst0 cam0"), BuildCameraEndpoint(Cfg, 0), FString(TEXT("tcp://0.0.0.0:5561"))); + TestEqual(TEXT("inst0 cam1"), BuildCameraEndpoint(Cfg, 1), FString(TEXT("tcp://0.0.0.0:5562"))); + TestEqual(TEXT("inst0 cam2"), BuildCameraEndpoint(Cfg, 2), FString(TEXT("tcp://0.0.0.0:5563"))); + } + + // Instance 1 (CamBasePort 5571): its camera block is strictly above + // instance 0's, so two instances never collide on a camera port. + { + FURLabBridgeServerConfig Cfg; + Cfg.InstanceIndex = 1; + DerivePorts(Cfg, false, false, false); // CamBasePort -> 5571 + TestEqual(TEXT("inst1 cam0"), BuildCameraEndpoint(Cfg, 0), FString(TEXT("tcp://0.0.0.0:5571"))); + TestEqual(TEXT("inst1 cam1"), BuildCameraEndpoint(Cfg, 1), FString(TEXT("tcp://0.0.0.0:5572"))); + } + + // BindAddress is honoured (consistent with the step/state sockets), and a + // negative index clamps to the block base rather than underflowing. + { + FURLabBridgeServerConfig Cfg; + Cfg.CamBasePort = 6000; + Cfg.BindAddress = TEXT("127.0.0.1"); + TestEqual(TEXT("bind addr honoured"), + BuildCameraEndpoint(Cfg, 0), FString(TEXT("tcp://127.0.0.1:6000"))); + TestEqual(TEXT("negative index clamps to base"), + BuildCameraEndpoint(Cfg, -5), FString(TEXT("tcp://127.0.0.1:6000"))); + } + + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjBridgeServerSubsystemTests.cpp b/Source/URLabEditor/Private/Tests/MjBridgeServerSubsystemTests.cpp index a4737f59..782b3555 100644 --- a/Source/URLabEditor/Private/Tests/MjBridgeServerSubsystemTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjBridgeServerSubsystemTests.cpp @@ -161,3 +161,164 @@ bool FMjBridgeServerOwnershipFlag::RunTest(const FString& Parameters) Server->RemoveFromRoot(); return true; } + +// --------------------------------------------------------------------------- +// 6. Cooperative lease: acquire / contended acquire / release (right + wrong +// id) / re-acquire, and IsLeaseHeld tracks state. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerLeaseAcquireRelease, + "URLab.BridgeServer.LeaseAcquireRelease", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerLeaseAcquireRelease::RunTest(const FString& Parameters) +{ + UURLabBridgeServer* Server = NewObject(); + Server->AddToRoot(); + + TestFalse(TEXT("No lease held initially"), Server->IsLeaseHeld()); + TestTrue(TEXT("Lease id empty initially"), Server->GetLeaseId().IsEmpty()); + + // Acquire succeeds and returns a non-empty lease id. + FString LeaseId, ExistingId; + const bool bAcquired = Server->TryAcquireLease(TEXT("client-a"), 60.0, LeaseId, ExistingId); + TestTrue(TEXT("First acquire succeeds"), bAcquired); + TestFalse(TEXT("Acquired lease id non-empty"), LeaseId.IsEmpty()); + TestTrue(TEXT("IsLeaseHeld true after acquire"), Server->IsLeaseHeld()); + TestEqual(TEXT("GetLeaseId matches acquired id"), Server->GetLeaseId(), LeaseId); + + // Second acquire while held fails and returns the current lease id. + FString LeaseId2, ExistingId2; + const bool bAcquired2 = Server->TryAcquireLease(TEXT("client-b"), 60.0, LeaseId2, ExistingId2); + TestFalse(TEXT("Second acquire fails while held"), bAcquired2); + TestEqual(TEXT("Busy reports the current lease id"), ExistingId2, LeaseId); + + // Release with a wrong id errors and leaves the lease held. + TestFalse(TEXT("Release with wrong id fails"), Server->ReleaseLease(TEXT("not-the-id"))); + TestTrue(TEXT("Lease still held after wrong-id release"), Server->IsLeaseHeld()); + + // Release with the correct id succeeds and frees the lease. + TestTrue(TEXT("Release with correct id succeeds"), Server->ReleaseLease(LeaseId)); + TestFalse(TEXT("IsLeaseHeld false after release"), Server->IsLeaseHeld()); + TestTrue(TEXT("Lease id empty after release"), Server->GetLeaseId().IsEmpty()); + + // A subsequent acquire succeeds again. + FString LeaseId3, ExistingId3; + TestTrue(TEXT("Re-acquire succeeds after release"), + Server->TryAcquireLease(TEXT("client-c"), 60.0, LeaseId3, ExistingId3)); + TestFalse(TEXT("Re-acquired id non-empty"), LeaseId3.IsEmpty()); + + Server->RemoveFromRoot(); + return true; +} + +// --------------------------------------------------------------------------- +// 7. Lazy TTL expiry frees an idle lease. Time is injected so the check is +// deterministic without sleeping. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerLeaseTtlExpiry, + "URLab.BridgeServer.LeaseTtlExpiry", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerLeaseTtlExpiry::RunTest(const FString& Parameters) +{ + UURLabBridgeServer* Server = NewObject(); + Server->AddToRoot(); + + // Pin the lease clock so TTL expiry is deterministic. + Server->SetLeaseClockForTest(100.0); + + // Acquire at t=100 with a 5s TTL. + FString LeaseId, ExistingId; + TestTrue(TEXT("Acquire at t=100 succeeds"), + Server->TryAcquireLease(TEXT("client-a"), 5.0, LeaseId, ExistingId)); + + // Still within TTL at t=104: held. + Server->SetLeaseClockForTest(104.0); + TestTrue(TEXT("Held within TTL"), Server->IsLeaseHeld()); + + // Past TTL at t=106 (>100+5): lazily expired. + Server->SetLeaseClockForTest(106.0); + TestFalse(TEXT("Auto-released past TTL"), Server->IsLeaseHeld()); + + // After expiry a fresh acquire succeeds. + Server->SetLeaseClockForTest(107.0); + FString LeaseId2, ExistingId2; + TestTrue(TEXT("Acquire succeeds after TTL expiry"), + Server->TryAcquireLease(TEXT("client-b"), 5.0, LeaseId2, ExistingId2)); + TestNotEqual(TEXT("New lease id differs from the expired one"), LeaseId2, LeaseId); + + Server->RemoveFromRoot(); + return true; +} + +// --------------------------------------------------------------------------- +// 8. Discovery polling must NOT keep a lease alive. Driving hello repeatedly +// past the TTL window still auto-expires the lease; a state op within the +// window refreshes it. Exercises the DispatchInternal TouchLease gate. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjBridgeServerLeaseHelloDoesNotRefresh, + "URLab.BridgeServer.LeaseHelloDoesNotRefresh", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjBridgeServerLeaseHelloDoesNotRefresh::RunTest(const FString& Parameters) +{ + UURLabBridgeServer* Server = NewObject(); + Server->AddToRoot(); + + // Empty endpoint: constructs the dispatcher (wired back to this server) + // with no transports, so we can drive Dispatch() directly. + Server->Start(TEXT("")); + FURLabRpcDispatcher* Dispatcher = Server->GetDispatcher(); + if (!Dispatcher) + { + AddError(TEXT("dispatcher not constructed")); + Server->RemoveFromRoot(); + return false; + } + + auto DispatchOp = [Dispatcher](const TCHAR* Op) { + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), Op); + Dispatcher->Dispatch(Req); + }; + + // --- hello polling does not keep the lease alive --- + Server->SetLeaseClockForTest(100.0); + FString LeaseId, ExistingId; + TestTrue(TEXT("Acquire at t=100"), + Server->TryAcquireLease(TEXT("owner"), 5.0, LeaseId, ExistingId)); + + // A pool client probes hello every second inside the TTL window; none of + // these count as owner activity. + for (double T = 101.0; T <= 104.0; T += 1.0) + { + Server->SetLeaseClockForTest(T); + DispatchOp(TEXT("hello")); + } + + // Past the original TTL (100+5): still expired despite the polling. + Server->SetLeaseClockForTest(106.0); + TestFalse(TEXT("hello polling did not keep the lease alive"), Server->IsLeaseHeld()); + + // --- a state op within the window refreshes the lease --- + Server->SetLeaseClockForTest(200.0); + FString LeaseId2, ExistingId2; + TestTrue(TEXT("Re-acquire at t=200"), + Server->TryAcquireLease(TEXT("owner"), 5.0, LeaseId2, ExistingId2)); + + // A real owner op at t=204 refreshes activity (rejected for missing + // manager/session, but the gate fires before that — TouchLease runs). + Server->SetLeaseClockForTest(204.0); + DispatchOp(TEXT("step")); + + // At t=207 the original TTL (200+5=205) has passed, but the step refresh + // (204+5=209) keeps it alive. + Server->SetLeaseClockForTest(207.0); + TestTrue(TEXT("state op refreshed the lease past its original TTL"), + Server->IsLeaseHeld()); + + Server->SetLeaseClockForTest(-1.0); + Server->Stop(); + Server->RemoveFromRoot(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjCameraHistoryTests.cpp b/Source/URLabEditor/Private/Tests/MjCameraHistoryTests.cpp new file mode 100644 index 00000000..f6298311 --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjCameraHistoryTests.cpp @@ -0,0 +1,313 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. +// +// Pure-logic tests for the decoupled camera retrieval added with the streaming +// rework: the frame-history ring (by-id / latest fetch + eviction) and the +// per-camera capture-gating state machine. These exercise no GPU, so they run +// fine under the -NullRHI automation harness. The actual GPU pixel readback is +// validated separately (it cannot run headless). + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "Bridge/RpcDispatcher.h" +#include "Tests/MjTestHelpers.h" +#include "Dom/JsonObject.h" + +namespace +{ +UMjCamera* MakeBareCamera() +{ + // Transient, unregistered component: the history ring and gating helpers + // touch only plain members (no render target / world), so this is enough. + return NewObject(GetTransientPackage(), UMjCamera::StaticClass()); +} + +FMjCameraFrame MakeColorFrame(uint64 FrameId, double SimTime) +{ + FMjCameraFrame F; + F.FrameId = FrameId; + F.SimTime = SimTime; + F.Width = 2; + F.Height = 1; + F.Color.Init(FColor(static_cast(FrameId & 0xFF), 0, 0, 255), 2); + return F; +} + +FMjCameraFrame MakeDelayFrame(uint64 FrameId, double SimTime, double RevealValue, uint64 Seq) +{ + FMjCameraFrame F = MakeColorFrame(FrameId, SimTime); + F.RevealValue = RevealValue; + F.Seq = Seq; + return F; +} +} // namespace + +// ============================================================================ +// URLab.CameraHistory.RingEvictsAndFetches +// Ring keeps the last HistoryCapacity frames; GetFrame returns latest (id 0) +// or the oldest retained frame >= MinFrameId; misses return false. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraHistoryRing, + "URLab.CameraHistory.RingEvictsAndFetches", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraHistoryRing::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeBareCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + Cam->HistoryCapacity = 3; + + FMjCameraFrame Out; + TestFalse(TEXT("empty history -> GetFrame(0) false"), Cam->GetFrame(0, Out)); + TestEqual(TEXT("empty history -> latest id 0"), Cam->GetLatestFrameId(), (uint64)0); + + for (uint64 Id = 1; Id <= 5; ++Id) + { + Cam->PushFrameToHistory(MakeColorFrame(Id, 0.01 * Id)); + } + + // Capacity 3 -> only frames 3,4,5 retained. + TestEqual(TEXT("latest id is 5"), Cam->GetLatestFrameId(), (uint64)5); + + TestTrue(TEXT("GetFrame(0) latest"), Cam->GetFrame(0, Out)); + TestEqual(TEXT("latest frame id"), Out.FrameId, (uint64)5); + + TestTrue(TEXT("GetFrame(4)"), Cam->GetFrame(4, Out)); + TestEqual(TEXT("exact >=4 is 4"), Out.FrameId, (uint64)4); + + // Frames 1,2 were evicted; oldest retained >= 2 is 3. + TestTrue(TEXT("GetFrame(2) -> oldest retained >=2"), Cam->GetFrame(2, Out)); + TestEqual(TEXT(">=2 resolves to 3"), Out.FrameId, (uint64)3); + + // Nothing newer than 5. + TestFalse(TEXT("GetFrame(6) misses"), Cam->GetFrame(6, Out)); + + // Payload + metadata survive the round trip. + TestTrue(TEXT("GetFrame(5)"), Cam->GetFrame(5, Out)); + TestEqual(TEXT("width preserved"), Out.Width, 2); + TestEqual(TEXT("pixel count preserved"), Out.Color.Num(), 2); + TestTrue(TEXT("sim_time preserved"), FMath::IsNearlyEqual(Out.SimTime, 0.05, 1e-9)); + + return true; +} + +// ============================================================================ +// URLab.CameraHistory.DelayedFrameSelection +// Latency emulation: SelectDelayedFrame returns the newest frame whose +// RevealValue <= now, but only when its Seq advances past the last published +// (monotonic, no repeats), and nothing when no frame is yet eligible. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraDelayedSelection, + "URLab.CameraHistory.DelayedFrameSelection", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraDelayedSelection::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeBareCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + // Arm latency emulation so PushFrameToHistory uses the time-windowed + // retention path. A large delay keeps the retain window wide; the SimTime + // spread here is tiny so nothing is evicted and selection sees all 4 frames. + Cam->DelaySeconds = 1.0f; + Cam->bDelayUseWallClock = false; + + // FrameId, SimTime, RevealValue, Seq + Cam->PushFrameToHistory(MakeDelayFrame(10, 0.00, 0.05, 1)); + Cam->PushFrameToHistory(MakeDelayFrame(20, 0.01, 0.06, 2)); + Cam->PushFrameToHistory(MakeDelayFrame(30, 0.02, 0.07, 3)); + Cam->PushFrameToHistory(MakeDelayFrame(40, 0.03, 0.08, 4)); + + FMjCameraFrame Out; + + // Nothing revealed yet at now=0.04 (earliest reveal is 0.05). + TestFalse(TEXT("nothing eligible yet"), Cam->SelectDelayedFrame(0.04, 0, Out)); + + // now=0.065 -> newest with reveal <= 0.065 is frame 20 (reveal 0.06). + TestTrue(TEXT("selects newest eligible"), Cam->SelectDelayedFrame(0.065, 0, Out)); + TestEqual(TEXT("picked frame 20"), Out.FrameId, (uint64)20); + TestEqual(TEXT("picked seq 2"), Out.Seq, (uint64)2); + + // Same instant, but we've already published seq 2 -> nothing new. + TestFalse(TEXT("no repeat past AfterSeq"), Cam->SelectDelayedFrame(0.065, 2, Out)); + + // Far future reveals everything; newest past seq 2 is frame 40. + TestTrue(TEXT("advances to newest"), Cam->SelectDelayedFrame(10.0, 2, Out)); + TestEqual(TEXT("picked frame 40"), Out.FrameId, (uint64)40); + + return true; +} + +// ============================================================================ +// URLab.CameraHistory.DelayConfig +// SetCameraDelay / SetCaptureRate set + clamp the latency / capture-rate +// knobs; defaults match the resource-smart, zero-latency baseline. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraDelayConfig, + "URLab.CameraHistory.DelayConfig", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraDelayConfig::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeBareCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + // Defaults: no latency, capture-on-state-change on, no fps cap. + TestEqual(TEXT("default delay 0"), Cam->DelaySeconds, 0.0f); + TestEqual(TEXT("default jitter 0"), Cam->DelayJitterSeconds, 0.0f); + TestFalse(TEXT("default sim clock"), Cam->bDelayUseWallClock); + TestTrue(TEXT("default state-change capture"), Cam->bCaptureOnStateChange); + TestEqual(TEXT("default uncapped"), Cam->CaptureMaxFps, 0.0f); + + Cam->SetCameraDelay(0.05f, 0.01f, /*wall=*/true, /*seed=*/123); + TestTrue(TEXT("delay set"), FMath::IsNearlyEqual(Cam->DelaySeconds, 0.05f)); + TestTrue(TEXT("jitter set"), FMath::IsNearlyEqual(Cam->DelayJitterSeconds, 0.01f)); + TestTrue(TEXT("wall clock set"), Cam->bDelayUseWallClock); + + // Negative inputs clamp to zero. + Cam->SetCameraDelay(-1.0f, -1.0f, /*wall=*/false, /*seed=*/0); + TestEqual(TEXT("delay clamps >=0"), Cam->DelaySeconds, 0.0f); + TestEqual(TEXT("jitter clamps >=0"), Cam->DelayJitterSeconds, 0.0f); + + Cam->SetCaptureRate(/*on_state_change=*/false, /*max_fps=*/30.0f); + TestFalse(TEXT("state-change off"), Cam->bCaptureOnStateChange); + TestTrue(TEXT("fps set"), FMath::IsNearlyEqual(Cam->CaptureMaxFps, 30.0f)); + + Cam->SetCaptureRate(true, -5.0f); + TestEqual(TEXT("fps clamps >=0"), Cam->CaptureMaxFps, 0.0f); + + return true; +} + +// ============================================================================ +// URLab.CameraHistory.CaptureGating +// A camera is dormant by default, active when a broadcast flag is set, and +// active after TouchRequested within the TTL. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraCaptureGating, + "URLab.CameraHistory.CaptureGating", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraCaptureGating::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeBareCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + Cam->bEnableZmqBroadcast = false; + Cam->bEnableShmBroadcast = false; + + TestFalse(TEXT("fresh camera is dormant"), Cam->IsCaptureActive()); + + // Broadcast-enabled cameras are always active. + Cam->bEnableZmqBroadcast = true; + TestTrue(TEXT("zmq broadcast -> active"), Cam->IsCaptureActive()); + Cam->bEnableZmqBroadcast = false; + Cam->bEnableShmBroadcast = true; + TestTrue(TEXT("shm broadcast -> active"), Cam->IsCaptureActive()); + Cam->bEnableShmBroadcast = false; + TestFalse(TEXT("flags cleared -> dormant again"), Cam->IsCaptureActive()); + + // A recent request keeps a non-broadcast camera active within the TTL. + Cam->RequestActiveTtlSeconds = 3600.0f; + Cam->TouchRequested(); + TestTrue(TEXT("touched within TTL -> active"), Cam->IsCaptureActive()); + + return true; +} + +// ============================================================================ +// URLab.CameraHistory.StreamingApply +// set_camera_streaming helpers: name resolution (canonical) and the +// game-thread apply (disable path — flags cleared, keyed by canonical name, +// unknown cameras omitted). The enable path binds real ZMQ sockets, covered +// by the existing MjCamera streaming tests. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraStreamingApply, + "URLab.CameraHistory.StreamingApply", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraStreamingApply::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + UMjCamera* Cam = NewObject(S.Robot, TEXT("StreamCam")); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + Cam->MjName = TEXT("stream_cam"); + Cam->CaptureMode = EMjCameraMode::Real; + Cam->RegisterComponent(); + Cam->AttachToComponent(S.Body, FAttachmentTransformRules::KeepRelativeTransform); + + // Name resolution: the canonical "/" name resolves to this camera. + const FString Canon = Cam->GetCanonicalName(); + TMap ByName; + FURLabRpcDispatcher::BuildCameraNameMap(S.Manager, ByName); + TestTrue(TEXT("canonical name registered"), ByName.Contains(Canon)); + if (UMjCamera** F = ByName.Find(Canon)) + { + TestEqual(TEXT("resolves to the camera"), *F, Cam); + } + + // Pre-set the flags so the disable path has something to clear. + Cam->bEnableZmqBroadcast = true; + Cam->bEnableShmBroadcast = true; + + TMap> Reqs; + Reqs.Add(Canon, TPair(false, false)); + TSharedPtr Cams = + FURLabRpcDispatcher::ApplyCameraStreamingGameThread(S.Manager, Reqs); + if (!TestTrue(TEXT("reply valid"), Cams.IsValid())) + return false; + + const TSharedPtr* CamReply = nullptr; + TestTrue(TEXT("camera keyed by canonical name in reply"), + Cams->TryGetObjectField(Canon, CamReply)); + bool bStreaming = true; + if (CamReply && CamReply->IsValid()) + { + (*CamReply)->TryGetBoolField(TEXT("streaming"), bStreaming); + } + TestFalse(TEXT("disabled -> not streaming"), bStreaming); + TestFalse(TEXT("zmq flag cleared"), Cam->bEnableZmqBroadcast); + TestFalse(TEXT("shm flag cleared"), Cam->bEnableShmBroadcast); + + // Unknown camera key is omitted from the reply. + TMap> Unknown; + Unknown.Add(TEXT("nope_not_a_camera"), TPair(false, false)); + TSharedPtr Cams2 = + FURLabRpcDispatcher::ApplyCameraStreamingGameThread(S.Manager, Unknown); + TestEqual(TEXT("unknown camera omitted"), Cams2->Values.Num(), 0); + + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjCameraReadbackQueueTests.cpp b/Source/URLabEditor/Private/Tests/MjCameraReadbackQueueTests.cpp new file mode 100644 index 00000000..6263359f --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjCameraReadbackQueueTests.cpp @@ -0,0 +1,181 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. +// +// Pure-logic tests for the async readback pipeline's game-thread surface: the +// shared-frame history retention (a fetch is a refcount bump, not a pixel copy), +// the reveal-aware RPC fetch that keeps include_cameras in step with the delayed +// stream, and the bounds-safe resolution accessor. These touch no GPU, so they +// run under the -NullRHI automation harness; the render-thread map/copy stage is +// exercised by the GPU RenderOnDemand test, which cannot run headless. + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" + +namespace +{ +UMjCamera* MakeReadbackTestCamera() +{ + // Transient, unregistered component: the history ring and fetch helpers touch + // only plain members (no render target / world), so this is enough. + return NewObject(GetTransientPackage(), UMjCamera::StaticClass()); +} + +// Reveal / Seq drive the delay policy; a tiny reveal value keeps frames eligible +// under both the wall-clock and sim-clock "now" without depending on a manager. +FMjCameraFrame MakeFrame(uint64 FrameId, double RevealValue, uint64 Seq) +{ + FMjCameraFrame F; + F.FrameId = FrameId; + F.SimTime = 0.001 * FrameId; + F.Width = 2; + F.Height = 1; + F.RevealValue = RevealValue; + F.Seq = Seq; + F.Color.Init(FColor(static_cast(FrameId & 0xFF), 0, 0, 255), 2); + return F; +} +} // namespace + +// ============================================================================ +// URLab.CameraReadback.SharedFetchAliasesHistory +// GetFrameShared hands back the retained frame by refcount, so two fetches of +// the same frame resolve to the identical object (no per-fetch pixel copy). +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraSharedFetchAliases, + "URLab.CameraReadback.SharedFetchAliasesHistory", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraSharedFetchAliases::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeReadbackTestCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + Cam->HistoryCapacity = 4; + Cam->PushFrameToHistory(MakeFrame(10, 0.0, 1)); + Cam->PushFrameToHistory(MakeFrame(20, 0.0, 2)); + + TSharedPtr A = Cam->GetFrameShared(0); + TSharedPtr B = Cam->GetFrameShared(0); + if (!TestTrue(TEXT("latest fetch valid"), A.IsValid() && B.IsValid())) + return false; + TestTrue(TEXT("two fetches share one frame object"), A.Get() == B.Get()); + TestEqual(TEXT("latest is frame 20"), A->FrameId, (uint64)20); + + // By-id fetch resolves the oldest frame at/after the floor. + TSharedPtr ById = Cam->GetFrameShared(15); + if (TestTrue(TEXT("by-id fetch valid"), ById.IsValid())) + TestEqual(TEXT(">=15 resolves to 20"), ById->FrameId, (uint64)20); + + // Nothing newer than 20. + TestFalse(TEXT("fetch past newest misses"), Cam->GetFrameShared(21).IsValid()); + + return true; +} + +// ============================================================================ +// URLab.CameraReadback.RequestFetchIsDelayAware +// GetFrameForRequest: with latency emulation active it returns the frame the +// delayed stream currently reveals (so RPC and stream agree); bIgnoreDelay or +// no delay falls back to the by-id / latest fetch. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraRequestFetchDelayAware, + "URLab.CameraReadback.RequestFetchIsDelayAware", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraRequestFetchDelayAware::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeReadbackTestCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + // Wall-clock delay so "now" is UtcNow epoch seconds (~1.7e9), well above the + // small reveal values below, making the reveal gate deterministic in-test. + Cam->DelaySeconds = 0.5f; + Cam->bDelayUseWallClock = true; + + // Frame 30 is captured but NOT yet revealed (reveal far in the future); the + // newest revealed frame is 20. + Cam->PushFrameToHistory(MakeFrame(10, 1.0, 1)); + Cam->PushFrameToHistory(MakeFrame(20, 2.0, 2)); + Cam->PushFrameToHistory(MakeFrame(30, 1.0e18, 3)); + + // Delay active + honour delay: the RPC fetch tracks the stream and returns the + // newest revealed frame (20), NOT the undelayed newest (30). + TSharedPtr Revealed = Cam->GetFrameForRequest(0, /*bIgnoreDelay=*/false); + if (TestTrue(TEXT("reveal-aware fetch valid"), Revealed.IsValid())) + TestEqual(TEXT("returns newest revealed (20), not undelayed 30"), Revealed->FrameId, (uint64)20); + + // bIgnoreDelay bypasses the delay policy and returns the freshest rendered + // (ground-truth) frame, which is the undelayed newest (30). + TSharedPtr Fresh = Cam->GetFrameForRequest(0, /*bIgnoreDelay=*/true); + if (TestTrue(TEXT("ground-truth fetch valid"), Fresh.IsValid())) + TestEqual(TEXT("ignore-delay returns undelayed newest (30)"), Fresh->FrameId, (uint64)30); + + // No delay configured: routing collapses to the plain by-id / latest fetch. + Cam->DelaySeconds = 0.0f; + TSharedPtr NoDelay = Cam->GetFrameForRequest(0, /*bIgnoreDelay=*/false); + if (TestTrue(TEXT("no-delay fetch valid"), NoDelay.IsValid())) + TestEqual(TEXT("no delay returns latest (30)"), NoDelay->FrameId, (uint64)30); + + return true; +} + +// ============================================================================ +// URLab.CameraReadback.ResolutionAccessorIsBoundsSafe +// GetResolution() substitutes the 640x480 default for any missing or +// non-positive element, so downstream pixel sizing can never index a malformed +// resolution array out of bounds. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraResolutionAccessor, + "URLab.CameraReadback.ResolutionAccessorIsBoundsSafe", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraResolutionAccessor::RunTest(const FString& Parameters) +{ + UMjCamera* Cam = MakeReadbackTestCamera(); + if (!TestNotNull(TEXT("camera"), Cam)) + return false; + + // Well-formed pair passes through. + Cam->resolution = {800, 600}; + TestEqual(TEXT("valid width"), Cam->GetResolution().X, 800); + TestEqual(TEXT("valid height"), Cam->GetResolution().Y, 600); + + // Single element (MJCF resolution="640"): height defaults. + Cam->resolution = {640}; + TestEqual(TEXT("single-element width kept"), Cam->GetResolution().X, 640); + TestEqual(TEXT("single-element height defaults"), Cam->GetResolution().Y, 480); + + // Empty array: both default. + Cam->resolution.Reset(); + TestEqual(TEXT("empty width defaults"), Cam->GetResolution().X, 640); + TestEqual(TEXT("empty height defaults"), Cam->GetResolution().Y, 480); + + // Non-positive entries: both default. + Cam->resolution = {0, -5}; + TestEqual(TEXT("non-positive width defaults"), Cam->GetResolution().X, 640); + TestEqual(TEXT("non-positive height defaults"), Cam->GetResolution().Y, 480); + + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjCameraTests.cpp b/Source/URLabEditor/Private/Tests/MjCameraTests.cpp index b57defd3..c35e7769 100644 --- a/Source/URLabEditor/Private/Tests/MjCameraTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjCameraTests.cpp @@ -26,11 +26,16 @@ #include "MuJoCo/Components/Sensors/MjCamera.h" #include "MuJoCo/Core/MjDebugVisualizer.h" #include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "State/MjCanonicalName.h" +#include "Bridge/RpcDispatcher.h" #include "Components/SceneCaptureComponent2D.h" #include "Components/StaticMeshComponent.h" #include "Engine/StaticMesh.h" #include "Engine/TextureRenderTarget2D.h" #include "UObject/ConstructorHelpers.h" +#include "RenderingThread.h" +#include "Misc/App.h" namespace { @@ -51,7 +56,7 @@ UMjCamera* SpawnCameraAndStream(FMjUESession& Sess, EMjCameraMode Mode) // ============================================================================ // URLab.Camera.RealMode_ConfiguresFinalColorBGRA -// Default Real mode → RT is RGBA8, CaptureSource is SCS_FinalToneCurveHDR. +// Default Real mode → RT is RGBA8, CaptureSource is SCS_FinalColorLDR. // ============================================================================ IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraRealModeConfig, "URLab.Camera.RealMode_ConfiguresFinalColorBGRA", @@ -84,7 +89,7 @@ bool FMjCameraRealModeConfig::RunTest(const FString& Parameters) { TestEqual(TEXT("capture source"), (int32)Cam->CaptureComponent->CaptureSource, - (int32)ESceneCaptureSource::SCS_FinalToneCurveHDR); + (int32)ESceneCaptureSource::SCS_FinalColorLDR); } S.Cleanup(); @@ -374,3 +379,111 @@ bool FMjCameraNonSegHidesSiblings::RunTest(const FString& Parameters) S.Cleanup(); return true; } + +// ============================================================================ +// URLab.Camera.RenderOnDemandSync +// Exercises the render-on-demand path: apply a physics frame, then +// IssueSyncCapture -> FlushRenderingCommands -> HarvestCompletedReadbacks, and +// check a frame with pixels lands in the history ring. The GPU part runs only +// with a real RHI; run with -RenderOffScreen to validate actual rendering. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraRenderOnDemandSync, + "URLab.Camera.RenderOnDemandSync", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraRenderOnDemandSync::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + UMjCamera* Cam = SpawnCameraAndStream(S, EMjCameraMode::Real); + if (!TestNotNull(TEXT("camera"), Cam)) + { + S.Cleanup(); + return false; + } + + // Produce a physics frame and apply it, so the sync readback has a real + // applied-state id to tag its frame with. + S.Manager->PhysicsEngine->StepSync(1); + S.Manager->ApplyLatestRenderState(); + const uint64 AppliedId = S.Manager->GetLastAppliedFrameId(); + TestTrue(TEXT("applied frame id advanced"), AppliedId > 0); + + if (!FApp::CanEverRender()) + { + AddInfo(TEXT("No RHI: GPU capture not exercised; run with -RenderOffScreen to validate rendering")); + S.Cleanup(); + return true; + } + + // Warm the freshly-enabled render target (render-thread allocation) before + // the first capture, matching RenderCamerasSync's cold-camera warmup. + FlushRenderingCommands(); + + // Synchronous render-on-demand: capture, submit, wait on the GPU fence, harvest. + Cam->IssueSyncCapture(); + const int32 InFlight = Cam->NumInFlightReadbacks(); + FlushRenderingCommands(); + Cam->WaitAndHarvestReadbacks(1.0); + AddInfo(FString::Printf(TEXT("RenderOnDemandSync: readbacks_enqueued=%d latest_frame=%llu"), + InFlight, (unsigned long long)Cam->GetLatestFrameId())); + + FMjCameraFrame Frame; + if (TestTrue(TEXT("sync capture produced a frame in history"), Cam->GetFrame(0, Frame))) + { + AddInfo(FString::Printf(TEXT("RenderOnDemandSync: frame_id=%llu applied=%llu size=%dx%d color_px=%d"), + (unsigned long long)Frame.FrameId, (unsigned long long)AppliedId, + Frame.Width, Frame.Height, Frame.Color.Num())); + TestTrue(TEXT("frame carries rendered pixels"), Frame.Color.Num() > 0); + TestEqual(TEXT("pixel count matches frame dimensions"), + Frame.Color.Num(), Frame.Width * Frame.Height); + } + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Camera.CanonicalName_ArtSlashPart +// A camera's canonical identity is the single "/" name (no +// "camera/" infix, no raw-name aliases), and BuildCameraNameMap resolves it +// by that name alone. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjCameraCanonicalName, + "URLab.Camera.CanonicalName_ArtSlashPart", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjCameraCanonicalName::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + UMjCamera* Cam = NewObject(S.Robot, TEXT("WristCam")); + Cam->RegisterComponent(); + Cam->AttachToComponent(S.Body, FAttachmentTransformRules::KeepRelativeTransform); + + const FString ArtSeg = FMjCanonicalName::ArtSegment(S.Robot).ToString(); + const FString Canon = Cam->GetCanonicalName(); + + TestEqual(TEXT("canonical is /"), Canon, ArtSeg + TEXT("/WristCam")); + TestFalse(TEXT("no camera/ infix"), Canon.Contains(TEXT("/camera/"))); + + TMap ByName; + FURLabRpcDispatcher::BuildCameraNameMap(S.Manager, ByName); + TestEqual(TEXT("canonical name resolves to the camera"), ByName.FindRef(Canon), Cam); + TestNull(TEXT("bare component-name alias dropped"), ByName.FindRef(TEXT("WristCam"))); + TestNull(TEXT("camera/ infix alias dropped"), + ByName.FindRef(ArtSeg + TEXT("/camera/WristCam"))); + + S.Cleanup(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjControlOwnershipTests.cpp b/Source/URLabEditor/Private/Tests/MjControlOwnershipTests.cpp new file mode 100644 index 00000000..d16024ed --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjControlOwnershipTests.cpp @@ -0,0 +1,405 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjControlOwnershipTests.cpp +// +// Per-articulation control arbitration (FMjControlOwnership + the claim / +// release RPC ops + the control-write gate in the step / twist / qpos / mocap +// handlers). TTL / heartbeat cases drive the ownership object directly with the +// deterministic clock seam; the gate cases drive the dispatcher over an +// FMjUESession manager, using two source ids over one test session. +// ============================================================================ + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "MjTestHelpers.h" +#include "Bridge/ControlOwnership.h" +#include "Bridge/RpcDispatcher.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Joints/MjJoint.h" +#include "State/MjStateCollector.h" +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" + +namespace +{ +TSharedPtr CtrlOwnReq(const TCHAR* Op, const TCHAR* Source, const FString& Art) +{ + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), Op); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), Source); + R->SetStringField(TEXT("articulation"), Art); + return R; +} + +FString CtrlOwnReplyOp(const TSharedPtr& Reply) +{ + FString Op; + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("op"), Op); + return Op; +} + +FString CtrlOwnReplyCode(const TSharedPtr& Reply) +{ + FString Code; + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("code"), Code); + return Code; +} + +FString CtrlOwnReplyOwner(const TSharedPtr& Reply) +{ + FString Owner; + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("owner"), Owner); + return Owner; +} +} // namespace + +// --------------------------------------------------------------------------- +// Claim / conflict / force-steal. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipClaimAndSteal, + "URLab.ControlOwnership.ClaimAndSteal", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipClaimAndSteal::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + + // Source A claims the free art. + TSharedPtr R1 = Disp->Dispatch(CtrlOwnReq(TEXT("claim_control"), TEXT("A"), ArtName)); + TestEqual(TEXT("A claim ok"), CtrlOwnReplyOp(R1), FString(TEXT("claim_control_ok"))); + TestEqual(TEXT("A is owner"), CtrlOwnReplyOwner(R1), FString(TEXT("A"))); + + // Source B is refused with the current owner. + TSharedPtr R2 = Disp->Dispatch(CtrlOwnReq(TEXT("claim_control"), TEXT("B"), ArtName)); + TestEqual(TEXT("B refused"), CtrlOwnReplyCode(R2), FString(TEXT("control_claimed"))); + TestEqual(TEXT("conflict reports current owner"), CtrlOwnReplyOwner(R2), FString(TEXT("A"))); + + // force:true steals for B. + TSharedPtr R3 = CtrlOwnReq(TEXT("claim_control"), TEXT("B"), ArtName); + R3->SetBoolField(TEXT("force"), true); + TSharedPtr R3Reply = Disp->Dispatch(R3); + TestEqual(TEXT("B force-steal ok"), CtrlOwnReplyOp(R3Reply), FString(TEXT("claim_control_ok"))); + TestEqual(TEXT("B is owner after steal"), CtrlOwnReplyOwner(R3Reply), FString(TEXT("B"))); + + // The old owner's next write is rejected. + FString CurrentOwner; + TestTrue(TEXT("old owner A no longer owns"), + Disp->GetControlOwnership().CheckWrite(Key, TEXT("A"), CurrentOwner) + == FMjControlOwnership::EWriteCheck::NotOwner); + TestEqual(TEXT("owner is now B"), CurrentOwner, FString(TEXT("B"))); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// The control-write gate: non-owner rejected, owner allowed, unclaimed rejected. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipWriteGate, + "URLab.ControlOwnership.WriteGate", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipWriteGate::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Hinge; + Sess.Joint->bOverride_Type = true; + })) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + + auto SetQpos = [Disp, &ArtName](const TCHAR* Source, double Value) { + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("set_qpos")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + Req->SetStringField(TEXT("source"), Source); + Req->SetStringField(TEXT("target"), ArtName); + Req->SetStringField(TEXT("target_by"), TEXT("actor_name")); + TArray> Q; + Q.Add(MakeShared(Value)); + Req->SetArrayField(TEXT("qpos"), Q); + return Disp->Dispatch(Req); + }; + + // Write to an UNCLAIMED art is rejected — the safe default. + TSharedPtr Unclaimed = SetQpos(TEXT("A"), 0.1); + TestEqual(TEXT("unclaimed set_qpos rejected"), + CtrlOwnReplyCode(Unclaimed), FString(TEXT("not_control_owner"))); + + // A claims the art. + TSharedPtr Claim = Disp->Dispatch(CtrlOwnReq(TEXT("claim_control"), TEXT("A"), ArtName)); + TestEqual(TEXT("A claim ok"), CtrlOwnReplyOp(Claim), FString(TEXT("claim_control_ok"))); + + // Non-owner set_twist -> not_control_owner + owner field (gate precedes the + // twist-controller check). + { + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("set_twist")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + Req->SetStringField(TEXT("source"), TEXT("B")); + Req->SetStringField(TEXT("articulation"), ArtName); + TArray> Lin; + Lin.Add(MakeShared(1.0)); + Lin.Add(MakeShared(0.0)); + Req->SetArrayField(TEXT("linear"), Lin); + TSharedPtr Reply = Disp->Dispatch(Req); + TestEqual(TEXT("non-owner set_twist rejected"), + CtrlOwnReplyCode(Reply), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("set_twist rejection names owner"), CtrlOwnReplyOwner(Reply), FString(TEXT("A"))); + } + + // Non-owner set_qpos -> not_control_owner + owner field. + TSharedPtr NonOwner = SetQpos(TEXT("B"), 0.2); + TestEqual(TEXT("non-owner set_qpos rejected"), + CtrlOwnReplyCode(NonOwner), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("set_qpos rejection names owner"), CtrlOwnReplyOwner(NonOwner), FString(TEXT("A"))); + + // Owner set_qpos succeeds. + TSharedPtr OwnerWrite = SetQpos(TEXT("A"), 0.3); + TestEqual(TEXT("owner set_qpos ok"), CtrlOwnReplyOp(OwnerWrite), FString(TEXT("set_qpos_ok"))); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// Step-carried control is gated; an observation-only step is not. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipStepControlGate, + "URLab.ControlOwnership.StepControlGate", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipStepControlGate::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + Disp->SetActiveStepMode(EStepMode::Live); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + + // A step carrying ctrl for an unowned art is rejected as a whole. + { + TSharedPtr ArtObj = MakeShared(); + TSharedPtr CtrlMap = MakeShared(); + CtrlMap->SetNumberField(TEXT("j0"), 0.5); + ArtObj->SetObjectField(TEXT("ctrl_map"), CtrlMap); + TSharedPtr PerArt = MakeShared(); + PerArt->SetObjectField(ArtName, ArtObj); + + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("step")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + Req->SetObjectField(TEXT("per_articulation"), PerArt); + + TSharedPtr Reply = Disp->Dispatch(Req); + TestEqual(TEXT("unowned control step rejected"), + CtrlOwnReplyCode(Reply), FString(TEXT("not_control_owner"))); + } + + // The same step without a control payload succeeds (observation is free). + { + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("step")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + TSharedPtr Reply = Disp->Dispatch(Req); + TestEqual(TEXT("observation-only step ok"), + CtrlOwnReplyOp(Reply), FString(TEXT("step_ok"))); + } + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// TTL frees a dropped owner. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipTtlExpiry, + "URLab.ControlOwnership.TtlExpiry", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipTtlExpiry::RunTest(const FString& Parameters) +{ + FMjControlOwnership Own; + const FName Art(TEXT("robot")); + FString Cur; + + Own.SetClockOverrideForTest(100.0); + TestTrue(TEXT("A claims with a 5s TTL"), + Own.Claim(Art, TEXT("A"), 5.0, false, Cur) == FMjControlOwnership::EClaimResult::Ok); + + // Past the TTL with no owner activity, the claim is free. + Own.SetClockOverrideForTest(107.0); + TestTrue(TEXT("B claims the expired art"), + Own.Claim(Art, TEXT("B"), 5.0, false, Cur) == FMjControlOwnership::EClaimResult::Ok); + TestTrue(TEXT("B now owns"), + Own.CheckWrite(Art, TEXT("B"), Cur) == FMjControlOwnership::EWriteCheck::Ok); + + return true; +} + +// --------------------------------------------------------------------------- +// A write heartbeats the claim: activity within the TTL keeps ownership alive. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipHeartbeat, + "URLab.ControlOwnership.Heartbeat", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipHeartbeat::RunTest(const FString& Parameters) +{ + FMjControlOwnership Own; + const FName Art(TEXT("robot")); + FString Cur; + + Own.SetClockOverrideForTest(100.0); + Own.Claim(Art, TEXT("A"), 5.0, false, Cur); + + // A write inside the window refreshes LastActivity. + Own.SetClockOverrideForTest(104.0); + TestTrue(TEXT("owner write ok, refreshes activity"), + Own.CheckWrite(Art, TEXT("A"), Cur) == FMjControlOwnership::EWriteCheck::Ok); + + // Now past the ORIGINAL TTL (100+5) but within the refreshed one (104+5): + // ownership holds because the write reset the clock. + Own.SetClockOverrideForTest(108.0); + TestTrue(TEXT("ownership held past original TTL after heartbeat"), + Own.CheckWrite(Art, TEXT("A"), Cur) == FMjControlOwnership::EWriteCheck::Ok); + TestTrue(TEXT("a second source still cannot claim the live art"), + Own.Claim(Art, TEXT("B"), 5.0, false, Cur) == FMjControlOwnership::EClaimResult::AlreadyOwned); + + return true; +} + +// --------------------------------------------------------------------------- +// Release by a non-owner fails; OnManagerGone clears every claim. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjControlOwnershipReleaseAndReset, + "URLab.ControlOwnership.ReleaseAndReset", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjControlOwnershipReleaseAndReset::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + + // A claims. + Disp->Dispatch(CtrlOwnReq(TEXT("claim_control"), TEXT("A"), ArtName)); + + // Release by a non-owner fails and reports the current owner. + TSharedPtr BadRelease = + Disp->Dispatch(CtrlOwnReq(TEXT("release_control"), TEXT("B"), ArtName)); + TestEqual(TEXT("non-owner release rejected"), + CtrlOwnReplyCode(BadRelease), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("release rejection names owner"), CtrlOwnReplyOwner(BadRelease), FString(TEXT("A"))); + + // Release by the owner frees it, and a new source can claim. + TSharedPtr GoodRelease = + Disp->Dispatch(CtrlOwnReq(TEXT("release_control"), TEXT("A"), ArtName)); + TestEqual(TEXT("owner release ok"), CtrlOwnReplyOp(GoodRelease), FString(TEXT("release_control_ok"))); + TSharedPtr ReClaim = + Disp->Dispatch(CtrlOwnReq(TEXT("claim_control"), TEXT("B"), ArtName)); + TestEqual(TEXT("freed art re-claimable"), CtrlOwnReplyOp(ReClaim), FString(TEXT("claim_control_ok"))); + + // OnManagerGone drops every claim (arts die with the world). + Disp->OnManagerGone(); + FString Cur; + TestTrue(TEXT("OnManagerGone cleared claims"), + Disp->GetControlOwnership().CheckWrite(Key, TEXT("B"), Cur) + == FMjControlOwnership::EWriteCheck::NotOwner); + + S.Cleanup(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjImportTests.cpp b/Source/URLabEditor/Private/Tests/MjImportTests.cpp index b1839b33..62ea8bb5 100644 --- a/Source/URLabEditor/Private/Tests/MjImportTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjImportTests.cpp @@ -297,6 +297,40 @@ bool FTest_MjImport_MJ_TendonArmature::RunTest(const FString&) return true; } +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FTest_MjImport_MJ_SensorSection, + "URLab.Import.MJ_SensorSection", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) +bool FTest_MjImport_MJ_SensorSection::RunTest(const FString&) +{ + // The container is a wrapper; the importer must recurse into its + // per-type children (like ). A regression drops the whole section. + FMjTestSession S; + if (!S.CompileXml(TEXT(R"( + + + + + + + + + + + + + + + )"))) + { + AddError(S.LastError); + return false; + } + + TestEqual(TEXT("nsensor (all three children imported)"), (int)S.m->nsensor, 3); + S.Cleanup(); + return true; +} + IMPLEMENT_SIMPLE_AUTOMATION_TEST(FTest_MjImport_MJ_EqualityPolycoef, "URLab.Import.MJ_EqualityPolycoef", EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) diff --git a/Source/URLabEditor/Private/Tests/MjJointStateTests.cpp b/Source/URLabEditor/Private/Tests/MjJointStateTests.cpp new file mode 100644 index 00000000..cee21d9a --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjJointStateTests.cpp @@ -0,0 +1,195 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjJointStateTests.cpp +// +// Correctness tests for UURLabRosPublishTransport::FillJointState, the pure +// IR -> sensor_msgs/JointState transform. FillJointState compiles in every +// configuration (it has no rcl dependency), so these tests need no ROS fence and +// run on every build. They cover: +// - free/ball joints are excluded so names stay aligned with positions / +// velocities (the free-base misalignment + truncation bug), +// - a fixed-base arm (all 1-DOF joints) is passed through unchanged, +// - the qpos - qpos0 shift is preserved, +// - effort is filled from the force of the actuator that drives each joint (keyed +// by target joint, not by the actuator's own name), zero for undriven joints, +// and empty when the art has no actuators. +// ============================================================================ + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" + +namespace +{ +FMjJointState MakeJoint(const TCHAR* Name, EMjJointType Type, + TArray QPos, TArray QVel, TArray RefPos = {}) +{ + FMjJointState J; + J.Name = FName(Name); + J.Type = Type; + J.QPos = MoveTemp(QPos); + J.QVel = MoveTemp(QVel); + J.RefPos = MoveTemp(RefPos); + return J; +} + +// Name and TargetJoint are deliberately distinct: an actuator's own name need not +// match the joint it drives, and effort must map by the target joint, not the name. +FMjActuatorState MakeActuator(const TCHAR* Name, const TCHAR* TargetJoint, double Force) +{ + FMjActuatorState A; + A.Name = FName(Name); + A.TargetJoint = FName(TargetJoint); + A.Force = Force; + return A; +} +} // namespace + +// --------------------------------------------------------------------------- +// 1. Free base + 1-DOF joints: the free root is dropped, and every remaining +// entry's name lines up with its own position / velocity / effort (no shift, +// no truncation). The free root is placed first, the layout a legged robot +// uses, so the old whole-slice append would have shifted every hinge. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjJointStateFreeBaseAlignment, + "URLab.JointState.FreeBaseAlignment", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjJointStateFreeBaseAlignment::RunTest(const FString& Parameters) +{ + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + Art.Joints.Add(MakeJoint(TEXT("root"), EMjJointType::Free, + {0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0}, {0.0, 0.0, 0.0, 0.0, 0.0, 0.0})); + Art.Joints.Add(MakeJoint(TEXT("fl_hip"), EMjJointType::Hinge, + {0.10}, {1.10}, {0.04})); + Art.Joints.Add(MakeJoint(TEXT("fl_knee"), EMjJointType::Hinge, + {0.20}, {1.20})); + Art.Joints.Add(MakeJoint(TEXT("fl_slide"), EMjJointType::Slide, + {0.30}, {1.30}, {0.05})); + + // fl_hip and fl_knee are driven 1:1 by differently-named actuators; fl_slide has + // no actuator. + Art.Actuators.Add(MakeActuator(TEXT("act_fl_hip"), TEXT("fl_hip"), 5.0)); + Art.Actuators.Add(MakeActuator(TEXT("act_fl_knee"), TEXT("fl_knee"), -3.0)); + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + // The free root is excluded; only the three 1-DOF joints remain, and every + // parallel array is the same length (no truncation). + TestEqual(TEXT("entry count == 1-DOF joint count"), Names.Num(), 3); + TestEqual(TEXT("positions align with names"), Positions.Num(), Names.Num()); + TestEqual(TEXT("velocities align with names"), Velocities.Num(), Names.Num()); + TestEqual(TEXT("efforts align with names"), Efforts.Num(), Names.Num()); + + TestEqual(TEXT("name[0]"), Names[0], FString(TEXT("fl_hip"))); + TestEqual(TEXT("name[1]"), Names[1], FString(TEXT("fl_knee"))); + TestEqual(TEXT("name[2]"), Names[2], FString(TEXT("fl_slide"))); + + // name[i] carries fl_hip's own values, proving the free root did not shift it. + TestEqual(TEXT("fl_hip position is qpos - qpos0"), Positions[0], 0.06, 1e-9); + TestEqual(TEXT("fl_knee position unshifted (no RefPos)"), Positions[1], 0.20, 1e-9); + TestEqual(TEXT("fl_slide position is qpos - qpos0"), Positions[2], 0.25, 1e-9); + TestEqual(TEXT("fl_hip velocity"), Velocities[0], 1.10, 1e-9); + TestEqual(TEXT("fl_knee velocity"), Velocities[1], 1.20, 1e-9); + TestEqual(TEXT("fl_slide velocity"), Velocities[2], 1.30, 1e-9); + + // Effort follows the actuator that drives the joint (by target joint, not by + // the actuator's own name); undriven joints report 0. + TestEqual(TEXT("fl_hip effort from actuator force"), Efforts[0], 5.0, 1e-9); + TestEqual(TEXT("fl_knee effort from actuator force"), Efforts[1], -3.0, 1e-9); + TestEqual(TEXT("fl_slide effort zero (no actuator)"), Efforts[2], 0.0, 1e-9); + + return true; +} + +// --------------------------------------------------------------------------- +// 2. Fixed-base arm (all 1-DOF joints, no free/ball): every joint is kept and +// passed through unchanged, so the fix does not disturb the common case. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjJointStateFixedBaseUnchanged, + "URLab.JointState.FixedBaseUnchanged", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjJointStateFixedBaseUnchanged::RunTest(const FString& Parameters) +{ + FMjArticulationState Art; + Art.Name = FName(TEXT("arm")); + Art.Joints.Add(MakeJoint(TEXT("j0"), EMjJointType::Hinge, {0.1}, {0.01})); + Art.Joints.Add(MakeJoint(TEXT("j1"), EMjJointType::Hinge, {0.2}, {0.02})); + Art.Joints.Add(MakeJoint(TEXT("j2"), EMjJointType::Slide, {0.3}, {0.03})); + Art.Actuators.Add(MakeActuator(TEXT("motor_j1"), TEXT("j1"), 7.5)); + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + TestEqual(TEXT("all joints kept"), Names.Num(), 3); + TestEqual(TEXT("positions align"), Positions.Num(), 3); + TestEqual(TEXT("velocities align"), Velocities.Num(), 3); + TestEqual(TEXT("efforts align"), Efforts.Num(), 3); + + TestEqual(TEXT("j0 position"), Positions[0], 0.1, 1e-9); + TestEqual(TEXT("j2 velocity"), Velocities[2], 0.03, 1e-9); + TestEqual(TEXT("j0 effort zero (undriven)"), Efforts[0], 0.0, 1e-9); + TestEqual(TEXT("j1 effort from actuator"), Efforts[1], 7.5, 1e-9); + TestEqual(TEXT("j2 effort zero (undriven)"), Efforts[2], 0.0, 1e-9); + + return true; +} + +// --------------------------------------------------------------------------- +// 3. No actuators: effort is left empty (distinct from an all-zero array) so a +// consumer can tell "no effort data" from "zero force". +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjJointStateEffortEmptyWithoutActuators, + "URLab.JointState.EffortEmptyWithoutActuators", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjJointStateEffortEmptyWithoutActuators::RunTest(const FString& Parameters) +{ + FMjArticulationState Art; + Art.Name = FName(TEXT("passive")); + Art.Joints.Add(MakeJoint(TEXT("j0"), EMjJointType::Hinge, {0.1}, {0.0})); + Art.Joints.Add(MakeJoint(TEXT("j1"), EMjJointType::Hinge, {0.2}, {0.0})); + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + TestEqual(TEXT("both joints present"), Names.Num(), 2); + TestEqual(TEXT("effort empty when no actuators drive the art"), Efforts.Num(), 0); + + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjLevelOpsTests.cpp b/Source/URLabEditor/Private/Tests/MjLevelOpsTests.cpp index 28bf2d6a..cc9c6bdd 100644 --- a/Source/URLabEditor/Private/Tests/MjLevelOpsTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjLevelOpsTests.cpp @@ -9,8 +9,10 @@ #include "Misc/FileHelper.h" #include "HAL/FileManager.h" #include "Dom/JsonObject.h" +#include "Containers/Ticker.h" #include "Bridge/OpRegistry.h" +#include "Bridge/RpcErrorCodes.h" #include "MjLevelOps.h" #include "MuJoCo/Core/AMjManager.h" @@ -560,21 +562,52 @@ IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjLevelOpsImportXmlErrors, bool FMjLevelOpsImportXmlErrors::RunTest(const FString& Parameters) { auto Handler = URLabOpRegistry::GetHandler(TEXT("import_xml")); - if (!Handler) + auto StatusHandler = URLabOpRegistry::GetHandler(TEXT("op_status")); + if (!Handler || !StatusHandler) { - AddError(TEXT("import_xml handler not installed")); + AddError(TEXT("import_xml / op_status handler not installed")); return false; } + // import_xml is async now: the handler returns op_started + job_id and the + // real work (plus its error reply) runs on the next game-thread tick, read + // back via op_status. Drive that to the terminal result here. + auto RunToResult = [&](TSharedPtr Req) -> TSharedPtr { + TSharedPtr Started = Handler(Req); + FString JobId; + Started->TryGetStringField(TEXT("job_id"), JobId); + if (JobId.IsEmpty()) + return Started; // synchronous reply (no job) -> use directly + for (int32 i = 0; i < 100; ++i) + { + FTSTicker::GetCoreTicker().Tick(0.05f); + TSharedPtr SReq = MakeShared(); + SReq->SetStringField(TEXT("op"), TEXT("op_status")); + SReq->SetStringField(TEXT("job_id"), JobId); + TSharedPtr SRep = StatusHandler(SReq); + FString State; + SRep->TryGetStringField(TEXT("state"), State); + if (State != TEXT("running")) + { + const TSharedPtr* Res = nullptr; + if (SRep->TryGetObjectField(TEXT("result"), Res) && Res) + return *Res; + return SRep; + } + } + return nullptr; + }; + // Missing 'path' field. { TSharedPtr Req = MakeShared(); Req->SetStringField(TEXT("op"), TEXT("import_xml")); - TSharedPtr Reply = Handler(Req); + TSharedPtr Reply = RunToResult(Req); FString Code; - Reply->TryGetStringField(TEXT("code"), Code); + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("code"), Code); TestEqual(TEXT("missing path -> missing_field"), - Code, FString(TEXT("missing_field"))); + Code, FString(URLabError::MissingField)); } // Non-existent file. @@ -582,9 +615,10 @@ bool FMjLevelOpsImportXmlErrors::RunTest(const FString& Parameters) TSharedPtr Req = MakeShared(); Req->SetStringField(TEXT("op"), TEXT("import_xml")); Req->SetStringField(TEXT("path"), TEXT("C:/no/such/path/never.xml")); - TSharedPtr Reply = Handler(Req); + TSharedPtr Reply = RunToResult(Req); FString Code; - Reply->TryGetStringField(TEXT("code"), Code); + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("code"), Code); TestEqual(TEXT("missing file -> import_failed"), Code, FString(TEXT("import_failed"))); } diff --git a/Source/URLabEditor/Private/Tests/MjModelUploadTests.cpp b/Source/URLabEditor/Private/Tests/MjModelUploadTests.cpp new file mode 100644 index 00000000..331f9b3e --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjModelUploadTests.cpp @@ -0,0 +1,302 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjModelUploadTests.cpp +// +// Unit coverage for network model upload (render farm phase 3/4): +// - FURLabAssetCache SHA-256 vector + atomic Put/Has/GetPath round-trip +// - upload_model_manifest: upload_id + need lists, cache-hit dedup, caps, +// path-traversal rejection +// - upload_model_chunk: reassembly, hash-mismatch reject, oversize reject, +// unknown-upload reject +// +// The full commit -> import_xml path needs a live editor (import_xml stands up +// a Blueprint asset); it is exercised by the orchestrator's live upload test. +// ============================================================================ + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Bridge/AssetCache.h" +#include "Bridge/RpcDispatcher.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/RpcErrorCodes.h" +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" +#include "HAL/FileManager.h" +#include "Misc/Base64.h" +#include "Misc/FileHelper.h" +#include "Misc/Guid.h" +#include "Misc/Paths.h" + +namespace +{ +TArray RandomBytes(int32 N) +{ + TArray Out; + Out.Reserve(N); + for (int32 i = 0; i < N; ++i) + Out.Add(static_cast(FMath::Rand() & 0xff)); + return Out; +} + +TSharedPtr MakeManifest(const FString& Session, const FString& XmlSha, + const TArray>& Assets /* name, sha */, + const TArray& Sizes, int64 TotalBytes) +{ + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("upload_model_manifest")); + R->SetStringField(TEXT("session_id"), Session); + R->SetStringField(TEXT("xml_sha256"), XmlSha); + TArray> Arr; + for (int32 i = 0; i < Assets.Num(); ++i) + { + TSharedPtr A = MakeShared(); + A->SetStringField(TEXT("name"), Assets[i].Key); + A->SetStringField(TEXT("sha256"), Assets[i].Value); + A->SetNumberField(TEXT("size"), static_cast(Sizes.IsValidIndex(i) ? Sizes[i] : 0)); + Arr.Add(MakeShared(A)); + } + R->SetArrayField(TEXT("assets"), Arr); + R->SetNumberField(TEXT("total_bytes"), static_cast(TotalBytes)); + return R; +} + +TSharedPtr MakeChunk(const FString& Session, const FString& UploadId, + const FString& Kind, const FString& Name, int64 Total, int64 Offset, + const TArray& Data) +{ + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("upload_model_chunk")); + R->SetStringField(TEXT("session_id"), Session); + R->SetStringField(TEXT("upload_id"), UploadId); + R->SetStringField(TEXT("kind"), Kind); + R->SetStringField(TEXT("name"), Name); + R->SetNumberField(TEXT("total"), static_cast(Total)); + R->SetNumberField(TEXT("offset"), static_cast(Offset)); + // Binary field arrives on the wire under a __b64__ key (see MsgpackHelpers). + R->SetStringField(TEXT("data__b64__"), FBase64::Encode(Data)); + return R; +} + +FString ReplyCode(const TSharedPtr& Reply) +{ + FString Code; + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("code"), Code); + return Code; +} +} // namespace + +// --------------------------------------------------------------------------- +// 1. Content-addressed cache: SHA-256 vector + atomic Put/Has/Get round-trip. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjModelUploadAssetCache, + "URLab.ModelUpload.AssetCache", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjModelUploadAssetCache::RunTest(const FString& Parameters) +{ + // Known FIPS 180-4 vector: SHA-256("abc"). + const FString Abc = TEXT("abc"); + FTCHARToUTF8 AbcUtf8(*Abc); + const FString AbcHash = FURLabAssetCache::Sha256Hex( + reinterpret_cast(AbcUtf8.Get()), AbcUtf8.Length()); + TestEqual(TEXT("SHA-256(\"abc\")"), AbcHash, + FString(TEXT("ba7816bf8f01cfea414140de5dae2223b00361a396177a9cb410ff61f20015ad"))); + + // Empty input vector. + TestEqual(TEXT("SHA-256(\"\")"), FURLabAssetCache::Sha256Hex(nullptr, 0), + FString(TEXT("e3b0c44298fc1c149afbf4c8996fb92427ae41e4649b934ca495991b7852b855"))); + + const FString Root = FPaths::Combine(FPaths::ProjectIntermediateDir(), + TEXT("URLabTest"), FGuid::NewGuid().ToString(EGuidFormats::Digits)); + FURLabAssetCache Cache(Root); + + const TArray Bytes = RandomBytes(4096); + const FString Sha = FURLabAssetCache::Sha256Hex(Bytes); + + TestFalse(TEXT("miss before Put"), Cache.Has(Sha)); + TestTrue(TEXT("Put succeeds"), Cache.Put(Sha, Bytes)); + TestTrue(TEXT("hit after Put"), Cache.Has(Sha)); + + FString Path; + TestTrue(TEXT("GetPath resolves"), Cache.GetPath(Sha, Path)); + TestTrue(TEXT("blob file exists"), IFileManager::Get().FileExists(*Path)); + TestTrue(TEXT("blob sharded under sha[:2]"), Path.Contains(Sha.Left(2))); + + TArray Read; + TestTrue(TEXT("blob reads back"), FFileHelper::LoadFileToArray(Read, *Path)); + TestEqual(TEXT("round-trip bytes match"), Read.Num(), Bytes.Num()); + TestTrue(TEXT("round-trip content identical"), Read == Bytes); + + // Idempotent: a second Put of identical content is a no-op success. + TestTrue(TEXT("second Put idempotent"), Cache.Put(Sha, Bytes)); + + IFileManager::Get().DeleteDirectory(*Root, /*RequireExists=*/false, /*Tree=*/true); + return true; +} + +// --------------------------------------------------------------------------- +// 2. Manifest + chunk: reassembly, dedup, and every 4.6 validation reject. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjModelUploadManifestChunk, + "URLab.ModelUpload.ManifestChunk", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjModelUploadManifestChunk::RunTest(const FString& Parameters) +{ + UURLabBridgeServer* Server = NewObject(); + Server->AddToRoot(); + Server->Start(TEXT("")); // dispatcher only, no ZMQ bind + FURLabRpcDispatcher* Disp = Server->GetDispatcher(); + if (!Disp) + { + AddError(TEXT("Server has no dispatcher")); + Server->RemoveFromRoot(); + return false; + } + const FString Session = TEXT("upload-session"); + Disp->SetActiveSessionIdForTest(Session); + + // Random content per run so the cache state (fresh vs seeded) is + // deterministic regardless of the shared on-disk cache root. + const TArray XmlBytes = RandomBytes(1024); + const TArray MeshBytes = RandomBytes(8192); + const FString XmlSha = FURLabAssetCache::Sha256Hex(XmlBytes); + const FString MeshSha = FURLabAssetCache::Sha256Hex(MeshBytes); + + // --- manifest: fresh hashes -> everything is needed. --- + TSharedPtr ManReply = Disp->Dispatch(MakeManifest(Session, XmlSha, + { + {TEXT("mesh.STL"), MeshSha} + }, + {MeshBytes.Num()}, XmlBytes.Num() + MeshBytes.Num())); + FString ManOp; + ManReply->TryGetStringField(TEXT("op"), ManOp); + TestEqual(TEXT("manifest ok"), ManOp, FString(TEXT("upload_model_manifest_ok"))); + + FString UploadId; + ManReply->TryGetStringField(TEXT("upload_id"), UploadId); + TestTrue(TEXT("upload_id issued"), !UploadId.IsEmpty()); + + bool bNeedXml = false; + ManReply->TryGetBoolField(TEXT("need_xml"), bNeedXml); + TestTrue(TEXT("need_xml true (cache miss)"), bNeedXml); + + const TArray>* Need = nullptr; + ManReply->TryGetArrayField(TEXT("need_assets"), Need); + TestTrue(TEXT("need_assets lists the mesh"), Need && Need->Num() == 1); + + double MaxAsset = 0, MaxTotal = 0; + ManReply->TryGetNumberField(TEXT("max_asset_bytes"), MaxAsset); + ManReply->TryGetNumberField(TEXT("max_total_bytes"), MaxTotal); + TestTrue(TEXT("max_asset_bytes advertised"), MaxAsset > 0); + TestTrue(TEXT("max_total_bytes advertised"), MaxTotal >= MaxAsset); + + // --- chunk: xml in one shot completes. --- + TSharedPtr XmlChunk = Disp->Dispatch( + MakeChunk(Session, UploadId, TEXT("xml"), TEXT(""), XmlBytes.Num(), 0, XmlBytes)); + bool bXmlComplete = false; + XmlChunk->TryGetBoolField(TEXT("complete"), bXmlComplete); + TestTrue(TEXT("xml chunk completes"), bXmlComplete); + + // --- chunk: hash mismatch (correct total, wrong bytes) is rejected. --- + TArray Corrupt = MeshBytes; + Corrupt[0] ^= 0xff; + TSharedPtr BadChunk = Disp->Dispatch( + MakeChunk(Session, UploadId, TEXT("asset"), TEXT("mesh.STL"), MeshBytes.Num(), 0, Corrupt)); + TestEqual(TEXT("hash mismatch rejected"), ReplyCode(BadChunk), FString(TEXT("hash_mismatch"))); + + // --- chunk: correct bytes complete the asset (retry after mismatch). --- + TSharedPtr GoodChunk = Disp->Dispatch( + MakeChunk(Session, UploadId, TEXT("asset"), TEXT("mesh.STL"), MeshBytes.Num(), 0, MeshBytes)); + bool bMeshComplete = false; + GoodChunk->TryGetBoolField(TEXT("complete"), bMeshComplete); + TestTrue(TEXT("mesh chunk completes after retry"), bMeshComplete); + + // --- re-manifest: both blobs now cached -> nothing needed (dedup). --- + TSharedPtr ReMan = Disp->Dispatch(MakeManifest(Session, XmlSha, + { + {TEXT("mesh.STL"), MeshSha} + }, + {MeshBytes.Num()}, XmlBytes.Num() + MeshBytes.Num())); + bool bNeedXml2 = true; + ReMan->TryGetBoolField(TEXT("need_xml"), bNeedXml2); + const TArray>* Need2 = nullptr; + ReMan->TryGetArrayField(TEXT("need_assets"), Need2); + TestFalse(TEXT("cache hit: need_xml false"), bNeedXml2); + TestTrue(TEXT("cache hit: need_assets empty"), Need2 && Need2->Num() == 0); + + // --- manifest: path-traversal asset name is rejected. --- + TSharedPtr Trav = Disp->Dispatch(MakeManifest(Session, XmlSha, + { + {TEXT("../evil.STL"), MeshSha} + }, + {1}, 1)); + TestEqual(TEXT("path traversal rejected"), ReplyCode(Trav), FString(URLabError::BadRequest)); + + TSharedPtr Abs = Disp->Dispatch(MakeManifest(Session, XmlSha, + { + {TEXT("C:\\evil.STL"), MeshSha} + }, + {1}, 1)); + TestEqual(TEXT("drive-letter name rejected"), ReplyCode(Abs), FString(URLabError::BadRequest)); + + // --- chunk: oversize (total beyond max_asset_bytes) -> payload_too_large. --- + TSharedPtr Oversize = Disp->Dispatch(MakeChunk(Session, UploadId, + TEXT("asset"), TEXT("mesh.STL"), static_cast(MaxAsset) + 1, 0, {0x00})); + TestEqual(TEXT("oversize chunk -> payload_too_large"), + ReplyCode(Oversize), FString(TEXT("payload_too_large"))); + + // --- chunk: unknown upload_id -> unknown_upload. --- + TSharedPtr Unknown = Disp->Dispatch(MakeChunk(Session, + FGuid::NewGuid().ToString(EGuidFormats::DigitsWithHyphens), + TEXT("xml"), TEXT(""), XmlBytes.Num(), 0, XmlBytes)); + TestEqual(TEXT("unknown upload_id -> unknown_upload"), + ReplyCode(Unknown), FString(TEXT("unknown_upload"))); + + // --- commit: unknown upload_id -> unknown_upload. --- + TSharedPtr CommitReq = MakeShared(); + CommitReq->SetStringField(TEXT("op"), TEXT("upload_model_commit")); + CommitReq->SetStringField(TEXT("session_id"), Session); + CommitReq->SetStringField(TEXT("upload_id"), + FGuid::NewGuid().ToString(EGuidFormats::DigitsWithHyphens)); + TSharedPtr CommitReply = Disp->Dispatch(CommitReq); + TestEqual(TEXT("commit unknown upload_id -> unknown_upload"), + ReplyCode(CommitReply), FString(TEXT("unknown_upload"))); + + // Hygiene: the dispatcher writes verified blobs into the shared process + // cache. Drop the two we seeded so the test leaves no residue. + { + FURLabAssetCache& Cache = FURLabAssetCache::Get(); + FString P; + if (Cache.GetPath(XmlSha, P)) + IFileManager::Get().Delete(*P, /*RequireExists=*/false, /*EvenReadOnly=*/true); + if (Cache.GetPath(MeshSha, P)) + IFileManager::Get().Delete(*P, /*RequireExists=*/false, /*EvenReadOnly=*/true); + } + + Server->Stop(); + Server->RemoveFromRoot(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjPhysicsTests.cpp b/Source/URLabEditor/Private/Tests/MjPhysicsTests.cpp index 1d868da3..5c6a2fed 100644 --- a/Source/URLabEditor/Private/Tests/MjPhysicsTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjPhysicsTests.cpp @@ -387,9 +387,8 @@ bool FMjPhysicsSleep_EnableFlagSet::RunTest(const FString& Parameters) return false; } - constexpr int MJ_ENBL_SLEEP = 1 << 5; TestTrue(TEXT("mjENBL_SLEEP bit set when bEnableSleep=true"), - (S.Manager->PhysicsEngine->m_model->opt.enableflags & MJ_ENBL_SLEEP) != 0); + (S.Manager->PhysicsEngine->m_model->opt.enableflags & mjENBL_SLEEP) != 0); TestTrue(TEXT("sleep_tolerance matches Options.SleepTolerance"), FMath::Abs((float)S.Manager->PhysicsEngine->m_model->opt.sleep_tolerance - 1e-3f) < 1e-6f); @@ -414,9 +413,8 @@ bool FMjPhysicsSleep_DisableFlagClear::RunTest(const FString& Parameters) return false; } - constexpr int MJ_ENBL_SLEEP = 1 << 5; TestTrue(TEXT("mjENBL_SLEEP NOT set when bEnableSleep=false (default)"), - (S.Manager->PhysicsEngine->m_model->opt.enableflags & MJ_ENBL_SLEEP) == 0); + (S.Manager->PhysicsEngine->m_model->opt.enableflags & mjENBL_SLEEP) == 0); S.Cleanup(); return true; diff --git a/Source/URLabEditor/Private/Tests/MjRosLinkTests.cpp b/Source/URLabEditor/Private/Tests/MjRosLinkTests.cpp new file mode 100644 index 00000000..5d68af8f --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjRosLinkTests.cpp @@ -0,0 +1,1029 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjRosLinkTests.cpp +// +// Link + behaviour tests for the in-process ROS 2 output: +// - FURLabRosContext init / shutdown round-trip and idempotent double-init +// - UURLabRosPublishTransport creates JointState publishers and publishes a +// collected IR against real rcl without error +// - FillJointState maps the IR to the JointState parallel arrays correctly +// +// The whole file is compiled only when ROS 2 is linked (URLAB_WITH_ROS2). The +// context-dependent tests early-return true with a log when no live ROS context +// is available, so CI stays green on machines without a running DDS. Byte-level +// rosidl fill correctness over the wire is covered by the standalone harness in +// ros/urlab_ros_ws. +// ============================================================================ + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Transport/RosContext.h" +#include "Transport/RosPublishTransport.h" +#include "Transport/RosRpcTransport.h" +#include "Transport/RosOutputProvider.h" +#include "Transport/SnapshotPublisher.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" +#include "MjTestHelpers.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "UserChannels/MjUserChannelComponent.h" +#include "Bridge/BridgeServer.h" +#include "Bridge/ControlOwnership.h" +#include "Bridge/RpcDispatcher.h" +#include "Dom/JsonValue.h" +#include "Dom/JsonObject.h" +#include + +namespace +{ +// A minimal collected IR: one articulation with two hinge joints and a free base, +// so the DOF flattening (1/1 hinge, 7/6 free) is exercised without a live model. +FMjStateSnapshot MakeSnapshot() +{ + FMjStateSnapshot Snap; + Snap.StructureVersion = 1; + Snap.Clock.SimSec = 3; + Snap.Clock.SimNsec = 500000000; + + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + FMjJointState Hip; + Hip.Name = FName(TEXT("fl_hip")); + Hip.Type = EMjJointType::Hinge; + Hip.QPos = {0.10}; + Hip.QVel = {1.10}; + Art.Joints.Add(Hip); + + FMjJointState Knee; + Knee.Name = FName(TEXT("fl_knee")); + Knee.Type = EMjJointType::Hinge; + Knee.QPos = {0.20}; + Knee.QVel = {1.20}; + Art.Joints.Add(Knee); + + FMjJointState Root; + Root.Name = FName(TEXT("root")); + Root.Type = EMjJointType::Free; + Root.QPos = {0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0}; + Root.QVel = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + Art.Joints.Add(Root); + + Snap.Articulations.Add(Art); + return Snap; +} +} // namespace + +// --------------------------------------------------------------------------- +// 1. Context round-trip + idempotent double-init +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosContextRoundTrip, + "URLab.Ros.ContextRoundTrip", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosContextRoundTrip::RunTest(const FString& Parameters) +{ + FURLabRosContext& Ctx = FURLabRosContext::Get(); + Ctx.Initialize(); + if (!Ctx.IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.ContextRoundTrip: no live ROS context; skipping.")); + return true; + } + + UrlabRclContext* const Handle = Ctx.GetHandle(); + TestNotNull(TEXT("context handle after init"), Handle); + + // Double-init is a no-op: the same handle, no second context. + Ctx.Initialize(); + TestTrue(TEXT("double-init keeps the same handle"), Ctx.GetHandle() == Handle); + + // Teardown then bring it back, leaving the process context available for the + // rest of the session. + Ctx.Shutdown(); + TestFalse(TEXT("unavailable after shutdown"), Ctx.IsAvailable()); + Ctx.Initialize(); + TestTrue(TEXT("available again after re-init"), Ctx.IsAvailable()); + + return true; +} + +// --------------------------------------------------------------------------- +// 2. Publisher create + publish a collected IR against real rcl +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosPublishJointState, + "URLab.Ros.PublishJointState", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosPublishJointState::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.PublishJointState: no live ROS context; skipping.")); + return true; + } + + UURLabRosPublishTransport* Transport = NewObject(); + TestNotNull(TEXT("transport object"), Transport); + TestTrue(TEXT("transport init"), Transport->TransportInit()); + + const FMjStateSnapshot Snap = MakeSnapshot(); + // First call builds the publisher set; second reuses it (StructureVersion + // unchanged). Neither should crash or tear the context down. + Transport->PublishState(Snap); + Transport->PublishState(Snap); + TestTrue(TEXT("context still available after publish"), + FURLabRosContext::Get().IsAvailable()); + + Transport->TransportShutdown(); + return true; +} + +// --------------------------------------------------------------------------- +// 3. FillJointState: names = part segments, arrays sized to joint DOF counts +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosFillJointState, + "URLab.Ros.FillJointState", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosFillJointState::RunTest(const FString& Parameters) +{ + // Pure IR -> arrays; no live ROS context needed. + const FMjStateSnapshot Snap = MakeSnapshot(); + const FMjArticulationState& Art = Snap.Articulations[0]; + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + // Only 1-DoF joints are emitted; the free "root" joint is excluded so + // name[i] aligns 1:1 with position[i] / velocity[i]. + TestEqual(TEXT("name count == 1-DoF joint count"), Names.Num(), 2); + TestEqual(TEXT("name[0]"), Names[0], FString(TEXT("fl_hip"))); + TestEqual(TEXT("name[1]"), Names[1], FString(TEXT("fl_knee"))); + + TestEqual(TEXT("positions aligned to names"), Positions.Num(), 2); + TestEqual(TEXT("velocities aligned to names"), Velocities.Num(), 2); + TestEqual(TEXT("position[0] is hip qpos"), Positions[0], 0.10); + TestEqual(TEXT("velocity[1] is knee qvel"), Velocities[1], 1.20); + + return true; +} + +// --------------------------------------------------------------------------- +// 4. FillImu: paired gyro+accel -> both fields; unpaired gyro -> angular only +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosFillImu, + "URLab.Ros.FillImu", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosFillImu::RunTest(const FString& Parameters) +{ + // Pure IR -> Imu components; no live ROS context needed. + + // Paired gyro + accel on one art. + { + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + FMjSensorState Gyro; + Gyro.Semantic = EMjSensorSemantic::Gyro; + Gyro.Values = {0.1, 0.2, 0.3}; + Art.Sensors.Add(Gyro); + FMjSensorState Accel; + Accel.Semantic = EMjSensorSemantic::Accel; + Accel.Values = {1.0, 2.0, 3.0}; + Art.Sensors.Add(Accel); + + double Ang[3] = {0, 0, 0}; + double Acc[3] = {0, 0, 0}; + bool bHasAng = false; + bool bHasAcc = false; + const bool bHas = UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc); + TestTrue(TEXT("paired imu present"), bHas); + TestTrue(TEXT("paired has angular velocity"), bHasAng); + TestTrue(TEXT("paired has linear acceleration"), bHasAcc); + TestEqual(TEXT("gyro x"), Ang[0], 0.1); + TestEqual(TEXT("gyro z"), Ang[2], 0.3); + TestEqual(TEXT("accel z"), Acc[2], 3.0); + } + + // Unpaired gyro: angular velocity only. + { + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + FMjSensorState Gyro; + Gyro.Semantic = EMjSensorSemantic::Gyro; + Gyro.Values = {0.5, 0.6, 0.7}; + Art.Sensors.Add(Gyro); + + double Ang[3] = {0, 0, 0}; + double Acc[3] = {0, 0, 0}; + bool bHasAng = false; + bool bHasAcc = false; + const bool bHas = UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc); + TestTrue(TEXT("gyro-only imu present"), bHas); + TestTrue(TEXT("gyro-only has angular velocity"), bHasAng); + TestFalse(TEXT("gyro-only has no linear acceleration"), bHasAcc); + TestEqual(TEXT("gyro-only y"), Ang[1], 0.6); + } + + // No imu sensors: nothing to publish. + { + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + double Ang[3] = {0, 0, 0}; + double Acc[3] = {0, 0, 0}; + bool bHasAng = false; + bool bHasAcc = false; + const bool bHas = UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc); + TestFalse(TEXT("no imu when art has no gyro/accel"), bHas); + } + + return true; +} + +// --------------------------------------------------------------------------- +// 5. FillClock: sec/nsec split matches AppendClockFields for the same sim time +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosFillClock, + "URLab.Ros.FillClock", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosFillClock::RunTest(const FString& Parameters) +{ + // AppendClockFields (RpcDispatcher.cpp) splits a sim-time double this way; the + // IR collector fills FMjClock identically and FillClock recombines to ns, so a + // round-trip through FillClock must reproduce the same sec/nsec pair. + const double SimTimeSec = 3.5; + const int32 ExpectedSec = static_cast(SimTimeSec); + const int32 ExpectedNsec = static_cast((SimTimeSec - ExpectedSec) * 1.0e9); + + FMjClock Clock; + Clock.SimSec = ExpectedSec; + Clock.SimNsec = ExpectedNsec; + + const int64 Ns = UURLabRosPublishTransport::FillClock(Clock); + TestEqual(TEXT("clock sec matches AppendClockFields"), + static_cast(Ns / 1000000000LL), ExpectedSec); + TestEqual(TEXT("clock nsec matches AppendClockFields"), + static_cast(Ns % 1000000000LL), ExpectedNsec); + return true; +} + +// --------------------------------------------------------------------------- +// 6. Publisher-set rebuild on StructureVersion change +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosPublisherRebuild, + "URLab.Ros.PublisherRebuild", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosPublisherRebuild::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.PublisherRebuild: no live ROS context; skipping.")); + return true; + } + + UURLabRosPublishTransport* Transport = NewObject(); + TestTrue(TEXT("transport init"), Transport->TransportInit()); + + // One art, version 1: one per-art publisher entry. + const FMjStateSnapshot Snap1 = MakeSnapshot(); + Transport->PublishState(Snap1); + TestEqual(TEXT("one art publisher after first publish"), + Transport->GetArtPublisherCountForTest(), 1); + TestEqual(TEXT("cached structure version tracks the snapshot"), + Transport->GetCachedStructureVersionForTest(), 1u); + + // A same-version re-publish must NOT rebuild. + Transport->PublishState(Snap1); + TestEqual(TEXT("no rebuild on unchanged structure version"), + Transport->GetArtPublisherCountForTest(), 1); + + // Add a second art and bump the version: the set rebuilds to match. + FMjStateSnapshot Snap2 = MakeSnapshot(); + FMjArticulationState Art2; + Art2.Name = FName(TEXT("arm")); + FMjJointState J; + J.Name = FName(TEXT("j0")); + J.Type = EMjJointType::Hinge; + J.QPos = {0.0}; + J.QVel = {0.0}; + Art2.Joints.Add(J); + Snap2.Articulations.Add(Art2); + Snap2.StructureVersion = 2; + + Transport->PublishState(Snap2); + TestEqual(TEXT("two art publishers after registry grows"), + Transport->GetArtPublisherCountForTest(), 2); + TestEqual(TEXT("cached structure version follows the bump"), + Transport->GetCachedStructureVersionForTest(), 2u); + + Transport->TransportShutdown(); + return true; +} + +// --------------------------------------------------------------------------- +// 7. Direct-mode fan-out: ROS publishes every step (distinct consumer) while the +// byte publishers stay paused (3.6 rule). A fake IMjSnapshotPublisher stands +// in for the ZMQ / SHM byte streams. +// --------------------------------------------------------------------------- +namespace +{ +struct FCountingSnapshotPublisher : public IMjSnapshotPublisher +{ + std::atomic Count{0}; + virtual void PublishSnapshot(const TArray& /*Bytes*/) override + { + ++Count; + } +}; +} // namespace + +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosDirectModeFanOut, + "URLab.Ros.DirectModeFanOut", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosDirectModeFanOut::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.DirectModeFanOut: no live ROS context; skipping.")); + return true; + } + + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + // Register a ROS publish transport into the manager's fan-out set, exactly as + // EnsureExternalTransportsBound does at runtime. + UURLabRosPublishTransport* Ros = NewObject(S.Manager); + TestTrue(TEXT("ros publish transport init"), Ros->TransportInit()); + S.Manager->ManagerOwnedPublishTransports.Add(Ros); + + // A fake byte publisher standing in for ZMQ / SHM. + FCountingSnapshotPublisher Fake; + S.Manager->RegisterSnapshotPublisher(&Fake, S.Manager); + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + Ros->TransportShutdown(); + S.Manager->UnregisterSnapshotPublisher(&Fake); + S.Cleanup(); + return false; + } + + // Direct mode pauses the byte fan-out (bPublishersPaused = true). + Disp->SetActiveStepMode(EStepMode::Direct); + TestTrue(TEXT("direct mode pauses byte publishers"), + S.Manager->bPublishersPaused.load()); + + mjModel* m = S.Manager->PhysicsEngine->m_model; + mjData* d = S.Manager->PhysicsEngine->m_data; + const int64 Before = Ros->GetPublishStateCountForTest(); + + S.Manager->FanOutStateSnapshot(m, d); + + TestEqual(TEXT("ROS published once despite pause (distinct consumer)"), + Ros->GetPublishStateCountForTest() - Before, static_cast(1)); + TestEqual(TEXT("byte publisher received nothing while paused"), + Fake.Count.load(), 0); + + // Restore Live so downstream tests see a clean cadence, then tear down. + Disp->SetActiveStepMode(EStepMode::Live); + Ros->TransportShutdown(); + S.Manager->UnregisterSnapshotPublisher(&Fake); + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 8. ROS control ownership across surfaces: a ROS-sourced claim blocks an +// RPC-session control write (and vice versa); a TTL frees the ROS claim. +// Drives FMjControlOwnership + Dispatch, so no live ROS runtime is needed. +// --------------------------------------------------------------------------- +namespace +{ +FString RosReplyField(const TSharedPtr& Reply, const TCHAR* Field) +{ + FString Out; + if (Reply.IsValid()) + Reply->TryGetStringField(Field, Out); + return Out; +} +} // namespace + +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosControlOwnershipAcrossSurfaces, + "URLab.Ros.ControlOwnershipAcrossSurfaces", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosControlOwnershipAcrossSurfaces::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + const FString RosSrc = UURLabRosRpcTransport::RosControlSourceId(); + const FString RpcSrc = TEXT("rpc-session-src"); + + auto Claim = [Disp, &ArtName](const FString& Source) { + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("claim_control")); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), Source); + R->SetStringField(TEXT("articulation"), ArtName); + return Disp->Dispatch(R); + }; + auto Release = [Disp, &ArtName](const FString& Source) { + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("release_control")); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), Source); + R->SetStringField(TEXT("articulation"), ArtName); + return Disp->Dispatch(R); + }; + auto SetTwist = [Disp, &ArtName](const FString& Source) { + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("set_twist")); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), Source); + R->SetStringField(TEXT("articulation"), ArtName); + TArray> Lin; + Lin.Add(MakeShared(1.0)); + Lin.Add(MakeShared(0.0)); + R->SetArrayField(TEXT("linear"), Lin); + return Disp->Dispatch(R); + }; + + // ROS claims; an RPC-session control write is then rejected, naming ROS owner. + TestEqual(TEXT("ROS claim ok"), RosReplyField(Claim(RosSrc), TEXT("op")), + FString(TEXT("claim_control_ok"))); + { + TSharedPtr R = SetTwist(RpcSrc); + TestEqual(TEXT("RPC write rejected while ROS owns"), + RosReplyField(R, TEXT("code")), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("rejection names the ROS owner"), + RosReplyField(R, TEXT("owner")), RosSrc); + } + + // Hand the art to the RPC session; a ROS write is now the one rejected. + TestEqual(TEXT("ROS release ok"), RosReplyField(Release(RosSrc), TEXT("op")), + FString(TEXT("release_control_ok"))); + TestEqual(TEXT("RPC claim ok"), RosReplyField(Claim(RpcSrc), TEXT("op")), + FString(TEXT("claim_control_ok"))); + { + TSharedPtr R = SetTwist(RosSrc); + TestEqual(TEXT("ROS write rejected while RPC owns"), + RosReplyField(R, TEXT("code")), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("rejection names the RPC owner"), + RosReplyField(R, TEXT("owner")), RpcSrc); + } + + // TTL frees a dropped ROS owner: past the TTL, another source can claim. + FMjControlOwnership& Own = Disp->GetControlOwnership(); + Own.Reset(); + Own.SetClockOverrideForTest(0.0); + FString Cur; + TestTrue(TEXT("ROS claims with a TTL"), + Own.Claim(Key, RosSrc, 5.0, false, Cur) == FMjControlOwnership::EClaimResult::Ok); + Own.SetClockOverrideForTest(10.0); + TestTrue(TEXT("TTL freed the ROS claim; RPC can claim"), + Own.Claim(Key, RpcSrc, 0.0, false, Cur) == FMjControlOwnership::EClaimResult::Ok); + Own.SetClockOverrideForTest(-1.0); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 9. Mode gating: a marshalled ROS ctrl write is dropped outside Live mode and +// applies in Live mode. Ownership is granted first so mode is the only gate; +// no live ROS runtime is needed (HandleRosCtrl touches no rcl). +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosCtrlModeGating, + "URLab.Ros.CtrlModeGating", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosCtrlModeGating::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Slide; + UMjActuator* A = NewObject(Sess.Robot, TEXT("TestActuator")); + A->Type = EMjActuatorType::Position; + A->TargetName = Sess.Joint->GetName(); + A->RegisterComponent(); + A->AttachToComponent(Sess.Robot->GetRootComponent(), + FAttachmentTransformRules::KeepRelativeTransform); + })) + { + AddInfo(FString::Printf(TEXT("Skipping CtrlModeGating: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + if (!m || !d || m->nu == 0) + { + AddInfo(TEXT("Skipping CtrlModeGating: no actuators in compiled model")); + S.Cleanup(); + return true; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + const FString RosSrc = UURLabRosRpcTransport::RosControlSourceId(); + + UURLabRosRpcTransport* Ros = NewObject(S.Manager); + Ros->SetOwningBridge(S.Manager->BridgeServer); + + // Grant ROS ownership so ownership never blocks; the mode is the only gate. + FString Cur; + Disp->GetControlOwnership().Claim(Key, RosSrc, 0.0, false, Cur); + + // Direct mode: the write is dropped, so d->ctrl stays at its initial value. + Disp->SetActiveStepMode(EStepMode::Direct); + Ros->ApplyRosCtrlForTest(ArtName, {0.5}); + Art->ApplyControls(/*bSkipController=*/true); + TestEqual(TEXT("direct-mode ROS ctrl dropped"), (double)d->ctrl[0], 0.0, 1e-6); + + // Live mode: the same write reaches the actuator staging and lands. + // bSkipController=false so ApplyControls reads NetworkValue → d->ctrl. + Disp->SetActiveStepMode(EStepMode::Live); + Ros->ApplyRosCtrlForTest(ArtName, {0.5}); + Art->ApplyControls(/*bSkipController=*/false); + TestEqual(TEXT("live-mode ROS ctrl applies"), (double)d->ctrl[0], 0.5, 1e-6); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 10. Availability-gated wire test: publish a Float64MultiArray on +// //cmd_ctrl via rcl and assert the staged ctrl value lands. Requires a +// live ROS context (skips otherwise) and a compiled actuator. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosCtrlWire, + "URLab.Ros.CtrlWire", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosCtrlWire::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, TEXT("URLab.Ros.CtrlWire: no live ROS context; skipping.")); + return true; + } + + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Slide; + UMjActuator* A = NewObject(Sess.Robot, TEXT("TestActuator")); + A->Type = EMjActuatorType::Position; + A->TargetName = Sess.Joint->GetName(); + A->RegisterComponent(); + A->AttachToComponent(Sess.Robot->GetRootComponent(), + FAttachmentTransformRules::KeepRelativeTransform); + })) + { + AddInfo(FString::Printf(TEXT("Skipping CtrlWire: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + if (!m || !d || m->nu == 0) + { + AddInfo(TEXT("Skipping CtrlWire: no actuators in compiled model")); + S.Cleanup(); + return true; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + const FString RosSrc = UURLabRosRpcTransport::RosControlSourceId(); + + UURLabRosRpcTransport* Ros = NewObject(S.Manager); + Ros->SetOwningBridge(S.Manager->BridgeServer); + + // Own the art as the ROS source and stay in Live mode so the marshalled write + // is applied rather than dropped. + FString Cur; + Disp->GetControlOwnership().Claim(Key, RosSrc, 0.0, false, Cur); + Disp->SetActiveStepMode(EStepMode::Live); + + const FString Segment = FMjCanonicalName::ArtSegment(Art).ToString(); + const FString Topic = FString::Printf(TEXT("/%s/cmd_ctrl"), *Segment); + + const bool bFired = Ros->PublishAndPumpCtrlForTest(Topic, {0.42}); + TestTrue(TEXT("ROS cmd_ctrl delivered over the wire"), bFired); + + // The callback staged the value on the actuator's NetworkValue; a step copies + // it into d->ctrl (mirroring the live physics tick). + Art->ApplyControls(/*bSkipController=*/false); + TestEqual(TEXT("wire ctrl landed in d->ctrl"), (double)d->ctrl[0], 0.42, 1e-6); + + Ros->TransportShutdown(); + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 11. Total sensor routing over the wire: an art carrying one of each typed +// sensor plus an unmapped one builds and publishes through the provider set +// against real rcl without error (Wrench / Range / MagneticField / Twist / +// Float64MultiArray create + publish paths). Availability-gated. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosSensorRoutingWire, + "URLab.Ros.SensorRoutingWire", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosSensorRoutingWire::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.SensorRoutingWire: no live ROS context; skipping.")); + return true; + } + + auto AddSensor = [](FMjArticulationState& Art, const TCHAR* Name, + EMjSensorSemantic Sem, TArray Values) + { + FMjSensorState S; + S.Name = FName(Name); + S.Semantic = Sem; + S.Values = MoveTemp(Values); + Art.Sensors.Add(S); + }; + + FMjStateSnapshot Snap; + Snap.StructureVersion = 1; + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + AddSensor(Art, TEXT("ft_force"), EMjSensorSemantic::Force, {1.0, 2.0, 3.0}); + AddSensor(Art, TEXT("ft_torque"), EMjSensorSemantic::Torque, {4.0, 5.0, 6.0}); + AddSensor(Art, TEXT("front_range"), EMjSensorSemantic::Rangefinder, {0.42}); + AddSensor(Art, TEXT("mag0"), EMjSensorSemantic::Magnetometer, {0.1, 0.2, 0.3}); + AddSensor(Art, TEXT("base_vel"), EMjSensorSemantic::Velocity, {0.5, 0.0, 0.0}); + AddSensor(Art, TEXT("belly_touch"), EMjSensorSemantic::Touch, {1.0}); + Snap.Articulations.Add(Art); + + UURLabRosPublishTransport* Transport = NewObject(); + TestTrue(TEXT("transport init"), Transport->TransportInit()); + + Transport->PublishState(Snap); + Transport->PublishState(Snap); + + // Every registered provider instantiated, and the run did not tear the context + // down (the wrench/range/mag/twist/multiarray wire paths all succeeded). + TestEqual(TEXT("all registered providers built"), + Transport->GetProviderCountForTest(), FMjRosOutputRegistry::Get().Num()); + TestTrue(TEXT("context still available after routing publish"), + FURLabRosContext::Get().IsAvailable()); + + Transport->TransportShutdown(); + return true; +} + +// --------------------------------------------------------------------------- +// 12. State-estimation outputs over the wire: a free-base art builds + publishes +// nav_msgs/Odometry (//odom) and geometry_msgs/PoseWithCovarianceStamped +// (//pose), and the REP-105 map->odom->world static chain publishes on +// /tf_static, all against real rcl without tearing the context down. +// Availability-gated. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosStateEstimationWire, + "URLab.Ros.StateEstimationWire", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosStateEstimationWire::RunTest(const FString& Parameters) +{ + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.StateEstimationWire: no live ROS context; skipping.")); + return true; + } + + // A free-base art with a matching base body and a genuinely asymmetric velocity + // (world linear, body angular) so the Odometry twist rotation path runs live. + FMjStateSnapshot Snap; + Snap.StructureVersion = 1; + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + FMjBodyState Base; + Base.Name = FName(TEXT("trunk")); + Base.Xpos[0] = 0.0; Base.Xpos[1] = 0.0; Base.Xpos[2] = 0.5; + Base.Xquat[0] = 1.0; + Art.Bodies.Add(Base); + + FMjJointState Free; + Free.Name = FName(TEXT("root")); + Free.Type = EMjJointType::Free; + Free.QPos = {0.0, 0.0, 0.5, 1.0, 0.0, 0.0, 0.0}; + Free.QVel = {0.3, 0.0, 0.0, 0.0, 0.0, 0.5}; + Art.Joints.Add(Free); + + Snap.Articulations.Add(Art); + + UURLabRosPublishTransport* Transport = NewObject(); + TestTrue(TEXT("transport init"), Transport->TransportInit()); + + Transport->PublishState(Snap); + Transport->PublishState(Snap); + + TestEqual(TEXT("all registered providers built"), + Transport->GetProviderCountForTest(), FMjRosOutputRegistry::Get().Num()); + TestTrue(TEXT("context still available after state-estimation publish"), + FURLabRosContext::Get().IsAvailable()); + + Transport->TransportShutdown(); + return true; +} + +// --------------------------------------------------------------------------- +// 13. User channels over ROS: the "user_channels" provider is registered, and a +// snapshot carrying a Bool + a Transform art channel plus a scene channel +// builds + publishes its typed topics (std_msgs/Bool, geometry_msgs/Pose +// Stamped, ...) against real rcl without tearing the context down. +// Registry check runs unconditionally; the wire check is availability-gated. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosUserChannelsWire, + "URLab.Ros.UserChannelsWire", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosUserChannelsWire::RunTest(const FString& Parameters) +{ + // Registry: the provider self-registers in every configuration. + TestTrue(TEXT("user_channels provider registered"), + FMjRosOutputRegistry::Get().GetRegisteredNames().Contains(FName(TEXT("user_channels")))); + + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Ros.UserChannelsWire: no live ROS context; skipping wire check.")); + return true; + } + + FMjStateSnapshot Snap; + Snap.StructureVersion = 1; + + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + FMjUserChannel Done; + Done.Name = FName(TEXT("task_done")); + Done.Kind = EMjUserChannelKind::Bool; + Done.Values = {1.0}; + Art.UserChannels.Add(Done); + + FMjUserChannel Target; + Target.Name = FName(TEXT("target")); + Target.Kind = EMjUserChannelKind::Transform; + Target.Values = {1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0}; // pos + quat wxyz + Art.UserChannels.Add(Target); + + Snap.Articulations.Add(Art); + + FMjUserChannel Phase; + Phase.Name = FName(TEXT("episode_phase")); + Phase.Kind = EMjUserChannelKind::Scalar; + Phase.Values = {2.0}; + Snap.UserChannels.Add(Phase); + + UURLabRosPublishTransport* Transport = NewObject(); + TestTrue(TEXT("transport init"), Transport->TransportInit()); + + Transport->PublishState(Snap); + Transport->PublishState(Snap); + + TestEqual(TEXT("all registered providers built"), + Transport->GetProviderCountForTest(), FMjRosOutputRegistry::Get().Num()); + TestTrue(TEXT("context still available after user-channel publish"), + FURLabRosContext::Get().IsAvailable()); + + Transport->TransportShutdown(); + return true; +} + +// --------------------------------------------------------------------------- +// 14. claim_control as a ROS service: a service-sourced claim (routed through +// Dispatch with the ROS source preset) claims the art, an RPC-session write +// is then rejected naming the ROS owner, and once the RPC session owns it a +// second ROS service claim fails. Drives Dispatch only; no live ROS runtime. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosClaimService, + "URLab.Ros.ClaimService", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosClaimService::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FString RosSrc = UURLabRosRpcTransport::RosControlSourceId(); + const FString RpcSrc = TEXT("rpc-session-src"); + + UURLabRosRpcTransport* Ros = NewObject(S.Manager); + Ros->SetOwningBridge(S.Manager->BridgeServer); + + auto SetTwist = [Disp, &ArtName](const FString& Source) { + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("set_twist")); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), Source); + R->SetStringField(TEXT("articulation"), ArtName); + TArray> Lin; + Lin.Add(MakeShared(1.0)); + Lin.Add(MakeShared(0.0)); + R->SetArrayField(TEXT("linear"), Lin); + return Disp->Dispatch(R); + }; + + // The ROS claim_control service claims the art. + TestTrue(TEXT("ROS claim service succeeds"), + Ros->ApplyRosClaimReleaseForTest(ArtName, /*bClaim=*/true)); + + // An RPC-session write is now rejected, naming the ROS owner. + { + TSharedPtr R = SetTwist(RpcSrc); + TestEqual(TEXT("RPC write rejected while ROS service owns"), + RosReplyField(R, TEXT("code")), FString(TEXT("not_control_owner"))); + TestEqual(TEXT("rejection names the ROS owner"), + RosReplyField(R, TEXT("owner")), RosSrc); + } + + // The ROS release service frees it; the RPC session then claims it. + TestTrue(TEXT("ROS release service succeeds"), + Ros->ApplyRosClaimReleaseForTest(ArtName, /*bClaim=*/false)); + { + TSharedPtr R = MakeShared(); + R->SetStringField(TEXT("op"), TEXT("claim_control")); + R->SetStringField(TEXT("session_id"), TEXT("test-session")); + R->SetStringField(TEXT("source"), RpcSrc); + R->SetStringField(TEXT("articulation"), ArtName); + TestEqual(TEXT("RPC claim ok"), RosReplyField(Disp->Dispatch(R), TEXT("op")), + FString(TEXT("claim_control_ok"))); + } + + // A second ROS service claim now fails: the RPC session owns the art. + TestFalse(TEXT("ROS claim service fails when RPC owns"), + Ros->ApplyRosClaimReleaseForTest(ArtName, /*bClaim=*/true)); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 15. JointState jog input: a joint_command naming an actuator's canonical joint +// is dropped outside Live mode and stages the actuator's position target in +// Live mode. Ownership is granted first so mode is the only gate; drives +// HandleRosJointCommand directly (no live ROS runtime needed). +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosJointCommandJog, + "URLab.Ros.JointCommandJog", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosJointCommandJog::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Slide; + UMjActuator* A = NewObject(Sess.Robot, TEXT("TestActuator")); + A->Type = EMjActuatorType::Position; + A->TargetName = Sess.Joint->GetName(); + A->RegisterComponent(); + A->AttachToComponent(Sess.Robot->GetRootComponent(), + FAttachmentTransformRules::KeepRelativeTransform); + })) + { + AddInfo(FString::Printf(TEXT("Skipping JointCommandJog: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + if (!m || !d || m->nu == 0) + { + AddInfo(TEXT("Skipping JointCommandJog: no actuators in compiled model")); + S.Cleanup(); + return true; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + const FName Key(*ArtName); + const FString RosSrc = UURLabRosRpcTransport::RosControlSourceId(); + + // The jog names the joint the actuator drives (not the actuator's own name), + // resolved through the same canonical-name convention JointState output uses. + UMjActuator* Act = Art->GetActuators()[0]; + const FString JointName = FMjCanonicalName::PartSegment(Art, Act->TargetName).ToString(); + + UURLabRosRpcTransport* Ros = NewObject(S.Manager); + Ros->SetOwningBridge(S.Manager->BridgeServer); + + // Grant ROS ownership so the mode is the only gate. + FString Cur; + Disp->GetControlOwnership().Claim(Key, RosSrc, 0.0, false, Cur); + + // Direct mode: the jog is dropped, so d->ctrl stays at its initial value. + Disp->SetActiveStepMode(EStepMode::Direct); + Ros->ApplyRosJointCommandForTest(ArtName, {JointName}, {0.5}); + Art->ApplyControls(/*bSkipController=*/true); + TestEqual(TEXT("direct-mode joint_command dropped"), (double)d->ctrl[0], 0.0, 1e-6); + + // Live mode: the same jog stages the actuator's position target. + Disp->SetActiveStepMode(EStepMode::Live); + Ros->ApplyRosJointCommandForTest(ArtName, {JointName}, {0.5}); + Art->ApplyControls(/*bSkipController=*/false); + TestEqual(TEXT("live-mode joint_command applies"), (double)d->ctrl[0], 0.5, 1e-6); + + S.Cleanup(); + return true; +} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabEditor/Private/Tests/MjRosProviderTests.cpp b/Source/URLabEditor/Private/Tests/MjRosProviderTests.cpp new file mode 100644 index 00000000..60c26084 --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjRosProviderTests.cpp @@ -0,0 +1,323 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjRosProviderTests.cpp +// +// Tests for the URLabRos output-provider library that need no rcl runtime, so +// they run in every configuration (ROS on or off): the total sensor routing +// table, the sensor topic naming, the Force+Torque wrench pairing, and the +// self-registering provider registry listing the built-ins. These prove the +// provider library is populated and total even when ROS is compiled out, which is +// the point of keeping the routing / registry ROS-agnostic. +// ============================================================================ + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Transport/RosSensorRouting.h" +#include "Transport/RosOutputProvider.h" +#include "Transport/RosStateEstimation.h" +#include "State/MjStateTypes.h" + +// --------------------------------------------------------------------------- +// 1. RouteForSemantic is total: every EMjSensorSemantic value maps, the typed +// routes match the spec, and everything else lands on the MultiArray fallback. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosRouteTableTotal, + "URLab.Ros.RouteTableTotal", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosRouteTableTotal::RunTest(const FString& Parameters) +{ + // Typed routes. + TestEqual(TEXT("Gyro -> Imu"), RouteForSemantic(EMjSensorSemantic::Gyro), ERosSensorRoute::Imu); + TestEqual(TEXT("Accel -> Imu"), RouteForSemantic(EMjSensorSemantic::Accel), ERosSensorRoute::Imu); + TestEqual(TEXT("Force -> Wrench"), RouteForSemantic(EMjSensorSemantic::Force), ERosSensorRoute::Wrench); + TestEqual(TEXT("Torque -> Wrench"), RouteForSemantic(EMjSensorSemantic::Torque), ERosSensorRoute::Wrench); + TestEqual(TEXT("Rangefinder -> Range"), RouteForSemantic(EMjSensorSemantic::Rangefinder), ERosSensorRoute::Range); + TestEqual(TEXT("Magnetometer -> MagneticField"), + RouteForSemantic(EMjSensorSemantic::Magnetometer), ERosSensorRoute::MagneticField); + TestEqual(TEXT("Velocity -> Twist"), RouteForSemantic(EMjSensorSemantic::Velocity), ERosSensorRoute::Twist); + + // A spread of the untyped semantics all fall back to MultiArray. + TestEqual(TEXT("Generic -> MultiArray"), RouteForSemantic(EMjSensorSemantic::Generic), ERosSensorRoute::MultiArray); + TestEqual(TEXT("Touch -> MultiArray"), RouteForSemantic(EMjSensorSemantic::Touch), ERosSensorRoute::MultiArray); + TestEqual(TEXT("JointPos -> MultiArray"), RouteForSemantic(EMjSensorSemantic::JointPos), ERosSensorRoute::MultiArray); + TestEqual(TEXT("FramePos -> MultiArray"), RouteForSemantic(EMjSensorSemantic::FramePos), ERosSensorRoute::MultiArray); + TestEqual(TEXT("SubtreeCom -> MultiArray"), RouteForSemantic(EMjSensorSemantic::SubtreeCom), ERosSensorRoute::MultiArray); + + // Totality: every value from Generic..Clock maps to a defined route. The + // compiler enforces this (the switch has no default); the loop documents it and + // guards against a value being dropped from the mapping. + for (uint8 V = 0; V <= static_cast(EMjSensorSemantic::Clock); ++V) + { + const ERosSensorRoute Route = RouteForSemantic(static_cast(V)); + const bool bValid = Route == ERosSensorRoute::Imu || Route == ERosSensorRoute::Wrench + || Route == ERosSensorRoute::Range || Route == ERosSensorRoute::MagneticField + || Route == ERosSensorRoute::Twist || Route == ERosSensorRoute::MultiArray; + TestTrue(*FString::Printf(TEXT("semantic %u maps to a route"), V), bValid); + } + return true; +} + +// --------------------------------------------------------------------------- +// 2. Sensor topic naming: the MultiArray fallback is the self-describing +// //sensors/; typed routes are ///. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosSensorTopics, + "URLab.Ros.SensorTopics", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosSensorTopics::RunTest(const FString& Parameters) +{ + using namespace MjRosSensorRouting; + TestEqual(TEXT("unmapped sensor -> /go2/sensors/"), + TopicFor(TEXT("go2"), TEXT("belly_touch"), ERosSensorRoute::MultiArray), + FString(TEXT("/go2/sensors/belly_touch"))); + TestEqual(TEXT("wrench topic"), + TopicFor(TEXT("go2"), TEXT("ankle_ft"), ERosSensorRoute::Wrench), + FString(TEXT("/go2/ankle_ft/wrench"))); + TestEqual(TEXT("range topic"), + TopicFor(TEXT("go2"), TEXT("front_range"), ERosSensorRoute::Range), + FString(TEXT("/go2/front_range/range"))); + TestEqual(TEXT("magnetic field topic"), + TopicFor(TEXT("go2"), TEXT("mag0"), ERosSensorRoute::MagneticField), + FString(TEXT("/go2/mag0/magnetic_field"))); + TestEqual(TEXT("velocimeter topic"), + TopicFor(TEXT("go2"), TEXT("base_vel"), ERosSensorRoute::Twist), + FString(TEXT("/go2/base_vel/velocity"))); + return true; +} + +// --------------------------------------------------------------------------- +// 3. Force+Torque pairing: one force + one torque sensor on an art pair into a +// single wrench row, named after the force sensor. Extra sensors that are not +// Force/Torque are ignored by the pairing. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosWrenchPairing, + "URLab.Ros.WrenchPairing", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosWrenchPairing::RunTest(const FString& Parameters) +{ + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + FMjSensorState Force; + Force.Name = FName(TEXT("ankle_force")); + Force.Semantic = EMjSensorSemantic::Force; + Force.Values = {1.0, 2.0, 3.0}; + Art.Sensors.Add(Force); + + FMjSensorState Torque; + Torque.Name = FName(TEXT("ankle_torque")); + Torque.Semantic = EMjSensorSemantic::Torque; + Torque.Values = {4.0, 5.0, 6.0}; + Art.Sensors.Add(Torque); + + // A touch sensor should not affect wrench pairing. + FMjSensorState Touch; + Touch.Name = FName(TEXT("foot_touch")); + Touch.Semantic = EMjSensorSemantic::Touch; + Touch.Values = {0.0}; + Art.Sensors.Add(Touch); + + TArray Pairs; + MjRosSensorRouting::GatherWrenchPairs(Art, Pairs); + + TestEqual(TEXT("one force + one torque -> one wrench pair"), Pairs.Num(), 1); + if (Pairs.Num() == 1) + { + TestEqual(TEXT("pair force sensor"), Pairs[0].ForceSensor, FName(TEXT("ankle_force"))); + TestEqual(TEXT("pair torque sensor"), Pairs[0].TorqueSensor, FName(TEXT("ankle_torque"))); + TestEqual(TEXT("pair named after the force sensor"), Pairs[0].TopicName, FName(TEXT("ankle_force"))); + } + + // An unpaired torque (no force) still produces a wrench row, named after itself. + FMjArticulationState TorqueOnly; + TorqueOnly.Name = FName(TEXT("arm")); + FMjSensorState LoneTorque; + LoneTorque.Name = FName(TEXT("wrist_torque")); + LoneTorque.Semantic = EMjSensorSemantic::Torque; + LoneTorque.Values = {0.1, 0.2, 0.3}; + TorqueOnly.Sensors.Add(LoneTorque); + + TArray LonePairs; + MjRosSensorRouting::GatherWrenchPairs(TorqueOnly, LonePairs); + TestEqual(TEXT("lone torque -> one wrench row"), LonePairs.Num(), 1); + if (LonePairs.Num() == 1) + { + TestTrue(TEXT("lone torque has no force sensor"), LonePairs[0].ForceSensor.IsNone()); + TestEqual(TEXT("lone torque names the row"), LonePairs[0].TopicName, FName(TEXT("wrist_torque"))); + } + return true; +} + +// --------------------------------------------------------------------------- +// 4. The provider registry is populated with the built-in outputs at module +// load, in every configuration (this test is not ROS-fenced). +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosProviderRegistryBuiltins, + "URLab.Ros.ProviderRegistryBuiltins", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosProviderRegistryBuiltins::RunTest(const FString& Parameters) +{ + const TArray Names = FMjRosOutputRegistry::Get().GetRegisteredNames(); + + const TCHAR* Expected[] = { + TEXT("joint_state"), TEXT("imu"), TEXT("cmd_twist"), TEXT("sensors"), + TEXT("tf"), TEXT("clock"), TEXT("robot_description"), + TEXT("odometry"), TEXT("pose"), TEXT("camera_info"), TEXT("rep105_frames") + }; + for (const TCHAR* Name : Expected) + { + TestTrue(*FString::Printf(TEXT("registry lists built-in '%s'"), Name), + Names.Contains(FName(Name))); + } + + // Instantiating the registry yields one live provider per registered entry. + TArray> Providers; + FMjRosOutputRegistry::Get().InstantiateAll(Providers); + TestEqual(TEXT("instantiated provider count matches registry"), + Providers.Num(), FMjRosOutputRegistry::Get().Num()); + TestTrue(TEXT("at least the built-ins are present"), + Providers.Num() >= UE_ARRAY_COUNT(Expected)); + return true; +} + +// --------------------------------------------------------------------------- +// 5. Free-base odometry math: the MuJoCo free-joint convention is asymmetric +// (qvel = [WORLD linear, BODY angular]). ComputeFreeBaseState must rotate the +// linear half into the base frame and pass the angular half through, and map +// the pose (wxyz -> xyzw) and base-body index correctly. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosFreeBaseTwist, + "URLab.Ros.FreeBaseTwist", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosFreeBaseTwist::RunTest(const FString& Parameters) +{ + using namespace MjRosStateEstimation; + + // Base rotated +90 degrees about world Z: quaternion wxyz = (cos45, 0, 0, sin45). + const double S = FMath::Sqrt(0.5); + + FMjArticulationState Art; + Art.Name = FName(TEXT("go2")); + + // A base body whose world pose matches the free joint qpos, so the base-body + // index resolves to it. + FMjBodyState Base; + Base.Name = FName(TEXT("trunk")); + Base.Xpos[0] = 1.0; Base.Xpos[1] = 2.0; Base.Xpos[2] = 3.0; + Base.Xquat[0] = S; Base.Xquat[1] = 0.0; Base.Xquat[2] = 0.0; Base.Xquat[3] = S; + Art.Bodies.Add(Base); + + FMjJointState Free; + Free.Name = FName(TEXT("root")); + Free.Type = EMjJointType::Free; + Free.QPos = {1.0, 2.0, 3.0, S, 0.0, 0.0, S}; // pos + wxyz + // qvel: WORLD linear (1,0,0), BODY angular (0.1, 0.2, 0.3). + Free.QVel = {1.0, 0.0, 0.0, 0.1, 0.2, 0.3}; + Art.Joints.Add(Free); + + FMjFreeBaseState State; + const bool bOk = ComputeFreeBaseState(Art, State); + TestTrue(TEXT("free-base state derived"), bOk); + TestTrue(TEXT("state valid"), State.bValid); + + // Pose position straight through; orientation reordered wxyz -> xyzw. + TestEqual(TEXT("pos x"), State.Position[0], 1.0); + TestEqual(TEXT("pos z"), State.Position[2], 3.0); + TestEqual(TEXT("quat x (from wxyz.x)"), State.OrientationXyzw[0], 0.0); + TestEqual(TEXT("quat z (from wxyz.z)"), State.OrientationXyzw[2], S); + TestEqual(TEXT("quat w (from wxyz.w)"), State.OrientationXyzw[3], S); + + // A world +X velocity, viewed from a base yawed +90 about Z, reads as base -Y. + TestEqual(TEXT("linear rotated world->body x"), State.LinearBody[0], 0.0, 1e-9); + TestEqual(TEXT("linear rotated world->body y"), State.LinearBody[1], -1.0, 1e-9); + TestEqual(TEXT("linear rotated world->body z"), State.LinearBody[2], 0.0, 1e-9); + + // Angular half is already body-frame: passed through unchanged. + TestEqual(TEXT("angular x passthrough"), State.AngularBody[0], 0.1); + TestEqual(TEXT("angular y passthrough"), State.AngularBody[1], 0.2); + TestEqual(TEXT("angular z passthrough"), State.AngularBody[2], 0.3); + + // The base body matched by world position. + TestEqual(TEXT("base body index"), State.BaseBodyIndex, 0); + + // A fixed-base art (no free joint) yields no odometry. + FMjArticulationState Fixed; + Fixed.Name = FName(TEXT("arm")); + FMjJointState Hinge; + Hinge.Name = FName(TEXT("j0")); + Hinge.Type = EMjJointType::Hinge; + Hinge.QPos = {0.0}; + Hinge.QVel = {0.0}; + Fixed.Joints.Add(Hinge); + FMjFreeBaseState None; + TestFalse(TEXT("fixed-base art has no free-base state"), ComputeFreeBaseState(Fixed, None)); + + return true; +} + +// --------------------------------------------------------------------------- +// 6. CameraInfo intrinsics: the pinhole K derived from a vertical FOV matches +// fy = (H/2)/tan(fovy/2), fx = fy, principal point at the image centre. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjRosCameraInfoIntrinsics, + "URLab.Ros.CameraInfoIntrinsics", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjRosCameraInfoIntrinsics::RunTest(const FString& Parameters) +{ + using namespace MjRosStateEstimation; + + // fovy = 90 deg, 640x480: fy = 240/tan(45) = 240, fx = fy, cx = 320, cy = 240. + double K[9]; + PinholeKFromFovy(90.0, 640, 480, K); + TestEqual(TEXT("fx"), K[0], 240.0, 1e-6); + TestEqual(TEXT("cx"), K[2], 320.0, 1e-6); + TestEqual(TEXT("fy"), K[4], 240.0, 1e-6); + TestEqual(TEXT("cy"), K[5], 240.0, 1e-6); + TestEqual(TEXT("K[8] == 1"), K[8], 1.0); + TestEqual(TEXT("K skew zero"), K[1], 0.0); + + // A non-square image keeps fx == fy (square pixels); the horizontal FOV differs + // from the vertical because the width differs. + double K2[9]; + PinholeKFromFovy(60.0, 800, 600, K2); + const double ExpectedFy = (600.0 * 0.5) / FMath::Tan(FMath::DegreesToRadians(60.0) * 0.5); + TestEqual(TEXT("fy from 60deg over 600px"), K2[4], ExpectedFy, 1e-6); + TestEqual(TEXT("fx == fy square pixels"), K2[0], K2[4], 1e-9); + TestEqual(TEXT("cx at width centre"), K2[2], 400.0, 1e-6); + + // A zero / unset fovy falls back to MuJoCo's 45-degree default rather than + // collapsing the focal length. + double K3[9]; + PinholeKFromFovy(0.0, 640, 480, K3); + const double ExpectedFy45 = (480.0 * 0.5) / FMath::Tan(FMath::DegreesToRadians(45.0) * 0.5); + TestEqual(TEXT("fovy 0 falls back to 45 deg"), K3[4], ExpectedFy45, 1e-6); + + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjSensorTypeInfoTests.cpp b/Source/URLabEditor/Private/Tests/MjSensorTypeInfoTests.cpp new file mode 100644 index 00000000..4c97430b --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjSensorTypeInfoTests.cpp @@ -0,0 +1,239 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "MuJoCo/Components/Sensors/MjUserSensor.h" +#include "MuJoCo/Components/Sensors/MjPluginSensor.h" +#include "MuJoCo/Generated/MjSensorTypeInfo.h" +#include "State/MjStateTypes.h" + +// The FMjSensorTypeInfo descriptor table is the single source of truth that +// replaced the six per-type switches in MjSensor.cpp and the editor XML +// parser. These tests pin the descriptor contract each of those consumers +// relies on. + +// ============================================================================ +// URLab.SensorTypeInfo.TableCoversEveryType +// Every EMjSensorType has exactly one descriptor row, and both the by-type +// and by-tag lookups round-trip against it. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoTableComplete, + "URLab.SensorTypeInfo.TableCoversEveryType", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoTableComplete::RunTest(const FString& Parameters) +{ + const UEnum* Enum = StaticEnum(); + if (!TestNotNull(TEXT("EMjSensorType reflection"), Enum)) + return false; + + TSet Seen; + for (int32 i = 0; i < Enum->NumEnums(); ++i) + { + const FString Name = Enum->GetNameStringByIndex(i); + if (Name.EndsWith(TEXT("_MAX"))) + continue; + + const EMjSensorType Type = static_cast(Enum->GetValueByIndex(i)); + const FMjSensorTypeInfo& Info = MjSensorTypeInfoFor(Type); + TestEqual(FString::Printf(TEXT("descriptor.Type matches for %s"), *Name), + Info.Type, Type); + TestNotNull(FString::Printf(TEXT("SensorClass set for %s"), *Name), + Info.SensorClass); + + // The MJCF tag must round-trip back to the same row. + const FMjSensorTypeInfo* ByTag = MjSensorTypeInfoForTag(FString(Info.Tag)); + if (TestNotNull(FString::Printf(TEXT("tag lookup for %s"), *Name), ByTag)) + TestEqual(FString::Printf(TEXT("tag round-trip for %s"), *Name), + ByTag->Type, Type); + + Seen.Add(Type); + } + + // Table row count matches the number of live enum values. + TestEqual(TEXT("one descriptor row per enum value"), + MjSensorTypeInfoTable().Num(), Seen.Num()); + return true; +} + +// ============================================================================ +// URLab.SensorTypeInfo.TagLookupIsCaseInsensitiveAndRejectsUnknown +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoTagLookup, + "URLab.SensorTypeInfo.TagLookupIsCaseInsensitiveAndRejectsUnknown", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoTagLookup::RunTest(const FString& Parameters) +{ + const FMjSensorTypeInfo* Lower = MjSensorTypeInfoForTag(TEXT("gyro")); + const FMjSensorTypeInfo* Upper = MjSensorTypeInfoForTag(TEXT("GyRo")); + if (TestNotNull(TEXT("lowercase gyro"), Lower) + && TestNotNull(TEXT("mixed-case gyro"), Upper)) + { + TestEqual(TEXT("case-insensitive tag maps to Gyro"), Lower->Type, EMjSensorType::Gyro); + TestEqual(TEXT("case variants resolve identically"), Lower->Type, Upper->Type); + } + + TestNull(TEXT("unknown tag returns null"), + MjSensorTypeInfoForTag(TEXT("not_a_sensor"))); + TestNull(TEXT("bare container is not a descriptor"), + MjSensorTypeInfoForTag(TEXT("sensor"))); + return true; +} + +// ============================================================================ +// URLab.SensorTypeInfo.ObjRefPolicyMatchesExportRules +// Spot-check the objtype/reftype export policy for each policy class. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoObjRefPolicy, + "URLab.SensorTypeInfo.ObjRefPolicyMatchesExportRules", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoObjRefPolicy::RunTest(const FString& Parameters) +{ + // Static objtype, no reference (site-attached sensor). + const FMjSensorTypeInfo& Touch = MjSensorTypeInfoFor(EMjSensorType::Touch); + TestEqual(TEXT("touch obj is static"), Touch.ObjSource, EMjSensorObjSource::Static); + TestEqual(TEXT("touch obj is site"), Touch.ObjType, (int32)mjOBJ_SITE); + TestEqual(TEXT("touch has no ref"), Touch.RefSource, EMjSensorObjSource::None); + + // Static objtype + static reftype (camprojection: site -> camera). + const FMjSensorTypeInfo& CamProj = MjSensorTypeInfoFor(EMjSensorType::CamProjection); + TestEqual(TEXT("camprojection ref is static"), CamProj.RefSource, EMjSensorObjSource::Static); + TestEqual(TEXT("camprojection ref is camera"), CamProj.RefType, (int32)mjOBJ_CAMERA); + + // Computed objtype (rangefinder: camera or site), no reference. + const FMjSensorTypeInfo& Range = MjSensorTypeInfoFor(EMjSensorType::RangeFinder); + TestEqual(TEXT("rangefinder obj is computed"), Range.ObjSource, EMjSensorObjSource::Computed); + TestEqual(TEXT("rangefinder has no ref"), Range.RefSource, EMjSensorObjSource::None); + + // Frame sensors read both objtype and reftype from the UE properties. + const FMjSensorTypeInfo& FramePos = MjSensorTypeInfoFor(EMjSensorType::FramePos); + TestEqual(TEXT("framepos obj from xml"), FramePos.ObjSource, EMjSensorObjSource::FromXml); + TestEqual(TEXT("framepos ref from xml"), FramePos.RefSource, EMjSensorObjSource::FromXml); + + // insidesite: objtype from xml, reftype fixed to site. + const FMjSensorTypeInfo& Inside = MjSensorTypeInfoFor(EMjSensorType::InsideSite); + TestEqual(TEXT("insidesite obj from xml"), Inside.ObjSource, EMjSensorObjSource::FromXml); + TestEqual(TEXT("insidesite ref is static site"), Inside.RefSource, EMjSensorObjSource::Static); + TestEqual(TEXT("insidesite ref is site"), Inside.RefType, (int32)mjOBJ_SITE); + + // user reads objtype from xml but never writes a reftype; plugin writes both. + TestEqual(TEXT("user ref is none"), + MjSensorTypeInfoFor(EMjSensorType::User).RefSource, EMjSensorObjSource::None); + TestEqual(TEXT("plugin ref from xml"), + MjSensorTypeInfoFor(EMjSensorType::Plugin).RefSource, EMjSensorObjSource::FromXml); + return true; +} + +// ============================================================================ +// URLab.SensorTypeInfo.SemanticValueKindAndDim +// The metadata that feeds DescribeState (Semantic), TransformSensorReading +// (ValueKind) and the MuJoCo output dimension. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoSemanticValueKindDim, + "URLab.SensorTypeInfo.SemanticValueKindAndDim", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoSemanticValueKindDim::RunTest(const FString& Parameters) +{ + const FMjSensorTypeInfo& Gyro = MjSensorTypeInfoFor(EMjSensorType::Gyro); + TestEqual(TEXT("gyro semantic"), Gyro.Semantic, EMjSensorSemantic::Gyro); + TestEqual(TEXT("gyro value kind"), Gyro.ValueKind, EMjSensorValueKind::Vector3); + TestEqual(TEXT("gyro dim"), Gyro.FixedDim, 3); + + const FMjSensorTypeInfo& FrameQuat = MjSensorTypeInfoFor(EMjSensorType::FrameQuat); + TestEqual(TEXT("framequat value kind"), FrameQuat.ValueKind, EMjSensorValueKind::Quaternion); + TestEqual(TEXT("framequat dim"), FrameQuat.FixedDim, 4); + + const FMjSensorTypeInfo& FramePos = MjSensorTypeInfoFor(EMjSensorType::FramePos); + TestEqual(TEXT("framepos value kind"), FramePos.ValueKind, EMjSensorValueKind::Position); + + const FMjSensorTypeInfo& FrameXAxis = MjSensorTypeInfoFor(EMjSensorType::FrameXAxis); + TestEqual(TEXT("framexaxis semantic"), FrameXAxis.Semantic, EMjSensorSemantic::FrameAxis); + TestEqual(TEXT("framexaxis value kind"), FrameXAxis.ValueKind, EMjSensorValueKind::Direction); + + const FMjSensorTypeInfo& FromTo = MjSensorTypeInfoFor(EMjSensorType::GeomFromTo); + TestEqual(TEXT("fromto value kind"), FromTo.ValueKind, EMjSensorValueKind::GeomFromTo); + TestEqual(TEXT("fromto dim"), FromTo.FixedDim, 6); + + // camprojection is a two-component scalar output. + TestEqual(TEXT("camprojection dim"), + MjSensorTypeInfoFor(EMjSensorType::CamProjection).FixedDim, 2); + + // Variable-length sensors report -1. + TestEqual(TEXT("user dim is variable"), + MjSensorTypeInfoFor(EMjSensorType::User).FixedDim, -1); + TestEqual(TEXT("contact dim is variable"), + MjSensorTypeInfoFor(EMjSensorType::Contact).FixedDim, -1); + + // A plain scalar (jointpos) carries the Generic->no-transform default. + TestEqual(TEXT("jointpos value kind"), + MjSensorTypeInfoFor(EMjSensorType::JointPos).ValueKind, EMjSensorValueKind::Scalar); + return true; +} + +// ============================================================================ +// URLab.SensorTypeInfo.UnknownTypeFallsBackToAccelerometer +// Mirrors the historical ExportTo default arm. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoFallback, + "URLab.SensorTypeInfo.UnknownTypeFallsBackToAccelerometer", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoFallback::RunTest(const FString& Parameters) +{ + const FMjSensorTypeInfo& Info = MjSensorTypeInfoFor(static_cast(0xFE)); + TestEqual(TEXT("unmapped type falls back to accelerometer"), + Info.Type, EMjSensorType::Accelerometer); + return true; +} + +// ============================================================================ +// URLab.SensorTypeInfo.UserAndPluginTagsResolveToTheirSubclasses +// Regression: the editor XML parser's tag -> UClass chain omitted the +// and cases, so those tags silently created a base +// UMjSensor. The descriptor table now drives that mapping. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjSensorTypeInfoUserPluginSubclass, + "URLab.SensorTypeInfo.UserAndPluginTagsResolveToTheirSubclasses", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjSensorTypeInfoUserPluginSubclass::RunTest(const FString& Parameters) +{ + const FMjSensorTypeInfo* User = MjSensorTypeInfoForTag(TEXT("user")); + const FMjSensorTypeInfo* Plugin = MjSensorTypeInfoForTag(TEXT("plugin")); + if (TestNotNull(TEXT("user descriptor"), User) + && TestNotNull(TEXT("plugin descriptor"), Plugin)) + { + TestEqual(TEXT("user tag -> UMjUserSensor"), + User->SensorClass, UMjUserSensor::StaticClass()); + TestEqual(TEXT("plugin tag -> UMjPluginSensor"), + Plugin->SensorClass, UMjPluginSensor::StaticClass()); + // The bug produced the base class; assert we are past it. + TestNotEqual(TEXT("user is not the base UMjSensor"), + User->SensorClass, UMjSensor::StaticClass()); + } + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjStateCollectorTests.cpp b/Source/URLabEditor/Private/Tests/MjStateCollectorTests.cpp new file mode 100644 index 00000000..36284d2f --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjStateCollectorTests.cpp @@ -0,0 +1,741 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjStateCollectorTests.cpp +// +// Unit tests for the state-serialization IR (Phase 0a): +// - FMjCanonicalName sanitize / part-segment stripping +// - FMjStateCollector produces the IR fields today's paths carried +// - Sensor values are emitted raw (IR == d->sensordata; GetReading == transform(IR)) +// - FMjMsgpackEncoder canonical schema + observation-level filter +// - StructureVersion bumps on a producer-cache rebuild, not a plain collect +// ============================================================================ + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "MjTestHelpers.h" +#include "State/MjCanonicalName.h" +#include "State/MjStateCollector.h" +#include "State/MjMsgpackEncoder.h" +#include "State/MjStateTypes.h" +#include "State/MjObservationLevel.h" +#include "Bridge/RpcDispatcher.h" +#include "Bridge/BridgeServer.h" +#include "Transport/SnapshotPublisher.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "MuJoCo/Components/Actuators/MjActuator.h" +#include "MuJoCo/Components/Sensors/MjSensor.h" +#include "Transport/RosPublishTransport.h" +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" + +using EObs = EObservationLevel; + +// --------------------------------------------------------------------------- +// 1. FMjCanonicalName::Sanitize + PartSegment +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateCanonicalName, + "URLab.State.CanonicalName", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateCanonicalName::RunTest(const FString& Parameters) +{ + TestEqual(TEXT("passthrough legal"), + FMjCanonicalName::Sanitize(TEXT("go2_imu_gyro")), FString(TEXT("go2_imu_gyro"))); + TestEqual(TEXT("illegal chars -> _"), + FMjCanonicalName::Sanitize(TEXT("arm/link-1.x")), FString(TEXT("arm_link_1_x"))); + TestEqual(TEXT("leading digit gets _ prefix"), + FMjCanonicalName::Sanitize(TEXT("3dof")), FString(TEXT("_3dof"))); + TestEqual(TEXT("empty stays empty"), + FMjCanonicalName::Sanitize(TEXT("")), FString(TEXT(""))); + + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + const FString ArtName = Art->GetName(); + + // PartSegment strips exactly one "_" prefix, then sanitizes. + TestEqual(TEXT("PartSegment strips art prefix"), + FMjCanonicalName::PartSegment(Art, ArtName + TEXT("_shoulder")), + FName(TEXT("shoulder"))); + // No prefix -> passthrough (sanitized). + TestEqual(TEXT("PartSegment no-prefix passthrough"), + FMjCanonicalName::PartSegment(Art, TEXT("free_body")), + FName(TEXT("free_body"))); + // ArtSegment mirrors the actor name (already legal in tests). + TestEqual(TEXT("ArtSegment == sanitized actor name"), + FMjCanonicalName::ArtSegment(Art), FName(*ArtName)); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 2. Canonical msgpack schema: EncodeSnapshot top-level keys + level filter. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateSchema, + "URLab.State.Schema", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateSchema::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + if (!m || !d) + { + AddError(TEXT("Model/data missing")); + S.Cleanup(); + return false; + } + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 7); + + TSharedPtr Full = FMjMsgpackEncoder::EncodeSnapshot(Snap, EObs::Full); + TestTrue(TEXT("snapshot has time"), Full->HasField(TEXT("time"))); + TestTrue(TEXT("snapshot has step"), Full->HasField(TEXT("step"))); + TestTrue(TEXT("snapshot has sim_time"), Full->HasField(TEXT("sim_time"))); + TestTrue(TEXT("snapshot has wall_time"), Full->HasField(TEXT("wall_time"))); + TestTrue(TEXT("snapshot has arts"), Full->HasField(TEXT("arts"))); + TestTrue(TEXT("snapshot has scene"), Full->HasField(TEXT("scene"))); + + FString Op; + Full->TryGetStringField(TEXT("op"), Op); + TestEqual(TEXT("op == state_full"), Op, FString(TEXT("state_full"))); + double Step = 0.0; + Full->TryGetNumberField(TEXT("step"), Step); + TestEqual(TEXT("step echoes collected index"), (int64)Step, (int64)7); + + // Minimal level: per-art block carries qpos/qvel only. + TSharedPtr MinArts = FMjMsgpackEncoder::EncodeArts(Snap, EObs::Minimal); + if (MinArts->Values.Num() > 0) + { + const TSharedPtr* ArtObj = nullptr; + MinArts->Values.CreateConstIterator()->Value->TryGetObject(ArtObj); + if (ArtObj && ArtObj->IsValid()) + { + TestTrue(TEXT("Minimal has qpos"), (*ArtObj)->HasField(TEXT("qpos"))); + TestTrue(TEXT("Minimal has qvel"), (*ArtObj)->HasField(TEXT("qvel"))); + TestFalse(TEXT("Minimal lacks ctrl"), (*ArtObj)->HasField(TEXT("ctrl"))); + TestFalse(TEXT("Minimal lacks sensors"), (*ArtObj)->HasField(TEXT("sensors"))); + TestFalse(TEXT("Minimal lacks bodies"), (*ArtObj)->HasField(TEXT("bodies"))); + } + } + else + { + AddError(TEXT("expected at least one articulation in the arts block")); + } + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 3. Joint slot widths: hinge is 1/1; a free base is 7/6. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateJointWidths, + "URLab.State.JointWidths", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateJointWidths::RunTest(const FString& Parameters) +{ + // Default rig has a single hinge joint -> per-art qpos/qvel width 1/1. + { + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Hinge; + Sess.Joint->bOverride_Type = true; + })) + { + AddError(S.LastError); + return false; + } + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + if (Snap.Articulations.Num() > 0 && Snap.Articulations[0].Joints.Num() > 0) + { + const FMjJointState& J = Snap.Articulations[0].Joints[0]; + TestEqual(TEXT("hinge type"), (int)J.Type, (int)EMjJointType::Hinge); + TestEqual(TEXT("hinge qpos width 1"), J.QPos.Num(), 1); + TestEqual(TEXT("hinge qvel width 1"), J.QVel.Num(), 1); + } + else + { + AddError(TEXT("expected a hinge joint in the IR")); + } + S.Cleanup(); + } + + // Free base -> 7/6. + { + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Free; + Sess.Joint->bOverride_Type = true; + })) + { + AddInfo(FString::Printf(TEXT("Skipping free-joint width: %s"), *S.LastError)); + return true; + } + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + if (Snap.Articulations.Num() > 0 && Snap.Articulations[0].Joints.Num() > 0) + { + const FMjJointState& J = Snap.Articulations[0].Joints[0]; + TestEqual(TEXT("free type"), (int)J.Type, (int)EMjJointType::Free); + TestEqual(TEXT("free qpos width 7"), J.QPos.Num(), 7); + TestEqual(TEXT("free qvel width 6"), J.QVel.Num(), 6); + } + else + { + AddError(TEXT("expected a free joint in the IR")); + } + S.Cleanup(); + } + + return true; +} + +// --------------------------------------------------------------------------- +// 4. Actuator ctrl / act / force equal the raw mjData values. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateActuatorParity, + "URLab.State.ActuatorParity", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateActuatorParity::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init([](FMjUESession& Sess) { + Sess.Joint->Type = EMjJointType::Slide; + Sess.Joint->bOverride_Type = true; + UMjActuator* A = NewObject(Sess.Robot, TEXT("TestActuator")); + A->Type = EMjActuatorType::Motor; + A->TargetName = Sess.Joint->GetName(); + A->RegisterComponent(); + A->AttachToComponent(Sess.Robot->GetRootComponent(), FAttachmentTransformRules::KeepRelativeTransform); + })) + { + AddInfo(FString::Printf(TEXT("Skipping ActuatorParity: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + if (!m || !d || m->nu == 0) + { + AddInfo(TEXT("Skipping ActuatorParity: no actuators compiled")); + S.Cleanup(); + return true; + } + + AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + TArray Acts = Art->GetActuators(); + if (Acts.Num() == 0 || !Acts[0] || Acts[0]->GetMjID() < 0) + { + AddInfo(TEXT("Skipping ActuatorParity: actuator did not bind")); + S.Cleanup(); + return true; + } + const int32 Aid = Acts[0]->GetMjID(); + + // Drive a known ctrl into d and recompute derived quantities. + d->ctrl[Aid] = 0.55; + mj_forward(m, d); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + + bool bFound = false; + for (const FMjArticulationState& AS : Snap.Articulations) + { + for (const FMjActuatorState& Act : AS.Actuators) + { + bFound = true; + TestEqual(TEXT("ctrl matches d->ctrl"), Act.Ctrl, (double)d->ctrl[Aid], 1e-9); + TestEqual(TEXT("force matches d->actuator_force"), + Act.Force, (double)d->actuator_force[Aid], 1e-9); + const int ActAddr = (m->actuator_actadr && m->actuator_actadr[Aid] >= 0) + ? m->actuator_actadr[Aid] + : -1; + const double ExpectedAct = (ActAddr >= 0 && ActAddr < m->na) ? d->act[ActAddr] : 0.0; + TestEqual(TEXT("act matches d->act (0 when stateless)"), Act.Act, ExpectedAct, 1e-9); + } + } + TestTrue(TEXT("actuator present in IR"), bFound); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 5. Sensor values are emitted raw: IR Values == d->sensordata (MuJoCo SI), +// while GetReading() applies the MuJoCo -> UE transform on top. For a +// framequat the quaternion reorder makes the two provably differ, proving the +// transform no longer contaminates the IR. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateSensorRawParity, + "URLab.State.SensorRawParity", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateSensorRawParity::RunTest(const FString& Parameters) +{ + // A framequat sensor exercises the quaternion reorder path in + // TransformSensorReading, so raw slots differ from the transformed reading. + const FString Xml = TEXT( + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""); + + FMjXmlImportSession S; + if (!S.Init(Xml) || !S.Compile()) + { + AddInfo(FString::Printf(TEXT("Skipping SensorTransformParity: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Model(); + mjData* d = S.Data(); + if (!m || !d || !S.Robot) + { + AddInfo(TEXT("Skipping SensorTransformParity: no model/robot")); + S.Cleanup(); + return true; + } + mj_forward(m, d); + + // Rotate the body so the quaternion is non-trivial, then recompute. + if (m->nq >= 7) + { + d->qpos[3] = 0.7071; // w + d->qpos[4] = 0.7071; // x + d->qpos[5] = 0.0; + d->qpos[6] = 0.0; + mj_forward(m, d); + } + + UMjSensor* Sensor = nullptr; + TArray Sensors; + S.Robot->GetComponents(Sensors); + for (UMjSensor* Sen : Sensors) + { + if (Sen && !Sen->bIsDefault && Sen->GetMjID() >= 0) + { + Sensor = Sen; + break; + } + } + if (!Sensor) + { + AddInfo(TEXT("Skipping SensorRawParity: no bound sensor")); + S.Cleanup(); + return true; + } + + const TArray Reading = Sensor->GetReading(); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + + const TArray* IRValues = nullptr; + for (const FMjArticulationState& AS : Snap.Articulations) + { + for (const FMjSensorState& Sen : AS.Sensors) + { + IRValues = &Sen.Values; + break; + } + if (IRValues) + break; + } + + if (!IRValues) + { + AddError(TEXT("sensor missing from the IR")); + S.Cleanup(); + return false; + } + + // The IR carries the raw MuJoCo sensordata slice verbatim (double precision). + const int SensorAdr = m->sensor_adr[Sensor->GetMjID()]; + const int SensorDim = m->sensor_dim[Sensor->GetMjID()]; + TestEqual(TEXT("IR sensor dim == model sensor_dim"), IRValues->Num(), SensorDim); + if (IRValues->Num() == SensorDim) + { + for (int32 i = 0; i < SensorDim; ++i) + TestEqual(*FString::Printf(TEXT("IR value[%d] == raw d->sensordata"), i), + (*IRValues)[i], d->sensordata[SensorAdr + i], 1e-12); + } + + // GetReading() applies the MuJoCo -> UE transform on top of the raw IR. For a + // framequat (wxyz -> UE xyzw with handedness flip) the two must differ, which + // proves the IR is genuinely raw and the transform lives only on the getter. + TestEqual(TEXT("GetReading dim == IR dim"), Reading.Num(), IRValues->Num()); + if (Reading.Num() == 4 && IRValues->Num() == 4) + { + const double mj_w = (*IRValues)[0], mj_x = (*IRValues)[1], + mj_y = (*IRValues)[2], mj_z = (*IRValues)[3]; + TestEqual(TEXT("GetReading[0] == -mjX"), (double)Reading[0], -mj_x, 1e-5); + TestEqual(TEXT("GetReading[1] == mjY"), (double)Reading[1], mj_y, 1e-5); + TestEqual(TEXT("GetReading[2] == -mjZ"), (double)Reading[2], -mj_z, 1e-5); + TestEqual(TEXT("GetReading[3] == mjW"), (double)Reading[3], mj_w, 1e-5); + // The reorder must actually move data: IR[0] is mjW, GetReading[0] is -mjX. + TestTrue(TEXT("IR differs from GetReading (transform is real)"), + FMath::Abs((*IRValues)[0] - (double)Reading[0]) > 1e-6); + } + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 5b. Gyro + accel raw values reach the IR unchanged (MuJoCo SI, no Y-negation), +// and FillImu emits them verbatim into the Imu components. MuJoCo's gyro/ +// accel convention already equals ROS's, so a correct IMU needs the raw IR +// and no flip anywhere on the ROS path. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateImuRawToFillImu, + "URLab.State.ImuRawToFillImu", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateImuRawToFillImu::RunTest(const FString& Parameters) +{ + const FString Xml = TEXT( + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + ""); + + FMjXmlImportSession S; + if (!S.Init(Xml) || !S.Compile()) + { + AddInfo(FString::Printf(TEXT("Skipping ImuRawToFillImu: %s"), *S.LastError)); + return true; + } + + mjModel* m = S.Model(); + mjData* d = S.Data(); + if (!m || !d || !S.Robot) + { + AddInfo(TEXT("Skipping ImuRawToFillImu: no model/robot")); + S.Cleanup(); + return true; + } + + // Give the base a nonzero body angular velocity so the gyro reads a distinct, + // asymmetric value per axis (proving no accidental Y flip), then recompute so + // the vel/acc sensor stages populate d->sensordata. + if (m->nv >= 6) + { + d->qvel[3] = 0.3; // body wx + d->qvel[4] = 0.5; // body wy + d->qvel[5] = 0.7; // body wz + } + mj_forward(m, d); + + // Locate the gyro and accel components by type so raw slices are read via the + // bound sensor id (compiled sensor names carry an articulation prefix, so a + // bare mj_name2id lookup would miss them). + auto RawSliceForType = [&](EMjSensorType Want) { + TArray Out; + TArray All; + S.Robot->GetComponents(All); + for (UMjSensor* Sen : All) + { + if (!Sen || Sen->bIsDefault || Sen->Type != Want || Sen->GetMjID() < 0) + continue; + const int Adr = m->sensor_adr[Sen->GetMjID()]; + const int Dim = m->sensor_dim[Sen->GetMjID()]; + for (int i = 0; i < Dim; ++i) + Out.Add(d->sensordata[Adr + i]); + break; + } + return Out; + }; + const TArray RawGyro = RawSliceForType(EMjSensorType::Gyro); + const TArray RawAccel = RawSliceForType(EMjSensorType::Accelerometer); + if (RawGyro.Num() != 3 || RawAccel.Num() != 3) + { + AddInfo(TEXT("Skipping ImuRawToFillImu: gyro/accel did not bind")); + S.Cleanup(); + return true; + } + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + + // The IR must carry each sensor's raw slice verbatim under the right semantic. + const FMjSensorState* IRGyro = nullptr; + const FMjSensorState* IRAccel = nullptr; + for (const FMjArticulationState& AS : Snap.Articulations) + { + for (const FMjSensorState& Sen : AS.Sensors) + { + if (Sen.Semantic == EMjSensorSemantic::Gyro) + IRGyro = &Sen; + else if (Sen.Semantic == EMjSensorSemantic::Accel) + IRAccel = &Sen; + } + } + + if (!IRGyro || !IRAccel) + { + AddError(TEXT("gyro/accel missing from the IR")); + S.Cleanup(); + return false; + } + + TestEqual(TEXT("IR gyro dim == 3"), IRGyro->Values.Num(), 3); + TestEqual(TEXT("IR accel dim == 3"), IRAccel->Values.Num(), 3); + if (IRGyro->Values.Num() == 3 && IRAccel->Values.Num() == 3) + { + for (int32 i = 0; i < 3; ++i) + { + TestEqual(*FString::Printf(TEXT("IR gyro[%d] == raw"), i), + IRGyro->Values[i], RawGyro[i], 1e-12); + TestEqual(*FString::Printf(TEXT("IR accel[%d] == raw"), i), + IRAccel->Values[i], RawAccel[i], 1e-12); + } + } + + // FillImu (the ROS Imu producer, pure and compiled in every config) emits the + // IR values with no coordinate flip: the Imu carries raw MuJoCo == ROS SI. + const FMjArticulationState& Art = Snap.Articulations[0]; + double Ang[3] = {0, 0, 0}; + double Acc[3] = {0, 0, 0}; + bool bHasAng = false; + bool bHasAcc = false; + const bool bHas = UURLabRosPublishTransport::FillImu(Art, Ang, bHasAng, Acc, bHasAcc); + TestTrue(TEXT("FillImu reports an Imu"), bHas); + TestTrue(TEXT("FillImu has angular velocity"), bHasAng); + TestTrue(TEXT("FillImu has linear acceleration"), bHasAcc); + for (int32 i = 0; i < 3; ++i) + { + TestEqual(*FString::Printf(TEXT("FillImu angular[%d] unflipped"), i), + Ang[i], RawGyro[i], 1e-12); + TestEqual(*FString::Printf(TEXT("FillImu linear[%d] unflipped"), i), + Acc[i], RawAccel[i], 1e-12); + } + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 6. StructureVersion bumps on a producer-cache rebuild, not a plain collect. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateStructureVersion, + "URLab.State.StructureVersion", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateStructureVersion::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + const uint32 V0 = C.GetStructureVersion(); + + // A plain collect does not change the version. + C.Collect(m, d, 0); + TestEqual(TEXT("collect leaves version unchanged"), C.GetStructureVersion(), V0); + + // A rebuild (registry change) bumps it. + C.RebuildProducerCacheGameThread(); + TestTrue(TEXT("rebuild bumps version"), C.GetStructureVersion() > V0); + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 7. A step reply's `arts` block matches EncodeArts of a fresh Collect. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateStepReplyArts, + "URLab.State.StepReplyArts", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateStepReplyArts::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + Disp->SetActiveStepMode(EStepMode::Live); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("step")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + TSharedPtr Reply = Disp->Dispatch(Req); + + FString Op; + Reply->TryGetStringField(TEXT("op"), Op); + TestEqual(TEXT("op == step_ok"), Op, FString(TEXT("step_ok"))); + + const TSharedPtr* ReplyArts = nullptr; + TestTrue(TEXT("reply carries arts"), Reply->TryGetObjectField(TEXT("arts"), ReplyArts)); + TestTrue(TEXT("reply carries scene"), Reply->HasField(TEXT("scene"))); + + // The reply's arts share the same articulation key(s) as a direct encode. + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + const FMjStateSnapshot& Snap = C.Collect(m, d, 0); + TSharedPtr DirectArts = FMjMsgpackEncoder::EncodeArts(Snap, EObs::Standard); + if (ReplyArts && ReplyArts->IsValid()) + { + TestEqual(TEXT("same art count as a fresh encode"), + (*ReplyArts)->Values.Num(), DirectArts->Values.Num()); + for (const auto& Pair : DirectArts->Values) + TestTrue(*FString::Printf(TEXT("reply arts has key %s"), *Pair.Key), + (*ReplyArts)->HasField(Pair.Key)); + } + + S.Cleanup(); + return true; +} + +// --------------------------------------------------------------------------- +// 8. Byte fan-out delivers snapshots to registered publishers, and the +// bPublishersPaused gate suppresses delivery (Path B replacement). +// --------------------------------------------------------------------------- +namespace +{ +struct FFakeSnapshotPublisher : public IMjSnapshotPublisher +{ + int32 Count = 0; + int32 LastBytes = 0; + virtual void PublishSnapshot(const TArray& Bytes) override + { + ++Count; + LastBytes = Bytes.Num(); + } +}; +} // namespace + +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStateByteFanOutPause, + "URLab.State.ByteFanOutPause", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStateByteFanOutPause::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + + FMjStateCollector& C = S.Manager->GetStateCollector(); + C.Init(S.Manager); + C.RebuildProducerCacheGameThread(); + + FFakeSnapshotPublisher Fake; + S.Manager->RegisterSnapshotPublisher(&Fake, S.Manager); + + // Live (unpaused): the fan-out encodes and delivers bytes to the publisher. + S.Manager->bPublishersPaused.store(false); + S.Manager->FanOutStateSnapshot(m, d); + TestTrue(TEXT("publisher receives bytes when live"), Fake.Count >= 1); + TestTrue(TEXT("delivered payload is non-empty"), Fake.LastBytes > 0); + + // Paused (Direct/Puppet): the byte fan-out is suppressed. + const int32 Before = Fake.Count; + S.Manager->bPublishersPaused.store(true); + S.Manager->FanOutStateSnapshot(m, d); + TestEqual(TEXT("no byte delivery while paused"), Fake.Count, Before); + + S.Manager->bPublishersPaused.store(false); + S.Manager->UnregisterSnapshotPublisher(&Fake); + S.Cleanup(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjStepServerTests.cpp b/Source/URLabEditor/Private/Tests/MjStepServerTests.cpp index dd6fac63..a6d1c9f8 100644 --- a/Source/URLabEditor/Private/Tests/MjStepServerTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjStepServerTests.cpp @@ -38,15 +38,20 @@ #include "CoreMinimal.h" #include "Misc/AutomationTest.h" #include "MjTestHelpers.h" -#include "Bridge/OpRegistry.h" #include "Bridge/RpcDispatcher.h" +#include "Bridge/OpRegistry.h" #include "Bridge/BridgeServer.h" #include "Transport/ZmqRpcTransport.h" #include "Bridge/MsgpackHelpers.h" +#include "Bridge/RpcErrorCodes.h" +#include "State/MjStateCollector.h" +#include "State/MjMsgpackEncoder.h" +#include "State/MjStateTypes.h" +#include "MuJoCo/Core/AMjManager.h" #include "MuJoCo/Components/Controllers/MjPDController.h" -#include "MuJoCo/Components/Sensors/MjCamera.h" #include "MuJoCo/Components/Actuators/MjActuator.h" #include "MuJoCo/Components/Bodies/MjBody.h" +#include "MuJoCo/Components/Sensors/MjCamera.h" #include "MuJoCo/Core/MjPhysicsEngine.h" #include "Dom/JsonObject.h" #include "Dom/JsonValue.h" @@ -114,7 +119,7 @@ bool FMjStepServerNoManagerGuard::RunTest(const FString& Parameters) FString Code; Reply->TryGetStringField(TEXT("code"), Code); TestEqual(*FString::Printf(TEXT("op %s -> no_active_manager"), *Op), - Code, FString(TEXT("no_active_manager"))); + Code, FString(URLabError::NoActiveManager)); }; AssertNoManager(TEXT("step")); @@ -257,9 +262,11 @@ bool FMjStepServerPauseFlag::RunTest(const FString& Parameters) } Disp->SetActiveStepMode(EStepMode::Direct); - TestTrue(TEXT("Direct mode flips manager pause flag true"), + TestTrue(TEXT("Direct mode flips manager (state/ctrl) pause flag true"), S.Manager->bPublishersPaused.load()); - TestTrue(TEXT("Direct mode flips camera pause flag true"), + // Cameras stream in EVERY step mode now (decoupled from the step reply), + // so the camera publisher pause flag must stay false regardless of mode. + TestFalse(TEXT("Direct mode keeps camera publishers live"), FCameraZmqWorker::bPublishersPaused.load()); TestEqual(TEXT("ActiveStepMode reflects the switch"), (int)Disp->GetActiveStepMode(), (int)EStepMode::Direct); @@ -267,12 +274,14 @@ bool FMjStepServerPauseFlag::RunTest(const FString& Parameters) Disp->SetActiveStepMode(EStepMode::Live); TestFalse(TEXT("Live resets manager pause flag"), S.Manager->bPublishersPaused.load()); - TestFalse(TEXT("Live resets camera pause flag"), + TestFalse(TEXT("Live keeps camera publishers live"), FCameraZmqWorker::bPublishersPaused.load()); Disp->SetActiveStepMode(EStepMode::Puppet); - TestTrue(TEXT("Puppet mode flips pause flag true"), + TestTrue(TEXT("Puppet mode flips manager pause flag true"), S.Manager->bPublishersPaused.load()); + TestFalse(TEXT("Puppet mode keeps camera publishers live"), + FCameraZmqWorker::bPublishersPaused.load()); // Cleanup: leave the camera worker pause flag reset for downstream tests. Disp->SetActiveStepMode(EStepMode::Live); @@ -282,14 +291,15 @@ bool FMjStepServerPauseFlag::RunTest(const FString& Parameters) } // --------------------------------------------------------------------------- -// 1b. EffectiveStepMode mirrors the resolved mode (regression: live 10 Hz lock) -// The physics loop paces off Manager->EffectiveStepMode. StepMode defaults -// to Auto; if that does not resolve to Live the loop falls into the -// step-request wait path and ticks at the 100 ms timeout (~10 Hz) instead -// of running real-time. +// 1b. The engine's resolved step mode tracks the dispatcher (regression: live +// 10 Hz lock). The physics loop paces off PhysicsEngine->GetStepMode(). +// StepMode defaults to Auto; if that does not resolve to Live the loop +// falls into the step-request wait path and ticks at the 100 ms timeout +// (~10 Hz) instead of running real-time. Each SetActiveStepMode must push +// the resolved mode down to the engine via the step strategy's OnEnter. // --------------------------------------------------------------------------- IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStepServerEffectiveMode, - "URLab.StepServer.EffectiveStepMode", + "URLab.StepServer.ResolvedStepMode", EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) bool FMjStepServerEffectiveMode::RunTest(const FString& Parameters) @@ -301,11 +311,12 @@ bool FMjStepServerEffectiveMode::RunTest(const FString& Parameters) return false; } - // StepMode is Auto by default; RegisterManager must resolve+mirror it to Live. + // StepMode is Auto by default; RegisterManager must resolve it to Live and + // push that down to the engine the physics worker paces off. TestEqual(TEXT("configured StepMode is Auto"), (int)S.Manager->StepMode, (int)EStepMode::Auto); - TestEqual(TEXT("EffectiveStepMode resolves Auto -> Live"), - (int)S.Manager->EffectiveStepMode.load(), (int)EStepMode::Live); + TestEqual(TEXT("engine resolves Auto -> Live"), + (int)S.Manager->PhysicsEngine->GetStepMode(), (int)EStepMode::Live); FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); if (!Disp) @@ -316,16 +327,16 @@ bool FMjStepServerEffectiveMode::RunTest(const FString& Parameters) } Disp->SetActiveStepMode(EStepMode::Direct); - TestEqual(TEXT("EffectiveStepMode tracks Direct"), - (int)S.Manager->EffectiveStepMode.load(), (int)EStepMode::Direct); + TestEqual(TEXT("engine tracks Direct"), + (int)S.Manager->PhysicsEngine->GetStepMode(), (int)EStepMode::Direct); Disp->SetActiveStepMode(EStepMode::Puppet); - TestEqual(TEXT("EffectiveStepMode tracks Puppet"), - (int)S.Manager->EffectiveStepMode.load(), (int)EStepMode::Puppet); + TestEqual(TEXT("engine tracks Puppet"), + (int)S.Manager->PhysicsEngine->GetStepMode(), (int)EStepMode::Puppet); Disp->SetActiveStepMode(EStepMode::Live); - TestEqual(TEXT("EffectiveStepMode tracks Live"), - (int)S.Manager->EffectiveStepMode.load(), (int)EStepMode::Live); + TestEqual(TEXT("engine tracks Live"), + (int)S.Manager->PhysicsEngine->GetStepMode(), (int)EStepMode::Live); S.Cleanup(); return true; @@ -527,7 +538,9 @@ bool FMjStepServerSessionId::RunTest(const FString& Parameters) } // --------------------------------------------------------------------------- -// 5. Puppet mode push-state writes qpos/qvel and fires OnPostStep +// 5. Puppet mode: a step RPC pushes qpos/qvel/time inline and fires OnPostStep. +// Drives the real dispatch path (SetActiveStepMode(Puppet) + step op) rather +// than a dead test-only queue. // --------------------------------------------------------------------------- IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStepServerPuppetHandler, "URLab.StepServer.PuppetHandler", @@ -550,6 +563,7 @@ bool FMjStepServerPuppetHandler::RunTest(const FString& Parameters) return false; } Disp->SetActiveStepMode(EStepMode::Puppet); + Disp->SetActiveSessionIdForTest(TEXT("test-session")); mjModel* m = S.Manager->PhysicsEngine->GetModel(); mjData* d = S.Manager->PhysicsEngine->GetData(); @@ -565,28 +579,24 @@ bool FMjStepServerPuppetHandler::RunTest(const FString& Parameters) OnPostStepCount++; }; - // Build a push-state request that sets qpos[0] and qvel[0] to known values. - FMjPushStateRequest Req; - Req.QPos.SetNum(m->nq); - Req.QVel.SetNum(m->nv); - if (m->nq > 0) - Req.QPos[0] = 0.42; - if (m->nv > 0) - Req.QVel[0] = 0.13; - Req.Time = 1.5; - - Disp->EnqueuePushStateRequestForTest(MoveTemp(Req)); + // Build a step request that pushes a full qpos/qvel with known slot-0 values. + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("step")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + TArray> QPos; + for (int i = 0; i < m->nq; ++i) + QPos.Add(MakeShared(i == 0 ? 0.42 : 0.0)); + TArray> QVel; + for (int i = 0; i < m->nv; ++i) + QVel.Add(MakeShared(i == 0 ? 0.13 : 0.0)); + Req->SetArrayField(TEXT("qpos"), QPos); + Req->SetArrayField(TEXT("qvel"), QVel); + Req->SetNumberField(TEXT("time"), 1.5); - // Drive the engine's CustomStepHandler explicitly. The puppet handler - // dequeues, writes, calls mj_forward, and fires OnPostStep. - if (S.Manager->PhysicsEngine->CustomStepHandler) - { - S.Manager->PhysicsEngine->CustomStepHandler(m, d); - } - else - { - AddError(TEXT("CustomStepHandler not installed in Puppet mode")); - } + TSharedPtr Reply = Disp->Dispatch(Req); + FString Op; + Reply->TryGetStringField(TEXT("op"), Op); + TestEqual(TEXT("puppet step -> step_ok"), Op, FString(TEXT("step_ok"))); if (m->nq > 0) TestEqual(TEXT("qpos[0] written"), (double)d->qpos[0], 0.42, 1e-9); @@ -821,9 +831,17 @@ bool FMjStepServerObservationLevels::RunTest(const FString& Parameters) return false; } + // The IR is built once; the msgpack encoder applies the observation-level + // filter. RebuildProducerCacheGameThread runs at PostCompile, but call it + // explicitly so the test does not depend on that ordering. + FMjStateCollector& Collector = S.Manager->GetStateCollector(); + Collector.Init(S.Manager); + Collector.RebuildProducerCacheGameThread(); + const FMjStateSnapshot& Snap = Collector.Collect(m, d, 0); + // Minimal: qpos / qvel only. - TSharedPtr Min = FURLabRpcDispatcher::BuildStepObservations( - S.Manager, m, d, FURLabRpcDispatcher::EObservationLevel::Minimal); + TSharedPtr Min = FMjMsgpackEncoder::EncodeArts( + Snap, EObservationLevel::Minimal); TestTrue(TEXT("Minimal returns object"), Min.IsValid()); if (Min.IsValid() && Min->Values.Num() > 0) { @@ -841,8 +859,8 @@ bool FMjStepServerObservationLevels::RunTest(const FString& Parameters) } // Standard: minimal + ctrl + act + sensors. - TSharedPtr Std = FURLabRpcDispatcher::BuildStepObservations( - S.Manager, m, d, FURLabRpcDispatcher::EObservationLevel::Standard); + TSharedPtr Std = FMjMsgpackEncoder::EncodeArts( + Snap, EObservationLevel::Standard); if (Std.IsValid() && Std->Values.Num() > 0) { const TSharedPtr* Art = nullptr; @@ -859,8 +877,8 @@ bool FMjStepServerObservationLevels::RunTest(const FString& Parameters) } // Full: standard + bodies + actuator_force. - TSharedPtr Full = FURLabRpcDispatcher::BuildStepObservations( - S.Manager, m, d, FURLabRpcDispatcher::EObservationLevel::Full); + TSharedPtr Full = FMjMsgpackEncoder::EncodeArts( + Snap, EObservationLevel::Full); if (Full.IsValid() && Full->Values.Num() > 0) { const TSharedPtr* Art = nullptr; @@ -971,7 +989,10 @@ bool FMjStepServerXfrcApplied::RunTest(const FString& Parameters) } // --------------------------------------------------------------------------- -// 11. ApplyControls is gated when StepMode == Puppet +// 11. ApplyControls gate input: the physics worker skips its ApplyControls pass +// when the resolved step mode is Puppet (client pushes qpos/qvel directly). +// The worker reads ResolvedStepMode, surfaced by GetStepMode(); verify +// SetStepMode drives that authoritative value the gate keys off. // --------------------------------------------------------------------------- IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStepServerApplyControlsGate, "URLab.StepServer.ApplyControlsGate", @@ -979,11 +1000,6 @@ IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStepServerApplyControlsGate, bool FMjStepServerApplyControlsGate::RunTest(const FString& Parameters) { - // Verify that AAMjManager::StepMode == Puppet causes the engine's - // ApplyControls call site to be skipped. The engine's async loop is - // not running in tests, but the gate logic is observable through the - // StepMode property that the call site reads on each iteration. The - // critical invariant: StepMode is a UPROPERTY visible to the gate. FMjUESession S; if (!S.Init()) { @@ -991,29 +1007,29 @@ bool FMjStepServerApplyControlsGate::RunTest(const FString& Parameters) return false; } - // Initial mode is Auto. - TestEqual(TEXT("Default StepMode is Auto"), - (int)S.Manager->StepMode, (int)EStepMode::Auto); - - // Puppet mode — the engine's ApplyControls gate should now report skip. - S.Manager->StepMode = EStepMode::Puppet; - TestEqual(TEXT("StepMode set to Puppet"), - (int)S.Manager->StepMode, (int)EStepMode::Puppet); - - // The gate itself: in MjPhysicsEngine.cpp we call - // if (Cast(GetOwner())->StepMode == EStepMode::Puppet) skip; - // This unit-level test verifies the property is reachable via Cast, - // mirroring the gate's own access pattern. - AAMjManager* OwnerMgr = Cast(S.Manager->PhysicsEngine->GetOwner()); - TestNotNull(TEXT("PhysicsEngine owner is AAMjManager"), OwnerMgr); - if (OwnerMgr) + UMjPhysicsEngine* Engine = S.Manager->PhysicsEngine; + if (!Engine) { - TestEqual(TEXT("Engine sees Puppet StepMode through GetOwner()"), - (int)OwnerMgr->StepMode, (int)EStepMode::Puppet); + AddError(TEXT("Manager has no PhysicsEngine")); + S.Cleanup(); + return false; } - // Restore so other tests aren't affected. - S.Manager->StepMode = EStepMode::Auto; + // The gate in RunMujocoAsync is `bSkipApplyControls = (Mode == Puppet)`, + // where Mode == ResolvedStepMode. SetStepMode is the single writer. + Engine->SetStepMode(EStepMode::Puppet); + TestEqual(TEXT("resolved mode is Puppet (ApplyControls skipped)"), + (int)Engine->GetStepMode(), (int)EStepMode::Puppet); + + Engine->SetStepMode(EStepMode::Direct); + TestEqual(TEXT("resolved mode is Direct (ApplyControls runs)"), + (int)Engine->GetStepMode(), (int)EStepMode::Direct); + + // Auto resolves to Live, and Live runs the ApplyControls pass too. + Engine->SetStepMode(EStepMode::Auto); + TestEqual(TEXT("Auto resolves to Live (ApplyControls runs)"), + (int)Engine->GetStepMode(), (int)EStepMode::Live); + S.Cleanup(); return true; } @@ -1084,6 +1100,64 @@ bool FMjStepServerDirectHandler::RunTest(const FString& Parameters) return true; } +// --------------------------------------------------------------------------- +// 12b. Render frame id advances only when the sim state actually advances. +// An idle worker wake (step handler dequeues nothing) must report no +// advance and leave the frame id unchanged; a real step bumps it by one. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjStepServerFrameIdGating, + "URLab.StepServer.FrameIdGating", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjStepServerFrameIdGating::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveStepMode(EStepMode::Direct); + + UMjPhysicsEngine* Engine = S.Manager->PhysicsEngine; + mjModel* m = Engine->GetModel(); + mjData* d = Engine->GetData(); + if (!m || !d || !Engine->CustomStepHandler) + { + AddError(TEXT("Direct mode did not install a CustomStepHandler")); + S.Cleanup(); + return false; + } + + // Idle wake: empty queue -> no advance, frame id frozen. + const int64 IdBefore = (int64)Engine->GetRenderFrameId(); + const bool bAdvancedIdle = Engine->CustomStepHandler(m, d); + TestFalse(TEXT("Empty-queue step reports no advance"), bAdvancedIdle); + TestEqual(TEXT("FrameId unchanged on idle wake"), + (int64)Engine->GetRenderFrameId(), IdBefore); + + // Real step: a queued request advances and bumps the frame id once. + FMjStepRequest Req; + Req.NSteps = 1; + Disp->EnqueueStepRequestForTest(MoveTemp(Req)); + const bool bAdvancedStep = Engine->CustomStepHandler(m, d); + TestTrue(TEXT("Queued step reports advance"), bAdvancedStep); + TestEqual(TEXT("FrameId +1 after a real step"), + (int64)Engine->GetRenderFrameId(), IdBefore + 1); + + Disp->SetActiveStepMode(EStepMode::Live); + S.Cleanup(); + return true; +} + // --------------------------------------------------------------------------- // 13. Perturbation snapshot is reachable from the step server (Puppet path) // --------------------------------------------------------------------------- @@ -1162,6 +1236,11 @@ bool FMjStepServerSetQposByName::RunTest(const FString& Parameters) int32 Jid = Art->GetJoints()[0]->GetMjID(); int32 QAddr = m->jnt_qposadr[Jid]; + // Control writes require an explicit claim; the session owns the art. + FString ClaimOwner; + Disp->GetControlOwnership().Claim(FName(*Art->GetName()), TEXT("test-session"), + 0.0, false, ClaimOwner); + TSharedPtr Req = MakeShared(); Req->SetStringField(TEXT("op"), TEXT("set_qpos")); Req->SetStringField(TEXT("session_id"), TEXT("test-session")); @@ -1209,6 +1288,11 @@ bool FMjStepServerSetQposActorId::RunTest(const FString& Parameters) FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); Disp->SetActiveSessionIdForTest(TEXT("test-session")); + // The claim is keyed by the canonical art name, even when addressed by actor_id. + FString ClaimOwner; + Disp->GetControlOwnership().Claim(FName(*Art->GetName()), TEXT("test-session"), + 0.0, false, ClaimOwner); + TSharedPtr Req = MakeShared(); Req->SetStringField(TEXT("op"), TEXT("set_qpos")); Req->SetStringField(TEXT("session_id"), TEXT("test-session")); @@ -1293,6 +1377,10 @@ bool FMjStepServerSetQposFreeBase::RunTest(const FString& Parameters) int32 HingeAdr = m->jnt_qposadr[JointsArr[1]->GetMjID()]; d->qpos[HingeAdr] = 1.5; // sentinel -- the shortcut must NOT touch this + FString ClaimOwner; + Disp->GetControlOwnership().Claim(FName(*Art->GetName()), TEXT("test-session"), + 0.0, false, ClaimOwner); + TSharedPtr Req = MakeShared(); Req->SetStringField(TEXT("op"), TEXT("set_qpos")); Req->SetStringField(TEXT("session_id"), TEXT("test-session")); @@ -1344,6 +1432,11 @@ bool FMjStepServerSetQposErrors::RunTest(const FString& Parameters) AMjArticulation* Art = S.Manager->GetAllArticulations()[0]; + // Own the art so the dim_mismatch path is reached past the control gate. + FString ClaimOwner; + Disp->GetControlOwnership().Claim(FName(*Art->GetName()), TEXT("test-session"), + 0.0, false, ClaimOwner); + // dim_mismatch: 3-vec into a 1-dim hinge articulation. { TSharedPtr Req = MakeShared(); @@ -1359,7 +1452,7 @@ bool FMjStepServerSetQposErrors::RunTest(const FString& Parameters) TSharedPtr Reply = Disp->Dispatch(Req); FString Code; Reply->TryGetStringField(TEXT("code"), Code); - TestEqual(TEXT("dim_mismatch"), Code, FString(TEXT("dim_mismatch"))); + TestEqual(TEXT("dim_mismatch"), Code, FString(URLabError::DimMismatch)); } // unknown_articulation: target with no matching actor_id. @@ -1374,7 +1467,7 @@ bool FMjStepServerSetQposErrors::RunTest(const FString& Parameters) TSharedPtr Reply = Disp->Dispatch(Req); FString Code; Reply->TryGetStringField(TEXT("code"), Code); - TestEqual(TEXT("unknown_articulation"), Code, FString(TEXT("unknown_articulation"))); + TestEqual(TEXT("unknown_articulation"), Code, FString(URLabError::UnknownArticulation)); } // missing_field: no target field at all. @@ -1388,7 +1481,7 @@ bool FMjStepServerSetQposErrors::RunTest(const FString& Parameters) TSharedPtr Reply = Disp->Dispatch(Req); FString Code; Reply->TryGetStringField(TEXT("code"), Code); - TestEqual(TEXT("missing_field"), Code, FString(TEXT("missing_field"))); + TestEqual(TEXT("missing_field"), Code, FString(URLabError::MissingField)); } S.Cleanup(); @@ -1604,7 +1697,7 @@ bool FMjStepServerShmRejectsEditorOps::RunTest(const FString& Parameters) Reply->TryGetStringField(TEXT("op"), Op); Reply->TryGetStringField(TEXT("code"), Code); TestEqual(TEXT("op == error"), Op, FString(TEXT("error"))); - TestEqual(TEXT("code == wrong_transport"), Code, FString(TEXT("wrong_transport"))); + TestEqual(TEXT("code == wrong_transport"), Code, FString(URLabError::WrongTransport)); // ZMQ stays universal — same payload through ZMQ transport invokes // the dispatcher (which will return its own missing-fields error, @@ -1619,7 +1712,7 @@ bool FMjStepServerShmRejectsEditorOps::RunTest(const FString& Parameters) if (ZmqReply.IsValid()) ZmqReply->TryGetStringField(TEXT("code"), ZmqCode); TestNotEqual(TEXT("ZMQ does not emit wrong_transport"), - ZmqCode, FString(TEXT("wrong_transport"))); + ZmqCode, FString(URLabError::WrongTransport)); Server->Stop(); Server->RemoveFromRoot(); @@ -1836,7 +1929,7 @@ bool FMjStepServerUnknownOpVsNotInEditor::RunTest(const FString& Parameters) FString Code; Reply->TryGetStringField(TEXT("code"), Code); TestEqual(TEXT("unknown_op for genuinely unknown name"), - Code, FString(TEXT("unknown_op"))); + Code, FString(URLabError::UnknownOp)); Server->Stop(); Server->RemoveFromRoot(); @@ -1881,7 +1974,7 @@ bool FMjStepServerRequiredFieldsValidated::RunTest(const FString& Parameters) // registry-level check is supposed to short-circuit BEFORE // OwnerMgr is touched — i.e. this test passes even with no manager. TestEqual(TEXT("missing_field on omitted required field"), - Code, FString(TEXT("missing_field"))); + Code, FString(URLabError::MissingField)); TestTrue(TEXT("error message names the field"), Msg.Contains(TEXT("paused"))); diff --git a/Source/URLabEditor/Private/Tests/MjThreadTests.cpp b/Source/URLabEditor/Private/Tests/MjThreadTests.cpp index 7c07ad30..af95097c 100644 --- a/Source/URLabEditor/Private/Tests/MjThreadTests.cpp +++ b/Source/URLabEditor/Private/Tests/MjThreadTests.cpp @@ -142,13 +142,13 @@ bool FMjThreadPauseResume::RunTest(const FString& Parameters) } // Pause - S.Manager->PhysicsEngine->bIsPaused = true; + S.Manager->PhysicsEngine->SetPaused(true); // Direct steps still execute; the async loop would honour the flag S.Step(10); // Resume - S.Manager->PhysicsEngine->bIsPaused = false; + S.Manager->PhysicsEngine->SetPaused(false); TestTrue(TEXT("Manager should be running after unpause"), S.Manager->IsRunning()); TestTrue(TEXT("Manager should be initialized after unpause"), S.Manager->IsInitialized()); @@ -192,3 +192,113 @@ bool FMjThreadModelIntegrity::RunTest(const FString& Parameters) S.Cleanup(); return true; } + +// ============================================================================ +// URLab.Thread.LivePacing +// Runs the async worker in live mode for a wall-clock window and checks that +// sim time advances at ~real time. Guards two runtime behaviours the headless +// suite otherwise can't see: the resolved-step-mode fix (a default Auto scene +// used to fall through to the ~10Hz step-event timeout instead of the pacer) +// and the hybrid-sleep pacer (must hold the rate, not overshoot into slow-mo). +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjThreadLivePacing, + "URLab.Thread.LivePacing", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjThreadLivePacing::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("Init() failed: %s"), *S.LastError)); + return false; + } + + UMjPhysicsEngine* Engine = S.Manager->PhysicsEngine; + + // Live mode, full speed, unpaused, worker running. + Engine->SetStepMode(EStepMode::Live); + Engine->SetSimSpeed(100.0f); + Engine->SetPaused(false); + Engine->RunMujocoAsync(); + + const double SimStart = Engine->GetSimTime(); + const double WallStart = FPlatformTime::Seconds(); + FPlatformProcess::Sleep(0.5f); + const double WallElapsed = FPlatformTime::Seconds() - WallStart; + const double SimElapsed = Engine->GetSimTime() - SimStart; + + // Stop and join the worker before the session tears the engine down. + Engine->bShouldStopTask = true; + if (Engine->StepRequestEvent) + Engine->StepRequestEvent->Trigger(); + if (Engine->AsyncPhysicsFuture.IsValid()) + Engine->AsyncPhysicsFuture.Wait(); + + const double Ratio = (WallElapsed > 0.0) ? (SimElapsed / WallElapsed) : 0.0; + AddInfo(FString::Printf(TEXT("LivePacing: sim=%.3fs wall=%.3fs ratio=%.2f"), + SimElapsed, WallElapsed, Ratio)); + + // Real-time pacing at 100%: sim advances ~= wall (ratio ~1). The old ~10Hz + // lock gives ratio ~0.02; a pacer that oversleeps gives ratio well under 1; + // no pacing at all gives ratio well over 1. Wide window to stay non-flaky. + TestTrue(FString::Printf(TEXT("Live sim advances ~ real time (ratio=%.2f, want 0.5-1.5)"), Ratio), + Ratio > 0.5 && Ratio < 1.5); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Thread.LiveSnapshotGating +// In live mode the worker steps continuously but should publish a render +// snapshot (bump FrameId) only when the game thread has asked for one, so the +// full-state copy runs at consumer rate rather than physics rate. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjThreadLiveSnapshotGating, + "URLab.Thread.LiveSnapshotGating", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjThreadLiveSnapshotGating::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("Init() failed: %s"), *S.LastError)); + return false; + } + + UMjPhysicsEngine* Engine = S.Manager->PhysicsEngine; + Engine->SetStepMode(EStepMode::Live); + Engine->SetSimSpeed(100.0f); + Engine->SetPaused(false); + Engine->RunMujocoAsync(); + + // Let the worker flush the initial pending publish (bSnapshotWanted defaults + // true), then clear it: with no consumer asking, FrameId must hold steady + // even though the worker keeps stepping. + FPlatformProcess::Sleep(0.05f); + Engine->bSnapshotWanted.store(false, std::memory_order_release); + const uint64 IdIdle0 = Engine->GetRenderFrameId(); + FPlatformProcess::Sleep(0.1f); + const uint64 IdIdle1 = Engine->GetRenderFrameId(); + + // Ask for one; the next step should publish. + Engine->bSnapshotWanted.store(true, std::memory_order_release); + FPlatformProcess::Sleep(0.05f); + const uint64 IdAfterRequest = Engine->GetRenderFrameId(); + + Engine->bShouldStopTask = true; + if (Engine->StepRequestEvent) + Engine->StepRequestEvent->Trigger(); + if (Engine->AsyncPhysicsFuture.IsValid()) + Engine->AsyncPhysicsFuture.Wait(); + + TestEqual(TEXT("FrameId holds steady while no consumer requests a snapshot"), + (int64)IdIdle1, (int64)IdIdle0); + TestTrue(TEXT("FrameId advances once a consumer requests a snapshot"), + IdAfterRequest > IdIdle1); + + S.Cleanup(); + return true; +} diff --git a/Source/URLabEditor/Private/Tests/MjUrdfExportTests.cpp b/Source/URLabEditor/Private/Tests/MjUrdfExportTests.cpp new file mode 100644 index 00000000..fae70229 --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjUrdfExportTests.cpp @@ -0,0 +1,537 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +// ============================================================================ +// MjUrdfExportTests.cpp +// +// Tests for the mjModel -> URDF exporter (FUrdfExporter) and its matched ROS +// pieces: the qpos0 /joint_states shift and the latched robot_description +// publisher. The exporter is a direct port of the Python prototype validated +// against the Franka (docs/plan_ros_urdf_port_spec.md). +// +// The whole file compiles only when ROS 2 is linked (URLAB_WITH_ROS2), so the +// ROS-off build is unaffected. Most cases need no live ROS runtime -- string +// generation and forward kinematics run against a plain mjModel; only the +// robot_description wire case gates on an available ROS context. +// ============================================================================ + +#if defined(URLAB_WITH_ROS2) && URLAB_WITH_ROS2 + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Misc/Paths.h" +#include "Misc/FileHelper.h" +#include "HAL/FileManager.h" + +#include "MjTestHelpers.h" +#include "Urdf/UrdfExporter.h" +#include "Transport/RosContext.h" +#include "Transport/RosPublishTransport.h" +#include "State/MjStateTypes.h" +#include "State/MjCanonicalName.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" + +#include "mujoco/mujoco.h" + +#include + +namespace +{ +// Two-link chain welded to the world: root "base" (no joint), "link1" on a +// limited hinge with a non-zero reference (exercises the qpos0 shift), "link2" +// on a limited slide carrying a mesh geom (exercises mesh emission). +const TCHAR* kChainXml = TEXT(R"XML( + + + + + + + + + + + + + + + + + + + +)XML"); + +// A jnt_pos anchor (non-zero) plus a two-joint wrist body, so the frame-shift +// and the multi-joint dummy-link chain are both exercised. +const TCHAR* kAnchorXml = TEXT(R"XML( + + + + + + + + + + + + + + + + + +)XML"); + +// --- double quaternion (w,x,y,z) helpers for the FK self-test ------------- + +struct FQd { double w, x, y, z; }; + +FQd QMul(const FQd& A, const FQd& B) +{ + return { + A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, + A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, + A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, + A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w}; +} + +FQd QConj(const FQd& Q) { return {Q.w, -Q.x, -Q.y, -Q.z}; } + +FVector QRot(const FQd& Q, const FVector& V) +{ + const double Tx = 2.0 * (Q.y * V.Z - Q.z * V.Y); + const double Ty = 2.0 * (Q.z * V.X - Q.x * V.Z); + const double Tz = 2.0 * (Q.x * V.Y - Q.y * V.X); + return FVector( + V.X + Q.w * Tx + (Q.y * Tz - Q.z * Ty), + V.Y + Q.w * Ty + (Q.z * Tx - Q.x * Tz), + V.Z + Q.w * Tz + (Q.x * Ty - Q.y * Tx)); +} + +// URDF rpy (extrinsic XYZ) -> quaternion: R = Rz(yaw)*Ry(pitch)*Rx(roll). +FQd QFromRpy(const FVector& Rpy) +{ + const double R = Rpy.X * 0.5, P = Rpy.Y * 0.5, Y = Rpy.Z * 0.5; + const FQd Qx{std::cos(R), std::sin(R), 0, 0}; + const FQd Qy{std::cos(P), 0, std::sin(P), 0}; + const FQd Qz{std::cos(Y), 0, 0, std::sin(Y)}; + return QMul(Qz, QMul(Qy, Qx)); +} + +struct FPose { FVector Pos = FVector::ZeroVector; FQd Rot{1, 0, 0, 0}; }; + +// Reproduce mjModel geom_xpos from the exported URDF at the zero pose (q=0 == +// qpos0) and check every emitted geom lands within Tol metres of MuJoCo, +// rebased into the URDF root-link frame. This validates the joint origins, the +// axes, the qpos0 shift, the jnt_pos frame-shift and the dummy chain together. +bool CheckForwardKinematics(FAutomationTestBase& Test, const FUrdfModel& Model, + mjModel* m, mjData* d, double Tol) +{ + mj_resetData(m, d); // qpos <- qpos0 + mj_forward(m, d); + + // Root body of the model = the body with world as parent among the exported + // links. Rebase everything into its frame. + int RootBodyId = -1; + for (int i = 1; i < m->nbody; ++i) + { + if (m->body_parentid[i] == 0) { RootBodyId = i; break; } + } + if (RootBodyId < 0) + { + Test.AddError(TEXT("no world-rooted body")); + return false; + } + const FVector RootPos(d->xpos[3 * RootBodyId + 0], d->xpos[3 * RootBodyId + 1], + d->xpos[3 * RootBodyId + 2]); + const FQd RootQuat{d->xquat[4 * RootBodyId + 0], d->xquat[4 * RootBodyId + 1], + d->xquat[4 * RootBodyId + 2], d->xquat[4 * RootBodyId + 3]}; + const FQd RootConj = QConj(RootQuat); + + // Link world poses in the root frame. Root links start at identity. + TMap LinkWorld; + for (const FUrdfLink& L : Model.Links) + LinkWorld.Add(L.Name, FPose()); + + // Joints are emitted in topological order, so a single pass resolves the tree. + for (const FUrdfJoint& J : Model.Joints) + { + const FPose* Parent = LinkWorld.Find(J.Parent); + if (!Parent) + { + Test.AddError(FString::Printf(TEXT("joint %s: unknown parent %s"), *J.Name, *J.Parent)); + return false; + } + FPose Child; + Child.Pos = Parent->Pos + QRot(Parent->Rot, J.OriginPos); + Child.Rot = QMul(Parent->Rot, QFromRpy(J.OriginRpy)); + LinkWorld.Add(J.Child, Child); + } + + bool bOk = true; + int Checked = 0; + for (const FUrdfLink& L : Model.Links) + { + const FPose& LW = LinkWorld[L.Name]; + for (const FUrdfGeomFrame& G : L.Geoms) + { + const FVector UrdfWorld = LW.Pos + QRot(LW.Rot, G.LocalPos); + const FVector MjWorld(d->geom_xpos[3 * G.MjGeomId + 0], + d->geom_xpos[3 * G.MjGeomId + 1], d->geom_xpos[3 * G.MjGeomId + 2]); + const FVector MjRebased = QRot(RootConj, MjWorld - RootPos); + const double Err = (UrdfWorld - MjRebased).Size(); + ++Checked; + if (Err > Tol) + { + bOk = false; + Test.AddError(FString::Printf( + TEXT("geom %d (%s): FK error %.3g m > %.3g"), G.MjGeomId, *L.Name, Err, Tol)); + } + } + } + Test.TestTrue(TEXT("FK checked at least one geom"), Checked > 0); + return bOk && Checked > 0; +} + +// Find a joint by its URDF name. +const FUrdfJoint* FindJoint(const FUrdfModel& Model, const TCHAR* Name) +{ + for (const FUrdfJoint& J : Model.Joints) + if (J.Name == Name) + return &J; + return nullptr; +} + +int CountJointType(const FUrdfModel& Model, const TCHAR* Type) +{ + int N = 0; + for (const FUrdfJoint& J : Model.Joints) + if (J.Type == Type) + ++N; + return N; +} +} // namespace + +// --------------------------------------------------------------------------- +// 1. Counts / types / limits / mesh emission on the two-link chain. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfExportCounts, + "URLab.Urdf.ExportCountsLimitsMeshes", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfExportCounts::RunTest(const FString& Parameters) +{ + FMjTestSession S; + if (!S.CompileXml(kChainXml)) + { + AddError(S.LastError); + return false; + } + mjModel* m = S.m; + + const FString OutDir = FPaths::Combine(FPaths::ProjectSavedDir(), + TEXT("URLab"), TEXT("Tests"), TEXT("urdf_chain")); + FUrdfExportConfig Cfg; + Cfg.bAppendTool0 = false; // test model has no single clear leaf body + const FUrdfModel Model = FUrdfExporter::ExportToDir(m, TEXT("chain"), FString(), OutDir, Cfg); + + TestEqual(TEXT("link count == nbody - 1 (excl. world)"), Model.Links.Num(), static_cast(m->nbody) - 1); + TestEqual(TEXT("two joints"), Model.Joints.Num(), 2); + TestEqual(TEXT("one revolute"), CountJointType(Model, TEXT("revolute")), 1); + TestEqual(TEXT("one prismatic"), CountJointType(Model, TEXT("prismatic")), 1); + + // Limits == jnt_range - qpos0. + const FUrdfJoint* J1 = FindJoint(Model, TEXT("j1")); + const FUrdfJoint* J2 = FindJoint(Model, TEXT("j2")); + TestNotNull(TEXT("j1 present"), J1); + TestNotNull(TEXT("j2 present"), J2); + if (J1) + { + const int Jid = mj_name2id(m, mjOBJ_JOINT, "j1"); + const double Q0 = m->qpos0[m->jnt_qposadr[Jid]]; + const double Margin = FUrdfExportConfig().LimitMargin; + TestEqual(TEXT("j1 is revolute"), J1->Type, FString(TEXT("revolute"))); + TestEqual(TEXT("j1 lower == range0 - qpos0 - margin"), J1->Lower, m->jnt_range[2 * Jid + 0] - Q0 - Margin, 1e-9); + TestEqual(TEXT("j1 upper == range1 - qpos0 + margin"), J1->Upper, m->jnt_range[2 * Jid + 1] - Q0 + Margin, 1e-9); + TestTrue(TEXT("j1 qpos0 non-zero (ref applied)"), FMath::Abs(Q0 - 0.5) < 1e-9); + } + if (J2) + { + const int Jid = mj_name2id(m, mjOBJ_JOINT, "j2"); + const double Q0 = m->qpos0[m->jnt_qposadr[Jid]]; + const double Margin = FUrdfExportConfig().LimitMargin; + TestEqual(TEXT("j2 is prismatic"), J2->Type, FString(TEXT("prismatic"))); + TestEqual(TEXT("j2 lower == range0 - qpos0 - margin"), J2->Lower, m->jnt_range[2 * Jid + 0] - Q0 - Margin, 1e-9); + TestEqual(TEXT("j2 upper == range1 - qpos0 + margin"), J2->Upper, m->jnt_range[2 * Jid + 1] - Q0 + Margin, 1e-9); + } + + // Mesh emitted, referenced and on disk. + TestEqual(TEXT("one mesh referenced"), Model.MeshIds.Num(), 1); + TestTrue(TEXT("URDF references a file:// mesh"), Model.Xml.Contains(TEXT("file://"))); + TestTrue(TEXT("URDF references an .stl"), Model.Xml.Contains(TEXT(".stl"))); + if (Model.MeshIds.Num() == 1) + { + const FString Stl = FPaths::Combine(OutDir, TEXT("meshes"), + FUrdfExporter::MeshBaseName(m, Model.MeshIds[0]) + TEXT(".stl")); + TestTrue(TEXT("STL written to disk"), FPaths::FileExists(Stl)); + const int64 Size = IFileManager::Get().FileSize(*Stl); + TestTrue(TEXT("STL is a valid binary header + triangles"), Size >= 84); + TestTrue(TEXT("URDF names the emitted mesh"), + Model.Xml.Contains(FUrdfExporter::MeshBaseName(m, Model.MeshIds[0]) + TEXT(".stl"))); + } + TestTrue(TEXT("model.urdf written"), + FPaths::FileExists(FPaths::Combine(OutDir, TEXT("model.urdf")))); + + return true; +} + +// --------------------------------------------------------------------------- +// 2. Forward-kinematics round-trip: URDF at q=0 reproduces mjModel geom_xpos. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfExportFk, + "URLab.Urdf.ForwardKinematics", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfExportFk::RunTest(const FString& Parameters) +{ + FMjTestSession S; + if (!S.CompileXml(kChainXml)) + { + AddError(S.LastError); + return false; + } + const FString MeshDir = FPaths::Combine(FPaths::ProjectSavedDir(), + TEXT("URLab"), TEXT("Tests"), TEXT("urdf_chain"), TEXT("meshes")); + FUrdfExportConfig Cfg; + const TArray Bodies = FUrdfExporter::BodyIdsForArt(S.m, FString()); + const FUrdfModel Model = FUrdfExporter::Build(S.m, TEXT("chain"), FString(), Bodies, MeshDir, Cfg); + + TestTrue(TEXT("chain FK matches geom_xpos"), + CheckForwardKinematics(*this, Model, S.m, S.d, 1e-6)); + return true; +} + +// --------------------------------------------------------------------------- +// 3. jnt_pos anchor frame-shift + multi-joint dummy chain: FK + dummy link. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfExportAnchorMultiJoint, + "URLab.Urdf.AnchorAndMultiJoint", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfExportAnchorMultiJoint::RunTest(const FString& Parameters) +{ + FMjTestSession S; + if (!S.CompileXml(kAnchorXml)) + { + AddError(S.LastError); + return false; + } + const FString MeshDir = FPaths::Combine(FPaths::ProjectSavedDir(), + TEXT("URLab"), TEXT("Tests"), TEXT("urdf_anchor"), TEXT("meshes")); + FUrdfExportConfig Cfg; + Cfg.bAppendTool0 = false; // single anchor+2joint wrist model, no clear leaf + const TArray Bodies = FUrdfExporter::BodyIdsForArt(S.m, FString()); + const FUrdfModel Model = FUrdfExporter::Build(S.m, TEXT("anchor"), FString(), Bodies, MeshDir, Cfg); + + // The two-joint wrist yields one dummy link and three joints total + // (ja, jw1 on the dummy, jw2 on the real wrist link). + bool bHasDummy = false; + for (const FUrdfLink& L : Model.Links) + if (L.Name == TEXT("wrist__j0")) + bHasDummy = true; + TestTrue(TEXT("dummy link inserted for two-joint body"), bHasDummy); + TestEqual(TEXT("three joints (ja + jw1 + jw2)"), Model.Joints.Num(), 3); + + // The jnt_pos anchor is non-zero, so this exercises the frame-shift path. + const int JaId = mj_name2id(S.m, mjOBJ_JOINT, "ja"); + TestTrue(TEXT("anchor joint has non-zero jnt_pos"), + FMath::Abs(S.m->jnt_pos[3 * JaId + 0]) > 1e-9); + + TestTrue(TEXT("anchor+wrist FK matches geom_xpos"), + CheckForwardKinematics(*this, Model, S.m, S.d, 1e-6)); + return true; +} + +// --------------------------------------------------------------------------- +// 4. Reference match: the real Franka. Gated on the menagerie checkout being +// present so CI without it stays green. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfExportPanda, + "URLab.Urdf.PandaReference", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfExportPanda::RunTest(const FString& Parameters) +{ + const FString PandaXml = TEXT("C:/dev/menagerie/franka_emika_panda/panda.xml"); + if (!FPaths::FileExists(PandaXml)) + { + AddInfo(TEXT("URLab.Urdf.PandaReference: menagerie panda.xml absent; skipping.")); + return true; + } + + char Err[1000] = ""; + mjModel* m = mj_loadXML(TCHAR_TO_UTF8(*PandaXml), nullptr, Err, sizeof(Err)); + if (!m) + { + AddError(FString::Printf(TEXT("mj_loadXML failed: %hs"), Err)); + return false; + } + mjData* d = mj_makeData(m); + + const FString OutDir = FPaths::Combine(FPaths::ProjectSavedDir(), + TEXT("URLab"), TEXT("Tests"), TEXT("urdf_panda")); + FUrdfExportConfig Cfg; + Cfg.bAppendTool0 = false; // tool0 opt-in for the Panda reference test + const FUrdfModel Model = FUrdfExporter::ExportToDir(m, TEXT("panda"), FString(), OutDir, Cfg); + + // The validated reference (docs/plan_ros_urdf_port_spec.md): 11 links, 10 + // joints (7 revolute, 2 prismatic, 1 fixed), 67 meshes. + TestEqual(TEXT("11 links"), Model.Links.Num(), 11); + TestEqual(TEXT("10 joints"), Model.Joints.Num(), 10); + TestEqual(TEXT("7 revolute"), CountJointType(Model, TEXT("revolute")), 7); + TestEqual(TEXT("2 prismatic"), CountJointType(Model, TEXT("prismatic")), 2); + TestEqual(TEXT("1 fixed"), CountJointType(Model, TEXT("fixed")), 1); + TestEqual(TEXT("67 meshes"), Model.MeshIds.Num(), 67); + + // Every 1-DOF joint limit is jnt_range - qpos0, widened by the safety margin. + const double Margin = FUrdfExportConfig().LimitMargin; + bool bLimitsOk = true; + for (const FUrdfJoint& J : Model.Joints) + { + if (!J.bHasLimit) + continue; + const int Jid = J.MjJointId; + const double Q0 = m->qpos0[m->jnt_qposadr[Jid]]; + if (FMath::Abs(J.Lower - (m->jnt_range[2 * Jid + 0] - Q0 - Margin)) > 1e-6 + || FMath::Abs(J.Upper - (m->jnt_range[2 * Jid + 1] - Q0 + Margin)) > 1e-6) + bLimitsOk = false; + } + TestTrue(TEXT("all limits == jnt_range - qpos0 +/- margin"), bLimitsOk); + + // FK reproduces mjModel geom_xpos across the whole Franka. + TestTrue(TEXT("panda FK matches geom_xpos"), + CheckForwardKinematics(*this, Model, m, d, 1e-5)); + + AddInfo(FString::Printf(TEXT("panda URDF written to %s (%d warnings)"), + *FPaths::Combine(OutDir, TEXT("model.urdf")), Model.Warnings.Num())); + + mj_deleteData(d); + mj_deleteModel(m); + return true; +} + +// --------------------------------------------------------------------------- +// 5. Matched qpos0 shift: FillJointState emits qpos - qpos0. Pure IR transform. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfJointStateShift, + "URLab.Urdf.JointStateQpos0Shift", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfJointStateShift::RunTest(const FString& Parameters) +{ + FMjArticulationState Art; + Art.Name = FName(TEXT("art")); + + // Hinge with a non-zero reference: JointState must report qpos - qpos0. + FMjJointState Hinge; + Hinge.Name = FName(TEXT("j1")); + Hinge.Type = EMjJointType::Hinge; + Hinge.QPos = {1.25}; + Hinge.QVel = {0.4}; + Hinge.RefPos = {0.5}; + Art.Joints.Add(Hinge); + + // Slide with no reference recorded: unshifted. + FMjJointState Slide; + Slide.Name = FName(TEXT("j2")); + Slide.Type = EMjJointType::Slide; + Slide.QPos = {0.30}; + Slide.QVel = {0.0}; + Art.Joints.Add(Slide); + + TArray Names; + TArray Positions; + TArray Velocities; + TArray Efforts; + UURLabRosPublishTransport::FillJointState(Art, Names, Positions, Velocities, Efforts); + + TestEqual(TEXT("two positions"), Positions.Num(), 2); + TestEqual(TEXT("hinge shifted by qpos0"), Positions[0], 0.75, 1e-9); + TestEqual(TEXT("slide unshifted (no RefPos)"), Positions[1], 0.30, 1e-9); + TestEqual(TEXT("velocity untouched"), Velocities[0], 0.4, 1e-9); + return true; +} + +// --------------------------------------------------------------------------- +// 6. Auto-export on compile + latched robot_description publish. The pure part +// (export cached) always runs; the rcl publish gates on a live ROS context. +// --------------------------------------------------------------------------- +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUrdfPublishRobotDescription, + "URLab.Urdf.PublishRobotDescription", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::EngineFilter) + +bool FMjUrdfPublishRobotDescription::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(S.LastError); + return false; + } + + // RefreshStateCaches runs the auto-export and builds the collector cache. + S.Manager->RefreshStateCaches(); + TestTrue(TEXT("at least one URDF cached after compile"), + S.Manager->GetRobotDescriptions().Num() >= 1); + + FURLabRosContext::Get().Initialize(); + if (!FURLabRosContext::Get().IsAvailable()) + { + UE_LOG(LogTemp, Display, + TEXT("URLab.Urdf.PublishRobotDescription: no live ROS context; publish leg skipped.")); + S.Cleanup(); + return true; + } + + UURLabRosPublishTransport* Ros = NewObject(S.Manager); + TestTrue(TEXT("transport init"), Ros->TransportInit()); + + mjModel* m = S.Manager->PhysicsEngine->GetModel(); + mjData* d = S.Manager->PhysicsEngine->GetData(); + const FMjStateSnapshot& Snap = S.Manager->GetStateCollector().Collect(m, d, 0); + + // Rebuilds the publisher set (JointState + latched robot_description via rcl) + // and publishes once; must not tear the context down. + Ros->PublishState(Snap); + TestTrue(TEXT("art publisher built"), Ros->GetArtPublisherCountForTest() >= 1); + TestTrue(TEXT("context still available after publish"), + FURLabRosContext::Get().IsAvailable()); + + Ros->TransportShutdown(); + S.Cleanup(); + return true; +} + +#endif // URLAB_WITH_ROS2 diff --git a/Source/URLabEditor/Private/Tests/MjUserChannelTests.cpp b/Source/URLabEditor/Private/Tests/MjUserChannelTests.cpp new file mode 100644 index 00000000..693796fa --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjUserChannelTests.cpp @@ -0,0 +1,335 @@ +// Copyright (c) 2026 Jonathan Embley-Riches. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// --- LEGAL DISCLAIMER --- +// UnrealRoboticsLab is an independent software plugin. It is NOT affiliated with, +// endorsed by, or sponsored by Epic Games, Inc. "Unreal" and "Unreal Engine" are +// trademarks or registered trademarks of Epic Games, Inc. in the US and elsewhere. +// +// This plugin incorporates third-party software: MuJoCo (Apache 2.0), +// CoACD (MIT), and libzmq (MPL 2.0). See ThirdPartyNotices.txt for details. + +#include "CoreMinimal.h" +#include "Misc/AutomationTest.h" +#include "Tests/MjTestHelpers.h" +#include "UserChannels/MjUserChannelComponent.h" +#include "State/MjStateTypes.h" +#include "State/MjStateCollector.h" +#include "State/MjObservationLevel.h" +#include "State/MjMsgpackEncoder.h" +#include "State/MjCanonicalName.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjArticulation.h" +#include "Bridge/RpcDispatcher.h" +#include "Dom/JsonObject.h" +#include "Dom/JsonValue.h" +#include "GameFramework/Actor.h" + +namespace +{ +const FMjUserChannel* FindChannel(const TArray& Channels, const TCHAR* Name) +{ + const FName Target(Name); + for (const FMjUserChannel& C : Channels) + { + if (C.Name == Target) + return &C; + } + return nullptr; +} +} // namespace + +// ============================================================================ +// URLab.UserChannels.ArtScope_CollectorAndEncoder +// A component on an articulation actor publishes a bool + a transform; the +// collector lands them in that art's IR block with the right kinds and raw +// MuJoCo values, and the msgpack encoder emits them under "user". +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUserChannelArtScope, + "URLab.UserChannels.ArtScope_CollectorAndEncoder", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjUserChannelArtScope::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + UMjUserChannelComponent* Comp = NewObject(S.Robot, TEXT("UserChannels")); + Comp->RegisterComponent(); + S.Manager->RegisterStateProducer(Comp); + + Comp->PublishBool(TEXT("task_done"), true); + // (100, 200, 300) cm, identity rotation -> MuJoCo (1, -2, 3) m, wxyz (1,0,0,0). + Comp->PublishTransform(TEXT("target"), + FTransform(FQuat::Identity, FVector(100.0, 200.0, 300.0)), /*bConvertFromUESpace=*/true); + + // Drive the game-thread cache rebuild synchronously (BeginPlay is bypassed in + // test worlds, so scope resolution has to be triggered explicitly). + FMjStateCollector& Collector = S.Manager->GetStateCollector(); + Collector.Init(S.Manager); + Collector.RebuildProducerCacheGameThread(); + + mjModel* M = S.Manager->PhysicsEngine->m_model; + mjData* D = S.Manager->PhysicsEngine->m_data; + const FMjStateSnapshot& Snap = Collector.Collect(M, D, 0); + + if (!TestEqual(TEXT("one articulation"), Snap.Articulations.Num(), 1)) + { + S.Cleanup(); + return false; + } + const FMjArticulationState& Art = Snap.Articulations[0]; + + // --- Bool channel --- + const FMjUserChannel* Done = FindChannel(Art.UserChannels, TEXT("task_done")); + if (TestNotNull(TEXT("task_done present"), Done)) + { + TestEqual(TEXT("task_done kind"), (int32)Done->Kind, (int32)EMjUserChannelKind::Bool); + TestTrue(TEXT("task_done value"), Done->Values.Num() == 1 && Done->Values[0] != 0.0); + } + + // --- Transform channel (raw MuJoCo SI) --- + const FMjUserChannel* Target = FindChannel(Art.UserChannels, TEXT("target")); + if (TestNotNull(TEXT("target present"), Target)) + { + TestEqual(TEXT("target kind"), (int32)Target->Kind, (int32)EMjUserChannelKind::Transform); + if (TestEqual(TEXT("target width"), Target->Values.Num(), 7)) + { + TestTrue(TEXT("target pos x"), MjTestMath::NearlyEqual(Target->Values[0], 1.0)); + TestTrue(TEXT("target pos y"), MjTestMath::NearlyEqual(Target->Values[1], -2.0)); + TestTrue(TEXT("target pos z"), MjTestMath::NearlyEqual(Target->Values[2], 3.0)); + TestTrue(TEXT("target quat w"), MjTestMath::NearlyEqual(Target->Values[3], 1.0)); + TestTrue(TEXT("target quat x"), MjTestMath::NearlyEqual(Target->Values[4], 0.0)); + TestTrue(TEXT("target quat y"), MjTestMath::NearlyEqual(Target->Values[5], 0.0)); + TestTrue(TEXT("target quat z"), MjTestMath::NearlyEqual(Target->Values[6], 0.0)); + } + } + + // --- Encoder: the channels appear under arts//user --- + TSharedPtr Encoded = FMjMsgpackEncoder::EncodeSnapshot(Snap, EObservationLevel::Full); + const TSharedPtr* ArtsObj = nullptr; + if (TestTrue(TEXT("arts block"), Encoded->TryGetObjectField(TEXT("arts"), ArtsObj))) + { + const TSharedPtr* ArtObj = nullptr; + if (TestTrue(TEXT("art entry"), (*ArtsObj)->TryGetObjectField(Art.Name.ToString(), ArtObj))) + { + const TSharedPtr* UserObj = nullptr; + if (TestTrue(TEXT("user block"), (*ArtObj)->TryGetObjectField(TEXT("user"), UserObj))) + { + bool bDone = false; + TestTrue(TEXT("encoded task_done readable"), (*UserObj)->TryGetBoolField(TEXT("task_done"), bDone)); + TestTrue(TEXT("encoded task_done true"), bDone); + + const TSharedPtr* TargetObj = nullptr; + if (TestTrue(TEXT("encoded target object"), (*UserObj)->TryGetObjectField(TEXT("target"), TargetObj))) + { + const TArray>* Pos = nullptr; + if (TestTrue(TEXT("encoded target pos"), (*TargetObj)->TryGetArrayField(TEXT("pos"), Pos)) + && TestEqual(TEXT("encoded pos width"), Pos->Num(), 3)) + { + TestTrue(TEXT("encoded pos x"), MjTestMath::NearlyEqual((*Pos)[0]->AsNumber(), 1.0)); + TestTrue(TEXT("encoded pos y"), MjTestMath::NearlyEqual((*Pos)[1]->AsNumber(), -2.0)); + TestTrue(TEXT("encoded pos z"), MjTestMath::NearlyEqual((*Pos)[2]->AsNumber(), 3.0)); + } + const TArray>* Quat = nullptr; + TestTrue(TEXT("encoded target quat"), (*TargetObj)->TryGetArrayField(TEXT("quat"), Quat)); + } + } + } + } + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.UserChannels.SceneScope_CollectorAndEncoder +// A component on a non-articulation actor publishes into the scene scope; the +// channel lands on the snapshot itself and encodes as a top-level "user" block. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUserChannelSceneScope, + "URLab.UserChannels.SceneScope_CollectorAndEncoder", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjUserChannelSceneScope::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + // A plain actor (not an AMjArticulation) => scene scope. + AActor* SceneActor = S.World->SpawnActor(); + if (!TestNotNull(TEXT("scene actor"), SceneActor)) + { + S.Cleanup(); + return false; + } + + UMjUserChannelComponent* Comp = NewObject(SceneActor, TEXT("SceneChannels")); + Comp->RegisterComponent(); + S.Manager->RegisterStateProducer(Comp); + + Comp->PublishInt(TEXT("episode_phase"), 2); + + FMjStateCollector& Collector = S.Manager->GetStateCollector(); + Collector.Init(S.Manager); + Collector.RebuildProducerCacheGameThread(); + + mjModel* M = S.Manager->PhysicsEngine->m_model; + mjData* D = S.Manager->PhysicsEngine->m_data; + const FMjStateSnapshot& Snap = Collector.Collect(M, D, 0); + + const FMjUserChannel* Phase = FindChannel(Snap.UserChannels, TEXT("episode_phase")); + if (TestNotNull(TEXT("episode_phase present"), Phase)) + { + TestEqual(TEXT("episode_phase kind"), (int32)Phase->Kind, (int32)EMjUserChannelKind::Int); + TestTrue(TEXT("episode_phase value"), Phase->Values.Num() == 1 && FMath::IsNearlyEqual(Phase->Values[0], 2.0)); + } + + // No art channel leakage: the art block must not carry the scene channel. + if (Snap.Articulations.Num() == 1) + TestNull(TEXT("not on art"), FindChannel(Snap.Articulations[0].UserChannels, TEXT("episode_phase"))); + + TSharedPtr Encoded = FMjMsgpackEncoder::EncodeSnapshot(Snap, EObservationLevel::Full); + const TSharedPtr* UserObj = nullptr; + if (TestTrue(TEXT("top-level user block"), Encoded->TryGetObjectField(TEXT("user"), UserObj))) + { + double PhaseVal = 0.0; + TestTrue(TEXT("encoded episode_phase readable"), (*UserObj)->TryGetNumberField(TEXT("episode_phase"), PhaseVal)); + TestTrue(TEXT("encoded episode_phase value"), FMath::IsNearlyEqual(PhaseVal, 2.0)); + } + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.UserChannels.InputRoundTrip +// A component on an art declares a Bool + a Transform input channel; the +// set_user_channels RPC op routes values to them through ApplyUserChannelInput, +// the GetInput* nodes read them back, undeclared names are rejected, and a +// kind-family mismatch is rejected. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjUserChannelInputRoundTrip, + "URLab.UserChannels.InputRoundTrip", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjUserChannelInputRoundTrip::RunTest(const FString& Parameters) +{ + FMjUESession S; + if (!S.Init()) + { + AddError(FString::Printf(TEXT("FMjUESession::Init failed: %s"), *S.LastError)); + return false; + } + + UMjUserChannelComponent* Comp = NewObject(S.Robot, TEXT("UserInput")); + Comp->RegisterComponent(); + S.Manager->RegisterStateProducer(Comp); + Comp->DeclareInputChannel(TEXT("go"), EMjUserInputKind::Bool); + Comp->DeclareInputChannel(TEXT("reset_goal"), EMjUserInputKind::Transform); + + const AMjArticulation* Art = Cast(S.Robot); + const FName Segment = FMjCanonicalName::ArtSegment(Art); + + // Direct manager routing: a Bool value lands and reads back true. + { + FMjUserChannel V; + V.Name = FName(TEXT("go")); + V.Kind = EMjUserChannelKind::Bool; + V.Values = {1.0}; + TestTrue(TEXT("declared bool input applied"), + S.Manager->ApplyUserChannelInput(Segment, FName(TEXT("go")), V)); + TestTrue(TEXT("GetInputBool reads the applied value"), + Comp->GetInputBool(TEXT("go"), false)); + } + + // Undeclared channel is rejected. + { + FMjUserChannel V; + V.Kind = EMjUserChannelKind::Bool; + V.Values = {1.0}; + TestFalse(TEXT("undeclared input rejected"), + S.Manager->ApplyUserChannelInput(Segment, FName(TEXT("bogus")), V)); + } + + // Kind-family mismatch (a string into a numeric channel) is rejected. + { + FMjUserChannel V; + V.Kind = EMjUserChannelKind::String; + V.Text = TEXT("nope"); + TestFalse(TEXT("kind-family mismatch rejected"), + S.Manager->ApplyUserChannelInput(Segment, FName(TEXT("go")), V)); + } + + // The set_user_channels RPC op routes a transform value end-to-end. + FURLabRpcDispatcher* Disp = S.Manager->GetStepDispatcher(); + if (!Disp) + { + AddError(TEXT("Manager has no StepDispatcher")); + S.Cleanup(); + return false; + } + Disp->SetActiveSessionIdForTest(TEXT("test-session")); + + { + TSharedPtr Req = MakeShared(); + Req->SetStringField(TEXT("op"), TEXT("set_user_channels")); + Req->SetStringField(TEXT("session_id"), TEXT("test-session")); + + TSharedPtr Xform = MakeShared(); + TArray> Pos; + Pos.Add(MakeShared(1.0)); + Pos.Add(MakeShared(2.0)); + Pos.Add(MakeShared(3.0)); + Xform->SetArrayField(TEXT("pos"), Pos); + TArray> Quat; + Quat.Add(MakeShared(1.0)); + Quat.Add(MakeShared(0.0)); + Quat.Add(MakeShared(0.0)); + Quat.Add(MakeShared(0.0)); + Xform->SetArrayField(TEXT("quat"), Quat); + + TSharedPtr Channels = MakeShared(); + Channels->SetObjectField(TEXT("reset_goal"), Xform); + TSharedPtr Arts = MakeShared(); + Arts->SetObjectField(Segment.ToString(), Channels); + Req->SetObjectField(TEXT("arts"), Arts); + + TSharedPtr Reply = Disp->Dispatch(Req); + FString ReplyOp; + if (Reply.IsValid()) + Reply->TryGetStringField(TEXT("op"), ReplyOp); + TestEqual(TEXT("set_user_channels_ok"), ReplyOp, FString(TEXT("set_user_channels_ok"))); + double Applied = 0.0; + if (Reply.IsValid()) + Reply->TryGetNumberField(TEXT("applied"), Applied); + TestEqual(TEXT("one channel applied"), (int32)Applied, 1); + + // Read the transform back in raw MuJoCo space (no UE conversion). + const FTransform T = Comp->GetInputTransform(TEXT("reset_goal"), /*bConvertToUESpace=*/false); + TestTrue(TEXT("reset_goal pos x"), FMath::IsNearlyEqual(T.GetLocation().X, 1.0)); + TestTrue(TEXT("reset_goal pos z"), FMath::IsNearlyEqual(T.GetLocation().Z, 3.0)); + } + + S.Cleanup(); + return true; +} diff --git a/Source/URLabEditor/Public/MjBridgeServerSubsystem.h b/Source/URLabEditor/Public/MjBridgeServerSubsystem.h index 4b1ab0b0..78f1fa79 100644 --- a/Source/URLabEditor/Public/MjBridgeServerSubsystem.h +++ b/Source/URLabEditor/Public/MjBridgeServerSubsystem.h @@ -7,6 +7,7 @@ #include "CoreMinimal.h" #include "EditorSubsystem.h" +#include "Containers/Ticker.h" #include "Bridge/BridgeServer.h" #include "Bridge/BridgeServerConfig.h" #include "MjBridgeServerSubsystem.generated.h" @@ -58,4 +59,13 @@ class URLABEDITOR_API UURLabBridgeServerSubsystem : public UEditorSubsystem TObjectPtr Server; FURLabBridgeServerConfig Config; + + /** Version string echoed into the registry heartbeat, cached at start. */ + FString CachedUrlabVersion; + + /** Periodic registry refresh: keeps the entry's mtime fresh (so discovery + * doesn't treat a live instance as stale) and republishes the live lease + * `busy` state. */ + FTSTicker::FDelegateHandle HeartbeatHandle; + bool RefreshRegistryHeartbeat(float DeltaTime); }; diff --git a/Source/URLabEditor/URLabEditor.Build.cs b/Source/URLabEditor/URLabEditor.Build.cs index f45b5b6d..924a9683 100644 --- a/Source/URLabEditor/URLabEditor.Build.cs +++ b/Source/URLabEditor/URLabEditor.Build.cs @@ -35,6 +35,7 @@ public URLabEditor(ReadOnlyTargetRules Target) : base(Target) "CoreUObject", "Engine", "URLab", + "URLabRos", "UnrealEd", "EditorSubsystem", "AssetTools", From 3dbd438832a2edb122976cb3bcd1580980be6989 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 10:03:39 +0100 Subject: [PATCH 10/32] Document the camera, streaming and ROS protocols --- Content/TestData/ros_maps_test.xml | 26 +++ docs/guides/ros.md | 281 ++++++++++++++++++++++++ docs/python/api.md | 38 +++- docs/python/quickstart.md | 17 +- docs/reference/protocol.md | 247 +++++++++++++++++++-- docs/ros2_link_facts.md | 177 +++++++++++++++ docs/ros_workspace_setup.md | 336 +++++++++++++++++++++++++++++ mkdocs.yml | 1 + 8 files changed, 1095 insertions(+), 28 deletions(-) create mode 100644 Content/TestData/ros_maps_test.xml create mode 100644 docs/guides/ros.md create mode 100644 docs/ros2_link_facts.md create mode 100644 docs/ros_workspace_setup.md diff --git a/Content/TestData/ros_maps_test.xml b/Content/TestData/ros_maps_test.xml new file mode 100644 index 00000000..9d81ea2f --- /dev/null +++ b/Content/TestData/ros_maps_test.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/docs/guides/ros.md b/docs/guides/ros.md new file mode 100644 index 00000000..bde7c8b6 --- /dev/null +++ b/docs/guides/ros.md @@ -0,0 +1,281 @@ +# ROS 2 Integration + +URLab publishes robot state and scene geometry as standard ROS 2 messages so +downstream tools — rviz, MoveIt, nav2, your own nodes — consume them without any +shim. It also accepts `cmd_vel` (`geometry_msgs/Twist`) and can route external ROS +control through MuJoCo actuators. + +ROS 2 is **optional and modular**. The core plugin has no ROS dependency; nothing +breaks when ROS is absent. The ROS pieces live in a separate `URLabRos` module that +compiles to a no-op when ROS 2 is not installed. + +The integration works on **Ubuntu 22.04 / 24.04** (apt or Pixi/Conda) and +**Windows 11** (Pixi/Conda via RoboStack). This guide covers Ubuntu; for Windows +setup see [ROS Workspace Setup](../ros_workspace_setup.md). + +--- + +## 1. Install ROS 2 + +**Pinned distribution: ROS 2 Lyrical Luth** (LTS, supported to 2031). + +=== "Ubuntu 24.04 (apt)" + + ```bash + sudo apt update + sudo apt install -y ros-lyrical-ros-base ros-dev-tools + ``` + + Installs into `/opt/ros/lyrical`. `ros-lyrical-ros-base` is enough; the + `ros-dev-tools` package gives you `ros2` CLI for verification. + +=== "Ubuntu 22.04 / 24.04 (Pixi)" + + Pixi is useful when you want an isolated ROS install that doesn't touch the + system, or when you're on 22.04 (apt doesn't ship Lyrical for 22.04). + + ```bash + curl -fsSL https://pixi.sh/install.sh | sh + # restart your shell, then: + pixi init urlab_ros2_env -c https://prefix.dev/robostack-lyrical -c conda-forge + cd urlab_ros2_env + pixi add ros-lyrical-desktop + ``` + + Activate with `pixi shell` from the `urlab_ros2_env` directory whenever you + build or launch the editor. + +--- + +## 2. Build URLab with ROS enabled + +### 2.1 Prerequisites + +Build the plugin's native dependencies first (MuJoCo, CoACD, libzmq). See +[Installation](../installation.md) for the full flow. From the plugin root: + +```bash +cd third_party +./build_all.sh --engine "$UE_ROOT" +``` + +### 2.2 Set the ROS root + +The `URLabRos` module's build logic probes the `URLAB_ROS2_ROOT` environment +variable. Point it at your ROS install prefix (the directory that holds `include/`, +`lib/`, and `bin/` subdirectories): + +=== "apt" + + ```bash + export URLAB_ROS2_ROOT=/opt/ros/lyrical + ``` + +=== "Pixi" + + ```bash + export URLAB_ROS2_ROOT=$CONDA_PREFIX + ``` + +If `URLAB_ROS2_ROOT` is unset or points at a missing directory, the module builds +without ROS (`URLAB_WITH_ROS2=0`) and prints a notice: + +``` +URLabRos: ROS 2 not found (set URLAB_ROS2_ROOT to enable) - building without ROS. +``` + +### 2.3 Build + +```bash +./Scripts/build_and_test_linux.sh \ + --engine "$UE_ROOT" \ + --project /path/to/YourProject.uproject +``` + +UBT links the minimal C-ABI set (`librcl.so`, `librosidl_runtime_c.so`, the +message-package `.so` files) and stages the transitive DDS cluster next to the +plugin binary. + +### 2.4 Runtime library path + +UE does not auto-stage `RuntimeDependencies` for editor builds on Linux, so the +ROS `.so` files must be reachable by the dynamic linker. Two options: + +**Option A — `LD_LIBRARY_PATH` (quickest for development):** + +```bash +export LD_LIBRARY_PATH="$URLAB_ROS2_ROOT/lib:$LD_LIBRARY_PATH" +./UnrealEditor YourProject.uproject +``` + +**Option B — symlink into the plugin's `Binaries/Linux/`:** + +```bash +./Scripts/setup_runtime_linux.sh +``` + +This symlinks the ROS `.so` cluster under `Binaries/Linux/` so UBT's `$ORIGIN` +RPATH resolves them without any env var. Idempotent; re-run after a build. + +--- + +## 3. Verify it works + +### 3.1 Check module load + +Launch the editor and look for the ROS context log line in the output: + +``` +LogURLabRos: ROS 2 context up (distro lyrical, node 'urlab'). +``` + +If you see this, the module loaded and connected to DDS. A warning instead means +the DLLs were found but `rcl_init` failed (usually a DDS configuration issue). + +If you see no `LogURLabRos` lines at all, the module didn't load — check that the +ROS `.so` files are on `LD_LIBRARY_PATH` and that `URLAB_ROS2_ROOT` was set during +the build (re-run `Build.sh` after setting it). + +### 3.2 Start PIE and check topics + +With the editor open, enter Play-In-Editor (PIE) with a level that contains an +`MjManager` and at least one robot articulation. In another terminal (with ROS 2 +sourced), run: + +```bash +ros2 topic list +``` + +You should see the standard ROS topics: + +| Topic | Message type | Notes | +|---|---|---| +| `/clock` | `rosgraph_msgs/Clock` | Sim time, published at sim rate | +| `/tf` | `tf2_msgs/TFMessage` | Per-body transforms, 50 Hz | +| `/tf_static` | `tf2_msgs/TFMessage` | Static transforms, latched | +| `//joint_states` | `sensor_msgs/JointState` | Per-articulation, 50 Hz | +| `//pose` | `geometry_msgs/PoseStamped` | Articulation root pose | +| `//odometry` | `nav_msgs/Odometry` | Root odometry (if free joint), 50 Hz | +| `//imu` | `sensor_msgs/Imu` | Per-IMU-sensor, 100 Hz | +| `//sensors` | `sensor_msgs/JointState` | Sensor readouts | +| `//robot_description` | `std_msgs/String` | URDF, latched | +| `//planning_scene` | `moveit_msgs/PlanningScene` | Includes world geometry, 10 Hz | +| `//image` | `sensor_msgs/Image` | One per streaming camera | +| `//camera_info` | `sensor_msgs/CameraInfo` | One per streaming camera | +| `/urlab/obstacle_cloud` | `sensor_msgs/PointCloud2` | Sampled world geometry, 10 Hz | +| `/map` | `nav_msgs/OccupancyGrid` | 2D occupancy raster, latched | +| `/octomap_binary` | `octomap_msgs/Octomap` | Volumetric occupancy, 5 Hz | +| `/urlab/cmd_vel` | `geometry_msgs/Twist` | Twist control input | + +### 3.3 Visualise in rviz + +```bash +ros2 run rviz2 rviz2 +``` + +Add a `RobotModel` display, set the description topic to +`//robot_description`, and add a `TF` display. The robot appears in rviz +with live joint state, driven by MuJoCo physics running inside Unreal. + +--- + +## 4. MoveIt planning + +To use URLab with MoveIt: + +1. Launch `urlab_moveit` from the `ros/urlab_moveit/` package: + ```bash + ros2 launch urlab_moveit franka.launch.py + ``` + This brings up `move_group` with the URDF from `/robot_description`, the SRDF + auto-generated by `generate_srdf.py`, and the planning scene fed from + `/planning_scene`. + +2. Use the MoveIt RViz plugin or the Python MoveIt API (`moveit_commander`) to + plan and execute trajectories. The bridge publishes joint trajectories to + URLab's control path. + +--- + +## 5. How it fits together + +``` +┌─────────────────────────────────────────────────────┐ +│ Unreal Editor (PIE) │ +│ ┌──────────┐ ┌──────────────┐ ┌───────────────┐ │ +│ │ MuJoCo │ │ FMjState- │ │ IMjRosOutput- │ │ +│ │ thread ├─►│ Collector ├─►│ Provider │ │ +│ │ (physics) │ │ (typed IR) │ │ (per topic) │ │ +│ └──────────┘ └──────────────┘ └───────┬───────┘ │ +│ │ rcl C ABI │ +│ ┌───────────────────────────────────────┴──────────┐│ +│ │ UrlabRclCore (extern "C", no rclcpp) ││ +│ │ Fills rosidl C structs → rcl_publish ││ +│ └──────────────────────────────────────────────────┘│ +└─────────────────────────────────────────────────────┘ + │ DDS (FastDDS) + ▼ + ┌──────────────────────────────────────────┐ + │ ROS 2 ecosystem (rviz, MoveIt, nav2, …) │ + └──────────────────────────────────────────┘ +``` + +State flows from MuJoCo through a transport-neutral typed IR +(`FMjStateCollector`). Each ROS provider reads the IR and publishes one topic. The +whole ROS side lives in `Source/URLabRos/` and links only the rcl C API — no +`rclcpp`, no `ament`, no colcon. + +The bridge (ZMQ + shared memory) is always available; ROS is an additional, +independent transport that publishes the same state in ROS-native formats. + +--- + +## 6. Architecture notes + +**No rclcpp.** The plugin links the rcl C API directly through a thin +`extern "C"` seam (`Source/URLabRos/Private/Ros/UrlabRclCore.h`). This avoids +the rclcpp dependency (and its `libstdc++` ABI mismatch with UE's bundled +`libc++`) and keeps link times small. + +**Self-registering providers.** Every ROS topic is published by a class +that implements `IMjRosOutputProvider` and registers itself with the +`REGISTER_MJ_ROS_OUTPUT_PROVIDER` macro. The `RosPublishTransport` discovers them +at module load and drives their `Build`/`Publish` lifecycle each step. To add a +new topic, write a provider and register it — no plumbing changes needed. + +**Graceful degradation.** When `URLAB_WITH_ROS2=0` (no ROS at build time) or when +`FURLabRosContext::Initialize()` fails at runtime (no DDS, bad config), every ROS +code path is fenced and degrades to a no-op. The simulation, bridge, and dashboard +continue normally. + +--- + +## 7. Troubleshooting + +**"ROS 2 not found" during build.** +`URLAB_ROS2_ROOT` is unset or points at a missing directory. Export it and +re-build. If the ROS install uses a different layout (e.g. `include/` is nested +under a `ros2/` prefix), point `URLAB_ROS2_ROOT` at the directory that directly +contains `include/`, `lib/`, and `bin/`. + +**Editor fails to load with "lib.so: cannot open shared object file".** +The ROS `.so` cluster is not on the linker's search path. Use +`LD_LIBRARY_PATH` or run `Scripts/setup_runtime_linux.sh` to symlink them. + +**Module loads but no topics appear.** +The ROS context failed to initialise. Look for this warning in the editor log: +``` +LogURLabRos: Warning: ROS 2 unavailable: rcl context init failed (…) +``` +This usually means DDS discovery cannot start — check that no firewall is blocking +UDP multicast and that `$ROS_DOMAIN_ID` is consistent across terminals. + +**`ros2 topic list` shows topics but rviz sees no TF.** +Wait a few seconds after PIE starts. The TF provider throttles to 50 Hz and needs +at least one sim step before the first message goes out. Also check that +`/tf_static` appears — some tools need it before they render anything. + +**Wrong message types or missing fields.** +URLab is pinned to ROS 2 Lyrical. If you sourced a different distro (Humble, +Jazzy, Kilted), message definitions may differ. Run `ros2 topic info ` to +check the type. diff --git a/docs/python/api.md b/docs/python/api.md index 1630252d..4c9c3e6e 100644 --- a/docs/python/api.md +++ b/docs/python/api.md @@ -180,7 +180,7 @@ client.scene.save_level() -> None client.scene.import_xml(xml_path, *, force_reimport=False) -> URLabBlueprint client.scene.spawn_actor(blueprint, actor_id, *, location=..., rotation_quat=..., rotation_euler=..., scale=...) -> URLabSpawnHandle client.scene.spawn_light(kind="directional", actor_id="", *, location=..., rotation_euler=..., intensity=5000.0, color=...) -> URLabLightHandle -client.scene.destroy_actor(target, *, by_name=False) -> None +client.scene.remove_actor(target, *, by_name=False) -> None client.scene.set_actor_transform(target, *, by_name=False, location=None, rotation_quat=None, rotation_euler=None) -> None client.scene.destroy_asset(asset_path) -> bool client.scene.current_level() -> str @@ -235,7 +235,9 @@ client.runtime.set_mocap_pose(body, *, pos=None, quat=None) -> MocapPose client.runtime.read_mocap_pose(body) -> MocapPose client.runtime.get_contacts(*, body1=None, body2=None, geom1=None, geom2=None, max_contacts=64) -> ContactsResult client.runtime.list_keyframes() -> List[KeyframeInfo] -client.runtime.set_sim_options(**opts) -> SimOptions # timestep, gravity, ... +client.runtime.set_sim_options(**opts) -> SimOptions # timestep, gravity, num_worker_threads, ... +client.runtime.set_camera_streaming(cameras) -> dict # {canon: True|False|{"zmq":b,"shm":b}} +client.runtime.set_camera_delay(cameras) -> dict # {canon: secs | {delay_s, jitter_s, clock, seed, ...}} client.runtime.set_mode(mode) -> StepMode client.runtime.forward() -> dict # mj_forward, no integration ``` @@ -581,13 +583,35 @@ cam.fovy # vertical FOV in degrees cam.dtype # uint8 (real / segm) or float32 (depth) cam.latest_frame # np.ndarray | None cam.sim_time # float | None +cam.frame_id # int | None — post-step state id this frame shows +cam.capture_unix_time # float | None — Unix secs the frame was captured (v2 header) ``` -Frames populate on demand via `client.step(include_cameras=...)`, or -continuously from per-camera SUB threads in `live` mode. -`include_cameras` accepts `True` / `False`, or a mapping such as -`{"head": "sync"}` (block for a fresh frame) or `{"head": "latest"}` -(ship the cached frame). +Camera capture is decoupled from stepping; `include_cameras` retrieval is +non-blocking (reads a per-camera history ring). `include_cameras` accepts +`True` / `False`, or a mapping whose per-camera value is `"latest"`, an int +`frame_id`, or `{"frame_id": N}`. For the post-step image, read `frame_id` +off the step reply and request it (pipeline one step to keep full rate): + +```python +r = client.step() # r["frame_id"] +r2 = client.step(include_cameras={"wrist": r["frame_id"]}) +frame = robot.cameras["wrist"].latest_frame +``` + +Dedicated ZMQ/SHM pub streams must be enabled per camera (UE's +`bEnableAllCameras` defaults off): +`client.runtime.set_camera_streaming({"wrist": {"zmq": True}})`. In `live` +mode the client enables + subscribes to them automatically. + +To emulate real-sensor latency, configure a server-side delay: +`client.runtime.set_camera_delay({"wrist": 0.05})` (or an object with +`delay_s` / `jitter_s` / `clock` / `seed` / `on_state_change` / `max_fps`). +The delay is applied natively in UE, so every consumer sees the +already-delayed stream and a delayed camera never blocks the step on the +current render. Each streamed frame carries its own `frame_id` / +`sim_time` / capture timestamp in the stream header (`cam.frame_id`, +`cam.sim_time`, `cam.capture_unix_time`). | Mode | Shape | Channels | |---|---|---| diff --git a/docs/python/quickstart.md b/docs/python/quickstart.md index c4816fba..84e15622 100644 --- a/docs/python/quickstart.md +++ b/docs/python/quickstart.md @@ -234,8 +234,21 @@ client.step(n_steps=1, include_cameras=True) frame = robot.cameras["wrist"].latest_frame # (H, W, 4) RGBA uint8 ``` -In `live` mode, per-camera SUB threads keep `latest_frame` fresh in the -background. See [API Reference](api.md#cameras) for the per-mode shapes. +Capture is decoupled from stepping and retrieval is non-blocking. For the +image of a specific step's state, request its `frame_id` (pipeline one step +to keep full rate): + +```python +r = client.step() +r2 = client.step(include_cameras={"wrist": r["frame_id"]}) +frame = robot.cameras["wrist"].latest_frame +``` + +For continuous ZMQ/SHM streams (enabled automatically in `live` mode, or +explicitly via `client.runtime.set_camera_streaming({"wrist": {"zmq": True}})` +since `bEnableAllCameras` now defaults off), per-camera SUB threads keep +`latest_frame` fresh in the background. See +[API Reference](api.md#cameras) for the per-mode shapes. ## Record and replay diff --git a/docs/reference/protocol.md b/docs/reference/protocol.md index 8814d6c5..4001e1ea 100644 --- a/docs/reference/protocol.md +++ b/docs/reference/protocol.md @@ -62,7 +62,7 @@ sequenceDiagram end loop policy C->>S: step { n_steps, observations, per_articulation } - S-->>C: step_ok { time, step, sim_time, wall_time, per_articulation, ... } + S-->>C: step_ok { time, step, sim_time, wall_time, arts, scene, ... } end ``` @@ -96,6 +96,12 @@ Reply (`hello_ok`), notable fields: "mujoco_version_int": 3xy, "manager_present": true, "shm_session_dir": "C:/.../Saved/URLabShm/", + "shm_rpc": { + "session": "live", + "req_path": "C:/.../live/req.shm", "rep_path": "C:/.../live/rep.shm", + "req_event": "Local\\URLab_live_req_ready", "rep_event": "Local\\URLab_live_rep_ready", + "req_stride": 1048576, "rep_stride": 16777216, "n_buffers": 2, "header_size": 64 + }, "mjb": "", "articulations": [ ... ], "entities": { ... }, @@ -114,6 +120,14 @@ Reply (`hello_ok`), notable fields: `mjb_base64` / `mjb_size` for JSON clients). Load it via a temp-file round-trip into `mujoco.MjModel`. - `shm_session_dir` is the absolute SHM region directory. +- `shm_rpc` is the explicit SHM RPC contract — use it verbatim instead of + assuming `session="live"` or re-deriving names. It carries the `req`/`rep` + file paths, the Windows wake-up event names, the per-direction slot + `stride`, the buffer count, and the 64-byte header size. The bridge should + **poll the rep `sequence`** (header: `buffer_stride`@8, `n_buffers`@12, + `sequence` u64@16, `latest_idx` u32@24; slots start at `header_size`) as a + fallback — the `Local\` event doesn't cross logon-session/elevation, so an + event-only wait can stall out the recv timeout. ### Articulations block @@ -139,12 +153,12 @@ model. "joints": {}, "sensors": {}, "bodies": {} }, "camera_topics": { - "head_rgbd": { + "g1/head_rgbd": { "mode": "depth", "resolution": [848, 480], "fovy": 58.0, "zmq_endpoint": "tcp://127.0.0.1:5558", - "zmq_topic": "g1/camera/head_rgbd" + "zmq_topic": "g1/head_rgbd" } } } @@ -282,9 +296,10 @@ request uses the same `per_articulation` shape as direct. "op": "step_ok", "time": 0.042, "step": 21, + "frame_id": 21, "sim_time": { "sec": 0, "nsec": 42000000 }, "wall_time": { "sec": 1714125000, "nsec": 123000000 }, - "per_articulation": { + "arts": { "g1": { "qpos": ["..."], "qvel": ["..."], "ctrl": ["..."], "act": ["..."], "sensors": { "imu_gyro": ["..."] }, @@ -292,13 +307,20 @@ request uses the same `per_articulation` shape as direct. "actions": 0 } }, - "entities": { + "scene": { "pallet": { "qpos": ["..."], "qvel": ["..."], "xpos": ["..."], "xquat": ["..."] }, "terrain_a": { "xpos": ["..."], "xquat": ["..."] } } } ``` +The observation blocks are named on the output side by what they contain: +`arts` is the per-articulation state (keyed by articulation name), `scene` +is the non-articulation dynamic bodies. The `step` *request* still carries +control input under `per_articulation` — input and output are separate +concerns. The streamed `state/full` snapshot uses the identical `arts` / +`scene` shape (plus `op: "state_full"`). + - `time` is `mjData->time`. `sim_time` / `wall_time` are ROS-Time-aligned `{sec, nsec}` blocks: `sim_time` mirrors `time` at nanosecond precision, `wall_time` is the publish moment in unix @@ -307,28 +329,121 @@ request uses the same `per_articulation` shape as direct. `UMjTwistController`: `twist.linear` carries `(vx, vy, 0)`, `twist.angular` carries `(0, 0, yaw_rate)`; `actions` is a discrete bitfield. +- `sensors` values are raw MuJoCo SI in the MuJoCo frame (`mjData->sensordata` + verbatim), consistent with `qpos` / `qvel` / body poses. No UE unit or + handedness transform is applied on the wire. - The fields present per articulation follow the observation level (see [Observation levels](#observation-levels)). +- `frame_id` is the post-step render-snapshot id (monotonic, bumped once + per step). It is the key for camera association: the frame that shows + *this* step's state is tagged with this `frame_id`. Pass it back as a + camera `frame_id` request (below) to fetch the matching image. ### Camera observations -When `include_cameras` is non-false, the reply gains a `cameras` block: +Camera capture is **decoupled from stepping** (like MuJoCo `simulate`): UE +captures asynchronously into a per-camera history ring and the step never +blocks on rendering. `include_cameras` retrieval is **non-blocking** — it +reads from the ring; a frame that isn't ready yet is simply omitted and the +client retries (or fetches by `frame_id` on a later step). + +When `include_cameras` resolves a frame, the reply gains a `cameras` block: ```json "cameras": { - "head_rgbd": { "width": 848, "height": 480, "dtype": "float32", "data": "" }, - "wrist_rgb": { "width": 640, "height": 480, "dtype": "bgra8", "data": "" } + "head_rgbd": { "width": 848, "height": 480, "frame_id": 21, "sim_time": 0.042, "dtype": "float32", "data": "" }, + "base_wrist_rgb": { "width": 640, "height": 480, "frame_id": 21, "sim_time": 0.042, "dtype": "bgra8", "data": "" } } ``` -`dtype` is `bgra8` (4 bytes/pixel) for Real / Semantic / Instance and -`float32` for Depth. The bridge swaps Real to RGBA on receive and keeps -seg modes BGRA so consumers can map color to class id. - -`include_cameras` accepts `true` (every camera, latest cached frame), -`false`, or a per-camera object like `{"head_rgbd": "sync"}`. `"sync"` -blocks the step until UE captures a fresh frame; `"latest"` returns the -cached one. +Each camera carries the `frame_id` / `sim_time` of the state it shows, so a +client can confirm it got the post-step frame. `dtype` is `bgra8` (4 +bytes/pixel) for Real / Semantic / Instance and `float32` for Depth. The +bridge swaps Real to RGBA on receive and keeps seg modes BGRA so consumers +can map color to class id. + +**Camera keys are canonical**: the single `/` name, where `` +is the articulation name and `` is the MJCF camera name with the art +prefix stripped, both sanitized to `[A-Za-z0-9_]` (e.g. art `g1`, camera +`g1_head_rgbd` → `g1/head_rgbd`). This one string is the `camera_topics` key, +the `zmq_topic`, and the key the SHM/ZMQ transports and `include_cameras` +requests use; the SHM file is `cam__.shm`. + +`include_cameras` accepts: + +- `true` — every registered camera, latest available frame. +- `false` — no cameras. +- a per-camera object, where each value is one of: + - `"latest"` (or legacy `"sync"`) — latest available frame from the ring. + - a number `N` — the frame showing post-step state `>= N` (pass the step + reply's `frame_id`). The deterministic "image for this step" path. + - `{ "frame_id": N }` — same, explicit. + +Typical downstream flow (post-step image, full step rate via one-step +pipelining): `result = step(...)` then on the *next* step request +`include_cameras = { "": result.frame_id }`. + +**Capture gating / streaming:** a camera captures only while it is +broadcast-enabled or has been requested recently. UE's `bEnableAllCameras` +now defaults **off**, so dedicated ZMQ/SHM pub streams must be turned on per +camera via [`set_camera_streaming`](#set_camera_streaming) (or rely on +`include_cameras`, which auto-activates capture). Oversize replies on the +SHM RPC transport (e.g. large multi-camera frames) return a +`reply_too_large` error so the bridge re-routes that one request to ZMQ. + +### Server-side camera waits (`wait_cameras`, `render`) + +`include_cameras` retrieval is non-blocking by default: a frame that +is not ready is omitted and the client retries. Two optional `step` +fields trade that round-trip for a server-side wait, for eval loops that +want the deterministic post-step image without polling. + +- `wait_cameras` (bool): block the reply until every requested camera + has the frame this step produced (`frame_id >= this step`), instead of + the client polling with a min `frame_id` and eating a round trip per + miss. `camera_timeout_ms` (number) bounds the wait; on expiry the + reply returns whatever is available. +- `render` (string): force a capture for the requested cameras this + step. Two values: + - `render: "sync"`: drive an immediate capture and wait for the fresh + frame (`frame_id == this step`). Flushes the game thread, so it is + the highest-latency, lowest-throughput path; use it when you need the + image that exactly matches this step. + - `render: "async"`: kick the capture but return the + most-recently-completed frame (typically one step stale) without + waiting, so back-to-back requests overlap render and readback for + higher throughput. (`async` was formerly named `pipelined`.) + + Both are eval-oriented, not for interactive use. An unrecognised + `render` string is ignored (neither sync nor async). + +### Camera stream frames + +Camera pixels are delivered on the dedicated PUB socket / SHM ring, not +in the step reply (the `cameras` block above is a snapshot the server +lifts off the same streams). Each streamed frame is prefixed with a +fixed 40-byte little-endian metadata header (`FMjCameraFrameMeta`, +magic `"UCM1"`) so a consumer can associate the pixels with the step +that produced them. The Python side unpacks it as `"= 0`, drawn from + a seeded per-camera RNG so it is reproducible. +- `clock`: `"sim"` (measure the delay in SimTime, deterministic, + default) or `"wall"` (wall-clock, real-latency emulation). +- `seed`: RNG seed for the jitter draw (`0` derives one from the name). +- `on_state_change`: capture / read back only when the physics state + advanced, skipping redundant GPU work between steps (default on). +- `max_fps`: optional hard wall-clock cap on capture rate (`0` = + uncapped). + +`delay_s = 0` with no jitter restores the zero-latency path. The reply +(`set_camera_delay_ok`) is keyed by canonical name; each entry echoes the +applied `delay_s` / `jitter_s` / `clock` / `on_state_change` / `max_fps`. +A camera name that does not resolve is skipped (logged, not an error); a +`cameras` object with no usable entries returns `bad_request`, and a +missing `cameras` object returns `missing_field`. The game-thread apply +returns `timeout` if it does not complete within 5 s. ## Recording @@ -572,7 +746,7 @@ Their wire names mirror the Python methods; see - `scene`: `import_xml`, `create_level`, `load_level`, `save_level`, `current_level`, `ensure_manager`, `spawn_actor`, `spawn_grid`, - `spawn_light`, `destroy_actor`, `destroy_asset`, + `spawn_light`, `remove_actor`, `destroy_asset`, `set_actor_transform`, `duplicate_actor`, `actor_hierarchy`, `snapshot`. - `sim` (PIE lifecycle): `begin_pie`, `stop_pie`, `pie_status`. @@ -589,6 +763,37 @@ Their wire names mirror the Python methods; see and on `ready` embeds a fresh handshake-shaped `handshake_payload` so the client re-discovers without an extra `hello`. +### Async editor ops (`op_started` / `op_status`) + +Long editor ops (`import_xml`, level create / load / save, some spawns) +block the game thread for seconds. Rather than hold the single RPC +worker for the whole run, the server kicks the work onto a game-thread +ticker and replies immediately with `op_started`: + +```json +{ "op": "op_started", "job_id": "job_42", "state": "running" } +``` + +The client then polls `op_status` with the `job_id` until the job goes +terminal: + +```json +{ "op": "op_status", "session_id": "uuid-v4", "job_id": "job_42" } + +{ "op": "op_status_ok", "job_id": "job_42", "state": "done", + "result": { "op": "import_xml_ok", "...": "..." } } +``` + +- `state` is `running`, `done`, or `failed`. `progress` is an optional + human string while `running`. +- On a terminal `state`, `result` carries the original handler reply + (the `_ok` payload, or an `error` reply that sets `state` to + `failed`). The job is popped when first reported terminal, so a second + `op_status` for the same `job_id` returns `unknown_job`. +- A handler may still answer synchronously with its `_ok`; clients + accept both the async (`op_started` + poll) and direct forms. +- `op_status` with an empty / missing `job_id` returns `bad_request`. + ## Errors A failed request returns: @@ -628,6 +833,10 @@ Codes raised by the dispatcher and op handlers: | `replay_session_not_found` | `set_active` / `start` named an unloaded session. | | `replay_requires_stepped` | `replay_start` issued while in `live`. | | `step_timeout` | A direct-mode step did not complete within 5 s. | +| `timeout` | A game-thread apply did not complete in time (e.g. `set_camera_delay` after 5 s). | +| `reply_too_large` | The reply exceeded the SHM RPC slot (e.g. large multi-camera frames). The bridge re-routes that one request over ZMQ. | +| `wrong_transport` | An op not served on the current transport was received on it (e.g. an editor-only or oversize op on the runtime-only SHM RPC). Message `use_zmq`; the bridge retries on ZMQ. | +| `unknown_job` | `op_status` named a `job_id` that was already collected or never existed. | | `shutting_down` | The bridge began draining mid-op (server stop / editor close). | Editor ops add their own per-op failure codes (for example diff --git a/docs/ros2_link_facts.md b/docs/ros2_link_facts.md new file mode 100644 index 00000000..a69fdaf8 --- /dev/null +++ b/docs/ros2_link_facts.md @@ -0,0 +1,177 @@ +# ROS 2 link facts (recorded during M1) + +Recorded from a green `urlab_rcl_test` build on Windows against the pinned +Lyrical install (see `docs/ros_workspace_setup.md`). These are user-observed +facts from the actual install, not guesses; the Unreal build wiring (`AddRos2` +in `Source/URLab/URLab.Build.cs`) consumes this file and it is authoritative +over any provisional list elsewhere. + +The install is Pixi/Conda (prefix.dev + RoboStack), so the files live under the +environment prefix, not a system path: + +``` +C:\dev\urlab_ros2_env\.pixi\envs\default\Library +``` + +Point `URLAB_ROS2_ROOT` at that `Library` directory (it holds `include\`, +`lib\`, `bin\`). + +--- + +## Distro / version + +| Field | Value | +|---|---| +| Distro | Lyrical Luth (codename `lyrical`) | +| Package versions (from cmake configure) | rcl 10.4.4, rmw_fastrtps_cpp 9.4.8, std_msgs/geometry_msgs/sensor_msgs 5.9.2, tf2_msgs 0.45.7, rosgraph_msgs 2.4.5, rosidl_generator_c 5.2.1 | +| Install method | Pixi / RoboStack (`https://prefix.dev/robostack-lyrical`) | +| RMW implementation in use | `rmw_fastrtps_cpp` (default; selected automatically at configure) | + +--- + +## Windows (primary) + +### Link libraries — the minimal set UBT must link + +UBT compiles only the C ABI (`UrlabRclCore.cpp`), so the link-time set is far +smaller than the full transitive list CMake pulls in. On Windows an import `.lib` +only requires the symbols our object files actually reference; every downstream +DLL dependency (rmw_implementation, the FastDDS cluster, the fastrtps +typesupports) is resolved by the loader at runtime, not at link. The minimal +correct set, all confirmed present under `Library\lib`: + +| Purpose | As-found `.lib` | +|---|---| +| rcl | `rcl.lib` | +| rcutils | `rcutils.lib` | +| rmw | `rmw.lib` | +| rosidl runtime | `rosidl_runtime_c.lib` | +| builtin_interfaces (gen + ts) | `builtin_interfaces__rosidl_generator_c.lib`, `builtin_interfaces__rosidl_typesupport_c.lib` | +| std_msgs (gen + ts) | `std_msgs__rosidl_generator_c.lib`, `std_msgs__rosidl_typesupport_c.lib` | +| geometry_msgs (gen + ts) | `geometry_msgs__rosidl_generator_c.lib`, `geometry_msgs__rosidl_typesupport_c.lib` | +| sensor_msgs (gen + ts) | `sensor_msgs__rosidl_generator_c.lib`, `sensor_msgs__rosidl_typesupport_c.lib` | +| tf2_msgs (gen + ts) | `tf2_msgs__rosidl_generator_c.lib`, `tf2_msgs__rosidl_typesupport_c.lib` | +| rosgraph_msgs (gen + ts) | `rosgraph_msgs__rosidl_generator_c.lib`, `rosgraph_msgs__rosidl_typesupport_c.lib` | + +`AddRos2` pins exactly this list. + +For reference, the FULL set the standalone CMake link line pulled in transitively +(via ament imported targets, not needed for the UBT link) additionally included: +`rmw_implementation`, `rosidl_typesupport_c`, `rosidl_dynamic_typesupport`, +`rosidl_buffer`, `rcl_yaml_param_parser`, `rcl_logging_interface`, +`fastcdr-2.3`, and the `*__rosidl_typesupport_fastrtps_c/cpp`, +`*__rosidl_typesupport_introspection_c/cpp`, `*__rosidl_typesupport_cpp` and +`*__rosidl_generator_py` variants for `rcl_interfaces`, `service_msgs`, +`type_description_interfaces`, `action_msgs`, `unique_identifier_msgs`. These are +runtime DLLs, staged (below), not linked. + +### Runtime DLL cluster (must be on PATH / staged) + +Direct dependencies of `urlab_rcl_test.exe` (from `dumpbin /dependents`), ROS +only: + +``` +rcl.dll rcutils.dll rmw.dll rosidl_runtime_c.dll +sensor_msgs__rosidl_typesupport_c.dll sensor_msgs__rosidl_generator_c.dll +geometry_msgs__rosidl_typesupport_c.dll geometry_msgs__rosidl_generator_c.dll +std_msgs__rosidl_typesupport_c.dll std_msgs__rosidl_generator_c.dll +tf2_msgs__rosidl_typesupport_c.dll tf2_msgs__rosidl_generator_c.dll +rosgraph_msgs__rosidl_typesupport_c.dll rosgraph_msgs__rosidl_generator_c.dll +``` + +Transitive cluster loaded at runtime by `rmw` -> `rmw_implementation` -> +`rmw_fastrtps_cpp` (from `dumpbin /dependents` on `rmw_implementation.dll` and +the FastDDS chain in `Library\bin`): + +``` +rmw_implementation.dll rmw_fastrtps_cpp.dll rmw_fastrtps_shared_cpp.dll +rmw_dds_common.dll rmw_dds_common__rosidl_*.dll +rcpputils.dll ament_index_cpp.dll +rosidl_typesupport_c.dll rosidl_typesupport_cpp.dll +rosidl_typesupport_fastrtps_c.dll rosidl_typesupport_fastrtps_cpp.dll +rosidl_typesupport_introspection_c.dll rosidl_typesupport_introspection_cpp.dll +rosidl_dynamic_typesupport.dll rosidl_dynamic_typesupport_fastrtps.dll +rcl_logging_interface.dll rcl_logging_spdlog.dll spdlog.dll +rcl_yaml_param_parser.dll +fastdds-3.6.dll fastcdr-2.3.dll foonathan_memory-0.7.4.dll tinyxml2.dll +libssl-3-x64.dll libcrypto-3-x64.dll dds_security_crypto.dll +__rosidl_typesupport_fastrtps_c.dll / _cpp.dll and +__rosidl_typesupport_introspection_c.dll / _cpp.dll for +builtin_interfaces, std_msgs, geometry_msgs, sensor_msgs, tf2_msgs, +rosgraph_msgs, rcl_interfaces, service_msgs, type_description_interfaces, +action_msgs, unique_identifier_msgs +``` + +Note the version-suffixed DDS basenames: `fastdds-3.6`, `fastcdr-2.3`, +`foonathan_memory-0.7.4`. `AddRos2` stages these with prefix patterns so the +suffix does not have to be hard-coded. + +### Include layout + +| Field | Value | +|---|---| +| Include root | `%URLAB_ROS2_ROOT%\include` (= `...\Library\include`) | +| Layout | per-package nested: `include\\\msg\.h` (e.g. `include\sensor_msgs\sensor_msgs\msg\joint_state.h`) | +| Consequence | each package needs its own `-I include\` entry; `#include ` resolves under `include\sensor_msgs`. Confirmed for `rcl`, `rmw`, `rosidl_runtime_c`, `rosidl_typesupport_interface`, `builtin_interfaces`. | +| Lib dir | `%URLAB_ROS2_ROOT%\lib` (`.lib`) | +| Bin dir (DLLs) | `%URLAB_ROS2_ROOT%\bin` | + +--- + +## Linux (secondary) + +Not yet run on this machine (Windows is the primary M1 platform). Fill from +`ldd build/urlab_rcl_test` after a green `build_and_test.sh`. The `AddRos2` +Linux branch mirrors the Windows pin: link the unversioned `lib.so` +symlinks for the same minimal set and stage the `*.so*` cluster under +`$ORIGIN`, exactly as `AddThirdPartyLibrary` does for libzmq. Expected +basenames: `librcl.so`, `librcutils.so`, `librmw.so`, `librosidl_runtime_c.so`, +`lib__rosidl_generator_c.so`, `lib__rosidl_typesupport_c.so`, plus the +runtime `librmw_fastrtps_cpp.so`, `libfastdds.so`, `libfastcdr.so` cluster. + +--- + +## rcl / rosidl API assumptions — all confirmed on Lyrical + +`UrlabRclCore.cpp` compiled and linked clean against the actual Lyrical install +and the selftest + cross-process `ros2 topic echo` passed, which confirms the +API assumptions the core was written against: + +| # | Assumption | Status | +|---|---|---| +| 1 | Header paths (`rcl/rcl.h`, `rmw/qos_profiles.h`, `rosidl_runtime_c/*`, `/msg/.h`) | OK (compiled clean; nested include layout above) | +| 2 | `rcl_init_options_set_domain_id(rcl_init_options_t*, size_t)`; `RCL_DEFAULT_DOMAIN_ID` | OK | +| 3 | `ROSIDL_GET_MSG_TYPE_SUPPORT(pkg, msg, Type)` resolves once `__rosidl_typesupport_c` is linked | OK | +| 4 | `rcl_wait_set_init` seven-count signature | OK | +| 5 | `wait_set.subscriptions[i]` index-aligned with add order | OK (selftest ctrl + twist callbacks fired) | +| 6 | `rcl_take(...)` signature; `rmw_get_zero_initialized_message_info()` | OK | +| 7 | Loaned-message API present (`rcl_publisher_can_loan_messages`, borrow/publish/return) | OK (compiled/linked) | +| 8 | `rcl_get_error_string()` -> `rcutils_error_string_t{.str}`; `rcl_reset_error()` | OK | +| 9 | rosidl C struct field names as used across JointState/Imu/Image/TF/Twist/Clock/Time/Float64MultiArray | OK (`ros2 topic echo` showed correct JointState `name/position/velocity/effort`) | +| 10 | Sequence + string helpers (`rosidl_runtime_c__String__assign`, `__Sequence__init`, `__init/fini`) | OK | +| 11 | QoS symbols (`rmw_qos_profile_default`, transient-local / reliable / keep-last) | OK | +| 12 | `/tf_static` transient-local latch delivers to late joiners | OK (design choice; publisher created without error) | +| 13 | Plain-CMake consumption via namespaced imported targets against an active Lyrical env | OK (`find_package(rcl)` etc. resolved; see `ros/urlab_ros_ws/CMakeLists.txt`) | +| 14 | Env activation via `pixi run` (non-interactive) / `AMENT_PREFIX_PATH` detection | OK (built via `pixi run --manifest-path C:\dev\urlab_ros2_env\pixi.toml`) | + +### M1 result + +`ros/urlab_ros_ws/build_and_test.ps1` ran fully green on Windows: + +``` +distro: lyrical +selftest: OK (ctrl + twist loopback verified) +reinit: OK +urlab_rcl_test: PASS +>>> Cross-process verify (ros2 topic echo)... +name: +- joint_a +- joint_b +- joint_c +position: [1.0, 2.0, 3.0] velocity: [0.1, 0.2, 0.3] effort: [10.0, 20.0, 30.0] +>>> Cross-process verify OK: JointState names received. +=== urlab_rcl_test: ALL CHECKS PASSED (Windows) === +``` + +The cross-process echo proves real inter-process DDS works in-process on +Windows — the key de-risk gate for the whole ROS design. diff --git a/docs/ros_workspace_setup.md b/docs/ros_workspace_setup.md new file mode 100644 index 00000000..9fb43b59 --- /dev/null +++ b/docs/ros_workspace_setup.md @@ -0,0 +1,336 @@ +# ROS 2 workspace setup and validation (M1) + +This document is the complete, self-contained instruction set for installing +ROS 2, building the standalone `urlab_rcl_test` harness, and validating the +in-process rcl publish/subscribe path that the UnrealRoboticsLab ROS integration +is built on. You do not need to read any design document to follow it. + +**Pinned distribution: ROS 2 Lyrical Luth** (codename `lyrical`, released +2026-05-22, an LTS supported to 2031). Everything below assumes Lyrical. + +**Why this matters most:** in-process ROS 2 on **Windows** has no public +reference implementation. Proving that a Windows process can link the rcl C API +and exchange messages over DDS with a second process is the single biggest +de-risking step for the whole ROS effort. Lyrical is pinned specifically because +the ROS project has made Windows a first-class target (Windows became Tier 1 in +Kilted, 2025, which also moved the default Windows install to **Pixi/Conda** via +prefix.dev + RoboStack; Lyrical continues that with improved Windows 11 support). +The Windows path below is therefore the primary one; do it first. Linux follows +as the secondary / CI path. + +The workspace at `ros/urlab_ros_ws/` is a plain CMake project (not colcon). It +consumes the ROS 2 installation you provide; it never builds ROS itself and never +installs anything. + +--- + +## Part 1 — Windows (primary, via Pixi/Conda) + +The default and recommended Windows install for current ROS 2 is **Pixi** +(prefix.dev) with the **RoboStack** conda channels. There is no system-wide +install and no `setup.bat` to source: ROS lives inside a Pixi project's `.pixi/` +environment, and you activate it with `pixi shell`. + +### 1.1 Prerequisites + +1. **Visual Studio 2022** with the **"Desktop development with C++"** workload + (MSVC toolchain + Windows SDK). RoboStack's ROS 2 binaries are built with + MSVC, and so is Unreal Engine, so both sides of the eventual link use the same + compiler and C runtime — there is no libc++/libstdc++ ABI concern on Windows + (contrast Linux, Part 2.4). You compile `UrlabRclCore.cpp` and `test_main.cpp` + with this toolchain. +2. **Enable Developer mode** (Settings -> System -> For developers -> Developer + mode). Pixi/Conda uses symlinks; Developer mode lets them be created without + admin rights. This is the official RoboStack recommendation for Windows. + +### 1.2 Install Pixi + +In PowerShell (verified against the official Pixi install docs): + +```powershell +powershell -ExecutionPolicy Bypass -c "irm -useb https://pixi.sh/install.ps1 | iex" +``` + +This downloads Pixi and adds it to your `PATH`. Open a new terminal afterwards so +`pixi` is available, and confirm with `pixi --version`. + +### 1.3 Create the ROS 2 Lyrical environment + +IMPORTANT (Windows): create the Pixi env at a SHORT, SPACE-FREE path such as +`C:\dev\urlab_ros2_env`. Do NOT put it inside the UE project tree — that path +contains a space (`Unreal Projects`) and is deeply nested, and ROS 2's Windows +launchers (`ros2` and other Python console scripts) fail with +`failed to create process.` when their interpreter path contains a space or +exceeds the Windows path limit. The ROS env is an install, not source; it does +not belong in the repo. (The harness project in `ros/urlab_ros_ws/` stays in the +repo; you activate this env from its `C:\dev` location, or via `URLAB_ROS2_SETUP`, +when building the harness.) + +Create a Pixi project that pulls ROS 2 Lyrical from the RoboStack channel. Use +the FULL prefix.dev channel URL, not the bare `robostack-lyrical` name: the +newer RoboStack channels (kilted, lyrical) are hosted only on prefix.dev, and a +bare `robostack-lyrical` resolves against `conda.anaconda.org` (where lyrical is +not published) and fails with a 404 on `noarch/repodata.json`. Older distros +(humble, jazzy) happen to work with the bare name because they are mirrored on +anaconda.org; lyrical is not. + +```powershell +mkdir C:\dev -Force +cd C:\dev +pixi init urlab_ros2_env -c https://prefix.dev/robostack-lyrical -c conda-forge +cd urlab_ros2_env +pixi add ros-lyrical-desktop +pixi shell +ros2 --help # sanity check: must succeed, not "failed to create process." +``` + +If you already ran `pixi init` with the bare `robostack-lyrical` name, edit the +`channels` line in the generated `pixi.toml` to +`["https://prefix.dev/robostack-lyrical", "conda-forge"]` and re-run +`pixi add ros-lyrical-desktop`. + +`ros-lyrical-desktop` includes the rcl C API, the message packages this workspace +needs (`sensor_msgs`, `geometry_msgs`, `tf2_msgs`, `std_msgs`, `rosgraph_msgs`), +the CMake config packages, and the `ros2` CLI used for verification. If you want a +smaller footprint, `ros-lyrical-ros-base` is a lighter alternative that still +provides rcl and the `ros2` CLI. + +This installs ROS 2 into `urlab_ros2_env\.pixi\`. Nothing is installed +system-wide. + +### 1.4 Activate the environment + +From the `urlab_ros2_env` directory: + +```powershell +pixi shell +``` + +`pixi shell` activates the environment for the current shell: it puts the ROS +libraries, headers, `cmake`, and the `ros2` CLI on `PATH` and sets the ROS +environment variables (`AMENT_PREFIX_PATH`, `CMAKE_PREFIX_PATH`, `CONDA_PREFIX`, +...). Confirm ROS is live: + +```powershell +ros2 --help +``` + +Run this `pixi shell` from a **"Developer PowerShell for VS 2022"** (or otherwise +ensure the MSVC `cl` compiler is on `PATH`) so CMake finds the C++ compiler when +it configures the workspace. You can alternatively run one-off commands without a +persistent shell via `pixi run `. + +### 1.5 Build and test + +With the environment active (from 1.4), build and run the harness: + +```powershell +cd \ros\urlab_ros_ws +.\build_and_test.ps1 +``` + +The script detects the active ROS environment (via `AMENT_PREFIX_PATH`), +configures and builds the harness (Release), runs the selftest, then runs a +cross-process `ros2 topic echo` check. It writes nothing outside +`ros\urlab_ros_ws\build\`. + +If you prefer not to use `pixi shell`, point the script at an activation script +instead: + +```powershell +.\build_and_test.ps1 -RosSetup 'C:\path\to\ros_or_conda_activate.bat' +# or: $env:URLAB_ROS2_SETUP = 'C:\path\to\activate.bat' +``` + +### 1.6 Verify expected output + +On success you will see, in order: + +- `distro: lyrical` printed at startup. +- `selftest: OK (ctrl + twist loopback verified)` — the harness published a + `Float64MultiArray` and a `Twist` to the core's own subscriptions and the + callbacks fired with the exact values. +- `reinit: OK` and `urlab_rcl_test: PASS`. +- The cross-process block, where a second process reads one JointState sample: + + ``` + >>> Cross-process verify (ros2 topic echo)... + header: + stamp: ... + frame_id: '' + name: + - joint_a + - joint_b + - joint_c + position: + - 1.0 + - 2.0 + - 3.0 + velocity: + - 0.1 + - 0.2 + - 0.3 + effort: + - 10.0 + - 20.0 + - 30.0 + ... + >>> Cross-process verify OK: JointState names received. + === urlab_rcl_test: ALL CHECKS PASSED (Windows) === + ``` + +The joint names `joint_a/joint_b/joint_c` and positions `1.0/2.0/3.0` are the +deterministic values the harness publishes; seeing them proves real inter-process +DDS works in-process on Windows. **This is the key gate — if it passes, the +in-process design is validated on the hard platform.** + +You can also run the echo manually while `urlab_rcl_test --publish 600` runs in +another activated shell: + +```powershell +ros2 topic echo --once /urlab_test/joint_states +ros2 topic echo --once /urlab_test/imu +ros2 topic echo --once /clock +ros2 topic list +``` + +--- + +## Part 2 — Linux (secondary / CI) + +Linux can use apt (system install) or the same Pixi/Conda flow as Windows. The +apt path is the default the script expects. + +### 2.1 Install ROS 2 Lyrical (apt) + +```bash +sudo apt update && sudo apt install -y ros-lyrical-ros-base ros-dev-tools +``` + +This installs Lyrical into `/opt/ros/lyrical`. + +(Alternatively, use Pixi exactly as in Part 1.2-1.4 but with `curl -fsSL +https://pixi.sh/install.sh | sh` to install Pixi.) + +### 2.2 Source / activate the environment + +`build_and_test.sh` picks up ROS in this order: `URLAB_ROS2_SETUP` if set, else +the apt default `/opt/ros/lyrical/setup.bash`, else an already-active environment +(e.g. inside `pixi shell`). To override the apt default: + +```bash +export URLAB_ROS2_SETUP=/opt/ros/lyrical/setup.bash +``` + +### 2.3 Build and test + +```bash +cd /ros/urlab_ros_ws +./build_and_test.sh +``` + +Success ends with `=== urlab_rcl_test: ALL CHECKS PASSED (Linux) ===` and the +same selftest / `ros2 topic echo` output shape as Windows (Part 1.6). + +### 2.4 clang + libc++ toolchain smoke (Linux only, deferred) + +```bash +./build_and_test.sh --libcxx +``` + +This rebuilds `UrlabRclCore.cpp` with **clang + libc++** and links it against the +libstdc++-built rcl cluster, pre-validating the exact standard-library mix that +an Unreal Engine Linux build produces (UE uses clang + bundled libc++, while +stock ROS is gcc + libstdc++). It is a **Linux-only** concern and is **deferred** +until the Linux leg is worked on: it does **not** apply on Windows, where MSVC is +used on both sides and there is no stdlib ABI mix to validate. Skip it entirely +for the initial Windows run. + +--- + +## Part 3 — Record the link facts + +The Unreal build wiring (a later phase) needs the exact library names, include +directories, and runtime dependency cluster that your build actually used. After +a green run, capture them into `docs/ros2_link_facts.md` (a template with labeled +blanks lives there already). + +On a Pixi/Conda install the ROS files live under the environment prefix, not a +system path. With the env active, find the prefix: + +- Windows (PowerShell): `echo $env:CONDA_PREFIX` — libraries are under + `%CONDA_PREFIX%\Library\bin` (DLLs), `%CONDA_PREFIX%\Library\lib` (`.lib`), + headers under `%CONDA_PREFIX%\Library\include`. +- Linux (bash): `echo $CONDA_PREFIX` — `.so` under `$CONDA_PREFIX/lib`, headers + under `$CONDA_PREFIX/include`. (apt install: `/opt/ros/lyrical/{lib,include}`.) + +### Windows + +- **Link library basenames** (the `.lib` files CMake linked): inspect the CMake + link line: + + ```powershell + cmake --build build --config Release --verbose 2>&1 | Select-String '\.lib' + ``` + + Record the `rcl`, `rcutils`, `rmw`, `rmw_implementation`/`rmw_fastrtps_cpp`, + `rosidl_runtime_c`, `rosidl_typesupport_c`, and the per-message-package + `*__rosidl_typesupport_c` / `*__rosidl_generator_c` basenames as found. + +- **Runtime DLL cluster** (must be staged next to a UE build): list the DLLs the + executable actually depends on: + + ```powershell + dumpbin /dependents build\Release\urlab_rcl_test.exe + ``` + + Record every ROS/DDS DLL it names (`rcl.dll`, `rmw*.dll`, + `rmw_fastrtps_cpp.dll`, `fastrtps.dll`/`fastdds.dll`, `fastcdr.dll`, + `rosidl_*`, the message-package DLLs, and their transitive deps). Repeat + `dumpbin /dependents` on those DLLs to catch the transitive set. Note their + directory (typically `%CONDA_PREFIX%\Library\bin`). + +- **Include directories**: from the CMake configure output or by inspecting + `%CONDA_PREFIX%\Library\include` — note whether headers are flat or nested per + package (`include///msg/...`). + +### Linux + +```bash +ldd build/urlab_rcl_test +``` + +Record the `librcl.so`, `librmw*.so`, `librosidl_runtime_c.so`, +`libfastrtps.so`/`libfastdds.so`, `libfastcdr.so`, and message-package `.so` +basenames (the unversioned symlink names) and their directory, plus the include +layout under `$CONDA_PREFIX/include` (Pixi) or `/opt/ros/lyrical/include` (apt). + +Paste all of this into `docs/ros2_link_facts.md`. Those user-observed facts — not +any agent guess — are what the Unreal build wiring consumes. + +--- + +## Validation gates (what "M1 passed" means) + +- **A2 (Windows pub/sub):** `build_and_test.ps1` fully green on Windows. This is + the de-risk gate for the whole in-process design; do it first. +- **A1 (Linux pub/sub):** `build_and_test.sh` fully green on Linux. +- **A3 (toolchain smoke):** `build_and_test.sh --libcxx` green on Linux + (deferred; Linux-only). +- **Facts recorded:** `docs/ros2_link_facts.md` filled in, including the + rcl/rosidl API assumptions confirmed against the actual Lyrical install. + +Once these pass and the facts are recorded, the `UrlabRclCore.h` contract is +frozen (additive changes only) and the Unreal-side wiring can begin. + +--- + +## Sources + +Install commands above were verified on 2026-07-26 against: + +- [ROS 2 Lyrical Luth release notes](https://docs.ros.org/en/lyrical/Releases/Release-Lyrical-Luth.html) +- [ROS 2 Kilted Kaiju release blog (Windows Tier 1 + Pixi/Conda default)](https://www.openrobotics.org/blog/2025/5/23/ros-2-kilted-kaiju-released) +- [RoboStack getting started](https://robostack.github.io/GettingStarted.html) +- [Pixi ROS 2 tutorial](https://pixi.prefix.dev/latest/tutorials/ros2/) +- [Pixi installation](https://pixi.prefix.dev/latest/installation/) diff --git a/mkdocs.yml b/mkdocs.yml index eb6e7edd..3a112f8b 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -99,6 +99,7 @@ nav: - Debug Visualization: guides/debug.md - Interaction: guides/interaction.md - Recording & Replay: guides/recording.md + - ROS 2 Integration: guides/ros.md - Python & External Control: - Overview: python/index.md - Quickstart: python/quickstart.md From 46d91336fff5a7c661241e51a2f19a94d2b887f4 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Wed, 5 Aug 2026 09:56:05 +0100 Subject: [PATCH 11/32] Bump MuJoCo to upstream main (3.11.1) The schema this rebuild generates from is MuJoCo's own, so the engine and the grammar move together. --- .gitmodules | 7 ++++++- third_party/MuJoCo/build.ps1 | 10 +++++++++- third_party/MuJoCo/build.sh | 4 +++- third_party/MuJoCo/src | 2 +- 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/.gitmodules b/.gitmodules index 08f5c1a2..bc043798 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,11 @@ [submodule "third_party/MuJoCo/src"] path = third_party/MuJoCo/src - url = https://github.com/google-deepmind/mujoco + # URLab's MuJoCo fork. It is upstream plus one directory, protospec/, which + # carries the MJCF schema front end, its generator and the C++ SDK that + # Source/URLab/*/MuJoCo/Gen compiles against. Keeping it in the submodule is + # what makes schema, generator, SDK and engine version as one unit; the fork + # branch commits nothing outside protospec/, so bumping MuJoCo stays a rebase. + url = https://github.com/URLab-Sim/mujoco [submodule "third_party/CoACD/src"] path = third_party/CoACD/src url = https://github.com/SarahWeiii/CoACD diff --git a/third_party/MuJoCo/build.ps1 b/third_party/MuJoCo/build.ps1 index 5551d309..74bc3210 100644 --- a/third_party/MuJoCo/build.ps1 +++ b/third_party/MuJoCo/build.ps1 @@ -8,6 +8,7 @@ param( if (-not [System.IO.Path]::IsPathRooted($InstallDir)) { $InstallDir = Join-Path $PSScriptRoot $InstallDir } +$InstallRoot = [System.IO.Path]::GetFullPath($InstallDir).Replace('\', '/') $InstallDir = Join-Path $InstallDir "MuJoCo" $InstallDir = [System.IO.Path]::GetFullPath($InstallDir) $InstallDir = $InstallDir.Replace('\', '/') @@ -61,7 +62,14 @@ $cmakeArgs = @( "-DMUJOCO_BUILD_EXAMPLES=OFF", "-DMUJOCO_BUILD_TESTS=OFF", "-DMUJOCO_BUILD_SIMULATE=OFF", - "-DCMAKE_MSVC_RUNTIME_LIBRARY=MultiThreaded$($BuildType.Replace('Release', '').Replace('Debug', 'Debug'))DLL" + "-DCMAKE_MSVC_RUNTIME_LIBRARY=MultiThreaded$($BuildType.Replace('Release', '').Replace('Debug', 'Debug'))DLL", + # MuJoCo patches its fetched qhull with `git apply --reject`, which is not + # idempotent: applied twice it rejects every hunk and returns non-zero. On + # the Visual Studio generator MSBuild re-invokes CMake mid-build once the + # FetchContent deps have landed, so a build from a clean tree would re-run + # that patch and fail. Suppressing regeneration keeps the one successful + # configure authoritative for the rest of the build. + "-DCMAKE_SUPPRESS_REGENERATION=ON" ) cmake @cmakeArgs if ($LASTEXITCODE -ne 0) { throw "CMake configuration failed for MuJoCo" } diff --git a/third_party/MuJoCo/build.sh b/third_party/MuJoCo/build.sh index 0192d254..6d9e142d 100644 --- a/third_party/MuJoCo/build.sh +++ b/third_party/MuJoCo/build.sh @@ -8,7 +8,9 @@ done # Resolve INSTALL_DIR to an absolute per-package path. URLab.Build.cs expects # headers/libs/dlls under install//, matching the .ps1 layout. -INSTALL_DIR="$(cd "$(dirname "$INSTALL_DIR")" && pwd)/$(basename "$INSTALL_DIR")/MuJoCo" +INSTALL_ROOT="$(cd "$(dirname "$INSTALL_DIR")" && pwd)/$(basename "$INSTALL_DIR")" +INSTALL_DIR="$INSTALL_ROOT/MuJoCo" +SCRIPT_DIR="$(cd "$(dirname "$0")" && pwd)" # Wipe any prior install of THIS package only - cmake --install is additive # and would otherwise leave stale files behind across version bumps. diff --git a/third_party/MuJoCo/src b/third_party/MuJoCo/src index 67a1ea6d..b6683ac1 160000 --- a/third_party/MuJoCo/src +++ b/third_party/MuJoCo/src @@ -1 +1 @@ -Subproject commit 67a1ea6dca4cd8f2c4d17ec5df8a3a5792a3d170 +Subproject commit b6683ac13dd353c8d66788a3beea6e84b2eb2dc2 From 8c415a1327a3364f7461564053ebb971c62822ba Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Mon, 10 Aug 2026 23:33:32 +0100 Subject: [PATCH 12/32] Add ProtoSpec: MJCF as an object model generated from MuJoCo's schema MuJoCo ships `src/xml/mjcf.schema` and drives its own reader and writer from it. This is the same idea carried into a library: the schema is the single source of truth, and the object model, the reader, the writer and their tables are emitted from it rather than written by hand. What is here is the library and its generator, standalone and with its own build and test suite. Nothing in the plugin uses it yet. The point is the cost of the next MuJoCo release. Hand-written binding code is what makes a version bump expensive, so anything derivable from the schema is derived, and a schema change that our tables do not account for fails generation by name rather than compiling into something subtly wrong. --- protospec/.gitattributes | 7 + protospec/.gitignore | 6 + protospec/README.md | 156 + protospec/build.ps1 | 105 + protospec/build.sh | 78 + protospec/corpus_net.ps1 | 70 + protospec/corpus_net.sh | 57 + protospec/lib/CMakeLists.txt | 185 + protospec/lib/core/resolve.cc | 312 + protospec/lib/core/resolve.h | 48 + protospec/lib/generated/defaults.cc | 1333 +++ protospec/lib/generated/defaults.h | 161 + protospec/lib/generated/keywords.cc | 904 ++ protospec/lib/generated/keywords.h | 112 + protospec/lib/generated/reflect.cc | 7488 +++++++++++++++++ protospec/lib/generated/reflect.h | 290 + protospec/lib/generated/types.cc | 4704 +++++++++++ protospec/lib/generated/types.h | 3884 +++++++++ protospec/lib/generated/visit.h | 4512 ++++++++++ protospec/lib/generated/xml_binding.cc | 2313 +++++ protospec/lib/generated/xml_binding.h | 86 + protospec/lib/harness/CMakeLists.txt | 105 + protospec/lib/harness/mj_model_diff.cc | 273 + protospec/lib/harness/model_diff_lib.cc | 298 + protospec/lib/harness/model_diff_lib.h | 98 + protospec/lib/harness/plugin_registry.cc | 73 + protospec/lib/harness/plugin_registry.h | 33 + protospec/lib/include/protospec/core.h | 176 + protospec/lib/include/protospec/diag.h | 80 + protospec/lib/include/protospec/io.h | 18 + protospec/lib/include/protospec/model.h | 35 + protospec/lib/include/protospec/reflect.h | 23 + protospec/lib/io/default_classes.cc | 12 + protospec/lib/io/default_classes.h | 80 + protospec/lib/io/include.cc | 275 + protospec/lib/io/include.h | 70 + protospec/lib/io/mjcf.h | 114 + protospec/lib/io/mjcf_common.cc | 608 ++ protospec/lib/io/mjcf_common.h | 203 + protospec/lib/io/mjcf_reader.inc | 822 ++ protospec/lib/io/mjcf_reader_plain.cc | 82 + protospec/lib/io/mjcf_writer.inc | 282 + protospec/lib/io/mjcf_writer_plain.cc | 46 + protospec/lib/io/numeric.cc | 196 + protospec/lib/io/numeric.h | 54 + protospec/lib/io/supported.json | 131 + protospec/lib/sdk/protospec/classes.h | 436 + protospec/lib/sdk/protospec/detail.h | 239 + protospec/lib/sdk/protospec/model_core.h | 465 + protospec/lib/sdk/protospec/parents.h | 137 + protospec/lib/sdk/protospec/plain_profile.h | 1438 ++++ protospec/lib/sdk/protospec/profile.h | 281 + protospec/lib/test/protospec/builders.h | 418 + protospec/lib/test/protospec/class_edits.h | 138 + protospec/lib/test/protospec/edits.h | 193 + protospec/lib/test/protospec/refs.h | 631 ++ protospec/lib/test/protospec/save.h | 84 + protospec/lib/test/protospec/sdk.h | 34 + protospec/lib/test/protospec/traversal.h | 313 + protospec/lib/test/save.cc | 76 + protospec/lib/test/test_io.cc | 1542 ++++ protospec/lib/test/test_model.cc | 422 + protospec/lib/test/test_public_api.cc | 112 + protospec/lib/test/test_sdk.cc | 981 +++ .../lib/third_party/tinyxml2/LICENSE-note | 18 + .../lib/third_party/tinyxml2/tinyxml2.cpp | 3021 +++++++ protospec/lib/third_party/tinyxml2/tinyxml2.h | 2387 ++++++ protospec/lib/tools/ps_roundtrip.cc | 36 + protospec/protospec_gen/__init__.py | 22 + protospec/protospec_gen/classified_attrs.json | 1537 ++++ protospec/protospec_gen/doc_anchors.json | 1018 +++ protospec/protospec_gen/emit.py | 1312 +++ protospec/protospec_gen/emit_ue.py | 3649 ++++++++ protospec/protospec_gen/frontend.py | 1203 +++ protospec/protospec_gen/mjdefaults.py | 113 + protospec/protospec_gen/mujoco_defaults.json | 2474 ++++++ protospec/protospec_gen/overlay.py | 518 ++ protospec/protospec_gen/overlay_ue.py | 680 ++ protospec/protospec_gen/xmlref.py | 425 + protospec/pyproject.toml | 24 + protospec/snapshots/corpus_report.json | 44 + protospec/tests/fixtures/annotations.spec | 19 + protospec/tests/fixtures/arity.spec | 22 + protospec/tests/fixtures/cardinality.spec | 10 + .../include_traversal/outside/secret.xml | 3 + .../include_traversal/root/inc_inside.xml | 3 + .../include_traversal/root/uses_inside.xml | 5 + .../include_traversal/root/uses_outside.xml | 5 + protospec/tests/fixtures/nesting/deep_199.xml | 6 + protospec/tests/fixtures/nesting/deep_201.xml | 6 + protospec/tests/fixtures/section5.spec | 85 + .../tests/fixtures/section5_expected.json | 946 +++ protospec/tests/fixtures/unions.spec | 17 + protospec/tests/test_asan.py | 46 + protospec/tests/test_corpus_coverage.py | 190 + protospec/tests/test_differential.py | 547 ++ protospec/tests/test_emit.py | 162 + protospec/tests/test_emit_ue.py | 418 + protospec/tests/test_frontend.py | 460 + protospec/tests/test_mjdefaults.py | 173 + protospec/tests/test_upstream_untouched.py | 118 + protospec/tests/test_xmlref.py | 154 + protospec/tools/corpus_net.py | 110 + protospec/tools/corpus_study.py | 515 ++ protospec/tools/refresh_mj_defaults.py | 491 ++ protospec/tools/run_asan.ps1 | 175 + protospec/tools/run_asan.sh | 101 + protospec/uv.lock | 161 + 108 files changed, 62367 insertions(+) create mode 100644 protospec/.gitattributes create mode 100644 protospec/.gitignore create mode 100644 protospec/README.md create mode 100644 protospec/build.ps1 create mode 100644 protospec/build.sh create mode 100644 protospec/corpus_net.ps1 create mode 100644 protospec/corpus_net.sh create mode 100644 protospec/lib/CMakeLists.txt create mode 100644 protospec/lib/core/resolve.cc create mode 100644 protospec/lib/core/resolve.h create mode 100644 protospec/lib/generated/defaults.cc create mode 100644 protospec/lib/generated/defaults.h create mode 100644 protospec/lib/generated/keywords.cc create mode 100644 protospec/lib/generated/keywords.h create mode 100644 protospec/lib/generated/reflect.cc create mode 100644 protospec/lib/generated/reflect.h create mode 100644 protospec/lib/generated/types.cc create mode 100644 protospec/lib/generated/types.h create mode 100644 protospec/lib/generated/visit.h create mode 100644 protospec/lib/generated/xml_binding.cc create mode 100644 protospec/lib/generated/xml_binding.h create mode 100644 protospec/lib/harness/CMakeLists.txt create mode 100644 protospec/lib/harness/mj_model_diff.cc create mode 100644 protospec/lib/harness/model_diff_lib.cc create mode 100644 protospec/lib/harness/model_diff_lib.h create mode 100644 protospec/lib/harness/plugin_registry.cc create mode 100644 protospec/lib/harness/plugin_registry.h create mode 100644 protospec/lib/include/protospec/core.h create mode 100644 protospec/lib/include/protospec/diag.h create mode 100644 protospec/lib/include/protospec/io.h create mode 100644 protospec/lib/include/protospec/model.h create mode 100644 protospec/lib/include/protospec/reflect.h create mode 100644 protospec/lib/io/default_classes.cc create mode 100644 protospec/lib/io/default_classes.h create mode 100644 protospec/lib/io/include.cc create mode 100644 protospec/lib/io/include.h create mode 100644 protospec/lib/io/mjcf.h create mode 100644 protospec/lib/io/mjcf_common.cc create mode 100644 protospec/lib/io/mjcf_common.h create mode 100644 protospec/lib/io/mjcf_reader.inc create mode 100644 protospec/lib/io/mjcf_reader_plain.cc create mode 100644 protospec/lib/io/mjcf_writer.inc create mode 100644 protospec/lib/io/mjcf_writer_plain.cc create mode 100644 protospec/lib/io/numeric.cc create mode 100644 protospec/lib/io/numeric.h create mode 100644 protospec/lib/io/supported.json create mode 100644 protospec/lib/sdk/protospec/classes.h create mode 100644 protospec/lib/sdk/protospec/detail.h create mode 100644 protospec/lib/sdk/protospec/model_core.h create mode 100644 protospec/lib/sdk/protospec/parents.h create mode 100644 protospec/lib/sdk/protospec/plain_profile.h create mode 100644 protospec/lib/sdk/protospec/profile.h create mode 100644 protospec/lib/test/protospec/builders.h create mode 100644 protospec/lib/test/protospec/class_edits.h create mode 100644 protospec/lib/test/protospec/edits.h create mode 100644 protospec/lib/test/protospec/refs.h create mode 100644 protospec/lib/test/protospec/save.h create mode 100644 protospec/lib/test/protospec/sdk.h create mode 100644 protospec/lib/test/protospec/traversal.h create mode 100644 protospec/lib/test/save.cc create mode 100644 protospec/lib/test/test_io.cc create mode 100644 protospec/lib/test/test_model.cc create mode 100644 protospec/lib/test/test_public_api.cc create mode 100644 protospec/lib/test/test_sdk.cc create mode 100644 protospec/lib/third_party/tinyxml2/LICENSE-note create mode 100644 protospec/lib/third_party/tinyxml2/tinyxml2.cpp create mode 100644 protospec/lib/third_party/tinyxml2/tinyxml2.h create mode 100644 protospec/lib/tools/ps_roundtrip.cc create mode 100644 protospec/protospec_gen/__init__.py create mode 100644 protospec/protospec_gen/classified_attrs.json create mode 100644 protospec/protospec_gen/doc_anchors.json create mode 100644 protospec/protospec_gen/emit.py create mode 100644 protospec/protospec_gen/emit_ue.py create mode 100644 protospec/protospec_gen/frontend.py create mode 100644 protospec/protospec_gen/mjdefaults.py create mode 100644 protospec/protospec_gen/mujoco_defaults.json create mode 100644 protospec/protospec_gen/overlay.py create mode 100644 protospec/protospec_gen/overlay_ue.py create mode 100644 protospec/protospec_gen/xmlref.py create mode 100644 protospec/pyproject.toml create mode 100644 protospec/snapshots/corpus_report.json create mode 100644 protospec/tests/fixtures/annotations.spec create mode 100644 protospec/tests/fixtures/arity.spec create mode 100644 protospec/tests/fixtures/cardinality.spec create mode 100644 protospec/tests/fixtures/include_traversal/outside/secret.xml create mode 100644 protospec/tests/fixtures/include_traversal/root/inc_inside.xml create mode 100644 protospec/tests/fixtures/include_traversal/root/uses_inside.xml create mode 100644 protospec/tests/fixtures/include_traversal/root/uses_outside.xml create mode 100644 protospec/tests/fixtures/nesting/deep_199.xml create mode 100644 protospec/tests/fixtures/nesting/deep_201.xml create mode 100644 protospec/tests/fixtures/section5.spec create mode 100644 protospec/tests/fixtures/section5_expected.json create mode 100644 protospec/tests/fixtures/unions.spec create mode 100644 protospec/tests/test_asan.py create mode 100644 protospec/tests/test_corpus_coverage.py create mode 100644 protospec/tests/test_differential.py create mode 100644 protospec/tests/test_emit.py create mode 100644 protospec/tests/test_emit_ue.py create mode 100644 protospec/tests/test_frontend.py create mode 100644 protospec/tests/test_mjdefaults.py create mode 100644 protospec/tests/test_upstream_untouched.py create mode 100644 protospec/tests/test_xmlref.py create mode 100644 protospec/tools/corpus_net.py create mode 100644 protospec/tools/corpus_study.py create mode 100644 protospec/tools/refresh_mj_defaults.py create mode 100644 protospec/tools/run_asan.ps1 create mode 100644 protospec/tools/run_asan.sh create mode 100644 protospec/uv.lock diff --git a/protospec/.gitattributes b/protospec/.gitattributes new file mode 100644 index 00000000..0b947bfb --- /dev/null +++ b/protospec/.gitattributes @@ -0,0 +1,7 @@ +# Collapse machine-produced files in review diffs; they are verified +# mechanically (emit --check / extractor re-runs), not read by humans. +lib/generated/** linguist-generated=true +lib/python/generated/** linguist-generated=true +snapshots/** linguist-generated=true +uv.lock linguist-generated=true +lib/third_party/** linguist-vendored=true linguist-generated=true diff --git a/protospec/.gitignore b/protospec/.gitignore new file mode 100644 index 00000000..7e4e1ee1 --- /dev/null +++ b/protospec/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ +*.pyc +.venv/ +.pytest_cache/ +lib/build*/ +MUJOCO_LOG.TXT diff --git a/protospec/README.md b/protospec/README.md new file mode 100644 index 00000000..e08823e0 --- /dev/null +++ b/protospec/README.md @@ -0,0 +1,156 @@ +# ProtoSpec + +ProtoSpec is an IDL-driven redesign of the MJCF authoring layer. One schema +file describes the model format; a generator emits the C++ object model, +serialization and reflection tables from it. ProtoSpec owns the FILE boundary +only — reading MJCF into a document and writing a document back out. +Everything past that boundary is mjSpec and MuJoCo's own compiler, so +correctness is defined as byte-exact agreement with stock MuJoCo over its own +corpus, enforced by a round-trip differential. + +## How it works + +``` +src/xml/mjcf.schema MuJoCo's own grammar: every element, field, type, + │ default, union and reference relationship + ▼ +protospec_gen/ the generator (pure Python, no deps) + │ emit.py → lib/generated/ C++ types, XML binding tables, + │ reflection, keywords, defaults (11 files) + ▼ +lib/ handwritten library around the generated core +``` + +Generated code is **checked in** and byte-gated: `uv run python -m +protospec_gen.emit --check` fails if `lib/generated/` disagrees with the +schema by a single byte, so drift between schema and code cannot exist. + +The object model is deliberately plain: generated structs of owned values, +every optional field presence-tracked (`std::optional`), references stored by +name with typed wrappers. No hidden compiler state, no pointers into a graph +— a `Model` is a value you can copy, diff, and serialize. + +The layers on top: + +- **`lib/io/`** — MJCF reader/writer (vendored tinyxml2), table-driven by the + generated XML binding, with handwritten quirk handlers for the format's + irregular corners. MuJoCo-free. +- **`lib/core/`** — canonicalization resolvers (orientation and inertia + spellings fold into canonical quat/diaginertia at parse end). MuJoCo-free. +- **`lib/sdk/`** — the emission-profile seam and the default-class query + (below). Its authoring verbs are a test fixture and live under `lib/test/`. + +## The SDK + +`lib/test/protospec/sdk.h` is a pure tree library over the generated types — +it is written once against the reflection/visit hooks and never needs +regenerating when the schema grows. It is a test fixture: the host that ships +ProtoSpec brings its own document profile and its own authoring UI, so only +the profile seam and the default-class query below are shipped surface. + +- `builders.h` — typed `Add*` verbs that insert into the right child list + (`AddBody`, `AddPrimitive`, `AddFreeJoint`, `AddMaterial`, …). +- `traversal.h` — `World`, `Find`, `ForEachOfType`, path-to-element. +- `parents.h` — `ParentMap`, the upward index (shipped: the default-class + query needs it). +- `refs.h` — typed reference handling: `SetRef`, `Resolve`, `FindReferrers`, + `Rename` (referrer-safe), `DeleteRecursive`. +- `classes.h` — defaults-class queries (`Effective`, and the allocation-free + per-field `EffectiveField` / `EffectiveRef`). Shipped surface: an editor + resolves an inherited value while a document is being edited. +- `class_edits.h` — the mutating class transforms (`FlattenDefaults`, + `ExtractClass`). +- `edits.h` — `Duplicate`, `Reparent`. + +### Emission profiles + +The object model above is one *profile*. The SDK and the MJCF reader/writer are +generic over a profile tag `P` (`profile.h`) carrying five policies — `Doc`, +`Str`, `Tree`, `Ref`, `Ident` — so a host with its own storage (presence-wrapped +engine properties, a component hierarchy, its own string type) runs the same +algorithms on its own objects. `plain_profile.h` is the reference profile and +the default for every verb, so a call site that never mentions a profile reads +exactly as it always did. + +A complete load → edit → save round trip (`lib/test/test_public_api.cc` runs +exactly this): + +```cpp +#include "protospec/sdk.h" +namespace mj = ps::mjcf; +namespace sdk = ps::sdk; + +auto parsed = ps::mjcf::io::ParseMjcfString(xml, "hello.xml"); +mj::Model& model = *parsed.model; + +mj::Body& box = sdk::AddBody(sdk::World(model), "box"); +box.pos = std::array{0, 0, 1}; +sdk::AddFreeJoint(box, "box_free"); +mj::Geom& g = sdk::AddPrimitive(box, mj::GeomType::box, "box_geom"); + +mj::Material& mat = sdk::AddMaterial(model, "grid_mat"); +sdk::SetRef(g.material, mat); // typed, name-backed reference + +sdk::Save(model, "hello.xml"); // canonical MJCF back to disk +``` + +## Correctness + +The round-trip differential is the permanent net: parse a corpus model, write +it back, `mj_loadXML` the result, and diff that `mjModel` field-by-field +(every sizes int, name table, and pointer array) against a stock `mj_loadXML` +of the original file. `lib/harness/model_diff_lib.cc` is the comparison core; +`mj_model_diff` is its CLI and `tests/test_differential.py` drives the sweep. + +The claim this suite enforces: **byte-exact vs the enclosing MuJoCo checkout** +over MuJoCo's own model corpus (last verified against main at 3.11.0, +2026-07-22). + +### Running the corpus net + +One entry point, same behaviour on both platforms. It builds `ps_roundtrip` and +`mj_model_diff` against the staged MuJoCo, runs the differential over the +corpus, and exits non-zero on anything but the recorded allowed failures: + +```powershell +# Windows +./corpus_net.ps1 # defaults below +./corpus_net.ps1 -MujocoRoot -Corpus -BuildType Release +``` + +```sh +# Linux +./corpus_net.sh # defaults below +./corpus_net.sh +``` + +Defaults: MuJoCo at `../third_party/install/MuJoCo`, corpus at +`../third_party/MuJoCo/src`, `Release`. Exit 0 the net holds, 1 it does not, +2 the harness could not run. The verdict is `tools/corpus_net.py`, shared by +both scripts: every failure must be one of the four recorded allowed failures +(named, with their diagnoses, at the top of that file) *and* at least +`_PARITY_FLOOR_IDENTICAL` models must have round-tripped identically, so a run +that silently skipped its subject fails rather than passing empty. + +## Building and testing + +Everything runs from this `protospec/` directory. Python tooling uses +[uv](https://docs.astral.sh/uv/); the C++ library is a standalone CMake +project with a MuJoCo-free core. + +```sh +# Generated code matches the schema, byte for byte. +uv run python -m protospec_gen.emit --check + +# C++ core (object model, io, SDK) + unit tests. No MuJoCo needed. +cmake -S lib -B lib/build && cmake --build lib/build -j && ctest --test-dir lib/build + +# Python suite: schema, generator, extractors, differentials. +uv run pytest +``` + +Tests that need MuJoCo *source* (the corpus study under `tools/`) default to +the enclosing checkout and honor `PROTOSPEC_MUJOCO_SRC` as an override. Tests +that need *prebuilt* binaries (the round-trip differential runs `ps_roundtrip` +and `mj_model_diff`) skip when those have not been built, so a plain +`uv run pytest` stays green everywhere. diff --git a/protospec/build.ps1 b/protospec/build.ps1 new file mode 100644 index 00000000..1fddae4e --- /dev/null +++ b/protospec/build.ps1 @@ -0,0 +1,105 @@ +# Build ProtoSpec's static libraries and stage them where URLab.Build.cs looks. +# +# ProtoSpec is URLab's own code, not a vendored dependency, so it builds on its +# own rather than as a step of the MuJoCo build. That separation is the point: +# a change here used to tear down and reconfigure the entire MuJoCo install to +# recompile one file. +# +# What is staged is exactly what URLab links: the object model, the +# canonicalization resolvers, and the profile-independent half of the MJCF +# reader/writer (protospec_mjcf). URLab instantiates the templated halves for +# its own two document profiles, so the plain-profile instantiation +# (protospec_io_plain) is a test fixture here and is neither built nor staged. +# +# The one MuJoCo-dependent target that is built is the comparison harness +# (protospec_harness), which URLab's compile-parity goldens call to diff two +# mjModels field by field. It needs mujoco.h and nothing else that URLab does +# not already link, so the staged MuJoCo install is enough to build it. + +param( + [string]$InstallDir = "../third_party/install", + [string]$MujocoRoot = "../third_party/install/MuJoCo", + [string]$BuildType = "Release" +) + +$ErrorActionPreference = "Stop" + +# Anchored on the script, not the caller's working directory, so it behaves the +# same however it is invoked. +if (-not [System.IO.Path]::IsPathRooted($InstallDir)) { + $InstallDir = Join-Path $PSScriptRoot $InstallDir +} +$InstallRoot = [System.IO.Path]::GetFullPath($InstallDir).Replace('\', '/') +$ProtospecInstallDir = "$InstallRoot/protospec" + +if (-not [System.IO.Path]::IsPathRooted($MujocoRoot)) { + $MujocoRoot = Join-Path $PSScriptRoot $MujocoRoot +} +$MujocoRoot = [System.IO.Path]::GetFullPath($MujocoRoot).Replace('\', '/') +if (-not (Test-Path "$MujocoRoot/include/mujoco/mujoco.h")) { + throw "No MuJoCo headers under $MujocoRoot. Run third_party/build_all.ps1 first, or pass -MujocoRoot." +} + +$Src = Join-Path $PSScriptRoot "lib" +if (-not (Test-Path (Join-Path $Src "CMakeLists.txt"))) { + throw "No ProtoSpec sources at $Src." +} +$Src = [System.IO.Path]::GetFullPath($Src).Replace('\', '/') +$Build = "$Src/build-urlab" + +Write-Host "Resolved install: $ProtospecInstallDir" -ForegroundColor Gray +Write-Host "Configuring ProtoSpec from $Src..." -ForegroundColor Gray +cmake -S $Src -B $Build -DCMAKE_BUILD_TYPE=$BuildType "-DMUJOCO_ROOT=$MujocoRoot" ` + "-DCMAKE_MSVC_RUNTIME_LIBRARY=MultiThreaded$($BuildType.Replace('Release', '').Replace('Debug', 'Debug'))DLL" +if ($LASTEXITCODE -ne 0) { throw "CMake configuration failed for ProtoSpec" } + +Write-Host "Building ProtoSpec..." -ForegroundColor Gray +cmake --build $Build --config $BuildType --target protospec protospec_core protospec_mjcf protospec_harness +if ($LASTEXITCODE -ne 0) { throw "Build failed for ProtoSpec" } + +# Staging is explicit because ProtoSpec's CMake declares no install() rules. +# The lib/ header layout is mirrored rather than flattened: the umbrella headers +# reach the generated tables through relative paths ("../../generated/types.h"), +# which only resolve in the original shape. URLab.Build.cs adds each of these +# directories to the include path, reproducing what ProtoSpec's own CMake +# target_include_directories does. +# +# Staged last, so a failed build leaves the previous install in place rather +# than none at all. +Write-Host "Staging ProtoSpec into $ProtospecInstallDir..." -ForegroundColor Gray +# Named rather than globbed: the build tree also holds the fixture-only archives +# (the plain profile and the SDK authoring layer), which URLab must not link. +# The harness is its own CMake subdirectory, so its archive lands one level +# deeper than the rest; both output directories are searched. +$StagedLibs = @("protospec", "protospec_core", "protospec_mjcf", "tinyxml2", "protospec_harness") +$LibDirs = @("$Build/$BuildType", "$Build/harness/$BuildType") +$BuiltLibs = @($StagedLibs | ForEach-Object { + $name = "$_.lib" + $found = $LibDirs | ForEach-Object { Join-Path $_ $name } | Where-Object { Test-Path $_ } | Select-Object -First 1 + if (-not $found) { throw "ProtoSpec did not build $name under $Build" } + Get-Item $found +}) + +if (Test-Path $ProtospecInstallDir) { Remove-Item -Recurse -Force $ProtospecInstallDir } +$Lib = Join-Path $ProtospecInstallDir "lib" +New-Item -ItemType Directory -Force -Path $Lib | Out-Null + +foreach ($dir in @("include", "sdk", "generated", "core", "io", "harness")) { + $dirSrc = Join-Path $Src $dir + if (-not (Test-Path $dirSrc)) { continue } + $dirSrcFull = [System.IO.Path]::GetFullPath($dirSrc) + Get-ChildItem -Path $dirSrc -Include "*.h", "*.inc" -File -Recurse | ForEach-Object { + $rel = $_.FullName.Substring($dirSrcFull.Length).TrimStart('\', '/') + $dest = Join-Path (Join-Path $ProtospecInstallDir $dir) $rel + New-Item -ItemType Directory -Force -Path (Split-Path -Parent $dest) | Out-Null + Copy-Item -Force $_.FullName $dest + } +} + +$TinyXml = Join-Path $ProtospecInstallDir "third_party/tinyxml2" +New-Item -ItemType Directory -Force -Path $TinyXml | Out-Null +Copy-Item -Force "$Src/third_party/tinyxml2/tinyxml2.h" $TinyXml + +$BuiltLibs | ForEach-Object { Copy-Item -Force $_.FullName $Lib } + +Write-Host "ProtoSpec staged: $($BuiltLibs.Count) libraries, headers under $ProtospecInstallDir" -ForegroundColor Gray diff --git a/protospec/build.sh b/protospec/build.sh new file mode 100644 index 00000000..d288c22e --- /dev/null +++ b/protospec/build.sh @@ -0,0 +1,78 @@ +#!/usr/bin/env bash +# Build ProtoSpec's static libraries and stage them where URLab.Build.cs looks. +# The twin of build.ps1; see it for the rationale. +set -u + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +INSTALL_DIR="${1:-$SCRIPT_DIR/../third_party/install}" +BUILD_TYPE="${2:-Release}" +MUJOCO_ROOT="${3:-$SCRIPT_DIR/../third_party/install/MuJoCo}" + +mkdir -p "$INSTALL_DIR" +INSTALL_ROOT="$(cd "$INSTALL_DIR" && pwd)" +PROTOSPEC_INSTALL_DIR="$INSTALL_ROOT/protospec" + +if [ ! -f "$MUJOCO_ROOT/include/mujoco/mujoco.h" ]; then + echo "No MuJoCo headers under $MUJOCO_ROOT. Run third_party/build_all.sh first, or pass it as the third argument." >&2 + exit 1 +fi +MUJOCO_ROOT="$(cd "$MUJOCO_ROOT" && pwd)" + +SRC="$SCRIPT_DIR/lib" +if [ ! -f "$SRC/CMakeLists.txt" ]; then + echo "No ProtoSpec sources at $SRC." >&2 + exit 1 +fi +SRC="$(cd "$SRC" && pwd)" +BUILD="$SRC/build-urlab" + +echo "Resolved install: $PROTOSPEC_INSTALL_DIR" +echo "Configuring ProtoSpec from $SRC..." +cmake -S "$SRC" -B "$BUILD" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DMUJOCO_ROOT="$MUJOCO_ROOT" +if [ $? -ne 0 ]; then echo "CMake configuration failed for ProtoSpec" >&2; exit 1; fi + +echo "Building ProtoSpec..." +cmake --build "$BUILD" --config "$BUILD_TYPE" \ + --target protospec protospec_core protospec_mjcf protospec_harness +if [ $? -ne 0 ]; then echo "Build failed for ProtoSpec" >&2; exit 1; fi + +# Staging is explicit because ProtoSpec's CMake declares no install() rules. The +# lib/ header layout is mirrored rather than flattened: the umbrella headers +# reach the generated tables through relative paths that only resolve in the +# original shape. URLab.Build.cs adds each of these directories to the include +# path. +# +# Named rather than globbed: the build tree also holds the fixture-only archives +# (the plain profile and the SDK authoring layer), which URLab must not link. +# Depth 2 is what reaches the harness, which is its own CMake subdirectory and +# archives one level down. The previous install is removed only once every +# expected archive is present, so a failure leaves what was there. +STAGED_LIBS=(protospec protospec_core protospec_mjcf tinyxml2 protospec_harness) +FOUND_LIBS=() +for name in "${STAGED_LIBS[@]}"; do + lib="$(find "$BUILD" -maxdepth 2 -name "lib$name.a" -print -quit)" + if [ -z "$lib" ]; then + echo "ProtoSpec did not build lib$name.a under $BUILD" >&2 + exit 1 + fi + FOUND_LIBS+=("$lib") +done + +echo "Staging ProtoSpec into $PROTOSPEC_INSTALL_DIR..." +rm -rf "$PROTOSPEC_INSTALL_DIR" +mkdir -p "$PROTOSPEC_INSTALL_DIR/lib" "$PROTOSPEC_INSTALL_DIR/third_party/tinyxml2" + +for dir in include sdk generated core io harness; do + [ -d "$SRC/$dir" ] || continue + (cd "$SRC/$dir" && find . \( -name "*.h" -o -name "*.inc" \) -print0 | + while IFS= read -r -d '' f; do + mkdir -p "$PROTOSPEC_INSTALL_DIR/$dir/$(dirname "$f")" + cp -f "$f" "$PROTOSPEC_INSTALL_DIR/$dir/$f" + done) +done +cp -f "$SRC/third_party/tinyxml2/tinyxml2.h" "$PROTOSPEC_INSTALL_DIR/third_party/tinyxml2/" + +for lib in "${FOUND_LIBS[@]}"; do cp -f "$lib" "$PROTOSPEC_INSTALL_DIR/lib/"; done +echo "ProtoSpec staged: ${#FOUND_LIBS[@]} libraries, headers under $PROTOSPEC_INSTALL_DIR" diff --git a/protospec/corpus_net.ps1 b/protospec/corpus_net.ps1 new file mode 100644 index 00000000..b761d6b0 --- /dev/null +++ b/protospec/corpus_net.ps1 @@ -0,0 +1,70 @@ +# The corpus net: build the round-trip differential's tools and run it. +# +# One entry point, so the net is a single command in CI and on a developer's +# machine: it configures and builds ps_roundtrip and mj_model_diff against a +# prebuilt MuJoCo, points the harness at the corpus, and exits non-zero on +# anything but the recorded allowed failures. The verdict itself lives in +# tools/corpus_net.py, shared with the Linux twin corpus_net.sh so the two +# cannot drift apart in what they accept. +# +# What it proves: for every model in MuJoCo's own corpus that the retained +# reader supports, parse -> write -> mj_loadXML produces the same mjModel, field +# by field, as a stock mj_loadXML of the original. + +param( + [string]$MujocoRoot = "../third_party/install/MuJoCo", + [string]$Corpus = "../third_party/MuJoCo/src", + [string]$BuildType = "Release" +) + +$ErrorActionPreference = "Stop" + +# Anchored on the script, not the caller's working directory. +function Resolve-Rooted([string]$Path) { + if (-not [System.IO.Path]::IsPathRooted($Path)) { + $Path = Join-Path $PSScriptRoot $Path + } + return [System.IO.Path]::GetFullPath($Path).Replace('\', '/') +} + +$MujocoRoot = Resolve-Rooted $MujocoRoot +$Corpus = Resolve-Rooted $Corpus + +if (-not (Test-Path "$MujocoRoot/include/mujoco/mujoco.h")) { + throw "No MuJoCo headers under $MujocoRoot. Run third_party/build_all.ps1 first, or pass -MujocoRoot." +} +if (-not (Test-Path $Corpus)) { + throw "No MuJoCo corpus at $Corpus. Pass -Corpus, or set it to a MuJoCo source checkout." +} + +$Src = Resolve-Rooted "lib" +$Build = "$Src/build-urlab" + +# The same build directory and the same configure line as build.ps1, so the two +# scripts share a cache instead of invalidating each other's. +Write-Host "Configuring ProtoSpec from $Src..." -ForegroundColor Gray +cmake -S $Src -B $Build -DCMAKE_BUILD_TYPE=$BuildType "-DMUJOCO_ROOT=$MujocoRoot" ` + "-DCMAKE_MSVC_RUNTIME_LIBRARY=MultiThreaded$($BuildType.Replace('Release', '').Replace('Debug', 'Debug'))DLL" +if ($LASTEXITCODE -ne 0) { throw "CMake configuration failed for ProtoSpec" } + +Write-Host "Building the differential tools..." -ForegroundColor Gray +cmake --build $Build --config $BuildType --target ps_roundtrip mj_model_diff +if ($LASTEXITCODE -ne 0) { throw "Build failed for ps_roundtrip / mj_model_diff" } + +Write-Host "Running the corpus net over $Corpus..." -ForegroundColor Gray +$env:PROTOSPEC_CORPUS = $Corpus +Push-Location $PSScriptRoot +try { + uv run python tools/corpus_net.py + $Code = $LASTEXITCODE +} +finally { + Pop-Location +} + +if ($Code -ne 0) { + Write-Host "Corpus net FAILED (exit $Code)" -ForegroundColor Red +} else { + Write-Host "Corpus net passed" -ForegroundColor Green +} +exit $Code diff --git a/protospec/corpus_net.sh b/protospec/corpus_net.sh new file mode 100644 index 00000000..5f89e92e --- /dev/null +++ b/protospec/corpus_net.sh @@ -0,0 +1,57 @@ +#!/usr/bin/env bash +# The corpus net: build the round-trip differential's tools and run it. +# The twin of corpus_net.ps1; see it for the rationale. The verdict is shared: +# both scripts end in tools/corpus_net.py. +set -u + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" + +MUJOCO_ROOT="${1:-$SCRIPT_DIR/../third_party/install/MuJoCo}" +CORPUS="${2:-$SCRIPT_DIR/../third_party/MuJoCo/src}" +BUILD_TYPE="${3:-Release}" + +if [ ! -f "$MUJOCO_ROOT/include/mujoco/mujoco.h" ]; then + echo "No MuJoCo headers under $MUJOCO_ROOT. Run third_party/build_all.sh first, or pass it as the first argument." >&2 + exit 2 +fi +MUJOCO_ROOT="$(cd "$MUJOCO_ROOT" && pwd)" + +if [ ! -d "$CORPUS" ]; then + echo "No MuJoCo corpus at $CORPUS. Pass it as the second argument." >&2 + exit 2 +fi +CORPUS="$(cd "$CORPUS" && pwd)" + +SRC="$SCRIPT_DIR/lib" +BUILD="$SRC/build-urlab" + +# The same build directory and the same configure line as build.sh, so the two +# scripts share a cache instead of invalidating each other's. +echo "Configuring ProtoSpec from $SRC..." +cmake -S "$SRC" -B "$BUILD" -DCMAKE_BUILD_TYPE="$BUILD_TYPE" \ + -DCMAKE_POSITION_INDEPENDENT_CODE=ON -DMUJOCO_ROOT="$MUJOCO_ROOT" +if [ $? -ne 0 ]; then echo "CMake configuration failed for ProtoSpec" >&2; exit 2; fi + +echo "Building the differential tools..." +cmake --build "$BUILD" --config "$BUILD_TYPE" --target ps_roundtrip mj_model_diff +if [ $? -ne 0 ]; then echo "Build failed for ps_roundtrip / mj_model_diff" >&2; exit 2; fi + +# The harness CMake copies the MuJoCo runtime next to mj_model_diff; an ELF +# loader does not look there on its own, so say so. Both the build tree's copy +# and the install's lib/ are named, because a single-config generator and a +# multi-config one put the executable in different places. +DIFF_DIR="$(dirname "$(find "$BUILD" -name mj_model_diff -type f -print -quit)")" +export LD_LIBRARY_PATH="${DIFF_DIR}:${MUJOCO_ROOT}/lib:${LD_LIBRARY_PATH:-}" + +echo "Running the corpus net over $CORPUS..." +export PROTOSPEC_CORPUS="$CORPUS" +cd "$SCRIPT_DIR" || exit 2 +uv run python tools/corpus_net.py +CODE=$? + +if [ $CODE -ne 0 ]; then + echo "Corpus net FAILED (exit $CODE)" >&2 +else + echo "Corpus net passed" +fi +exit $CODE diff --git a/protospec/lib/CMakeLists.txt b/protospec/lib/CMakeLists.txt new file mode 100644 index 00000000..4a4eab51 --- /dev/null +++ b/protospec/lib/CMakeLists.txt @@ -0,0 +1,185 @@ +cmake_minimum_required(VERSION 3.20) +project(protospec CXX) + +set(CMAKE_CXX_STANDARD 20) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# MuJoCo-free static library: the generated object model + handwritten core. +add_library(protospec STATIC + generated/types.cc + generated/reflect.cc + generated/keywords.cc + generated/defaults.cc + generated/xml_binding.cc +) +target_include_directories(protospec PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_SOURCE_DIR}/generated +) + +if(MSVC) + target_compile_options(protospec PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec PRIVATE -Wall -Wextra) +endif() + +# Vendored tinyxml2 (zlib license). Third-party: built with default warnings so +# the project's /W4 does not flag it. Only the MJCF IO libraries link it; the +# core stays dependency-free. +add_library(tinyxml2 STATIC third_party/tinyxml2/tinyxml2.cpp) +target_include_directories(tinyxml2 PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/third_party/tinyxml2 +) + +# Core resolvers: orientation + inertia canonicalization, a MuJoCo-free +# translation unit of lifted math. The reader folds authored orientation and +# inertia spellings into their canonical quat/diaginertia forms at parse end +# through this library, so the MJCF IO stays MuJoCo-free (no mujoco.h, no +# mujoco.dll runtime dependency for ps_roundtrip). +add_library(protospec_core STATIC core/resolve.cc) +target_include_directories(protospec_core PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/core) +target_link_libraries(protospec_core PUBLIC protospec) +if(MSVC) + target_compile_options(protospec_core PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec_core PRIVATE -Wall -Wextra) +endif() + +# MJCF IO, profile-independent half: everything keyed on ElementType, raw XML or +# diagnostics, plus numeric formatting and expansion. This is the half +# URLab links; it instantiates the templated reader and writer for its own two +# document profiles in its own translation units. +add_library(protospec_mjcf STATIC + io/numeric.cc + io/include.cc + io/mjcf_common.cc +) +target_include_directories(protospec_mjcf PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/io + ${CMAKE_CURRENT_SOURCE_DIR}/sdk +) +target_link_libraries(protospec_mjcf PUBLIC protospec protospec_core PRIVATE tinyxml2) +if(MSVC) + target_compile_options(protospec_mjcf PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec_mjcf PRIVATE -Wall -Wextra) +endif() + +# The plain profile: the reader and writer instantiated over the generated value +# types. It is a TEST FIXTURE, not shipped surface -- it exists so the retained +# reader and writer can be exercised over a corpus without booting Unreal. It is +# neither staged nor linked by URLab, which brings its own profiles. +add_library(protospec_io_plain STATIC + io/mjcf_reader_plain.cc + io/mjcf_writer_plain.cc + io/default_classes.cc +) +target_include_directories(protospec_io_plain PUBLIC + ${CMAKE_CURRENT_SOURCE_DIR}/test +) +target_link_libraries(protospec_io_plain PUBLIC protospec_mjcf PRIVATE tinyxml2) +if(MSVC) + # /bigobj: the reader TU's per-element template instantiations exceed the COFF + # section limit without it. + target_compile_options(protospec_io_plain PRIVATE /W4 /permissive- /bigobj) +else() + target_compile_options(protospec_io_plain PRIVATE -Wall -Wextra) +endif() + +# Round-trip tool: the harness contract (ps_roundtrip ). +add_executable(ps_roundtrip tools/ps_roundtrip.cc) +target_link_libraries(ps_roundtrip PRIVATE protospec_io_plain) + +enable_testing() +add_executable(protospec_tests test/test_model.cc) +target_link_libraries(protospec_tests PRIVATE protospec) +if(MSVC) + target_compile_options(protospec_tests PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec_tests PRIVATE -Wall -Wextra) +endif() +add_test(NAME protospec_tests COMMAND protospec_tests) + +add_executable(protospec_io_tests test/test_io.cc) +target_link_libraries(protospec_io_tests PRIVATE protospec_io_plain) +if(MSVC) + target_compile_options(protospec_io_tests PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec_io_tests PRIVATE -Wall -Wextra) +endif() +add_test(NAME protospec_io_tests COMMAND protospec_io_tests) + +# The differential/corpus harness lives in harness/ (owned separately) and needs +# a prebuilt MuJoCo. MUJOCO_ROOT is the same cache variable the harness uses; +# point it at a prebuilt MuJoCo tree (contains include/ and build/, or a staged +# install with lib/ and bin/) to enable it. +set(MUJOCO_ROOT "" + CACHE PATH "Root of a prebuilt MuJoCo (a source build tree, or a staged install)") + +set(MUJOCO_INCLUDE_DIR "${MUJOCO_ROOT}/include") +if(EXISTS "${MUJOCO_ROOT}/build/lib/Release") + set(MUJOCO_LIB_DIR "${MUJOCO_ROOT}/build/lib/Release") +else() + set(MUJOCO_LIB_DIR "${MUJOCO_ROOT}/lib") +endif() +if(WIN32) + set(MUJOCO_IMPORT_LIB "${MUJOCO_LIB_DIR}/mujoco.lib") +else() + set(MUJOCO_IMPORT_LIB "${MUJOCO_LIB_DIR}/libmujoco.so") +endif() + +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/harness/CMakeLists.txt + AND EXISTS "${MUJOCO_INCLUDE_DIR}/mujoco/mujoco.h" + AND EXISTS "${MUJOCO_IMPORT_LIB}") + add_subdirectory(harness) +else() + message(STATUS "protospec: no prebuilt MuJoCo at MUJOCO_ROOT; harness targets skipped") +endif() + +# SDK: the ergonomic authoring layer over the generated object model. Header- +# only (templated over the generated Visit/reflect hooks), MuJoCo-free. The +# query half (profile, detail, model_core, parents, classes) is shipped surface; +# the authoring verbs are fixture-only and live beside the tests they serve. +add_library(protospec_sdk INTERFACE) +target_include_directories(protospec_sdk INTERFACE + ${CMAKE_CURRENT_SOURCE_DIR}/sdk + ${CMAKE_CURRENT_SOURCE_DIR}/test +) +target_link_libraries(protospec_sdk INTERFACE protospec) + +# SDK save surface (protospec/save.h): the one compiled piece of the SDK. It +# reaches to disk and pulls in the MJCF writer, so it is a small static library +# kept separate from the header-only pure-tree SDK. Fixture-only, like the rest +# of the authoring layer. +add_library(protospec_sdk_io STATIC test/save.cc) +target_link_libraries(protospec_sdk_io PUBLIC protospec_sdk protospec_io_plain) +if(MSVC) + target_compile_options(protospec_sdk_io PRIVATE /W4 /permissive-) +else() + target_compile_options(protospec_sdk_io PRIVATE -Wall -Wextra) +endif() + +add_executable(protospec_sdk_tests test/test_sdk.cc) +target_link_libraries(protospec_sdk_tests PRIVATE protospec_sdk protospec_sdk_io protospec_io_plain) +if(MSVC) + # /bigobj: the SDK test TU (heavily templated builders + the added save cases) + # exceeds the COFF section limit without it. + target_compile_options(protospec_sdk_tests PRIVATE /W4 /permissive- /bigobj) +else() + target_compile_options(protospec_sdk_tests PRIVATE -Wall -Wextra) +endif() +add_test(NAME protospec_sdk_tests COMMAND protospec_sdk_tests) + +# Public-surface test: includes ONLY the curated umbrella headers +# (never a generated or io header) and drives load -> edit -> save. Compiling it +# is the guarantee that the surface is self-sufficient. +add_executable(protospec_public_api_tests test/test_public_api.cc) +target_link_libraries(protospec_public_api_tests PRIVATE + protospec_sdk protospec_sdk_io protospec_io_plain) +if(MSVC) + target_compile_options(protospec_public_api_tests PRIVATE /W4 /permissive- /bigobj) +else() + target_compile_options(protospec_public_api_tests PRIVATE -Wall -Wextra) +endif() +add_test(NAME protospec_public_api_tests COMMAND protospec_public_api_tests) diff --git a/protospec/lib/core/resolve.cc b/protospec/lib/core/resolve.cc new file mode 100644 index 00000000..400f9d8f --- /dev/null +++ b/protospec/lib/core/resolve.cc @@ -0,0 +1,312 @@ +// Lifted from MuJoCo (pin mjVERSION_HEADER 3010000, Apache-2.0, (c) +// DeepMind Technologies Limited -- see NOTICE). The numeric kernels below are +// lifted verbatim from the vendored user-layer math pool and registered in +// snapshots/lifted_code.json (ids: resolve_orientation, full_inertia; sources +// user_objects.cc ResolveOrientation and user_util.cc mjuu_fullInertia + its +// mjuu_* dependencies). This module is MuJoCo-free (no mujoco.h) so the reader +// (protospec_mjcf) can canonicalize orientation/inertia at parse end without +// linking MuJoCo. The mjuu_* helpers are a +// self-contained copy of the same functions in attic/compile/lifted/mjuu_util.cc; +// they carry no state and are drift-gated against the upstream originals. +#include "resolve.h" + +#include + +namespace ps::core { +namespace { + +// Constants (verbatim from mjuu_util.h). mjPI matches MuJoCo's mjPI. +constexpr double mjEPS = 1E-14; +constexpr double mjPI = 3.14159265358979323846; + +// --- mjuu_* vector/quaternion/matrix helpers (verbatim, user_util.cc) ------ // +bool mjuu_defined(double num) { return !std::isnan(num); } + +void mjuu_setvec(double* dest, double x, double y, double z, double w) { + dest[0] = x; dest[1] = y; dest[2] = z; dest[3] = w; +} + +template +void mjuu_copyvec(T1* dest, const T2* src, int n) { + for (int i = 0; i < n; i++) dest[i] = (T1)src[i]; +} + +double mjuu_dot3(const double* a, const double* b) { + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2]; +} + +// normalize vector to unit length, return previous length +double mjuu_normvec(double* vec, const int n) { + double nrm = 0; + for (int i = 0; i < n; i++) nrm += vec[i]*vec[i]; + if (nrm < mjEPS) return 0; + nrm = std::sqrt(nrm); + if (std::abs(nrm - 1) > mjEPS) { + for (int i = 0; i < n; i++) vec[i] /= nrm; + } + return nrm; +} + +void mjuu_crossvec(double* a, const double* b, const double* c) { + a[0] = b[1]*c[2] - b[2]*c[1]; + a[1] = b[2]*c[0] - b[0]*c[2]; + a[2] = b[0]*c[1] - b[1]*c[0]; +} + +// convert unit quaternion to 3-by-3 rotation matrix +void mjuu_quat2mat(double* res, const double* quat) { + if (quat[0] == 1 && quat[1] == 0 && quat[2] == 0 && quat[3] == 0) { + res[0] = 1; res[1] = 0; res[2] = 0; + res[3] = 0; res[4] = 1; res[5] = 0; + res[6] = 0; res[7] = 0; res[8] = 1; + return; + } + double q00 = quat[0]*quat[0], q01 = quat[0]*quat[1], q02 = quat[0]*quat[2]; + double q03 = quat[0]*quat[3], q11 = quat[1]*quat[1], q12 = quat[1]*quat[2]; + double q13 = quat[1]*quat[3], q22 = quat[2]*quat[2], q23 = quat[2]*quat[3]; + double q33 = quat[3]*quat[3]; + res[0] = q00 + q11 - q22 - q33; + res[4] = q00 - q11 + q22 - q33; + res[8] = q00 - q11 - q22 + q33; + res[1] = 2*(q12 - q03); res[2] = 2*(q13 + q02); + res[3] = 2*(q12 + q03); res[5] = 2*(q23 - q01); + res[6] = 2*(q13 - q02); res[7] = 2*(q23 + q01); +} + +// multiply two unit quaternions +void mjuu_mulquat(double* res, const double* qa, const double* qb) { + double tmp[4]; + tmp[0] = qa[0]*qb[0] - qa[1]*qb[1] - qa[2]*qb[2] - qa[3]*qb[3]; + tmp[1] = qa[0]*qb[1] + qa[1]*qb[0] + qa[2]*qb[3] - qa[3]*qb[2]; + tmp[2] = qa[0]*qb[2] - qa[1]*qb[3] + qa[2]*qb[0] + qa[3]*qb[1]; + tmp[3] = qa[0]*qb[3] + qa[1]*qb[2] - qa[2]*qb[1] + qa[3]*qb[0]; + mjuu_normvec(tmp, 4); + mjuu_copyvec(res, tmp, 4); +} + +// multiply two matrices, all 3-by-3: res = A * B +void mjuu_mulmat(double* res, const double* A, const double* B) { + double tmp[9]; + tmp[0] = A[0]*B[0] + A[1]*B[3] + A[2]*B[6]; + tmp[1] = A[0]*B[1] + A[1]*B[4] + A[2]*B[7]; + tmp[2] = A[0]*B[2] + A[1]*B[5] + A[2]*B[8]; + tmp[3] = A[3]*B[0] + A[4]*B[3] + A[5]*B[6]; + tmp[4] = A[3]*B[1] + A[4]*B[4] + A[5]*B[7]; + tmp[5] = A[3]*B[2] + A[4]*B[5] + A[5]*B[8]; + tmp[6] = A[6]*B[0] + A[7]*B[3] + A[8]*B[6]; + tmp[7] = A[6]*B[1] + A[7]*B[4] + A[8]*B[7]; + tmp[8] = A[6]*B[2] + A[7]*B[5] + A[8]*B[8]; + mjuu_copyvec(res, tmp, 9); +} + +// transpose 3-by-3 matrix +void mjuu_transposemat(double* res, const double* mat) { + double tmp[9] = {mat[0], mat[3], mat[6], + mat[1], mat[4], mat[7], + mat[2], mat[5], mat[8]}; + mjuu_copyvec(res, tmp, 9); +} + +// compute quaternion as minimal rotation from [0;0;1] to vec +void mjuu_z2quat(double* quat, const double* vec) { + double z[3] = {0, 0, 1}; + (void)z; + mjuu_crossvec(quat+1, z, vec); + double s = mjuu_normvec(quat+1, 3); + if (s < 1E-10) { + quat[1] = 1; + quat[2] = quat[3] = 0; + } + double ang = std::atan2(s, vec[2]); + quat[0] = std::cos(ang/2); + quat[1] *= std::sin(ang/2); + quat[2] *= std::sin(ang/2); + quat[3] *= std::sin(ang/2); +} + +// compute quaternion given frame (axes are in matrix columns) +void mjuu_frame2quat(double* quat, const double* x, const double* y, + const double* z) { + const double* mat[3] = {x, y, z}; // mat[c][r] indexing + if (mat[0][0]+mat[1][1]+mat[2][2] > 0) { + quat[0] = 0.5 * std::sqrt(1 + mat[0][0] + mat[1][1] + mat[2][2]); + quat[1] = 0.25 * (mat[1][2] - mat[2][1]) / quat[0]; + quat[2] = 0.25 * (mat[2][0] - mat[0][2]) / quat[0]; + quat[3] = 0.25 * (mat[0][1] - mat[1][0]) / quat[0]; + } else if (mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2]) { + quat[1] = 0.5 * std::sqrt(1 + mat[0][0] - mat[1][1] - mat[2][2]); + quat[0] = 0.25 * (mat[1][2] - mat[2][1]) / quat[1]; + quat[2] = 0.25 * (mat[1][0] + mat[0][1]) / quat[1]; + quat[3] = 0.25 * (mat[2][0] + mat[0][2]) / quat[1]; + } else if (mat[1][1] > mat[2][2]) { + quat[2] = 0.5 * std::sqrt(1 - mat[0][0] + mat[1][1] - mat[2][2]); + quat[0] = 0.25 * (mat[2][0] - mat[0][2]) / quat[2]; + quat[1] = 0.25 * (mat[1][0] + mat[0][1]) / quat[2]; + quat[3] = 0.25 * (mat[2][1] + mat[1][2]) / quat[2]; + } else { + quat[3] = 0.5 * std::sqrt(1 - mat[0][0] - mat[1][1] + mat[2][2]); + quat[0] = 0.25 * (mat[0][1] - mat[1][0]) / quat[3]; + quat[1] = 0.25 * (mat[2][0] + mat[0][2]) / quat[3]; + quat[2] = 0.25 * (mat[2][1] + mat[1][2]) / quat[3]; + } + mjuu_normvec(quat, 4); +} + +// eigenvalue decomposition of symmetric 3x3 matrix (Jacobi) +constexpr double kEigEPS = 1E-12; +int mjuu_eig3(double eigval[3], double eigvec[9], double quat[4], + const double mat[9]) { + double D[9], tmp[9], tmp2[9]; + double tau, t, c; + int iter, rk, ck, rotk; + + quat[0] = 1; + quat[1] = quat[2] = quat[3] = 0; + + for (iter = 0; iter < 500; iter++) { + // make quaternion matrix eigvec, compute D = eigvec'*mat*eigvec + mjuu_quat2mat(eigvec, quat); + mjuu_transposemat(tmp2, eigvec); + mjuu_mulmat(tmp, tmp2, mat); + mjuu_mulmat(D, tmp, eigvec); + + eigval[0] = D[0]; + eigval[1] = D[4]; + eigval[2] = D[8]; + + if (std::abs(D[1]) > std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) { + rk = 0; ck = 1; rotk = 2; + } else if (std::abs(D[2]) > std::abs(D[5])) { + rk = 0; ck = 2; rotk = 1; + } else { + rk = 1; ck = 2; rotk = 0; + } + + if (std::abs(D[3*rk+ck]) < kEigEPS) break; + + tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]); + if (tau >= 0) { + t = 1.0/(tau + std::sqrt(1 + tau*tau)); + } else { + t = -1.0/(-tau + std::sqrt(1 + tau*tau)); + } + c = 1.0/std::sqrt(1 + t*t); + + if (c > 1.0-kEigEPS) break; + + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[rotk+1] = (tau >= 0 ? -std::sqrt(0.5-0.5*c) : std::sqrt(0.5-0.5*c)); + if (rotk == 1) tmp[rotk+1] = -tmp[rotk+1]; + tmp[0] = std::sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); + mjuu_normvec(tmp, 4); + + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + + // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) + for (int j = 0; j < 3; j++) { + int j1 = j%2; + if (eigval[j1]+kEigEPS < eigval[j1+1]) { + t = eigval[j1]; + eigval[j1] = eigval[j1+1]; + eigval[j1+1] = t; + tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[(j1+2)%3+1] = tmp[0]; + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + } + + mjuu_quat2mat(eigvec, quat); + return iter; +} + +} // namespace + +// --- Public resolvers ------------------------------------------------------ // +// Lifted from ResolveOrientation (user_objects.cc:241-349): the five authored +// forms + degree + eulerseq, over raw authored values instead of an mjsOrientation. +std::array ResolveOrientation(OrientKind kind, const double* raw, + const OrientContext& ctx) { + double quat[4] = {1, 0, 0, 0}; + const bool degree = ctx.degree; + + switch (kind) { + case OrientKind::Quat: { + quat[0] = raw[0]; quat[1] = raw[1]; quat[2] = raw[2]; quat[3] = raw[3]; + mjuu_normvec(quat, 4); + break; + } + case OrientKind::AxisAngle: { + double ax[4] = {raw[0], raw[1], raw[2], raw[3]}; + if (degree) ax[3] = ax[3] / 180.0 * mjPI; + if (mjuu_normvec(ax, 3) < mjEPS) break; + double ang2 = ax[3] / 2; + quat[0] = std::cos(ang2); quat[1] = std::sin(ang2) * ax[0]; + quat[2] = std::sin(ang2) * ax[1]; quat[3] = std::sin(ang2) * ax[2]; + break; + } + case OrientKind::XYAxes: { + double a[6]; for (int k = 0; k < 6; ++k) a[k] = raw[k]; + if (mjuu_normvec(a, 3) < mjEPS) break; + double d = mjuu_dot3(a, a + 3); + a[3] -= a[0] * d; a[4] -= a[1] * d; a[5] -= a[2] * d; + if (mjuu_normvec(a + 3, 3) < mjEPS) break; + double z[3]; + mjuu_crossvec(z, a, a + 3); + if (mjuu_normvec(z, 3) < mjEPS) break; + mjuu_frame2quat(quat, a, a + 3, z); + break; + } + case OrientKind::ZAxis: { + double z[3] = {raw[0], raw[1], raw[2]}; + if (mjuu_normvec(z, 3) < mjEPS) break; + mjuu_z2quat(quat, z); + break; + } + case OrientKind::Euler: { + double e[3] = {raw[0], raw[1], raw[2]}; + if (degree) for (int i = 0; i < 3; ++i) e[i] = e[i] / 180.0 * mjPI; + mjuu_setvec(quat, 1, 0, 0, 0); + const std::string& seq = ctx.eulerseq; + for (int i = 0; i < 3 && i < static_cast(seq.size()); ++i) { + double tmp[4], qrot[4] = {std::cos(e[i] / 2), 0, 0, 0}; + double sa = std::sin(e[i] / 2); + char ch = seq[i]; + if (ch == 'x' || ch == 'X') qrot[1] = sa; + else if (ch == 'y' || ch == 'Y') qrot[2] = sa; + else if (ch == 'z' || ch == 'Z') qrot[3] = sa; + if (ch == 'x' || ch == 'y' || ch == 'z') mjuu_mulquat(tmp, quat, qrot); + else mjuu_mulquat(tmp, qrot, quat); + mjuu_copyvec(quat, tmp, 4); + } + mjuu_normvec(quat, 4); + break; + } + } + return {quat[0], quat[1], quat[2], quat[3]}; +} + +// Lifted from mjuu_fullInertia (user_util.cc:872-901). +const char* FullInertiaToDiag(const double fullinertia[6], double diag[3], + double quat[4]) { + if (!mjuu_defined(fullinertia[0])) return nullptr; + + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { + fullinertia[0], fullinertia[3], fullinertia[4], + fullinertia[3], fullinertia[1], fullinertia[5], + fullinertia[4], fullinertia[5], fullinertia[2] + }; + mjuu_eig3(eigval, eigvec, quattmp, full); + + if (eigval[2] < mjEPS) return "inertia must have positive eigenvalues"; + + if (quat) mjuu_copyvec(quat, quattmp, 4); + if (diag) mjuu_copyvec(diag, eigval, 3); + return nullptr; +} + +} // namespace ps::core diff --git a/protospec/lib/core/resolve.h b/protospec/lib/core/resolve.h new file mode 100644 index 00000000..f3998a79 --- /dev/null +++ b/protospec/lib/core/resolve.h @@ -0,0 +1,48 @@ +// Orientation + inertia canonicalization resolvers. +// +// ProtoSpec stores orientation as a single canonical unit quaternion and inertia +// as diaginertia + iquat (docs/plan_canonicalization.md, Wave A). The MJCF reader +// accepts every authored spelling (quat/euler/axisangle/xyaxes/zaxis; +// diaginertia/fullinertia) and folds it here, at parse end, against the effective +// compiler context. Placing the fold in this MuJoCo-free module keeps the +// reader (protospec_mjcf) MuJoCo-free while sharing the exact math MuJoCo +// compiles: the resolvers are lifted verbatim from the vendored +// tree (see resolve.cc for provenance and snapshots/lifted_code.json). +#ifndef PROTOSPEC_CORE_RESOLVE_H +#define PROTOSPEC_CORE_RESOLVE_H + +#include +#include + +namespace ps::core { + +// The compiler context an orientation fold consumes. Folded document-order +// independently from Model.compilers (later authored attributes win), matching +// MuJoCo's accumulate-into-one-spec behavior. Defaults: degrees, eulerseq "xyz". +struct OrientContext { + bool degree = true; // compiler.angle == "degree" + std::string eulerseq = "xyz"; // compiler.eulerseq (per-character intrinsic/ + // extrinsic, lower/upper case) +}; + +// Which MJCF orientation spelling was authored. +enum class OrientKind { Quat, AxisAngle, XYAxes, ZAxis, Euler }; + +// Fold an authored orientation spelling into a unit quaternion (w, x, y, z), +// lifted from ResolveOrientation (user_objects.cc). `raw` holds the authored +// numbers for the kind: Quat -> [w,x,y,z]; AxisAngle -> [ax,ay,az,angle]; +// XYAxes -> [x0,x1,x2,y0,y1,y2]; ZAxis -> [z0,z1,z2]; Euler -> [e0,e1,e2]. +// A degenerate input yields the identity quaternion, exactly as MuJoCo does. +std::array ResolveOrientation(OrientKind kind, const double* raw, + const OrientContext& ctx); + +// Eigendecompose a symmetric full inertia matrix (fullinertia = [xx,yy,zz,xy, +// xz,yz]) into principal moments (diag, descending) and the quaternion of its +// principal frame, lifted from mjuu_fullInertia (user_util.cc). Returns nullptr +// on success, or a MuJoCo-verbatim error string for a non-positive inertia. +const char* FullInertiaToDiag(const double fullinertia[6], double diag[3], + double quat[4]); + +} // namespace ps::core + +#endif // PROTOSPEC_CORE_RESOLVE_H diff --git a/protospec/lib/generated/defaults.cc b/protospec/lib/generated/defaults.cc new file mode 100644 index 00000000..209edc7b --- /dev/null +++ b/protospec/lib/generated/defaults.cc @@ -0,0 +1,1333 @@ +// Generated by protospec_gen.emit — do not edit. +#include "defaults.h" + +namespace ps::mjcf { + +void ApplyDefault(Model& e) { + (void)e; +} + +void ApplyDefault(Compiler& e) { + e.autolimits = true; + e.boundmass = 0.0; + e.boundinertia = 0.0; + e.settotalmass = -1.0; + e.balanceinertia = false; + e.angle = AngleUnit::degree; + e.fitaabb = false; + e.eulerseq = std::string("xyz"); + e.discardvisual = false; + e.usethread = true; + e.fusestatic = false; + e.inertiafromgeom = TriState::auto_; + e.inertiagrouprange = std::array{{0, 5}}; + e.saveinertial = false; + e.alignfree = false; + e.conflict = Conflict::warning; +} + +void ApplyDefault(LengthRange& e) { + e.mode = LRMode::muscle; + e.useexisting = true; + e.uselimit = false; + e.accel = 20.0; + e.maxforce = 0.0; + e.timeconst = 1.0; + e.timestep = 0.01; + e.inttotal = 10.0; + e.interval = 2.0; + e.tolrange = 0.05; +} + +void ApplyDefault(Option& e) { + e.timestep = 0.002; + e.impratio = 1.0; + e.tolerance = 1e-08; + e.ls_tolerance = 0.01; + e.noslip_tolerance = 1e-06; + e.ccd_tolerance = 1e-06; + e.sleep_tolerance = 0.001; + e.gravity = std::array{{0.0, 0.0, -9.81}}; + e.wind = std::array{{0.0, 0.0, 0.0}}; + e.magnetic = std::array{{0.0, -0.5, 0.0}}; + e.density = 0.0; + e.viscosity = 0.0; + e.o_margin = 0.0; + e.o_solref = ps::InlineVec{0.02, 1.0}; + e.o_solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.o_friction = ps::InlineVec{1.0, 1.0, 0.005, 0.0001, 0.0001}; + e.integrator = Integrator::Euler; + e.cone = Cone::pyramidal; + e.jacobian = JacobianType::auto_; + e.solver = SolverType::Newton; + e.iterations = 100; + e.ls_iterations = 50; + e.noslip_iterations = 0; + e.ccd_iterations = 35; + e.sdf_iterations = 10; + e.sdf_initpoints = 40; +} + +void ApplyDefault(Flag& e) { + e.constraint = Enable::enable; + e.equality = Enable::enable; + e.frictionloss = Enable::enable; + e.limit = Enable::enable; + e.contact = Enable::enable; + e.spring = Enable::enable; + e.damper = Enable::enable; + e.gravity = Enable::enable; + e.clampctrl = Enable::enable; + e.warmstart = Enable::enable; + e.filterparent = Enable::enable; + e.actuation = Enable::enable; + e.refsafe = Enable::enable; + e.sensor = Enable::enable; + e.midphase = Enable::enable; + e.eulerdamp = Enable::enable; + e.autoreset = Enable::enable; + e.nativeccd = Enable::enable; + e.island = Enable::enable; + e.multiccd = Enable::enable; + e.override_ = Enable::disable; + e.energy = Enable::disable; + e.fwdinv = Enable::disable; + e.invdiscrete = Enable::disable; + e.sleep = Enable::disable; + e.diagexact = Enable::disable; +} + +void ApplyDefault(Size& e) { + e.njmax = -1; + e.nconmax = -1; + e.nstack = -1; + e.nuserdata = 0; + e.nkey = 0; + e.nuser_body = -1; + e.nuser_jnt = -1; + e.nuser_geom = -1; + e.nuser_site = -1; + e.nuser_cam = -1; + e.nuser_tendon = -1; + e.nuser_actuator = -1; + e.nuser_sensor = -1; +} + +void ApplyDefault(Statistic& e) { + (void)e; +} + +void ApplyDefault(Visual& e) { + (void)e; +} + +void ApplyDefault(VisualGlobal& e) { + e.cameraid = -1; + e.orthographic = false; + e.fovy = 45.0f; + e.ipd = 0.068f; + e.azimuth = 90.0f; + e.elevation = -45.0f; + e.linewidth = 1.0f; + e.glow = 0.3f; + e.offwidth = 640; + e.offheight = 480; + e.realtime = 1.0f; + e.ellipsoidinertia = false; + e.bvactive = true; +} + +void ApplyDefault(VisualQuality& e) { + e.shadowsize = 4096; + e.offsamples = 4; + e.numslices = 28; + e.numstacks = 16; + e.numquads = 4; +} + +void ApplyDefault(VisualHeadlight& e) { + e.ambient = std::array{{0.1f, 0.1f, 0.1f}}; + e.diffuse = std::array{{0.4f, 0.4f, 0.4f}}; + e.specular = std::array{{0.5f, 0.5f, 0.5f}}; + e.active = 1; +} + +void ApplyDefault(VisualMap& e) { + e.stiffness = 100.0f; + e.stiffnessrot = 500.0f; + e.force = 0.005f; + e.torque = 0.1f; + e.alpha = 0.3f; + e.fogstart = 3.0f; + e.fogend = 10.0f; + e.znear = 0.01f; + e.zfar = 50.0f; + e.haze = 0.3f; + e.shadowclip = 1.0f; + e.shadowscale = 0.6f; + e.actuatortendon = 2.0f; +} + +void ApplyDefault(VisualScale& e) { + e.forcewidth = 0.1f; + e.contactwidth = 0.3f; + e.contactheight = 0.1f; + e.connect = 0.2f; + e.com = 0.4f; + e.camera = 0.3f; + e.light = 0.3f; + e.selectpoint = 0.2f; + e.jointlength = 1.0f; + e.jointwidth = 0.1f; + e.actuatorlength = 0.7f; + e.actuatorwidth = 0.2f; + e.framelength = 1.0f; + e.framewidth = 0.1f; + e.constraint = 0.1f; + e.slidercrank = 0.2f; + e.frustum = 10.0f; +} + +void ApplyDefault(VisualRgba& e) { + e.fog = std::array{{0.0f, 0.0f, 0.0f, 1.0f}}; + e.haze = std::array{{1.0f, 1.0f, 1.0f, 1.0f}}; + e.force = std::array{{1.0f, 0.5f, 0.5f, 1.0f}}; + e.inertia = std::array{{0.800000011920929f, 0.20000000298023224f, 0.20000000298023224f, 0.6000000238418579f}}; + e.joint = std::array{{0.20000000298023224f, 0.6000000238418579f, 0.800000011920929f, 1.0f}}; + e.actuator = std::array{{0.20000000298023224f, 0.25f, 0.20000000298023224f, 1.0f}}; + e.actuatornegative = std::array{{0.20000000298023224f, 0.6000000238418579f, 0.8999999761581421f, 1.0f}}; + e.actuatorpositive = std::array{{0.8999999761581421f, 0.4000000059604645f, 0.20000000298023224f, 1.0f}}; + e.com = std::array{{0.8999999761581421f, 0.8999999761581421f, 0.8999999761581421f, 1.0f}}; + e.camera = std::array{{0.6000000238418579f, 0.8999999761581421f, 0.6000000238418579f, 1.0f}}; + e.light = std::array{{0.6000000238418579f, 0.6000000238418579f, 0.8999999761581421f, 1.0f}}; + e.selectpoint = std::array{{0.8999999761581421f, 0.8999999761581421f, 0.10000000149011612f, 1.0f}}; + e.connect = std::array{{0.20000000298023224f, 0.20000000298023224f, 0.800000011920929f, 1.0f}}; + e.contactpoint = std::array{{0.8999999761581421f, 0.6000000238418579f, 0.20000000298023224f, 1.0f}}; + e.contactforce = std::array{{0.699999988079071f, 0.8999999761581421f, 0.8999999761581421f, 1.0f}}; + e.contactfriction = std::array{{0.8999999761581421f, 0.800000011920929f, 0.4000000059604645f, 1.0f}}; + e.contacttorque = std::array{{0.8999999761581421f, 0.699999988079071f, 0.8999999761581421f, 1.0f}}; + e.contactgap = std::array{{0.5f, 0.800000011920929f, 0.8999999761581421f, 1.0f}}; + e.rangefinder = std::array{{1.0f, 1.0f, 0.10000000149011612f, 1.0f}}; + e.constraint = std::array{{0.8999999761581421f, 0.0f, 0.0f, 1.0f}}; + e.slidercrank = std::array{{0.5f, 0.30000001192092896f, 0.800000011920929f, 1.0f}}; + e.crankbroken = std::array{{0.8999999761581421f, 0.0f, 0.0f, 1.0f}}; + e.frustum = std::array{{1.0f, 1.0f, 0.0f, 0.20000000298023224f}}; + e.bv = std::array{{0.0f, 1.0f, 0.0f, 0.5f}}; + e.bvactive = std::array{{1.0f, 0.0f, 0.0f, 0.5f}}; +} + +void ApplyDefault(Default& e) { + (void)e; +} + +void ApplyDefault(MaterialLayer& e) { + (void)e; +} + +void ApplyDefault(Extension& e) { + (void)e; +} + +void ApplyDefault(PluginDef& e) { + (void)e; +} + +void ApplyDefault(PluginInstance& e) { + (void)e; +} + +void ApplyDefault(Config& e) { + (void)e; +} + +void ApplyDefault(Asset& e) { + (void)e; +} + +void ApplyDefault(Mesh& e) { + e.refpos = std::array{{0.0, 0.0, 0.0}}; + e.refquat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.scale = std::array{{1.0, 1.0, 1.0}}; + e.smoothnormal = false; + e.maxhullvert = -1; + e.inertia = MeshInertia::legacy; +} + +void ApplyDefault(PluginRef& e) { + (void)e; +} + +void ApplyDefault(Hfield& e) { + e.nrow = 0; + e.ncol = 0; + e.size = std::array{{0.0, 0.0, 0.0, 0.0}}; +} + +void ApplyDefault(Skin& e) { + e.rgba = std::array{{0.5f, 0.5f, 0.5f, 1.0f}}; + e.inflate = 0.0f; + e.group = 0; +} + +void ApplyDefault(SkinBone& e) { + (void)e; +} + +void ApplyDefault(Texture& e) { + e.type = TextureType::cube; + e.colorspace = ColorSpace::auto_; + e.gridsize = std::array{{1, 1}}; + e.gridlayout = std::string("............"); + e.builtin = TextureBuiltin::none; + e.rgb1 = std::array{{0.8, 0.8, 0.8}}; + e.rgb2 = std::array{{0.5, 0.5, 0.5}}; + e.mark = TextureMark::none; + e.markrgb = std::array{{0.0, 0.0, 0.0}}; + e.random = 0.01; + e.width = 0; + e.height = 0; + e.hflip = false; + e.vflip = false; + e.nchannel = 3; +} + +void ApplyDefault(Material& e) { + e.texrepeat = std::array{{1.0f, 1.0f}}; + e.texuniform = false; + e.emission = 0.0f; + e.specular = 0.5f; + e.shininess = 0.5f; + e.reflectance = 0.0f; + e.metallic = -1.0f; + e.roughness = -1.0f; + e.rgba = std::array{{1.0f, 1.0f, 1.0f, 1.0f}}; +} + +void ApplyDefault(ModelAsset& e) { + (void)e; +} + +void ApplyDefault(Body& e) { + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.mocap = false; + e.gravcomp = 0.0; + e.sleep = BodySleep::auto_; + e.simple = SimpleMode::auto_; +} + +void ApplyDefault(Inertial& e) { + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.mass = 0.0; + e.diaginertia = std::array{{0.0, 0.0, 0.0}}; +} + +void ApplyDefault(Joint& e) { + e.type = JointType::hinge; + e.group = 0; + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.axis = std::array{{0.0, 0.0, 1.0}}; + e.springdamper = std::array{{0.0, 0.0}}; + e.limited = TriState::auto_; + e.actuatorfrclimited = TriState::auto_; + e.solreflimit = ps::InlineVec{0.02, 1.0}; + e.solimplimit = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.solreffriction = ps::InlineVec{0.02, 1.0}; + e.solimpfriction = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.stiffness = ps::InlineVec{0.0, 0.0, 0.0}; + e.range = std::array{{0.0, 0.0}}; + e.actuatorfrcrange = std::array{{0.0, 0.0}}; + e.actuatorgravcomp = false; + e.margin = 0.0; + e.ref = 0.0; + e.springref = 0.0; + e.armature = 0.0; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.frictionloss = 0.0; +} + +void ApplyDefault(FreeJoint& e) { + e.group = 0; + e.align = TriState::auto_; +} + +void ApplyDefault(Geom& e) { + e.type = GeomType::sphere; + e.contype = 1; + e.conaffinity = 1; + e.condim = 3; + e.group = 0; + e.priority = 0; + e.size = ps::InlineVec{0.0, 0.0, 0.0}; + e.friction = ps::InlineVec{1.0, 0.005, 0.0001}; + e.density = 1000.0; + e.shellinertia = false; + e.solmix = 1.0; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.margin = 0.0; + e.gap = 0.0; + e.surfacevel = ps::InlineVec{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.adhesion = 0.0; + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.fitscale = 1.0; + e.rgba = std::array{{0.5f, 0.5f, 0.5f, 1.0f}}; + e.fluidshape = FluidShape::none; + e.fluidcoef = ps::InlineVec{0.5, 0.25, 1.5, 1.0, 1.0}; +} + +void ApplyDefault(Attach& e) { + (void)e; +} + +void ApplyDefault(Site& e) { + e.type = GeomType::sphere; + e.group = 0; + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.size = ps::InlineVec{0.005, 0.005, 0.005}; + e.rgba = std::array{{0.5f, 0.5f, 0.5f, 1.0f}}; +} + +void ApplyDefault(Camera& e) { + e.projection = CameraProjection::perspective; + e.fovy = 45.0; + e.ipd = 0.068; + e.resolution = std::array{{1, 1}}; + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; + e.mode = CamLightMode::fixed; + e.focal = std::array{{0.0f, 0.0f}}; + e.focalpixel = std::array{{0.0f, 0.0f}}; + e.principal = std::array{{0.0f, 0.0f}}; + e.principalpixel = std::array{{0.0f, 0.0f}}; + e.sensorsize = std::array{{0.0f, 0.0f}}; +} + +void ApplyDefault(Light& e) { + e.type = LightType::spot; + e.castshadow = true; + e.active = true; + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.dir = std::array{{0.0, 0.0, -1.0}}; + e.bulbradius = 0.02f; + e.intensity = 0.0f; + e.range = 10.0f; + e.attenuation = std::array{{1.0f, 0.0f, 0.0f}}; + e.cutoff = 45.0f; + e.exponent = 10.0f; + e.ambient = std::array{{0.0f, 0.0f, 0.0f}}; + e.diffuse = std::array{{0.7f, 0.7f, 0.7f}}; + e.specular = std::array{{0.3f, 0.3f, 0.3f}}; + e.mode = CamLightMode::fixed; +} + +void ApplyDefault(Composite& e) { + (void)e; +} + +void ApplyDefault(CompositeJoint& e) { + (void)e; +} + +void ApplyDefault(CompositeSkin& e) { + (void)e; +} + +void ApplyDefault(CompositeGeom& e) { + (void)e; +} + +void ApplyDefault(CompositeSite& e) { + (void)e; +} + +void ApplyDefault(Flexcomp& e) { + (void)e; +} + +void ApplyDefault(FlexcompEdge& e) { + (void)e; +} + +void ApplyDefault(FlexElasticity& e) { + e.young = 0.0; + e.poisson = 0.0; + e.damping = 0.0; + e.thickness = -1.0; + e.elastic2d = Elastic2D::none; +} + +void ApplyDefault(FlexContact& e) { + e.contype = 1; + e.conaffinity = 1; + e.condim = 3; + e.priority = 0; + e.friction = ps::InlineVec{1.0, 0.005, 0.0001}; + e.solmix = 1.0; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.margin = 0.0; + e.gap = 0.0; + e.internal = false; + e.selfcollide = FlexSelfCollide::auto_; + e.activelayers = 1; + e.passive = false; +} + +void ApplyDefault(FlexcompPin& e) { + (void)e; +} + +void ApplyDefault(Deformable& e) { + (void)e; +} + +void ApplyDefault(Flex& e) { + e.group = 0; + e.dim = 2; + e.radius = 0.005; + e.rgba = std::array{{0.5f, 0.5f, 0.5f, 1.0f}}; + e.flatskin = false; + e.cellcount = std::array{{1, 1, 1}}; +} + +void ApplyDefault(FlexEdge& e) { + e.stiffness = 0.0; + e.damping = 0.0; +} + +void ApplyDefault(Contact& e) { + (void)e; +} + +void ApplyDefault(Pair& e) { + e.condim = 3; + e.friction = ps::InlineVec{1.0, 1.0, 0.005, 0.0001, 0.0001}; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solreffriction = ps::InlineVec{0.0, 0.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.gap = 0.0; + e.margin = 0.0; + e.adhesion = 0.0; +} + +void ApplyDefault(Exclude& e) { + (void)e; +} + +void ApplyDefault(Tendon& e) { + (void)e; +} + +void ApplyDefault(Spatial& e) { + e.group = 0; + e.limited = TriState::auto_; + e.actuatorfrclimited = TriState::false_; + e.range = std::array{{0.0, 0.0}}; + e.actuatorfrcrange = std::array{{0.0, 0.0}}; + e.solreflimit = ps::InlineVec{0.02, 1.0}; + e.solimplimit = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.solreffriction = ps::InlineVec{0.02, 1.0}; + e.solimpfriction = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.frictionloss = 0.0; + e.springlength = ps::InlineVec{-1.0, -1.0}; + e.width = 0.003; + e.margin = 0.0; + e.stiffness = ps::InlineVec{0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; + e.rgba = std::array{{0.5f, 0.5f, 0.5f, 1.0f}}; +} + +void ApplyDefault(SpatialSite& e) { + (void)e; +} + +void ApplyDefault(SpatialGeom& e) { + (void)e; +} + +void ApplyDefault(Pulley& e) { + (void)e; +} + +void ApplyDefault(Fixed& e) { + e.group = 0; + e.limited = TriState::auto_; + e.actuatorfrclimited = TriState::false_; + e.range = std::array{{0.0, 0.0}}; + e.actuatorfrcrange = std::array{{0.0, 0.0}}; + e.solreflimit = ps::InlineVec{0.02, 1.0}; + e.solimplimit = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.solreffriction = ps::InlineVec{0.02, 1.0}; + e.solimpfriction = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.frictionloss = 0.0; + e.springlength = ps::InlineVec{-1.0, -1.0}; + e.margin = 0.0; + e.stiffness = ps::InlineVec{0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(FixedJoint& e) { + (void)e; +} + +void ApplyDefault(Equality& e) { + (void)e; +} + +void ApplyDefault(Connect& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(Weld& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(EqualityJoint& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(EqualityTendon& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(EqualityFlex& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(Flexvert& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(Flexstrain& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(Actuator& e) { + (void)e; +} + +void ApplyDefault(ActuatorGeneral& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.actlimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.actrange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; + e.actdim = -1; + e.velrange = std::array{{0.0, 0.0}}; + e.ffrange = std::array{{0.0, 0.0}}; + e.dyntype = DynType::none; + e.gaintype = GainType::fixed; + e.biastype = BiasType::none; + e.dynprm = ps::InlineVec{1.0}; + e.gainprm = ps::InlineVec{1.0}; + e.biasprm = ps::InlineVec{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.actearly = false; +} + +void ApplyDefault(Motor& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Position& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.inheritrange = 0.0; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Velocity& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(IntVelocity& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.actlimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.actrange = std::array{{0.0, 0.0}}; + e.inheritrange = 0.0; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(OrientationActuator& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; +} + +void ApplyDefault(Pid& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.posrange = std::array{{0.0, 0.0}}; + e.velrange = std::array{{0.0, 0.0}}; + e.ffrange = std::array{{0.0, 0.0}}; + e.forcerange = std::array{{0.0, 0.0}}; + e.inheritrange = 0.0; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Damper& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Cylinder& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Muscle& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(Adhesion& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.forcelimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; +} + +void ApplyDefault(DcMotor& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; +} + +void ApplyDefault(ActuatorPlugin& e) { + e.group = 0; + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.ctrlrange = std::array{{0.0, 0.0}}; + e.ctrllimited = TriState::auto_; + e.forcelimited = TriState::auto_; + e.actlimited = TriState::auto_; + e.forcerange = std::array{{0.0, 0.0}}; + e.actrange = std::array{{0.0, 0.0}}; + e.lengthrange = std::array{{0.0, 0.0}}; + e.gear = ps::InlineVec{1.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + e.damping = ps::InlineVec{0.0, 0.0, 0.0}; + e.armature = 0.0; + e.actdim = -1; + e.dyntype = DynType::none; + e.dynprm = ps::InlineVec{1.0}; + e.actearly = false; +} + +void ApplyDefault(Sensor& e) { + (void)e; +} + +void ApplyDefault(Touch& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Accelerometer& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Velocimeter& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Gyro& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Force& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Torque& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Magnetometer& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Camprojection& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Rangefinder& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointpos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonpos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Actuatorpos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Actuatorvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Actuatorfrc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointactuatorfrc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonactuatorfrc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Ballquat& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Ballangvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointlimitpos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointlimitvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Jointlimitfrc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonlimitpos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonlimitvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tendonlimitfrc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framepos& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framequat& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framexaxis& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Frameyaxis& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framezaxis& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framelinvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Frameangvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Framelinacc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Frameangacc& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Subtreecom& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Subtreelinvel& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Subtreeangmom& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Insidesite& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Distance& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Normal& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Fromto& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(SensorContact& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(EPotential& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(EKinetic& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Clock& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(Tactile& e) { + e.nsample = 0; + e.interp = InterpType::zoh; + e.delay = 0.0; + e.interval = ps::InlineVec{0.0, 0.0}; +} + +void ApplyDefault(SensorUser& e) { + e.datatype = DataType::real; + e.needstage = NeedStage::acc; + e.dim = 0; + e.cutoff = 0.0; + e.noise = 0.0; +} + +void ApplyDefault(SensorPlugin& e) { + e.cutoff = 0.0; +} + +void ApplyDefault(Custom& e) { + (void)e; +} + +void ApplyDefault(Numeric& e) { + e.size = 0; +} + +void ApplyDefault(Text& e) { + (void)e; +} + +void ApplyDefault(Tuple& e) { + (void)e; +} + +void ApplyDefault(TupleElement& e) { + (void)e; +} + +void ApplyDefault(Keyframe& e) { + (void)e; +} + +void ApplyDefault(Key& e) { + e.time = 0.0; +} + +void ApplyDefault(Frame& e) { + e.pos = std::array{{0.0, 0.0, 0.0}}; + e.quat = std::array{{1.0, 0.0, 0.0, 0.0}}; +} + +void ApplyDefault(Replicate& e) { + (void)e; +} + +void ApplyDefault(EqualityDefault& e) { + e.active = true; + e.solref = ps::InlineVec{0.02, 1.0}; + e.solimp = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; +} + +void ApplyDefault(TendonDefault& e) { + e.group = 0; + e.limited = TriState::auto_; + e.range = std::array{{0.0, 0.0}}; + e.solreflimit = ps::InlineVec{0.02, 1.0}; + e.solimplimit = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.solreffriction = ps::InlineVec{0.02, 1.0}; + e.solimpfriction = ps::InlineVec{0.9, 0.95, 0.001, 0.5, 2.0}; + e.frictionloss = 0.0; + e.springlength = ps::InlineVec{-1.0, -1.0}; + e.width = 0.003; + e.margin = 0.0; + e.stiffness = 0.0; + e.damping = 0.0; + e.rgba = std::array{{0.5, 0.5, 0.5, 1.0}}; +} + +} // namespace ps::mjcf diff --git a/protospec/lib/generated/defaults.h b/protospec/lib/generated/defaults.h new file mode 100644 index 00000000..2cca9770 --- /dev/null +++ b/protospec/lib/generated/defaults.h @@ -0,0 +1,161 @@ +// Generated by protospec_gen.emit — do not edit. +// +// IDL `=` defaults, applied on request; never silently written. +// ApplyDefault populates only fields the IDL gives a default; every other +// presence-tracked field stays unset. HasDefaults() reports whether an +// element has any (an empty ApplyDefault is still generated for symmetry). +#ifndef PROTOSPEC_GENERATED_DEFAULTS_H +#define PROTOSPEC_GENERATED_DEFAULTS_H + +#include "types.h" + +namespace ps::mjcf { + +void ApplyDefault(Model& e); +void ApplyDefault(Compiler& e); +void ApplyDefault(LengthRange& e); +void ApplyDefault(Option& e); +void ApplyDefault(Flag& e); +void ApplyDefault(Size& e); +void ApplyDefault(Statistic& e); +void ApplyDefault(Visual& e); +void ApplyDefault(VisualGlobal& e); +void ApplyDefault(VisualQuality& e); +void ApplyDefault(VisualHeadlight& e); +void ApplyDefault(VisualMap& e); +void ApplyDefault(VisualScale& e); +void ApplyDefault(VisualRgba& e); +void ApplyDefault(Default& e); +void ApplyDefault(MaterialLayer& e); +void ApplyDefault(Extension& e); +void ApplyDefault(PluginDef& e); +void ApplyDefault(PluginInstance& e); +void ApplyDefault(Config& e); +void ApplyDefault(Asset& e); +void ApplyDefault(Mesh& e); +void ApplyDefault(PluginRef& e); +void ApplyDefault(Hfield& e); +void ApplyDefault(Skin& e); +void ApplyDefault(SkinBone& e); +void ApplyDefault(Texture& e); +void ApplyDefault(Material& e); +void ApplyDefault(ModelAsset& e); +void ApplyDefault(Body& e); +void ApplyDefault(Inertial& e); +void ApplyDefault(Joint& e); +void ApplyDefault(FreeJoint& e); +void ApplyDefault(Geom& e); +void ApplyDefault(Attach& e); +void ApplyDefault(Site& e); +void ApplyDefault(Camera& e); +void ApplyDefault(Light& e); +void ApplyDefault(Composite& e); +void ApplyDefault(CompositeJoint& e); +void ApplyDefault(CompositeSkin& e); +void ApplyDefault(CompositeGeom& e); +void ApplyDefault(CompositeSite& e); +void ApplyDefault(Flexcomp& e); +void ApplyDefault(FlexcompEdge& e); +void ApplyDefault(FlexElasticity& e); +void ApplyDefault(FlexContact& e); +void ApplyDefault(FlexcompPin& e); +void ApplyDefault(Deformable& e); +void ApplyDefault(Flex& e); +void ApplyDefault(FlexEdge& e); +void ApplyDefault(Contact& e); +void ApplyDefault(Pair& e); +void ApplyDefault(Exclude& e); +void ApplyDefault(Tendon& e); +void ApplyDefault(Spatial& e); +void ApplyDefault(SpatialSite& e); +void ApplyDefault(SpatialGeom& e); +void ApplyDefault(Pulley& e); +void ApplyDefault(Fixed& e); +void ApplyDefault(FixedJoint& e); +void ApplyDefault(Equality& e); +void ApplyDefault(Connect& e); +void ApplyDefault(Weld& e); +void ApplyDefault(EqualityJoint& e); +void ApplyDefault(EqualityTendon& e); +void ApplyDefault(EqualityFlex& e); +void ApplyDefault(Flexvert& e); +void ApplyDefault(Flexstrain& e); +void ApplyDefault(Actuator& e); +void ApplyDefault(ActuatorGeneral& e); +void ApplyDefault(Motor& e); +void ApplyDefault(Position& e); +void ApplyDefault(Velocity& e); +void ApplyDefault(IntVelocity& e); +void ApplyDefault(OrientationActuator& e); +void ApplyDefault(Pid& e); +void ApplyDefault(Damper& e); +void ApplyDefault(Cylinder& e); +void ApplyDefault(Muscle& e); +void ApplyDefault(Adhesion& e); +void ApplyDefault(DcMotor& e); +void ApplyDefault(ActuatorPlugin& e); +void ApplyDefault(Sensor& e); +void ApplyDefault(Touch& e); +void ApplyDefault(Accelerometer& e); +void ApplyDefault(Velocimeter& e); +void ApplyDefault(Gyro& e); +void ApplyDefault(Force& e); +void ApplyDefault(Torque& e); +void ApplyDefault(Magnetometer& e); +void ApplyDefault(Camprojection& e); +void ApplyDefault(Rangefinder& e); +void ApplyDefault(Jointpos& e); +void ApplyDefault(Jointvel& e); +void ApplyDefault(Tendonpos& e); +void ApplyDefault(Tendonvel& e); +void ApplyDefault(Actuatorpos& e); +void ApplyDefault(Actuatorvel& e); +void ApplyDefault(Actuatorfrc& e); +void ApplyDefault(Jointactuatorfrc& e); +void ApplyDefault(Tendonactuatorfrc& e); +void ApplyDefault(Ballquat& e); +void ApplyDefault(Ballangvel& e); +void ApplyDefault(Jointlimitpos& e); +void ApplyDefault(Jointlimitvel& e); +void ApplyDefault(Jointlimitfrc& e); +void ApplyDefault(Tendonlimitpos& e); +void ApplyDefault(Tendonlimitvel& e); +void ApplyDefault(Tendonlimitfrc& e); +void ApplyDefault(Framepos& e); +void ApplyDefault(Framequat& e); +void ApplyDefault(Framexaxis& e); +void ApplyDefault(Frameyaxis& e); +void ApplyDefault(Framezaxis& e); +void ApplyDefault(Framelinvel& e); +void ApplyDefault(Frameangvel& e); +void ApplyDefault(Framelinacc& e); +void ApplyDefault(Frameangacc& e); +void ApplyDefault(Subtreecom& e); +void ApplyDefault(Subtreelinvel& e); +void ApplyDefault(Subtreeangmom& e); +void ApplyDefault(Insidesite& e); +void ApplyDefault(Distance& e); +void ApplyDefault(Normal& e); +void ApplyDefault(Fromto& e); +void ApplyDefault(SensorContact& e); +void ApplyDefault(EPotential& e); +void ApplyDefault(EKinetic& e); +void ApplyDefault(Clock& e); +void ApplyDefault(Tactile& e); +void ApplyDefault(SensorUser& e); +void ApplyDefault(SensorPlugin& e); +void ApplyDefault(Custom& e); +void ApplyDefault(Numeric& e); +void ApplyDefault(Text& e); +void ApplyDefault(Tuple& e); +void ApplyDefault(TupleElement& e); +void ApplyDefault(Keyframe& e); +void ApplyDefault(Key& e); +void ApplyDefault(Frame& e); +void ApplyDefault(Replicate& e); +void ApplyDefault(EqualityDefault& e); +void ApplyDefault(TendonDefault& e); + +} // namespace ps::mjcf + +#endif // PROTOSPEC_GENERATED_DEFAULTS_H diff --git a/protospec/lib/generated/keywords.cc b/protospec/lib/generated/keywords.cc new file mode 100644 index 00000000..a37c76d2 --- /dev/null +++ b/protospec/lib/generated/keywords.cc @@ -0,0 +1,904 @@ +// Generated by protospec_gen.emit — do not edit. +#include "keywords.h" + +namespace ps::mjcf { + +std::string_view ToMjcf(Coordinate value) { + switch (value) { + case Coordinate::local: return "local"; + case Coordinate::global: return "global"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Coordinate& out) { + if (text == "local") { out = Coordinate::local; return true; } + if (text == "global") { out = Coordinate::global; return true; } + return false; +} + +std::string_view ToMjcf(AngleUnit value) { + switch (value) { + case AngleUnit::radian: return "radian"; + case AngleUnit::degree: return "degree"; + } + return ""; +} + +bool FromMjcf(std::string_view text, AngleUnit& out) { + if (text == "radian") { out = AngleUnit::radian; return true; } + if (text == "degree") { out = AngleUnit::degree; return true; } + return false; +} + +std::string_view ToMjcf(FluidShape value) { + switch (value) { + case FluidShape::none: return "none"; + case FluidShape::ellipsoid: return "ellipsoid"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FluidShape& out) { + if (text == "none") { out = FluidShape::none; return true; } + if (text == "ellipsoid") { out = FluidShape::ellipsoid; return true; } + return false; +} + +std::string_view ToMjcf(Enable value) { + switch (value) { + case Enable::disable: return "disable"; + case Enable::enable: return "enable"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Enable& out) { + if (text == "disable") { out = Enable::disable; return true; } + if (text == "enable") { out = Enable::enable; return true; } + return false; +} + +std::string_view ToMjcf(TriState value) { + switch (value) { + case TriState::false_: return "false"; + case TriState::true_: return "true"; + case TriState::auto_: return "auto"; + } + return ""; +} + +bool FromMjcf(std::string_view text, TriState& out) { + if (text == "false") { out = TriState::false_; return true; } + if (text == "true") { out = TriState::true_; return true; } + if (text == "auto") { out = TriState::auto_; return true; } + return false; +} + +std::string_view ToMjcf(SimpleMode value) { + switch (value) { + case SimpleMode::false_: return "false"; + case SimpleMode::auto_: return "auto"; + } + return ""; +} + +bool FromMjcf(std::string_view text, SimpleMode& out) { + if (text == "false") { out = SimpleMode::false_; return true; } + if (text == "auto") { out = SimpleMode::auto_; return true; } + return false; +} + +std::string_view ToMjcf(BodySleep value) { + switch (value) { + case BodySleep::auto_: return "auto"; + case BodySleep::never: return "never"; + case BodySleep::allowed: return "allowed"; + case BodySleep::init: return "init"; + } + return ""; +} + +bool FromMjcf(std::string_view text, BodySleep& out) { + if (text == "auto") { out = BodySleep::auto_; return true; } + if (text == "never") { out = BodySleep::never; return true; } + if (text == "allowed") { out = BodySleep::allowed; return true; } + if (text == "init") { out = BodySleep::init; return true; } + return false; +} + +std::string_view ToMjcf(JointType value) { + switch (value) { + case JointType::free: return "free"; + case JointType::ball: return "ball"; + case JointType::slide: return "slide"; + case JointType::hinge: return "hinge"; + } + return ""; +} + +bool FromMjcf(std::string_view text, JointType& out) { + if (text == "free") { out = JointType::free; return true; } + if (text == "ball") { out = JointType::ball; return true; } + if (text == "slide") { out = JointType::slide; return true; } + if (text == "hinge") { out = JointType::hinge; return true; } + return false; +} + +std::string_view ToMjcf(GeomType value) { + switch (value) { + case GeomType::plane: return "plane"; + case GeomType::hfield: return "hfield"; + case GeomType::sphere: return "sphere"; + case GeomType::capsule: return "capsule"; + case GeomType::ellipsoid: return "ellipsoid"; + case GeomType::cylinder: return "cylinder"; + case GeomType::box: return "box"; + case GeomType::mesh: return "mesh"; + case GeomType::sdf: return "sdf"; + } + return ""; +} + +bool FromMjcf(std::string_view text, GeomType& out) { + if (text == "plane") { out = GeomType::plane; return true; } + if (text == "hfield") { out = GeomType::hfield; return true; } + if (text == "sphere") { out = GeomType::sphere; return true; } + if (text == "capsule") { out = GeomType::capsule; return true; } + if (text == "ellipsoid") { out = GeomType::ellipsoid; return true; } + if (text == "cylinder") { out = GeomType::cylinder; return true; } + if (text == "box") { out = GeomType::box; return true; } + if (text == "mesh") { out = GeomType::mesh; return true; } + if (text == "sdf") { out = GeomType::sdf; return true; } + return false; +} + +std::string_view ToMjcf(CameraProjection value) { + switch (value) { + case CameraProjection::perspective: return "perspective"; + case CameraProjection::orthographic: return "orthographic"; + } + return ""; +} + +bool FromMjcf(std::string_view text, CameraProjection& out) { + if (text == "perspective") { out = CameraProjection::perspective; return true; } + if (text == "orthographic") { out = CameraProjection::orthographic; return true; } + return false; +} + +std::string_view ToMjcf(CamLightMode value) { + switch (value) { + case CamLightMode::fixed: return "fixed"; + case CamLightMode::track: return "track"; + case CamLightMode::trackcom: return "trackcom"; + case CamLightMode::targetbody: return "targetbody"; + case CamLightMode::targetbodycom: return "targetbodycom"; + } + return ""; +} + +bool FromMjcf(std::string_view text, CamLightMode& out) { + if (text == "fixed") { out = CamLightMode::fixed; return true; } + if (text == "track") { out = CamLightMode::track; return true; } + if (text == "trackcom") { out = CamLightMode::trackcom; return true; } + if (text == "targetbody") { out = CamLightMode::targetbody; return true; } + if (text == "targetbodycom") { out = CamLightMode::targetbodycom; return true; } + return false; +} + +std::string_view ToMjcf(LightType value) { + switch (value) { + case LightType::spot: return "spot"; + case LightType::directional: return "directional"; + case LightType::point: return "point"; + case LightType::image: return "image"; + } + return ""; +} + +bool FromMjcf(std::string_view text, LightType& out) { + if (text == "spot") { out = LightType::spot; return true; } + if (text == "directional") { out = LightType::directional; return true; } + if (text == "point") { out = LightType::point; return true; } + if (text == "image") { out = LightType::image; return true; } + return false; +} + +std::string_view ToMjcf(TexRole value) { + switch (value) { + case TexRole::rgb: return "rgb"; + case TexRole::occlusion: return "occlusion"; + case TexRole::roughness: return "roughness"; + case TexRole::metallic: return "metallic"; + case TexRole::normal: return "normal"; + case TexRole::opacity: return "opacity"; + case TexRole::emissive: return "emissive"; + case TexRole::rgba: return "rgba"; + case TexRole::orm: return "orm"; + } + return ""; +} + +bool FromMjcf(std::string_view text, TexRole& out) { + if (text == "rgb") { out = TexRole::rgb; return true; } + if (text == "occlusion") { out = TexRole::occlusion; return true; } + if (text == "roughness") { out = TexRole::roughness; return true; } + if (text == "metallic") { out = TexRole::metallic; return true; } + if (text == "normal") { out = TexRole::normal; return true; } + if (text == "opacity") { out = TexRole::opacity; return true; } + if (text == "emissive") { out = TexRole::emissive; return true; } + if (text == "rgba") { out = TexRole::rgba; return true; } + if (text == "orm") { out = TexRole::orm; return true; } + return false; +} + +std::string_view ToMjcf(Integrator value) { + switch (value) { + case Integrator::Euler: return "Euler"; + case Integrator::RK4: return "RK4"; + case Integrator::implicit: return "implicit"; + case Integrator::implicitfast: return "implicitfast"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Integrator& out) { + if (text == "Euler") { out = Integrator::Euler; return true; } + if (text == "RK4") { out = Integrator::RK4; return true; } + if (text == "implicit") { out = Integrator::implicit; return true; } + if (text == "implicitfast") { out = Integrator::implicitfast; return true; } + return false; +} + +std::string_view ToMjcf(Cone value) { + switch (value) { + case Cone::pyramidal: return "pyramidal"; + case Cone::elliptic: return "elliptic"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Cone& out) { + if (text == "pyramidal") { out = Cone::pyramidal; return true; } + if (text == "elliptic") { out = Cone::elliptic; return true; } + return false; +} + +std::string_view ToMjcf(JacobianType value) { + switch (value) { + case JacobianType::dense: return "dense"; + case JacobianType::sparse: return "sparse"; + case JacobianType::auto_: return "auto"; + } + return ""; +} + +bool FromMjcf(std::string_view text, JacobianType& out) { + if (text == "dense") { out = JacobianType::dense; return true; } + if (text == "sparse") { out = JacobianType::sparse; return true; } + if (text == "auto") { out = JacobianType::auto_; return true; } + return false; +} + +std::string_view ToMjcf(SolverType value) { + switch (value) { + case SolverType::PGS: return "PGS"; + case SolverType::CG: return "CG"; + case SolverType::Newton: return "Newton"; + } + return ""; +} + +bool FromMjcf(std::string_view text, SolverType& out) { + if (text == "PGS") { out = SolverType::PGS; return true; } + if (text == "CG") { out = SolverType::CG; return true; } + if (text == "Newton") { out = SolverType::Newton; return true; } + return false; +} + +std::string_view ToMjcf(EqualityType value) { + switch (value) { + case EqualityType::connect: return "connect"; + case EqualityType::weld: return "weld"; + case EqualityType::joint: return "joint"; + case EqualityType::tendon: return "tendon"; + case EqualityType::flex: return "flex"; + case EqualityType::flexvert: return "flexvert"; + case EqualityType::flexstrain: return "flexstrain"; + case EqualityType::distance: return "distance"; + } + return ""; +} + +bool FromMjcf(std::string_view text, EqualityType& out) { + if (text == "connect") { out = EqualityType::connect; return true; } + if (text == "weld") { out = EqualityType::weld; return true; } + if (text == "joint") { out = EqualityType::joint; return true; } + if (text == "tendon") { out = EqualityType::tendon; return true; } + if (text == "flex") { out = EqualityType::flex; return true; } + if (text == "flexvert") { out = EqualityType::flexvert; return true; } + if (text == "flexstrain") { out = EqualityType::flexstrain; return true; } + if (text == "distance") { out = EqualityType::distance; return true; } + return false; +} + +std::string_view ToMjcf(TextureType value) { + switch (value) { + case TextureType::twod: return "2d"; + case TextureType::cube: return "cube"; + case TextureType::skybox: return "skybox"; + } + return ""; +} + +bool FromMjcf(std::string_view text, TextureType& out) { + if (text == "2d") { out = TextureType::twod; return true; } + if (text == "cube") { out = TextureType::cube; return true; } + if (text == "skybox") { out = TextureType::skybox; return true; } + return false; +} + +std::string_view ToMjcf(ColorSpace value) { + switch (value) { + case ColorSpace::auto_: return "auto"; + case ColorSpace::linear: return "linear"; + case ColorSpace::sRGB: return "sRGB"; + } + return ""; +} + +bool FromMjcf(std::string_view text, ColorSpace& out) { + if (text == "auto") { out = ColorSpace::auto_; return true; } + if (text == "linear") { out = ColorSpace::linear; return true; } + if (text == "sRGB") { out = ColorSpace::sRGB; return true; } + return false; +} + +std::string_view ToMjcf(TextureBuiltin value) { + switch (value) { + case TextureBuiltin::none: return "none"; + case TextureBuiltin::gradient: return "gradient"; + case TextureBuiltin::checker: return "checker"; + case TextureBuiltin::flat: return "flat"; + } + return ""; +} + +bool FromMjcf(std::string_view text, TextureBuiltin& out) { + if (text == "none") { out = TextureBuiltin::none; return true; } + if (text == "gradient") { out = TextureBuiltin::gradient; return true; } + if (text == "checker") { out = TextureBuiltin::checker; return true; } + if (text == "flat") { out = TextureBuiltin::flat; return true; } + return false; +} + +std::string_view ToMjcf(TextureMark value) { + switch (value) { + case TextureMark::none: return "none"; + case TextureMark::edge: return "edge"; + case TextureMark::cross: return "cross"; + case TextureMark::random: return "random"; + } + return ""; +} + +bool FromMjcf(std::string_view text, TextureMark& out) { + if (text == "none") { out = TextureMark::none; return true; } + if (text == "edge") { out = TextureMark::edge; return true; } + if (text == "cross") { out = TextureMark::cross; return true; } + if (text == "random") { out = TextureMark::random; return true; } + return false; +} + +std::string_view ToMjcf(DynType value) { + switch (value) { + case DynType::none: return "none"; + case DynType::integrator: return "integrator"; + case DynType::filter: return "filter"; + case DynType::filterexact: return "filterexact"; + case DynType::muscle: return "muscle"; + case DynType::dcmotor: return "dcmotor"; + case DynType::pid: return "pid"; + case DynType::user: return "user"; + } + return ""; +} + +bool FromMjcf(std::string_view text, DynType& out) { + if (text == "none") { out = DynType::none; return true; } + if (text == "integrator") { out = DynType::integrator; return true; } + if (text == "filter") { out = DynType::filter; return true; } + if (text == "filterexact") { out = DynType::filterexact; return true; } + if (text == "muscle") { out = DynType::muscle; return true; } + if (text == "dcmotor") { out = DynType::dcmotor; return true; } + if (text == "pid") { out = DynType::pid; return true; } + if (text == "user") { out = DynType::user; return true; } + return false; +} + +std::string_view ToMjcf(DcMotorInput value) { + switch (value) { + case DcMotorInput::voltage: return "voltage"; + case DcMotorInput::position: return "position"; + case DcMotorInput::velocity: return "velocity"; + } + return ""; +} + +bool FromMjcf(std::string_view text, DcMotorInput& out) { + if (text == "voltage") { out = DcMotorInput::voltage; return true; } + if (text == "position") { out = DcMotorInput::position; return true; } + if (text == "velocity") { out = DcMotorInput::velocity; return true; } + return false; +} + +std::string_view ToMjcf(GainType value) { + switch (value) { + case GainType::fixed: return "fixed"; + case GainType::affine: return "affine"; + case GainType::muscle: return "muscle"; + case GainType::dcmotor: return "dcmotor"; + case GainType::so3: return "so3"; + case GainType::pid: return "pid"; + case GainType::user: return "user"; + } + return ""; +} + +bool FromMjcf(std::string_view text, GainType& out) { + if (text == "fixed") { out = GainType::fixed; return true; } + if (text == "affine") { out = GainType::affine; return true; } + if (text == "muscle") { out = GainType::muscle; return true; } + if (text == "dcmotor") { out = GainType::dcmotor; return true; } + if (text == "so3") { out = GainType::so3; return true; } + if (text == "pid") { out = GainType::pid; return true; } + if (text == "user") { out = GainType::user; return true; } + return false; +} + +std::string_view ToMjcf(InputChart value) { + switch (value) { + case InputChart::expmap: return "expmap"; + case InputChart::quat: return "quat"; + } + return ""; +} + +bool FromMjcf(std::string_view text, InputChart& out) { + if (text == "expmap") { out = InputChart::expmap; return true; } + if (text == "quat") { out = InputChart::quat; return true; } + return false; +} + +std::string_view ToMjcf(InputBit value) { + switch (value) { + case InputBit::pos: return "pos"; + case InputBit::vel: return "vel"; + case InputBit::ff: return "ff"; + } + return ""; +} + +bool FromMjcf(std::string_view text, InputBit& out) { + if (text == "pos") { out = InputBit::pos; return true; } + if (text == "vel") { out = InputBit::vel; return true; } + if (text == "ff") { out = InputBit::ff; return true; } + return false; +} + +std::string_view ToMjcf(BiasType value) { + switch (value) { + case BiasType::none: return "none"; + case BiasType::affine: return "affine"; + case BiasType::muscle: return "muscle"; + case BiasType::dcmotor: return "dcmotor"; + case BiasType::so3: return "so3"; + case BiasType::user: return "user"; + } + return ""; +} + +bool FromMjcf(std::string_view text, BiasType& out) { + if (text == "none") { out = BiasType::none; return true; } + if (text == "affine") { out = BiasType::affine; return true; } + if (text == "muscle") { out = BiasType::muscle; return true; } + if (text == "dcmotor") { out = BiasType::dcmotor; return true; } + if (text == "so3") { out = BiasType::so3; return true; } + if (text == "user") { out = BiasType::user; return true; } + return false; +} + +std::string_view ToMjcf(InterpType value) { + switch (value) { + case InterpType::zoh: return "zoh"; + case InterpType::linear: return "linear"; + case InterpType::cubic: return "cubic"; + } + return ""; +} + +bool FromMjcf(std::string_view text, InterpType& out) { + if (text == "zoh") { out = InterpType::zoh; return true; } + if (text == "linear") { out = InterpType::linear; return true; } + if (text == "cubic") { out = InterpType::cubic; return true; } + return false; +} + +std::string_view ToMjcf(NeedStage value) { + switch (value) { + case NeedStage::none: return "none"; + case NeedStage::pos: return "pos"; + case NeedStage::vel: return "vel"; + case NeedStage::acc: return "acc"; + } + return ""; +} + +bool FromMjcf(std::string_view text, NeedStage& out) { + if (text == "none") { out = NeedStage::none; return true; } + if (text == "pos") { out = NeedStage::pos; return true; } + if (text == "vel") { out = NeedStage::vel; return true; } + if (text == "acc") { out = NeedStage::acc; return true; } + return false; +} + +std::string_view ToMjcf(DataType value) { + switch (value) { + case DataType::real: return "real"; + case DataType::positive: return "positive"; + case DataType::axis: return "axis"; + case DataType::quaternion: return "quaternion"; + } + return ""; +} + +bool FromMjcf(std::string_view text, DataType& out) { + if (text == "real") { out = DataType::real; return true; } + if (text == "positive") { out = DataType::positive; return true; } + if (text == "axis") { out = DataType::axis; return true; } + if (text == "quaternion") { out = DataType::quaternion; return true; } + return false; +} + +std::string_view ToMjcf(FrameObject value) { + switch (value) { + case FrameObject::body: return "body"; + case FrameObject::xbody: return "xbody"; + case FrameObject::geom: return "geom"; + case FrameObject::site: return "site"; + case FrameObject::camera: return "camera"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FrameObject& out) { + if (text == "body") { out = FrameObject::body; return true; } + if (text == "xbody") { out = FrameObject::xbody; return true; } + if (text == "geom") { out = FrameObject::geom; return true; } + if (text == "site") { out = FrameObject::site; return true; } + if (text == "camera") { out = FrameObject::camera; return true; } + return false; +} + +std::string_view ToMjcf(ContactData value) { + switch (value) { + case ContactData::found: return "found"; + case ContactData::force: return "force"; + case ContactData::torque: return "torque"; + case ContactData::dist: return "dist"; + case ContactData::pos: return "pos"; + case ContactData::normal: return "normal"; + case ContactData::tangent: return "tangent"; + } + return ""; +} + +bool FromMjcf(std::string_view text, ContactData& out) { + if (text == "found") { out = ContactData::found; return true; } + if (text == "force") { out = ContactData::force; return true; } + if (text == "torque") { out = ContactData::torque; return true; } + if (text == "dist") { out = ContactData::dist; return true; } + if (text == "pos") { out = ContactData::pos; return true; } + if (text == "normal") { out = ContactData::normal; return true; } + if (text == "tangent") { out = ContactData::tangent; return true; } + return false; +} + +std::string_view ToMjcf(RayData value) { + switch (value) { + case RayData::dist: return "dist"; + case RayData::dir: return "dir"; + case RayData::origin: return "origin"; + case RayData::point: return "point"; + case RayData::normal: return "normal"; + case RayData::depth: return "depth"; + } + return ""; +} + +bool FromMjcf(std::string_view text, RayData& out) { + if (text == "dist") { out = RayData::dist; return true; } + if (text == "dir") { out = RayData::dir; return true; } + if (text == "origin") { out = RayData::origin; return true; } + if (text == "point") { out = RayData::point; return true; } + if (text == "normal") { out = RayData::normal; return true; } + if (text == "depth") { out = RayData::depth; return true; } + return false; +} + +std::string_view ToMjcf(CameraOutput value) { + switch (value) { + case CameraOutput::rgb: return "rgb"; + case CameraOutput::depth: return "depth"; + case CameraOutput::distance: return "distance"; + case CameraOutput::normal: return "normal"; + case CameraOutput::segmentation: return "segmentation"; + } + return ""; +} + +bool FromMjcf(std::string_view text, CameraOutput& out) { + if (text == "rgb") { out = CameraOutput::rgb; return true; } + if (text == "depth") { out = CameraOutput::depth; return true; } + if (text == "distance") { out = CameraOutput::distance; return true; } + if (text == "normal") { out = CameraOutput::normal; return true; } + if (text == "segmentation") { out = CameraOutput::segmentation; return true; } + return false; +} + +std::string_view ToMjcf(ContactReduce value) { + switch (value) { + case ContactReduce::none: return "none"; + case ContactReduce::mindist: return "mindist"; + case ContactReduce::maxforce: return "maxforce"; + case ContactReduce::netforce: return "netforce"; + } + return ""; +} + +bool FromMjcf(std::string_view text, ContactReduce& out) { + if (text == "none") { out = ContactReduce::none; return true; } + if (text == "mindist") { out = ContactReduce::mindist; return true; } + if (text == "maxforce") { out = ContactReduce::maxforce; return true; } + if (text == "netforce") { out = ContactReduce::netforce; return true; } + return false; +} + +std::string_view ToMjcf(Conflict value) { + switch (value) { + case Conflict::warning: return "warning"; + case Conflict::merge: return "merge"; + case Conflict::error: return "error"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Conflict& out) { + if (text == "warning") { out = Conflict::warning; return true; } + if (text == "merge") { out = Conflict::merge; return true; } + if (text == "error") { out = Conflict::error; return true; } + return false; +} + +std::string_view ToMjcf(LRMode value) { + switch (value) { + case LRMode::none: return "none"; + case LRMode::muscle: return "muscle"; + case LRMode::muscleuser: return "muscleuser"; + case LRMode::all: return "all"; + } + return ""; +} + +bool FromMjcf(std::string_view text, LRMode& out) { + if (text == "none") { out = LRMode::none; return true; } + if (text == "muscle") { out = LRMode::muscle; return true; } + if (text == "muscleuser") { out = LRMode::muscleuser; return true; } + if (text == "all") { out = LRMode::all; return true; } + return false; +} + +std::string_view ToMjcf(CompositeType value) { + switch (value) { + case CompositeType::particle: return "particle"; + case CompositeType::grid: return "grid"; + case CompositeType::rope: return "rope"; + case CompositeType::loop: return "loop"; + case CompositeType::cable: return "cable"; + case CompositeType::cloth: return "cloth"; + } + return ""; +} + +bool FromMjcf(std::string_view text, CompositeType& out) { + if (text == "particle") { out = CompositeType::particle; return true; } + if (text == "grid") { out = CompositeType::grid; return true; } + if (text == "rope") { out = CompositeType::rope; return true; } + if (text == "loop") { out = CompositeType::loop; return true; } + if (text == "cable") { out = CompositeType::cable; return true; } + if (text == "cloth") { out = CompositeType::cloth; return true; } + return false; +} + +std::string_view ToMjcf(JointKind value) { + switch (value) { + case JointKind::main: return "main"; + } + return ""; +} + +bool FromMjcf(std::string_view text, JointKind& out) { + if (text == "main") { out = JointKind::main; return true; } + return false; +} + +std::string_view ToMjcf(CurveShape value) { + switch (value) { + case CurveShape::s: return "s"; + case CurveShape::cos_s: return "cos(s)"; + case CurveShape::sin_s: return "sin(s)"; + case CurveShape::zero: return "0"; + } + return ""; +} + +bool FromMjcf(std::string_view text, CurveShape& out) { + if (text == "s") { out = CurveShape::s; return true; } + if (text == "cos(s)") { out = CurveShape::cos_s; return true; } + if (text == "sin(s)") { out = CurveShape::sin_s; return true; } + if (text == "0") { out = CurveShape::zero; return true; } + return false; +} + +std::string_view ToMjcf(MeshInertia value) { + switch (value) { + case MeshInertia::convex: return "convex"; + case MeshInertia::legacy: return "legacy"; + case MeshInertia::exact: return "exact"; + case MeshInertia::shell: return "shell"; + } + return ""; +} + +bool FromMjcf(std::string_view text, MeshInertia& out) { + if (text == "convex") { out = MeshInertia::convex; return true; } + if (text == "legacy") { out = MeshInertia::legacy; return true; } + if (text == "exact") { out = MeshInertia::exact; return true; } + if (text == "shell") { out = MeshInertia::shell; return true; } + return false; +} + +std::string_view ToMjcf(MeshBuiltin value) { + switch (value) { + case MeshBuiltin::none: return "none"; + case MeshBuiltin::sphere: return "sphere"; + case MeshBuiltin::hemisphere: return "hemisphere"; + case MeshBuiltin::cone: return "cone"; + case MeshBuiltin::supertorus: return "supertorus"; + case MeshBuiltin::supersphere: return "supersphere"; + case MeshBuiltin::wedge: return "wedge"; + case MeshBuiltin::plate: return "plate"; + } + return ""; +} + +bool FromMjcf(std::string_view text, MeshBuiltin& out) { + if (text == "none") { out = MeshBuiltin::none; return true; } + if (text == "sphere") { out = MeshBuiltin::sphere; return true; } + if (text == "hemisphere") { out = MeshBuiltin::hemisphere; return true; } + if (text == "cone") { out = MeshBuiltin::cone; return true; } + if (text == "supertorus") { out = MeshBuiltin::supertorus; return true; } + if (text == "supersphere") { out = MeshBuiltin::supersphere; return true; } + if (text == "wedge") { out = MeshBuiltin::wedge; return true; } + if (text == "plate") { out = MeshBuiltin::plate; return true; } + return false; +} + +std::string_view ToMjcf(FlexcompType value) { + switch (value) { + case FlexcompType::grid: return "grid"; + case FlexcompType::box: return "box"; + case FlexcompType::cylinder: return "cylinder"; + case FlexcompType::ellipsoid: return "ellipsoid"; + case FlexcompType::square: return "square"; + case FlexcompType::disc: return "disc"; + case FlexcompType::circle: return "circle"; + case FlexcompType::mesh: return "mesh"; + case FlexcompType::gmsh: return "gmsh"; + case FlexcompType::direct: return "direct"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FlexcompType& out) { + if (text == "grid") { out = FlexcompType::grid; return true; } + if (text == "box") { out = FlexcompType::box; return true; } + if (text == "cylinder") { out = FlexcompType::cylinder; return true; } + if (text == "ellipsoid") { out = FlexcompType::ellipsoid; return true; } + if (text == "square") { out = FlexcompType::square; return true; } + if (text == "disc") { out = FlexcompType::disc; return true; } + if (text == "circle") { out = FlexcompType::circle; return true; } + if (text == "mesh") { out = FlexcompType::mesh; return true; } + if (text == "gmsh") { out = FlexcompType::gmsh; return true; } + if (text == "direct") { out = FlexcompType::direct; return true; } + return false; +} + +std::string_view ToMjcf(FlexDof value) { + switch (value) { + case FlexDof::full: return "full"; + case FlexDof::radial: return "radial"; + case FlexDof::trilinear: return "trilinear"; + case FlexDof::quadratic: return "quadratic"; + case FlexDof::twod: return "2d"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FlexDof& out) { + if (text == "full") { out = FlexDof::full; return true; } + if (text == "radial") { out = FlexDof::radial; return true; } + if (text == "trilinear") { out = FlexDof::trilinear; return true; } + if (text == "quadratic") { out = FlexDof::quadratic; return true; } + if (text == "2d") { out = FlexDof::twod; return true; } + return false; +} + +std::string_view ToMjcf(FlexSelfCollide value) { + switch (value) { + case FlexSelfCollide::none: return "none"; + case FlexSelfCollide::narrow: return "narrow"; + case FlexSelfCollide::bvh: return "bvh"; + case FlexSelfCollide::sap: return "sap"; + case FlexSelfCollide::auto_: return "auto"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FlexSelfCollide& out) { + if (text == "none") { out = FlexSelfCollide::none; return true; } + if (text == "narrow") { out = FlexSelfCollide::narrow; return true; } + if (text == "bvh") { out = FlexSelfCollide::bvh; return true; } + if (text == "sap") { out = FlexSelfCollide::sap; return true; } + if (text == "auto") { out = FlexSelfCollide::auto_; return true; } + return false; +} + +std::string_view ToMjcf(Elastic2D value) { + switch (value) { + case Elastic2D::none: return "none"; + case Elastic2D::bend: return "bend"; + case Elastic2D::stretch: return "stretch"; + case Elastic2D::both: return "both"; + } + return ""; +} + +bool FromMjcf(std::string_view text, Elastic2D& out) { + if (text == "none") { out = Elastic2D::none; return true; } + if (text == "bend") { out = Elastic2D::bend; return true; } + if (text == "stretch") { out = Elastic2D::stretch; return true; } + if (text == "both") { out = Elastic2D::both; return true; } + return false; +} + +std::string_view ToMjcf(FlexEquality value) { + switch (value) { + case FlexEquality::false_: return "false"; + case FlexEquality::true_: return "true"; + case FlexEquality::vert: return "vert"; + case FlexEquality::strain: return "strain"; + } + return ""; +} + +bool FromMjcf(std::string_view text, FlexEquality& out) { + if (text == "false") { out = FlexEquality::false_; return true; } + if (text == "true") { out = FlexEquality::true_; return true; } + if (text == "vert") { out = FlexEquality::vert; return true; } + if (text == "strain") { out = FlexEquality::strain; return true; } + return false; +} + +} // namespace ps::mjcf diff --git a/protospec/lib/generated/keywords.h b/protospec/lib/generated/keywords.h new file mode 100644 index 00000000..e3524d6e --- /dev/null +++ b/protospec/lib/generated/keywords.h @@ -0,0 +1,112 @@ +// Generated by protospec_gen.emit — do not edit. +// +// Enum <-> MJCF keyword string tables (from the IDL enum defs). +#ifndef PROTOSPEC_GENERATED_KEYWORDS_H +#define PROTOSPEC_GENERATED_KEYWORDS_H + +#include + +#include "types.h" + +namespace ps::mjcf { + +std::string_view ToMjcf(Coordinate value); +bool FromMjcf(std::string_view text, Coordinate& out); +std::string_view ToMjcf(AngleUnit value); +bool FromMjcf(std::string_view text, AngleUnit& out); +std::string_view ToMjcf(FluidShape value); +bool FromMjcf(std::string_view text, FluidShape& out); +std::string_view ToMjcf(Enable value); +bool FromMjcf(std::string_view text, Enable& out); +std::string_view ToMjcf(TriState value); +bool FromMjcf(std::string_view text, TriState& out); +std::string_view ToMjcf(SimpleMode value); +bool FromMjcf(std::string_view text, SimpleMode& out); +std::string_view ToMjcf(BodySleep value); +bool FromMjcf(std::string_view text, BodySleep& out); +std::string_view ToMjcf(JointType value); +bool FromMjcf(std::string_view text, JointType& out); +std::string_view ToMjcf(GeomType value); +bool FromMjcf(std::string_view text, GeomType& out); +std::string_view ToMjcf(CameraProjection value); +bool FromMjcf(std::string_view text, CameraProjection& out); +std::string_view ToMjcf(CamLightMode value); +bool FromMjcf(std::string_view text, CamLightMode& out); +std::string_view ToMjcf(LightType value); +bool FromMjcf(std::string_view text, LightType& out); +std::string_view ToMjcf(TexRole value); +bool FromMjcf(std::string_view text, TexRole& out); +std::string_view ToMjcf(Integrator value); +bool FromMjcf(std::string_view text, Integrator& out); +std::string_view ToMjcf(Cone value); +bool FromMjcf(std::string_view text, Cone& out); +std::string_view ToMjcf(JacobianType value); +bool FromMjcf(std::string_view text, JacobianType& out); +std::string_view ToMjcf(SolverType value); +bool FromMjcf(std::string_view text, SolverType& out); +std::string_view ToMjcf(EqualityType value); +bool FromMjcf(std::string_view text, EqualityType& out); +std::string_view ToMjcf(TextureType value); +bool FromMjcf(std::string_view text, TextureType& out); +std::string_view ToMjcf(ColorSpace value); +bool FromMjcf(std::string_view text, ColorSpace& out); +std::string_view ToMjcf(TextureBuiltin value); +bool FromMjcf(std::string_view text, TextureBuiltin& out); +std::string_view ToMjcf(TextureMark value); +bool FromMjcf(std::string_view text, TextureMark& out); +std::string_view ToMjcf(DynType value); +bool FromMjcf(std::string_view text, DynType& out); +std::string_view ToMjcf(DcMotorInput value); +bool FromMjcf(std::string_view text, DcMotorInput& out); +std::string_view ToMjcf(GainType value); +bool FromMjcf(std::string_view text, GainType& out); +std::string_view ToMjcf(InputChart value); +bool FromMjcf(std::string_view text, InputChart& out); +std::string_view ToMjcf(InputBit value); +bool FromMjcf(std::string_view text, InputBit& out); +std::string_view ToMjcf(BiasType value); +bool FromMjcf(std::string_view text, BiasType& out); +std::string_view ToMjcf(InterpType value); +bool FromMjcf(std::string_view text, InterpType& out); +std::string_view ToMjcf(NeedStage value); +bool FromMjcf(std::string_view text, NeedStage& out); +std::string_view ToMjcf(DataType value); +bool FromMjcf(std::string_view text, DataType& out); +std::string_view ToMjcf(FrameObject value); +bool FromMjcf(std::string_view text, FrameObject& out); +std::string_view ToMjcf(ContactData value); +bool FromMjcf(std::string_view text, ContactData& out); +std::string_view ToMjcf(RayData value); +bool FromMjcf(std::string_view text, RayData& out); +std::string_view ToMjcf(CameraOutput value); +bool FromMjcf(std::string_view text, CameraOutput& out); +std::string_view ToMjcf(ContactReduce value); +bool FromMjcf(std::string_view text, ContactReduce& out); +std::string_view ToMjcf(Conflict value); +bool FromMjcf(std::string_view text, Conflict& out); +std::string_view ToMjcf(LRMode value); +bool FromMjcf(std::string_view text, LRMode& out); +std::string_view ToMjcf(CompositeType value); +bool FromMjcf(std::string_view text, CompositeType& out); +std::string_view ToMjcf(JointKind value); +bool FromMjcf(std::string_view text, JointKind& out); +std::string_view ToMjcf(CurveShape value); +bool FromMjcf(std::string_view text, CurveShape& out); +std::string_view ToMjcf(MeshInertia value); +bool FromMjcf(std::string_view text, MeshInertia& out); +std::string_view ToMjcf(MeshBuiltin value); +bool FromMjcf(std::string_view text, MeshBuiltin& out); +std::string_view ToMjcf(FlexcompType value); +bool FromMjcf(std::string_view text, FlexcompType& out); +std::string_view ToMjcf(FlexDof value); +bool FromMjcf(std::string_view text, FlexDof& out); +std::string_view ToMjcf(FlexSelfCollide value); +bool FromMjcf(std::string_view text, FlexSelfCollide& out); +std::string_view ToMjcf(Elastic2D value); +bool FromMjcf(std::string_view text, Elastic2D& out); +std::string_view ToMjcf(FlexEquality value); +bool FromMjcf(std::string_view text, FlexEquality& out); + +} // namespace ps::mjcf + +#endif // PROTOSPEC_GENERATED_KEYWORDS_H diff --git a/protospec/lib/generated/reflect.cc b/protospec/lib/generated/reflect.cc new file mode 100644 index 00000000..746d9cd8 --- /dev/null +++ b/protospec/lib/generated/reflect.cc @@ -0,0 +1,7488 @@ +// Generated by protospec_gen.emit — do not edit. +#include "reflect.h" + +#include +#include +#include +#include + +namespace ps::mjcf::reflect { +namespace { + +constexpr FieldDescriptor kFields_Model[] = { + {"model", "model", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Model[] = { + {"compilers", "Compiler", false, Cardinality::ZeroOrMore}, + {"options", "Option", false, Cardinality::ZeroOrMore}, + {"sizes", "Size", false, Cardinality::ZeroOrMore}, + {"statistics", "Statistic", false, Cardinality::ZeroOrMore}, + {"visuals", "Visual", false, Cardinality::ZeroOrMore}, + {"defaults", "Default", false, Cardinality::ZeroOrMore}, + {"extensions", "Extension", false, Cardinality::ZeroOrMore}, + {"assets", "Asset", false, Cardinality::ZeroOrMore}, + {"worldbody", "Body", false, Cardinality::ZeroOrMore}, + {"deformables", "Deformable", false, Cardinality::ZeroOrMore}, + {"contacts", "Contact", false, Cardinality::ZeroOrMore}, + {"tendons", "Tendon", false, Cardinality::ZeroOrMore}, + {"equalities", "Equality", false, Cardinality::ZeroOrMore}, + {"actuators", "Actuator", false, Cardinality::ZeroOrMore}, + {"sensors", "Sensor", false, Cardinality::ZeroOrMore}, + {"customs", "Custom", false, Cardinality::ZeroOrMore}, + {"keyframes", "Keyframe", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Compiler[] = { + {"autolimits", "autolimits", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"boundmass", "boundmass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"boundinertia", "boundinertia", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"settotalmass", "settotalmass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"balanceinertia", "balanceinertia", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"strippath", "strippath", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, false, "", "stored on the spec"}, + {"coordinate", "coordinate", "Coordinate", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", "deprecation error"}, + {"angle", "angle", "AngleUnit", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fitaabb", "fitaabb", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"eulerseq", "eulerseq", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"meshdir", "meshdir", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"texturedir", "texturedir", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"discardvisual", "discardvisual", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"usethread", "usethread", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fusestatic", "fusestatic", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"inertiafromgeom", "inertiafromgeom", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"inertiagrouprange", "inertiagrouprange", "int32", FieldKind::Int32, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"saveinertial", "saveinertial", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"assetdir", "assetdir", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", "fans out to mesh/texturedir"}, + {"alignfree", "alignfree", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"conflict", "conflict", "Conflict", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; +constexpr ChildDescriptor kChildren_Compiler[] = { + {"lengthRanges", "LengthRange", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_LengthRange[] = { + {"mode", "mode", "LRMode", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"useexisting", "useexisting", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"uselimit", "uselimit", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"accel", "accel", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"maxforce", "maxforce", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"timeconst", "timeconst", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"timestep", "timestep", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"inttotal", "inttotal", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"tolrange", "tolrange", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Option[] = { + {"timestep", "timestep", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"impratio", "impratio", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"tolerance", "tolerance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ls_tolerance", "ls_tolerance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noslip_tolerance", "noslip_tolerance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ccd_tolerance", "ccd_tolerance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sleep_tolerance", "sleep_tolerance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gravity", "gravity", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"wind", "wind", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"magnetic", "magnetic", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"density", "density", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"viscosity", "viscosity", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"o_margin", "o_margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"o_solref", "o_solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"o_solimp", "o_solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"o_friction", "o_friction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"integrator", "integrator", "Integrator", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cone", "cone", "Cone", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"jacobian", "jacobian", "JacobianType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solver", "solver", "SolverType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"iterations", "iterations", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ls_iterations", "ls_iterations", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noslip_iterations", "noslip_iterations", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ccd_iterations", "ccd_iterations", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sdf_iterations", "sdf_iterations", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sdf_initpoints", "sdf_initpoints", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatorgroupdisable", "actuatorgroupdisable", "int32", FieldKind::Int32, ArityKind::Unbounded, 0, 0, true, false, "", "bits of disableactuator"}, +}; +constexpr ChildDescriptor kChildren_Option[] = { + {"flags", "Flag", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_Flag[] = { + {"constraint", "constraint", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"equality", "equality", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"frictionloss", "frictionloss", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"limit", "limit", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"contact", "contact", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"spring", "spring", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"damper", "damper", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gravity", "gravity", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"clampctrl", "clampctrl", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"warmstart", "warmstart", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"filterparent", "filterparent", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuation", "actuation", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"refsafe", "refsafe", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sensor", "sensor", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"midphase", "midphase", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"eulerdamp", "eulerdamp", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"autoreset", "autoreset", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nativeccd", "nativeccd", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"island", "island", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"multiccd", "multiccd", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"override", "override", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"energy", "energy", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fwdinv", "fwdinv", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"invdiscrete", "invdiscrete", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sleep", "sleep", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"diagexact", "diagexact", "Enable", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Size[] = { + {"memory", "memory", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", "suffixed byte count"}, + {"njmax", "njmax", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", "range/exclusivity checks"}, + {"nconmax", "nconmax", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", "range check"}, + {"nstack", "nstack", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", "range/exclusivity checks"}, + {"nuserdata", "nuserdata", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nkey", "nkey", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_body", "nuser_body", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_jnt", "nuser_jnt", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_geom", "nuser_geom", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_site", "nuser_site", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_cam", "nuser_cam", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_tendon", "nuser_tendon", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_actuator", "nuser_actuator", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nuser_sensor", "nuser_sensor", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; +constexpr int kBundle_Size_0_0[] = { 0 }; +constexpr int kBundle_Size_0_1[] = { 3 }; +constexpr ConstraintBundle kBundles_Size_0[] = { + {kBundle_Size_0_0, 1}, + {kBundle_Size_0_1, 1}, +}; +constexpr int kBundle_Size_1_0[] = { 0 }; +constexpr int kBundle_Size_1_1[] = { 1 }; +constexpr ConstraintBundle kBundles_Size_1[] = { + {kBundle_Size_1_0, 1}, + {kBundle_Size_1_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Size[] = { + {ConstraintKind::Exclusive, kBundles_Size_0, 2, ""}, + {ConstraintKind::Exclusive, kBundles_Size_1, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Statistic[] = { + {"meaninertia", "meaninertia", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"meanmass", "meanmass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"meansize", "meansize", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"extent", "extent", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "when defined"}, + {"center", "center", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Visual[] = { + {"visualGlobals", "VisualGlobal", false, Cardinality::ZeroOrOne}, + {"visualQualities", "VisualQuality", false, Cardinality::ZeroOrOne}, + {"visualHeadlights", "VisualHeadlight", false, Cardinality::ZeroOrOne}, + {"visualMaps", "VisualMap", false, Cardinality::ZeroOrOne}, + {"visualScales", "VisualScale", false, Cardinality::ZeroOrOne}, + {"visualRgbas", "VisualRgba", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_VisualGlobal[] = { + {"cameraid", "cameraid", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"orthographic", "orthographic", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fovy", "fovy", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ipd", "ipd", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"azimuth", "azimuth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"elevation", "elevation", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"linewidth", "linewidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"glow", "glow", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"offwidth", "offwidth", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"offheight", "offheight", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"realtime", "realtime", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ellipsoidinertia", "ellipsoidinertia", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"bvactive", "bvactive", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_VisualQuality[] = { + {"shadowsize", "shadowsize", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"offsamples", "offsamples", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"numslices", "numslices", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"numstacks", "numstacks", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"numquads", "numquads", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_VisualHeadlight[] = { + {"ambient", "ambient", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"diffuse", "diffuse", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"specular", "specular", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"active", "active", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_VisualMap[] = { + {"stiffness", "stiffness", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"stiffnessrot", "stiffnessrot", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"force", "force", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"torque", "torque", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"alpha", "alpha", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fogstart", "fogstart", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fogend", "fogend", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"znear", "znear", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"zfar", "zfar", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"haze", "haze", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"shadowclip", "shadowclip", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"shadowscale", "shadowscale", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatortendon", "actuatortendon", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_VisualScale[] = { + {"forcewidth", "forcewidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"contactwidth", "contactwidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"contactheight", "contactheight", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"connect", "connect", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"com", "com", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"camera", "camera", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"light", "light", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"selectpoint", "selectpoint", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"jointlength", "jointlength", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"jointwidth", "jointwidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatorlength", "actuatorlength", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatorwidth", "actuatorwidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"framelength", "framelength", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"framewidth", "framewidth", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"constraint", "constraint", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"slidercrank", "slidercrank", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"frustum", "frustum", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_VisualRgba[] = { + {"fog", "fog", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"haze", "haze", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"force", "force", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"inertia", "inertia", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"joint", "joint", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"actuator", "actuator", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"actuatornegative", "actuatornegative", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"actuatorpositive", "actuatorpositive", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"com", "com", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"camera", "camera", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"light", "light", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"selectpoint", "selectpoint", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"connect", "connect", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"contactpoint", "contactpoint", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"contactforce", "contactforce", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"contactfriction", "contactfriction", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"contacttorque", "contacttorque", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"contactgap", "contactgap", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"rangefinder", "rangefinder", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"constraint", "constraint", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"slidercrank", "slidercrank", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"crankbroken", "crankbroken", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"frustum", "frustum", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"bv", "bv", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"bvactive", "bvactive", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Default[] = { + {"dclass", "class", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Default[] = { + {"subclasses", "Default", false, Cardinality::ZeroOrMore}, + {"mesh", "Mesh", false, Cardinality::ZeroOrOne}, + {"material", "Material", false, Cardinality::ZeroOrOne}, + {"joint", "Joint", false, Cardinality::ZeroOrOne}, + {"geom", "Geom", false, Cardinality::ZeroOrOne}, + {"site", "Site", false, Cardinality::ZeroOrOne}, + {"camera", "Camera", false, Cardinality::ZeroOrOne}, + {"light", "Light", false, Cardinality::ZeroOrOne}, + {"pair", "Pair", false, Cardinality::ZeroOrOne}, + {"equality", "EqualityDefault", false, Cardinality::ZeroOrOne}, + {"tendon", "TendonDefault", false, Cardinality::ZeroOrOne}, + {"general", "ActuatorGeneral", false, Cardinality::ZeroOrOne}, + {"motor", "Motor", false, Cardinality::ZeroOrOne}, + {"position", "Position", false, Cardinality::ZeroOrOne}, + {"velocity", "Velocity", false, Cardinality::ZeroOrOne}, + {"intvelocity", "IntVelocity", false, Cardinality::ZeroOrOne}, + {"orientation", "OrientationActuator", false, Cardinality::ZeroOrOne}, + {"pid", "Pid", false, Cardinality::ZeroOrOne}, + {"damper", "Damper", false, Cardinality::ZeroOrOne}, + {"cylinder", "Cylinder", false, Cardinality::ZeroOrOne}, + {"muscle", "Muscle", false, Cardinality::ZeroOrOne}, + {"adhesion", "Adhesion", false, Cardinality::ZeroOrOne}, + {"dcmotor", "DcMotor", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_MaterialLayer[] = { + {"texture", "texture", "Texture", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"role", "role", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Extension[] = { + {"pluginDefs", "PluginDef", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_PluginDef[] = { + {"plugin", "plugin", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_PluginDef[] = { + {"pluginInstances", "PluginInstance", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_PluginInstance[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_PluginInstance[] = { + {"config", "Config", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Config[] = { + {"key", "key", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"value", "value", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Asset[] = { + {"meshes", "Mesh", false, Cardinality::ZeroOrMore}, + {"hfields", "Hfield", false, Cardinality::ZeroOrMore}, + {"skins", "Skin", false, Cardinality::ZeroOrMore}, + {"textures", "Texture", false, Cardinality::ZeroOrMore}, + {"materials", "Material", false, Cardinality::ZeroOrMore}, + {"modelAssets", "ModelAsset", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Mesh[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"content_type", "content_type", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"vertex", "vertex", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"normal", "normal", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"texcoord", "texcoord", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"face", "face", "int32", FieldKind::Int32, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"refpos", "refpos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"refquat", "refquat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"scale", "scale", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"smoothnormal", "smoothnormal", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"maxhullvert", "maxhullvert", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"inertia", "inertia", "MeshInertia", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"builtin", "builtin", "MeshBuiltin", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"params", "params", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Mesh[] = { + {"plugin", "PluginRef", false, Cardinality::ZeroOrMore}, +}; +constexpr int kBundle_Mesh_0_0[] = { 14 }; +constexpr int kBundle_Mesh_0_1[] = { 3 }; +constexpr ConstraintBundle kBundles_Mesh_0[] = { + {kBundle_Mesh_0_0, 1}, + {kBundle_Mesh_0_1, 1}, +}; +constexpr int kBundle_Mesh_1_0[] = { 14 }; +constexpr int kBundle_Mesh_1_1[] = { 4 }; +constexpr ConstraintBundle kBundles_Mesh_1[] = { + {kBundle_Mesh_1_0, 1}, + {kBundle_Mesh_1_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Mesh[] = { + {ConstraintKind::Exclusive, kBundles_Mesh_0, 2, ""}, + {ConstraintKind::Exclusive, kBundles_Mesh_1, 2, ""}, +}; + +constexpr FieldDescriptor kFields_PluginRef[] = { + {"plugin", "plugin", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"instance", "instance", "PluginInstance", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_PluginRef[] = { + {"config", "Config", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Hfield[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"content_type", "content_type", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nrow", "nrow", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ncol", "ncol", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, false, true, "", ""}, + {"elevation", "elevation", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", "flipped and zero-filled"}, +}; + +constexpr FieldDescriptor kFields_Skin[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"inflate", "inflate", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"vertex", "vertex", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"texcoord", "texcoord", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"face", "face", "int32", FieldKind::Int32, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; +constexpr ChildDescriptor kChildren_Skin[] = { + {"bones", "SkinBone", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_SkinBone[] = { + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"bindpos", "bindpos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"bindquat", "bindquat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"vertid", "vertid", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"vertweight", "vertweight", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Texture[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "TextureType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"colorspace", "colorspace", "ColorSpace", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"content_type", "content_type", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"gridsize", "gridsize", "int32", FieldKind::Int32, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gridlayout", "gridlayout", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, true, "", "length must equal the gridsize product"}, + {"fileright", "fileright", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fileleft", "fileleft", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fileup", "fileup", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"filedown", "filedown", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"filefront", "filefront", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fileback", "fileback", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"builtin", "builtin", "TextureBuiltin", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"rgb1", "rgb1", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"rgb2", "rgb2", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"mark", "mark", "TextureMark", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"markrgb", "markrgb", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"random", "random", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"width", "width", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"height", "height", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"hflip", "hflip", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"vflip", "vflip", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nchannel", "nchannel", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Material[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"texrepeat", "texrepeat", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"texuniform", "texuniform", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"emission", "emission", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"specular", "specular", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"shininess", "shininess", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"reflectance", "reflectance", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"metallic", "metallic", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"roughness", "roughness", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, +}; +constexpr ChildDescriptor kChildren_Material[] = { + {"layers", "MaterialLayer", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_ModelAsset[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"content_type", "content_type", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Body[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"childclass", "childclass", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"mocap", "mocap", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gravcomp", "gravcomp", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"sleep", "sleep", "BodySleep", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"simple", "simple", "SimpleMode", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Body[] = { + {"subtree", "BodyChildAny", true, Cardinality::ZeroOrMore}, + {"inertial", "Inertial", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_Inertial[] = { + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, false, false, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"mass", "mass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, false, true, "", ""}, + {"diaginertia", "diaginertia", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Joint[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "JointType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", "saved unless free"}, + {"axis", "axis", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", "saved for slide/hinge"}, + {"springdamper", "springdamper", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", "compile directive: saved as stiffness/damping"}, + {"limited", "limited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", "saved unless free"}, + {"actuatorfrclimited", "actuatorfrclimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", "saved for slide/hinge"}, + {"solreflimit", "solreflimit", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimplimit", "solimplimit", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimpfriction", "solimpfriction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "spring polynomial, 1+mjNPOLY coefficients"}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actuatorfrcrange", "actuatorfrcrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actuatorgravcomp", "actuatorgravcomp", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ref", "ref", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"springref", "springref", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"frictionloss", "frictionloss", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_FreeJoint[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"align", "align", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Geom[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "GeomType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"contype", "contype", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"conaffinity", "conaffinity", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"condim", "condim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"priority", "priority", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "saved length is type-dependent"}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"friction", "friction", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", ""}, + {"mass", "mass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "mass/density: one is saved"}, + {"density", "density", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"shellinertia", "shellinertia", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", "saved unless mesh"}, + {"solmix", "solmix", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gap", "gap", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"surfacevel", "surfacevel", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"adhesion", "adhesion", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fromto", "fromto", "double", FieldKind::Double, ArityKind::Fixed, 6, 6, true, false, "", "compile directive: saved as pos/quat/size"}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", "saved in the mesh-corrected frame"}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"hfield", "hfield", "Hfield", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"mesh", "mesh", "Mesh", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fitscale", "fitscale", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", "compile directive: not saved"}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"fluidshape", "fluidshape", "FluidShape", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fluidcoef", "fluidcoef", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Geom[] = { + {"plugin", "PluginRef", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Attach[] = { + {"model", "model", "ModelAsset", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"frame", "frame", "Frame", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"prefix", "prefix", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; +constexpr int kBundle_Attach_0_0[] = { 1 }; +constexpr int kBundle_Attach_0_1[] = { 2 }; +constexpr ConstraintBundle kBundles_Attach_0[] = { + {kBundle_Attach_0_0, 1}, + {kBundle_Attach_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Attach[] = { + {ConstraintKind::Exclusive, kBundles_Attach_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Site[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "GeomType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "saved length is type-dependent"}, + {"fromto", "fromto", "double", FieldKind::Double, ArityKind::Fixed, 6, 6, true, false, "", "compile directive: saved as pos/quat/size"}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Camera[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"projection", "projection", "CameraProjection", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"fovy", "fovy", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", "fovy or the intrinsics family is saved"}, + {"ipd", "ipd", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"resolution", "resolution", "int32", FieldKind::Int32, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"output", "output", "CameraOutput", FieldKind::Enum, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"mode", "mode", "CamLightMode", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"target", "target", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"focal", "focal", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"focalpixel", "focalpixel", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"principal", "principal", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"principalpixel", "principalpixel", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"sensorsize", "sensorsize", "float", FieldKind::Float, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Camera_0_0[] = { 3 }; +constexpr int kBundle_Camera_0_1[] = { 15 }; +constexpr ConstraintBundle kBundles_Camera_0[] = { + {kBundle_Camera_0_0, 1}, + {kBundle_Camera_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Camera[] = { + {ConstraintKind::Exclusive, kBundles_Camera_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Light[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "LightType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"castshadow", "castshadow", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"dir", "dir", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"bulbradius", "bulbradius", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"intensity", "intensity", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"range", "range", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"attenuation", "attenuation", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"cutoff", "cutoff", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"exponent", "exponent", "float", FieldKind::Float, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ambient", "ambient", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"diffuse", "diffuse", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"specular", "specular", "float", FieldKind::Float, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"mode", "mode", "CamLightMode", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"target", "target", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"texture", "texture", "Texture", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Composite[] = { + {"prefix", "prefix", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"type", "type", "CompositeType", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"count", "count", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"offset", "offset", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"vertex", "vertex", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"initial", "initial", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"curve", "curve", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Composite[] = { + {"compositeJoints", "CompositeJoint", false, Cardinality::ZeroOrMore}, + {"compositeSkins", "CompositeSkin", false, Cardinality::ZeroOrOne}, + {"compositeGeoms", "CompositeGeom", false, Cardinality::ZeroOrOne}, + {"compositeSites", "CompositeSite", false, Cardinality::ZeroOrOne}, + {"plugin", "PluginRef", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_CompositeJoint[] = { + {"kind", "kind", "JointKind", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solreffix", "solreffix", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"solimpfix", "solimpfix", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, + {"type", "type", "JointType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"axis", "axis", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"limited", "limited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, false, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solreflimit", "solreflimit", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"solimplimit", "solimplimit", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, + {"frictionloss", "frictionloss", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"solimpfriction", "solimpfriction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_CompositeSkin[] = { + {"texcoord", "texcoord", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"inflate", "inflate", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"subgrid", "subgrid", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_CompositeGeom[] = { + {"type", "type", "GeomType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"contype", "contype", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"conaffinity", "conaffinity", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"condim", "condim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"priority", "priority", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"friction", "friction", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"mass", "mass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"density", "density", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solmix", "solmix", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"gap", "gap", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"surfacevel", "surfacevel", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, false, "", ""}, + {"adhesion", "adhesion", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_CompositeSite[] = { + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"size", "size", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Flexcomp[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"type", "type", "FlexcompType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dim", "dim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dof", "dof", "FlexDof", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"count", "count", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"cellcount", "cellcount", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"spacing", "spacing", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"radius", "radius", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rigid", "rigid", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"mass", "mass", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"inertiabox", "inertiabox", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"scale", "scale", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"file", "file", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"point", "point", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"element", "element", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"texcoord", "texcoord", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"flatskin", "flatskin", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"axisangle", "axisangle", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, false, "", ""}, + {"xyaxes", "xyaxes", "double", FieldKind::Double, ArityKind::Fixed, 6, 6, true, false, "", ""}, + {"zaxis", "zaxis", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"euler", "euler", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"origin", "origin", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Flexcomp[] = { + {"flexcompEdges", "FlexcompEdge", false, Cardinality::ZeroOrOne}, + {"flexElasticities", "FlexElasticity", false, Cardinality::ZeroOrOne}, + {"flexContacts", "FlexContact", false, Cardinality::ZeroOrOne}, + {"flexcompPins", "FlexcompPin", false, Cardinality::ZeroOrMore}, + {"plugin", "PluginRef", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_FlexcompEdge[] = { + {"equality", "equality", "FlexEquality", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_FlexElasticity[] = { + {"young", "young", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"poisson", "poisson", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"thickness", "thickness", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"elastic2d", "elastic2d", "Elastic2D", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_FlexContact[] = { + {"contype", "contype", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"conaffinity", "conaffinity", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"condim", "condim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"priority", "priority", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"friction", "friction", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", ""}, + {"solmix", "solmix", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gap", "gap", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"internal", "internal", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"selfcollide", "selfcollide", "FlexSelfCollide", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"activelayers", "activelayers", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"passive", "passive", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_FlexcompPin[] = { + {"id", "id", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"grid", "grid", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"gridrange", "gridrange", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Deformable[] = { + {"flexs", "Flex", false, Cardinality::ZeroOrMore}, + {"skins", "Skin", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Flex[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"dim", "dim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"radius", "radius", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"flatskin", "flatskin", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"body", "body", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", "space-separated body names"}, + {"vertex", "vertex", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"element", "element", "int32", FieldKind::Int32, ArityKind::Unbounded, 0, 0, false, false, "", ""}, + {"texcoord", "texcoord", "float", FieldKind::Float, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"elemtexcoord", "elemtexcoord", "int32", FieldKind::Int32, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"node", "node", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", "space-separated body names"}, + {"cellcount", "cellcount", "int32", FieldKind::Int32, ArityKind::Fixed, 3, 3, true, true, "", "seeded to {1,1,1} before reading"}, + {"dof", "dof", "FlexDof", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", "lowers to interpolation order"}, +}; +constexpr ChildDescriptor kChildren_Flex[] = { + {"flexContacts", "FlexContact", false, Cardinality::ZeroOrOne}, + {"flexEdges", "FlexEdge", false, Cardinality::ZeroOrOne}, + {"flexElasticities", "FlexElasticity", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_FlexEdge[] = { + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Contact[] = { + {"pairs", "Pair", false, Cardinality::ZeroOrMore}, + {"excludes", "Exclude", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Pair[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom1", "geom1", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom2", "geom2", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"condim", "condim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"friction", "friction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"gap", "gap", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"adhesion", "adhesion", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Exclude[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Tendon[] = { + {"tendons", "TendonAny", true, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Spatial[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"limited", "limited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatorfrclimited", "actuatorfrclimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actuatorfrcrange", "actuatorfrcrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"solreflimit", "solreflimit", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimplimit", "solimplimit", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimpfriction", "solimpfriction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"frictionloss", "frictionloss", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"springlength", "springlength", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", "one value: copied to both"}, + {"width", "width", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "spring polynomial, 1+mjNPOLY coefficients"}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"rgba", "rgba", "float", FieldKind::Float, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Spatial[] = { + {"path", "PathItemAny", true, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_SpatialSite[] = { + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_SpatialGeom[] = { + {"geom", "geom", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"sidesite", "sidesite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Pulley[] = { + {"divisor", "divisor", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Fixed[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"limited", "limited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actuatorfrclimited", "actuatorfrclimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actuatorfrcrange", "actuatorfrcrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"solreflimit", "solreflimit", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimplimit", "solimplimit", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimpfriction", "solimpfriction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"frictionloss", "frictionloss", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"springlength", "springlength", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", "one value: copied to both"}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "spring polynomial, 1+mjNPOLY coefficients"}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Fixed[] = { + {"fixedJoints", "FixedJoint", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_FixedJoint[] = { + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"coef", "coef", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Equality[] = { + {"equalities", "EqualityAny", true, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Connect[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"anchor", "anchor", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"site1", "site1", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site2", "site2", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Connect_0_0[] = { 8, 9 }; +constexpr int kBundle_Connect_0_1[] = { 5, 6, 7 }; +constexpr ConstraintBundle kBundles_Connect_0[] = { + {kBundle_Connect_0_0, 2}, + {kBundle_Connect_0_1, 3}, +}; +constexpr int kBundle_Connect_1_0[] = { 8, 9 }; +constexpr int kBundle_Connect_1_1[] = { 5, 7 }; +constexpr ConstraintBundle kBundles_Connect_1[] = { + {kBundle_Connect_1_0, 2}, + {kBundle_Connect_1_1, 2}, +}; +constexpr int kBundle_Connect_2_0[] = { 8 }; +constexpr int kBundle_Connect_2_1[] = { 9 }; +constexpr ConstraintBundle kBundles_Connect_2[] = { + {kBundle_Connect_2_0, 1}, + {kBundle_Connect_2_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Connect[] = { + {ConstraintKind::Exclusive, kBundles_Connect_0, 2, "site and body semantics cannot mix"}, + {ConstraintKind::OneOf, kBundles_Connect_1, 2, ""}, + {ConstraintKind::Together, kBundles_Connect_2, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Weld[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"relpose", "relpose", "double", FieldKind::Double, ArityKind::Fixed, 7, 7, true, false, "", ""}, + {"anchor", "anchor", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"site1", "site1", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site2", "site2", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"torquescale", "torquescale", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Weld_0_0[] = { 9, 10 }; +constexpr int kBundle_Weld_0_1[] = { 5, 6, 8, 7 }; +constexpr ConstraintBundle kBundles_Weld_0[] = { + {kBundle_Weld_0_0, 2}, + {kBundle_Weld_0_1, 4}, +}; +constexpr int kBundle_Weld_1_0[] = { 9, 10 }; +constexpr int kBundle_Weld_1_1[] = { 5 }; +constexpr ConstraintBundle kBundles_Weld_1[] = { + {kBundle_Weld_1_0, 2}, + {kBundle_Weld_1_1, 1}, +}; +constexpr int kBundle_Weld_2_0[] = { 9 }; +constexpr int kBundle_Weld_2_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Weld_2[] = { + {kBundle_Weld_2_0, 1}, + {kBundle_Weld_2_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Weld[] = { + {ConstraintKind::Exclusive, kBundles_Weld_0, 2, ""}, + {ConstraintKind::OneOf, kBundles_Weld_1, 2, ""}, + {ConstraintKind::Together, kBundles_Weld_2, 2, ""}, +}; + +constexpr FieldDescriptor kFields_EqualityJoint[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"joint1", "joint1", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"joint2", "joint2", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"polycoef", "polycoef", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_EqualityTendon[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"tendon1", "tendon1", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"tendon2", "tendon2", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"polycoef", "polycoef", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_EqualityFlex[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"flex", "flex", "FlexAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Flexvert[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"flex", "flex", "FlexAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Flexstrain[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"flex", "flex", "FlexAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"cell", "cell", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Actuator[] = { + {"actuators", "ActuatorAny", true, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_ActuatorGeneral[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actlimited", "actlimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actrange", "actrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", "transmission target"}, + {"actdim", "actdim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", "saved default depends on dyntype"}, + {"input", "input", "InputChart", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", "so3 chart keyword, or servo token subset"}, + {"velrange", "velrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"ffrange", "ffrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"dyntype", "dyntype", "DynType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"gaintype", "gaintype", "GainType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", "gain/bias family is not"}, + {"biastype", "biastype", "BiasType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", "saved for plugin actuators"}, + {"dynprm", "dynprm", "double", FieldKind::Double, ArityKind::Range, 1, 10, true, true, "", ""}, + {"gainprm", "gainprm", "double", FieldKind::Double, ArityKind::Range, 1, 10, true, true, "", ""}, + {"biasprm", "biasprm", "double", FieldKind::Double, ArityKind::Range, 1, 10, true, true, "", ""}, + {"actearly", "actearly", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_Motor[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Position[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"inheritrange", "inheritrange", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kp", "kp", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dampratio", "dampratio", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"timeconst", "timeconst", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Velocity[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_IntVelocity[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actlimited", "actlimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actrange", "actrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"inheritrange", "inheritrange", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kp", "kp", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dampratio", "dampratio", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_OrientationActuator[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kp", "kp", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dampratio", "dampratio", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"input", "input", "InputChart", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Pid[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"posrange", "posrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", "alias: the position-setpoint range"}, + {"velrange", "velrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"ffrange", "ffrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"inheritrange", "inheritrange", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kp", "kp", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dampratio", "dampratio", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"ki", "ki", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"imax", "imax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slewmax", "slewmax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"input", "input", "InputBit", FieldKind::Enum, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Damper[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"kv", "kv", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Cylinder[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"timeconst", "timeconst", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"area", "area", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"bias", "bias", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Muscle[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"timeconst", "timeconst", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, false, "", ""}, + {"tausmooth", "tausmooth", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, false, "", ""}, + {"force", "force", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"scale", "scale", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"lmin", "lmin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"lmax", "lmax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"vmax", "vmax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fpmax", "fpmax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"fvmax", "fvmax", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Adhesion[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"gain", "gain", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_DcMotor[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refsite", "refsite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"motorconst", "motorconst", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"resistance", "resistance", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nominal", "nominal", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"saturation", "saturation", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"inductance", "inductance", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, false, "", ""}, + {"cogging", "cogging", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, false, "", ""}, + {"controller", "controller", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, false, "", ""}, + {"thermal", "thermal", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, false, "", ""}, + {"lugre", "lugre", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, false, "", ""}, + {"input", "input", "DcMotorInput", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_ActuatorPlugin[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"dclass", "class", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"ctrlrange", "ctrlrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"plugin", "plugin", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"instance", "instance", "PluginInstance", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"ctrllimited", "ctrllimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcelimited", "forcelimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"actlimited", "actlimited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"forcerange", "forcerange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"actrange", "actrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"lengthrange", "lengthrange", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"gear", "gear", "double", FieldKind::Double, ArityKind::Range, 1, 6, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Range, 1, 3, true, true, "", "damper polynomial, 1+mjNPOLY coefficients"}, + {"armature", "armature", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cranklength", "cranklength", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", "slidercrank-only, validated"}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"jointinparent", "jointinparent", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"actdim", "actdim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"dyntype", "dyntype", "DynType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"dynprm", "dynprm", "double", FieldKind::Double, ArityKind::Range, 1, 10, true, true, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cranksite", "cranksite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"slidersite", "slidersite", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"actearly", "actearly", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, +}; +constexpr ChildDescriptor kChildren_ActuatorPlugin[] = { + {"config", "Config", false, Cardinality::ZeroOrMore}, +}; + +constexpr ChildDescriptor kChildren_Sensor[] = { + {"sensors", "SensorAny", true, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Touch[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Accelerometer[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Velocimeter[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Gyro[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Force[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Torque[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Magnetometer[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Camprojection[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"camera", "camera", "Camera", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Rangefinder[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"camera", "camera", "Camera", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"data", "data", "RayData", FieldKind::Enum, ArityKind::Unbounded, 0, 0, true, false, "", "ordering-checked"}, +}; +constexpr int kBundle_Rangefinder_0_0[] = { 8 }; +constexpr int kBundle_Rangefinder_0_1[] = { 9 }; +constexpr ConstraintBundle kBundles_Rangefinder_0[] = { + {kBundle_Rangefinder_0_0, 1}, + {kBundle_Rangefinder_0_1, 1}, +}; +constexpr int kBundle_Rangefinder_1_0[] = { 8 }; +constexpr int kBundle_Rangefinder_1_1[] = { 9 }; +constexpr ConstraintBundle kBundles_Rangefinder_1[] = { + {kBundle_Rangefinder_1_0, 1}, + {kBundle_Rangefinder_1_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Rangefinder[] = { + {ConstraintKind::Exclusive, kBundles_Rangefinder_0, 2, ""}, + {ConstraintKind::OneOf, kBundles_Rangefinder_1, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Jointpos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Jointvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonpos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Actuatorpos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"actuator", "actuator", "ActuatorAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Actuatorvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"actuator", "actuator", "ActuatorAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Actuatorfrc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"actuator", "actuator", "ActuatorAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Jointactuatorfrc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonactuatorfrc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Ballquat[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Ballangvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Jointlimitpos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Jointlimitvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Jointlimitfrc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"joint", "joint", "JointAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonlimitpos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonlimitvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tendonlimitfrc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"tendon", "tendon", "TendonAny", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Framepos[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Framepos_0_0[] = { 10 }; +constexpr int kBundle_Framepos_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Framepos_0[] = { + {kBundle_Framepos_0_0, 1}, + {kBundle_Framepos_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Framepos[] = { + {ConstraintKind::Together, kBundles_Framepos_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Framequat[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Framequat_0_0[] = { 10 }; +constexpr int kBundle_Framequat_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Framequat_0[] = { + {kBundle_Framequat_0_0, 1}, + {kBundle_Framequat_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Framequat[] = { + {ConstraintKind::Together, kBundles_Framequat_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Framexaxis[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Framexaxis_0_0[] = { 10 }; +constexpr int kBundle_Framexaxis_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Framexaxis_0[] = { + {kBundle_Framexaxis_0_0, 1}, + {kBundle_Framexaxis_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Framexaxis[] = { + {ConstraintKind::Together, kBundles_Framexaxis_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Frameyaxis[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Frameyaxis_0_0[] = { 10 }; +constexpr int kBundle_Frameyaxis_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Frameyaxis_0[] = { + {kBundle_Frameyaxis_0_0, 1}, + {kBundle_Frameyaxis_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Frameyaxis[] = { + {ConstraintKind::Together, kBundles_Frameyaxis_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Framezaxis[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Framezaxis_0_0[] = { 10 }; +constexpr int kBundle_Framezaxis_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Framezaxis_0[] = { + {kBundle_Framezaxis_0_0, 1}, + {kBundle_Framezaxis_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Framezaxis[] = { + {ConstraintKind::Together, kBundles_Framezaxis_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Framelinvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Framelinvel_0_0[] = { 10 }; +constexpr int kBundle_Framelinvel_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Framelinvel_0[] = { + {kBundle_Framelinvel_0_0, 1}, + {kBundle_Framelinvel_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Framelinvel[] = { + {ConstraintKind::Together, kBundles_Framelinvel_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Frameangvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"reftype", "reftype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, +}; +constexpr int kBundle_Frameangvel_0_0[] = { 10 }; +constexpr int kBundle_Frameangvel_0_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Frameangvel_0[] = { + {kBundle_Frameangvel_0_0, 1}, + {kBundle_Frameangvel_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Frameangvel[] = { + {ConstraintKind::Together, kBundles_Frameangvel_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Framelinacc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, +}; + +constexpr FieldDescriptor kFields_Frameangacc[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, +}; + +constexpr FieldDescriptor kFields_Subtreecom[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Subtreelinvel[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Subtreeangmom[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"body", "body", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Insidesite[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objtype", "objtype", "FrameObject", FieldKind::Enum, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, +}; + +constexpr FieldDescriptor kFields_Distance[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"geom1", "geom1", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom2", "geom2", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Distance_0_0[] = { 8 }; +constexpr int kBundle_Distance_0_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Distance_0[] = { + {kBundle_Distance_0_0, 1}, + {kBundle_Distance_0_1, 1}, +}; +constexpr int kBundle_Distance_1_0[] = { 8 }; +constexpr int kBundle_Distance_1_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Distance_1[] = { + {kBundle_Distance_1_0, 1}, + {kBundle_Distance_1_1, 1}, +}; +constexpr int kBundle_Distance_2_0[] = { 9 }; +constexpr int kBundle_Distance_2_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Distance_2[] = { + {kBundle_Distance_2_0, 1}, + {kBundle_Distance_2_1, 1}, +}; +constexpr int kBundle_Distance_3_0[] = { 9 }; +constexpr int kBundle_Distance_3_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Distance_3[] = { + {kBundle_Distance_3_0, 1}, + {kBundle_Distance_3_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Distance[] = { + {ConstraintKind::Exclusive, kBundles_Distance_0, 2, ""}, + {ConstraintKind::OneOf, kBundles_Distance_1, 2, ""}, + {ConstraintKind::Exclusive, kBundles_Distance_2, 2, ""}, + {ConstraintKind::OneOf, kBundles_Distance_3, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Normal[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"geom1", "geom1", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom2", "geom2", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Normal_0_0[] = { 8 }; +constexpr int kBundle_Normal_0_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Normal_0[] = { + {kBundle_Normal_0_0, 1}, + {kBundle_Normal_0_1, 1}, +}; +constexpr int kBundle_Normal_1_0[] = { 8 }; +constexpr int kBundle_Normal_1_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Normal_1[] = { + {kBundle_Normal_1_0, 1}, + {kBundle_Normal_1_1, 1}, +}; +constexpr int kBundle_Normal_2_0[] = { 9 }; +constexpr int kBundle_Normal_2_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Normal_2[] = { + {kBundle_Normal_2_0, 1}, + {kBundle_Normal_2_1, 1}, +}; +constexpr int kBundle_Normal_3_0[] = { 9 }; +constexpr int kBundle_Normal_3_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Normal_3[] = { + {kBundle_Normal_3_0, 1}, + {kBundle_Normal_3_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Normal[] = { + {ConstraintKind::Exclusive, kBundles_Normal_0, 2, ""}, + {ConstraintKind::OneOf, kBundles_Normal_1, 2, ""}, + {ConstraintKind::Exclusive, kBundles_Normal_2, 2, ""}, + {ConstraintKind::OneOf, kBundles_Normal_3, 2, ""}, +}; + +constexpr FieldDescriptor kFields_Fromto[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"geom1", "geom1", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom2", "geom2", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_Fromto_0_0[] = { 8 }; +constexpr int kBundle_Fromto_0_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Fromto_0[] = { + {kBundle_Fromto_0_0, 1}, + {kBundle_Fromto_0_1, 1}, +}; +constexpr int kBundle_Fromto_1_0[] = { 8 }; +constexpr int kBundle_Fromto_1_1[] = { 10 }; +constexpr ConstraintBundle kBundles_Fromto_1[] = { + {kBundle_Fromto_1_0, 1}, + {kBundle_Fromto_1_1, 1}, +}; +constexpr int kBundle_Fromto_2_0[] = { 9 }; +constexpr int kBundle_Fromto_2_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Fromto_2[] = { + {kBundle_Fromto_2_0, 1}, + {kBundle_Fromto_2_1, 1}, +}; +constexpr int kBundle_Fromto_3_0[] = { 9 }; +constexpr int kBundle_Fromto_3_1[] = { 11 }; +constexpr ConstraintBundle kBundles_Fromto_3[] = { + {kBundle_Fromto_3_0, 1}, + {kBundle_Fromto_3_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_Fromto[] = { + {ConstraintKind::Exclusive, kBundles_Fromto_0, 2, ""}, + {ConstraintKind::OneOf, kBundles_Fromto_1, 2, ""}, + {ConstraintKind::Exclusive, kBundles_Fromto_2, 2, ""}, + {ConstraintKind::OneOf, kBundles_Fromto_3, 2, ""}, +}; + +constexpr FieldDescriptor kFields_SensorContact[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"geom1", "geom1", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom2", "geom2", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body1", "body1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"body2", "body2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"subtree1", "subtree1", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"subtree2", "subtree2", "Body", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"site", "site", "Site", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"num", "num", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"data", "data", "ContactData", FieldKind::Enum, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"reduce", "reduce", "ContactReduce", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_SensorContact_0_0[] = { 8 }; +constexpr int kBundle_SensorContact_0_1[] = { 10 }; +constexpr int kBundle_SensorContact_0_2[] = { 12 }; +constexpr int kBundle_SensorContact_0_3[] = { 14 }; +constexpr ConstraintBundle kBundles_SensorContact_0[] = { + {kBundle_SensorContact_0_0, 1}, + {kBundle_SensorContact_0_1, 1}, + {kBundle_SensorContact_0_2, 1}, + {kBundle_SensorContact_0_3, 1}, +}; +constexpr int kBundle_SensorContact_1_0[] = { 9 }; +constexpr int kBundle_SensorContact_1_1[] = { 11 }; +constexpr int kBundle_SensorContact_1_2[] = { 13 }; +constexpr ConstraintBundle kBundles_SensorContact_1[] = { + {kBundle_SensorContact_1_0, 1}, + {kBundle_SensorContact_1_1, 1}, + {kBundle_SensorContact_1_2, 1}, +}; +constexpr ConstraintDescriptor kConstraints_SensorContact[] = { + {ConstraintKind::Exclusive, kBundles_SensorContact_0, 4, ""}, + {ConstraintKind::Exclusive, kBundles_SensorContact_1, 3, ""}, +}; + +constexpr FieldDescriptor kFields_EPotential[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_EKinetic[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Clock[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tactile[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"geom", "geom", "Geom", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"mesh", "mesh", "Mesh", FieldKind::Ref, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"nsample", "nsample", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interp", "interp", "InterpType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"delay", "delay", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"interval", "interval", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_SensorUser[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"objtype", "objtype", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "objtype", ""}, + {"datatype", "datatype", "DataType", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"needstage", "needstage", "NeedStage", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"dim", "dim", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"noise", "noise", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr int kBundle_SensorUser_0_0[] = { 1 }; +constexpr int kBundle_SensorUser_0_1[] = { 2 }; +constexpr ConstraintBundle kBundles_SensorUser_0[] = { + {kBundle_SensorUser_0_0, 1}, + {kBundle_SensorUser_0_1, 1}, +}; +constexpr ConstraintDescriptor kConstraints_SensorUser[] = { + {ConstraintKind::Together, kBundles_SensorUser_0, 2, ""}, +}; + +constexpr FieldDescriptor kFields_SensorPlugin[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"plugin", "plugin", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"instance", "instance", "PluginInstance", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"cutoff", "cutoff", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"objtype", "objtype", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", "objtype/objname, reftype/refname: pairwise"}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "objtype", "co-occurrence enforced by the reader"}, + {"reftype", "reftype", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"refname", "refname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "reftype", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_SensorPlugin[] = { + {"config", "Config", false, Cardinality::ZeroOrMore}, +}; + +constexpr ChildDescriptor kChildren_Custom[] = { + {"numerics", "Numeric", false, Cardinality::ZeroOrMore}, + {"texts", "Text", false, Cardinality::ZeroOrMore}, + {"tuples", "Tuple", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Numeric[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"size", "size", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"data", "data", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Text[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"data", "data", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Tuple[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Tuple[] = { + {"tupleElements", "TupleElement", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_TupleElement[] = { + {"objtype", "objtype", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"objname", "objname", "string", FieldKind::String, ArityKind::Scalar, 0, 0, false, false, "objtype", ""}, + {"prm", "prm", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; + +constexpr ChildDescriptor kChildren_Keyframe[] = { + {"keys", "Key", false, Cardinality::ZeroOrMore}, +}; + +constexpr FieldDescriptor kFields_Key[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", "set even when absent"}, + {"time", "time", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"qpos", "qpos", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"qvel", "qvel", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"act", "act", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"mpos", "mpos", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"mquat", "mquat", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, + {"ctrl", "ctrl", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +constexpr FieldDescriptor kFields_Frame[] = { + {"name", "name", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"childclass", "childclass", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"pos", "pos", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, true, "", ""}, + {"quat", "quat", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, +}; +constexpr ChildDescriptor kChildren_Frame[] = { + {"subtree", "BodyChildAny", true, Cardinality::ZeroOrMore}, + {"inertial", "Inertial", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_Replicate[] = { + {"count", "count", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, false, false, "", ""}, + {"offset", "offset", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"euler", "euler", "double", FieldKind::Double, ArityKind::Fixed, 3, 3, true, false, "", ""}, + {"sep", "sep", "string", FieldKind::String, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"childclass", "childclass", "Default", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, +}; +constexpr ChildDescriptor kChildren_Replicate[] = { + {"subtree", "BodyChildAny", true, Cardinality::ZeroOrMore}, + {"inertial", "Inertial", false, Cardinality::ZeroOrOne}, +}; + +constexpr FieldDescriptor kFields_EqualityDefault[] = { + {"active", "active", "bool", FieldKind::Bool, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"solref", "solref", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimp", "solimp", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, +}; + +constexpr FieldDescriptor kFields_TendonDefault[] = { + {"group", "group", "int32", FieldKind::Int32, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"limited", "limited", "TriState", FieldKind::Enum, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"range", "range", "double", FieldKind::Double, ArityKind::Fixed, 2, 2, true, true, "", ""}, + {"solreflimit", "solreflimit", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimplimit", "solimplimit", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"solreffriction", "solreffriction", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"solimpfriction", "solimpfriction", "double", FieldKind::Double, ArityKind::Range, 1, 5, true, true, "", ""}, + {"frictionloss", "frictionloss", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"springlength", "springlength", "double", FieldKind::Double, ArityKind::Range, 1, 2, true, true, "", ""}, + {"width", "width", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"material", "material", "Material", FieldKind::Ref, ArityKind::Scalar, 0, 0, true, false, "", ""}, + {"margin", "margin", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"stiffness", "stiffness", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"damping", "damping", "double", FieldKind::Double, ArityKind::Scalar, 0, 0, true, true, "", ""}, + {"rgba", "rgba", "double", FieldKind::Double, ArityKind::Fixed, 4, 4, true, true, "", ""}, + {"user", "user", "double", FieldKind::Double, ArityKind::Unbounded, 0, 0, true, false, "", ""}, +}; + +bool Present_Model(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.model.has_value(); + default: return false; + } +} +void Clear_Model(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.model.reset(); break; + default: break; + } +} + +bool Present_Compiler(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.autolimits.has_value(); + case 1: return e.boundmass.has_value(); + case 2: return e.boundinertia.has_value(); + case 3: return e.settotalmass.has_value(); + case 4: return e.balanceinertia.has_value(); + case 5: return e.strippath.has_value(); + case 6: return e.coordinate.has_value(); + case 7: return e.angle.has_value(); + case 8: return e.fitaabb.has_value(); + case 9: return e.eulerseq.has_value(); + case 10: return e.meshdir.has_value(); + case 11: return e.texturedir.has_value(); + case 12: return e.discardvisual.has_value(); + case 13: return e.usethread.has_value(); + case 14: return e.fusestatic.has_value(); + case 15: return e.inertiafromgeom.has_value(); + case 16: return e.inertiagrouprange.has_value(); + case 17: return e.saveinertial.has_value(); + case 18: return e.assetdir.has_value(); + case 19: return e.alignfree.has_value(); + case 20: return e.conflict.has_value(); + default: return false; + } +} +void Clear_Compiler(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.autolimits.reset(); break; + case 1: e.boundmass.reset(); break; + case 2: e.boundinertia.reset(); break; + case 3: e.settotalmass.reset(); break; + case 4: e.balanceinertia.reset(); break; + case 5: e.strippath.reset(); break; + case 6: e.coordinate.reset(); break; + case 7: e.angle.reset(); break; + case 8: e.fitaabb.reset(); break; + case 9: e.eulerseq.reset(); break; + case 10: e.meshdir.reset(); break; + case 11: e.texturedir.reset(); break; + case 12: e.discardvisual.reset(); break; + case 13: e.usethread.reset(); break; + case 14: e.fusestatic.reset(); break; + case 15: e.inertiafromgeom.reset(); break; + case 16: e.inertiagrouprange.reset(); break; + case 17: e.saveinertial.reset(); break; + case 18: e.assetdir.reset(); break; + case 19: e.alignfree.reset(); break; + case 20: e.conflict.reset(); break; + default: break; + } +} + +bool Present_LengthRange(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.mode.has_value(); + case 1: return e.useexisting.has_value(); + case 2: return e.uselimit.has_value(); + case 3: return e.accel.has_value(); + case 4: return e.maxforce.has_value(); + case 5: return e.timeconst.has_value(); + case 6: return e.timestep.has_value(); + case 7: return e.inttotal.has_value(); + case 8: return e.interval.has_value(); + case 9: return e.tolrange.has_value(); + default: return false; + } +} +void Clear_LengthRange(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.mode.reset(); break; + case 1: e.useexisting.reset(); break; + case 2: e.uselimit.reset(); break; + case 3: e.accel.reset(); break; + case 4: e.maxforce.reset(); break; + case 5: e.timeconst.reset(); break; + case 6: e.timestep.reset(); break; + case 7: e.inttotal.reset(); break; + case 8: e.interval.reset(); break; + case 9: e.tolrange.reset(); break; + default: break; + } +} + +bool Present_Option(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.timestep.has_value(); + case 1: return e.impratio.has_value(); + case 2: return e.tolerance.has_value(); + case 3: return e.ls_tolerance.has_value(); + case 4: return e.noslip_tolerance.has_value(); + case 5: return e.ccd_tolerance.has_value(); + case 6: return e.sleep_tolerance.has_value(); + case 7: return e.gravity.has_value(); + case 8: return e.wind.has_value(); + case 9: return e.magnetic.has_value(); + case 10: return e.density.has_value(); + case 11: return e.viscosity.has_value(); + case 12: return e.o_margin.has_value(); + case 13: return e.o_solref.has_value(); + case 14: return e.o_solimp.has_value(); + case 15: return e.o_friction.has_value(); + case 16: return e.integrator.has_value(); + case 17: return e.cone.has_value(); + case 18: return e.jacobian.has_value(); + case 19: return e.solver.has_value(); + case 20: return e.iterations.has_value(); + case 21: return e.ls_iterations.has_value(); + case 22: return e.noslip_iterations.has_value(); + case 23: return e.ccd_iterations.has_value(); + case 24: return e.sdf_iterations.has_value(); + case 25: return e.sdf_initpoints.has_value(); + case 26: return e.actuatorgroupdisable.has_value(); + default: return false; + } +} +void Clear_Option(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.timestep.reset(); break; + case 1: e.impratio.reset(); break; + case 2: e.tolerance.reset(); break; + case 3: e.ls_tolerance.reset(); break; + case 4: e.noslip_tolerance.reset(); break; + case 5: e.ccd_tolerance.reset(); break; + case 6: e.sleep_tolerance.reset(); break; + case 7: e.gravity.reset(); break; + case 8: e.wind.reset(); break; + case 9: e.magnetic.reset(); break; + case 10: e.density.reset(); break; + case 11: e.viscosity.reset(); break; + case 12: e.o_margin.reset(); break; + case 13: e.o_solref.reset(); break; + case 14: e.o_solimp.reset(); break; + case 15: e.o_friction.reset(); break; + case 16: e.integrator.reset(); break; + case 17: e.cone.reset(); break; + case 18: e.jacobian.reset(); break; + case 19: e.solver.reset(); break; + case 20: e.iterations.reset(); break; + case 21: e.ls_iterations.reset(); break; + case 22: e.noslip_iterations.reset(); break; + case 23: e.ccd_iterations.reset(); break; + case 24: e.sdf_iterations.reset(); break; + case 25: e.sdf_initpoints.reset(); break; + case 26: e.actuatorgroupdisable.reset(); break; + default: break; + } +} + +bool Present_Flag(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.constraint.has_value(); + case 1: return e.equality.has_value(); + case 2: return e.frictionloss.has_value(); + case 3: return e.limit.has_value(); + case 4: return e.contact.has_value(); + case 5: return e.spring.has_value(); + case 6: return e.damper.has_value(); + case 7: return e.gravity.has_value(); + case 8: return e.clampctrl.has_value(); + case 9: return e.warmstart.has_value(); + case 10: return e.filterparent.has_value(); + case 11: return e.actuation.has_value(); + case 12: return e.refsafe.has_value(); + case 13: return e.sensor.has_value(); + case 14: return e.midphase.has_value(); + case 15: return e.eulerdamp.has_value(); + case 16: return e.autoreset.has_value(); + case 17: return e.nativeccd.has_value(); + case 18: return e.island.has_value(); + case 19: return e.multiccd.has_value(); + case 20: return e.override_.has_value(); + case 21: return e.energy.has_value(); + case 22: return e.fwdinv.has_value(); + case 23: return e.invdiscrete.has_value(); + case 24: return e.sleep.has_value(); + case 25: return e.diagexact.has_value(); + default: return false; + } +} +void Clear_Flag(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.constraint.reset(); break; + case 1: e.equality.reset(); break; + case 2: e.frictionloss.reset(); break; + case 3: e.limit.reset(); break; + case 4: e.contact.reset(); break; + case 5: e.spring.reset(); break; + case 6: e.damper.reset(); break; + case 7: e.gravity.reset(); break; + case 8: e.clampctrl.reset(); break; + case 9: e.warmstart.reset(); break; + case 10: e.filterparent.reset(); break; + case 11: e.actuation.reset(); break; + case 12: e.refsafe.reset(); break; + case 13: e.sensor.reset(); break; + case 14: e.midphase.reset(); break; + case 15: e.eulerdamp.reset(); break; + case 16: e.autoreset.reset(); break; + case 17: e.nativeccd.reset(); break; + case 18: e.island.reset(); break; + case 19: e.multiccd.reset(); break; + case 20: e.override_.reset(); break; + case 21: e.energy.reset(); break; + case 22: e.fwdinv.reset(); break; + case 23: e.invdiscrete.reset(); break; + case 24: e.sleep.reset(); break; + case 25: e.diagexact.reset(); break; + default: break; + } +} + +bool Present_Size(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.memory.has_value(); + case 1: return e.njmax.has_value(); + case 2: return e.nconmax.has_value(); + case 3: return e.nstack.has_value(); + case 4: return e.nuserdata.has_value(); + case 5: return e.nkey.has_value(); + case 6: return e.nuser_body.has_value(); + case 7: return e.nuser_jnt.has_value(); + case 8: return e.nuser_geom.has_value(); + case 9: return e.nuser_site.has_value(); + case 10: return e.nuser_cam.has_value(); + case 11: return e.nuser_tendon.has_value(); + case 12: return e.nuser_actuator.has_value(); + case 13: return e.nuser_sensor.has_value(); + default: return false; + } +} +void Clear_Size(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.memory.reset(); break; + case 1: e.njmax.reset(); break; + case 2: e.nconmax.reset(); break; + case 3: e.nstack.reset(); break; + case 4: e.nuserdata.reset(); break; + case 5: e.nkey.reset(); break; + case 6: e.nuser_body.reset(); break; + case 7: e.nuser_jnt.reset(); break; + case 8: e.nuser_geom.reset(); break; + case 9: e.nuser_site.reset(); break; + case 10: e.nuser_cam.reset(); break; + case 11: e.nuser_tendon.reset(); break; + case 12: e.nuser_actuator.reset(); break; + case 13: e.nuser_sensor.reset(); break; + default: break; + } +} + +bool Present_Statistic(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.meaninertia.has_value(); + case 1: return e.meanmass.has_value(); + case 2: return e.meansize.has_value(); + case 3: return e.extent.has_value(); + case 4: return e.center.has_value(); + default: return false; + } +} +void Clear_Statistic(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.meaninertia.reset(); break; + case 1: e.meanmass.reset(); break; + case 2: e.meansize.reset(); break; + case 3: e.extent.reset(); break; + case 4: e.center.reset(); break; + default: break; + } +} + +bool Present_Visual(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Visual(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_VisualGlobal(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.cameraid.has_value(); + case 1: return e.orthographic.has_value(); + case 2: return e.fovy.has_value(); + case 3: return e.ipd.has_value(); + case 4: return e.azimuth.has_value(); + case 5: return e.elevation.has_value(); + case 6: return e.linewidth.has_value(); + case 7: return e.glow.has_value(); + case 8: return e.offwidth.has_value(); + case 9: return e.offheight.has_value(); + case 10: return e.realtime.has_value(); + case 11: return e.ellipsoidinertia.has_value(); + case 12: return e.bvactive.has_value(); + default: return false; + } +} +void Clear_VisualGlobal(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.cameraid.reset(); break; + case 1: e.orthographic.reset(); break; + case 2: e.fovy.reset(); break; + case 3: e.ipd.reset(); break; + case 4: e.azimuth.reset(); break; + case 5: e.elevation.reset(); break; + case 6: e.linewidth.reset(); break; + case 7: e.glow.reset(); break; + case 8: e.offwidth.reset(); break; + case 9: e.offheight.reset(); break; + case 10: e.realtime.reset(); break; + case 11: e.ellipsoidinertia.reset(); break; + case 12: e.bvactive.reset(); break; + default: break; + } +} + +bool Present_VisualQuality(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.shadowsize.has_value(); + case 1: return e.offsamples.has_value(); + case 2: return e.numslices.has_value(); + case 3: return e.numstacks.has_value(); + case 4: return e.numquads.has_value(); + default: return false; + } +} +void Clear_VisualQuality(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.shadowsize.reset(); break; + case 1: e.offsamples.reset(); break; + case 2: e.numslices.reset(); break; + case 3: e.numstacks.reset(); break; + case 4: e.numquads.reset(); break; + default: break; + } +} + +bool Present_VisualHeadlight(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.ambient.has_value(); + case 1: return e.diffuse.has_value(); + case 2: return e.specular.has_value(); + case 3: return e.active.has_value(); + default: return false; + } +} +void Clear_VisualHeadlight(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.ambient.reset(); break; + case 1: e.diffuse.reset(); break; + case 2: e.specular.reset(); break; + case 3: e.active.reset(); break; + default: break; + } +} + +bool Present_VisualMap(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.stiffness.has_value(); + case 1: return e.stiffnessrot.has_value(); + case 2: return e.force.has_value(); + case 3: return e.torque.has_value(); + case 4: return e.alpha.has_value(); + case 5: return e.fogstart.has_value(); + case 6: return e.fogend.has_value(); + case 7: return e.znear.has_value(); + case 8: return e.zfar.has_value(); + case 9: return e.haze.has_value(); + case 10: return e.shadowclip.has_value(); + case 11: return e.shadowscale.has_value(); + case 12: return e.actuatortendon.has_value(); + default: return false; + } +} +void Clear_VisualMap(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.stiffness.reset(); break; + case 1: e.stiffnessrot.reset(); break; + case 2: e.force.reset(); break; + case 3: e.torque.reset(); break; + case 4: e.alpha.reset(); break; + case 5: e.fogstart.reset(); break; + case 6: e.fogend.reset(); break; + case 7: e.znear.reset(); break; + case 8: e.zfar.reset(); break; + case 9: e.haze.reset(); break; + case 10: e.shadowclip.reset(); break; + case 11: e.shadowscale.reset(); break; + case 12: e.actuatortendon.reset(); break; + default: break; + } +} + +bool Present_VisualScale(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.forcewidth.has_value(); + case 1: return e.contactwidth.has_value(); + case 2: return e.contactheight.has_value(); + case 3: return e.connect.has_value(); + case 4: return e.com.has_value(); + case 5: return e.camera.has_value(); + case 6: return e.light.has_value(); + case 7: return e.selectpoint.has_value(); + case 8: return e.jointlength.has_value(); + case 9: return e.jointwidth.has_value(); + case 10: return e.actuatorlength.has_value(); + case 11: return e.actuatorwidth.has_value(); + case 12: return e.framelength.has_value(); + case 13: return e.framewidth.has_value(); + case 14: return e.constraint.has_value(); + case 15: return e.slidercrank.has_value(); + case 16: return e.frustum.has_value(); + default: return false; + } +} +void Clear_VisualScale(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.forcewidth.reset(); break; + case 1: e.contactwidth.reset(); break; + case 2: e.contactheight.reset(); break; + case 3: e.connect.reset(); break; + case 4: e.com.reset(); break; + case 5: e.camera.reset(); break; + case 6: e.light.reset(); break; + case 7: e.selectpoint.reset(); break; + case 8: e.jointlength.reset(); break; + case 9: e.jointwidth.reset(); break; + case 10: e.actuatorlength.reset(); break; + case 11: e.actuatorwidth.reset(); break; + case 12: e.framelength.reset(); break; + case 13: e.framewidth.reset(); break; + case 14: e.constraint.reset(); break; + case 15: e.slidercrank.reset(); break; + case 16: e.frustum.reset(); break; + default: break; + } +} + +bool Present_VisualRgba(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.fog.has_value(); + case 1: return e.haze.has_value(); + case 2: return e.force.has_value(); + case 3: return e.inertia.has_value(); + case 4: return e.joint.has_value(); + case 5: return e.actuator.has_value(); + case 6: return e.actuatornegative.has_value(); + case 7: return e.actuatorpositive.has_value(); + case 8: return e.com.has_value(); + case 9: return e.camera.has_value(); + case 10: return e.light.has_value(); + case 11: return e.selectpoint.has_value(); + case 12: return e.connect.has_value(); + case 13: return e.contactpoint.has_value(); + case 14: return e.contactforce.has_value(); + case 15: return e.contactfriction.has_value(); + case 16: return e.contacttorque.has_value(); + case 17: return e.contactgap.has_value(); + case 18: return e.rangefinder.has_value(); + case 19: return e.constraint.has_value(); + case 20: return e.slidercrank.has_value(); + case 21: return e.crankbroken.has_value(); + case 22: return e.frustum.has_value(); + case 23: return e.bv.has_value(); + case 24: return e.bvactive.has_value(); + default: return false; + } +} +void Clear_VisualRgba(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.fog.reset(); break; + case 1: e.haze.reset(); break; + case 2: e.force.reset(); break; + case 3: e.inertia.reset(); break; + case 4: e.joint.reset(); break; + case 5: e.actuator.reset(); break; + case 6: e.actuatornegative.reset(); break; + case 7: e.actuatorpositive.reset(); break; + case 8: e.com.reset(); break; + case 9: e.camera.reset(); break; + case 10: e.light.reset(); break; + case 11: e.selectpoint.reset(); break; + case 12: e.connect.reset(); break; + case 13: e.contactpoint.reset(); break; + case 14: e.contactforce.reset(); break; + case 15: e.contactfriction.reset(); break; + case 16: e.contacttorque.reset(); break; + case 17: e.contactgap.reset(); break; + case 18: e.rangefinder.reset(); break; + case 19: e.constraint.reset(); break; + case 20: e.slidercrank.reset(); break; + case 21: e.crankbroken.reset(); break; + case 22: e.frustum.reset(); break; + case 23: e.bv.reset(); break; + case 24: e.bvactive.reset(); break; + default: break; + } +} + +bool Present_Default(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.dclass.has_value(); + default: return false; + } +} +void Clear_Default(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.dclass.reset(); break; + default: break; + } +} + +bool Present_MaterialLayer(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.texture.has_value(); + case 1: return true; + default: return false; + } +} +void Clear_MaterialLayer(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.texture.reset(); break; + default: break; + } +} + +bool Present_Extension(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Extension(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_PluginDef(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.plugin.has_value(); + default: return false; + } +} +void Clear_PluginDef(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.plugin.reset(); break; + default: break; + } +} + +bool Present_PluginInstance(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + default: return false; + } +} +void Clear_PluginInstance(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Config(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.value.has_value(); + default: return false; + } +} +void Clear_Config(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.value.reset(); break; + default: break; + } +} + +bool Present_Asset(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Asset(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Mesh(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.content_type.has_value(); + case 3: return e.file.has_value(); + case 4: return e.vertex.has_value(); + case 5: return e.normal.has_value(); + case 6: return e.texcoord.has_value(); + case 7: return e.face.has_value(); + case 8: return e.refpos.has_value(); + case 9: return e.refquat.has_value(); + case 10: return e.scale.has_value(); + case 11: return e.smoothnormal.has_value(); + case 12: return e.maxhullvert.has_value(); + case 13: return e.inertia.has_value(); + case 14: return e.builtin.has_value(); + case 15: return e.params.has_value(); + case 16: return e.material.has_value(); + default: return false; + } +} +void Clear_Mesh(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.content_type.reset(); break; + case 3: e.file.reset(); break; + case 4: e.vertex.reset(); break; + case 5: e.normal.reset(); break; + case 6: e.texcoord.reset(); break; + case 7: e.face.reset(); break; + case 8: e.refpos.reset(); break; + case 9: e.refquat.reset(); break; + case 10: e.scale.reset(); break; + case 11: e.smoothnormal.reset(); break; + case 12: e.maxhullvert.reset(); break; + case 13: e.inertia.reset(); break; + case 14: e.builtin.reset(); break; + case 15: e.params.reset(); break; + case 16: e.material.reset(); break; + default: break; + } +} + +bool Present_PluginRef(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.plugin.has_value(); + case 1: return e.instance.has_value(); + default: return false; + } +} +void Clear_PluginRef(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.plugin.reset(); break; + case 1: e.instance.reset(); break; + default: break; + } +} + +bool Present_Hfield(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.content_type.has_value(); + case 2: return e.file.has_value(); + case 3: return e.nrow.has_value(); + case 4: return e.ncol.has_value(); + case 5: return true; + case 6: return e.elevation.has_value(); + default: return false; + } +} +void Clear_Hfield(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.content_type.reset(); break; + case 2: e.file.reset(); break; + case 3: e.nrow.reset(); break; + case 4: e.ncol.reset(); break; + case 6: e.elevation.reset(); break; + default: break; + } +} + +bool Present_Skin(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.file.has_value(); + case 2: return e.material.has_value(); + case 3: return e.rgba.has_value(); + case 4: return e.inflate.has_value(); + case 5: return e.vertex.has_value(); + case 6: return e.texcoord.has_value(); + case 7: return e.face.has_value(); + case 8: return e.group.has_value(); + default: return false; + } +} +void Clear_Skin(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.file.reset(); break; + case 2: e.material.reset(); break; + case 3: e.rgba.reset(); break; + case 4: e.inflate.reset(); break; + case 5: e.vertex.reset(); break; + case 6: e.texcoord.reset(); break; + case 7: e.face.reset(); break; + case 8: e.group.reset(); break; + default: break; + } +} + +bool Present_SkinBone(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.bindpos.has_value(); + case 2: return e.bindquat.has_value(); + case 3: return e.vertid.has_value(); + case 4: return e.vertweight.has_value(); + default: return false; + } +} +void Clear_SkinBone(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.bindpos.reset(); break; + case 2: e.bindquat.reset(); break; + case 3: e.vertid.reset(); break; + case 4: e.vertweight.reset(); break; + default: break; + } +} + +bool Present_Texture(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.type.has_value(); + case 2: return e.colorspace.has_value(); + case 3: return e.content_type.has_value(); + case 4: return e.file.has_value(); + case 5: return e.gridsize.has_value(); + case 6: return e.gridlayout.has_value(); + case 7: return e.fileright.has_value(); + case 8: return e.fileleft.has_value(); + case 9: return e.fileup.has_value(); + case 10: return e.filedown.has_value(); + case 11: return e.filefront.has_value(); + case 12: return e.fileback.has_value(); + case 13: return e.builtin.has_value(); + case 14: return e.rgb1.has_value(); + case 15: return e.rgb2.has_value(); + case 16: return e.mark.has_value(); + case 17: return e.markrgb.has_value(); + case 18: return e.random.has_value(); + case 19: return e.width.has_value(); + case 20: return e.height.has_value(); + case 21: return e.hflip.has_value(); + case 22: return e.vflip.has_value(); + case 23: return e.nchannel.has_value(); + default: return false; + } +} +void Clear_Texture(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.type.reset(); break; + case 2: e.colorspace.reset(); break; + case 3: e.content_type.reset(); break; + case 4: e.file.reset(); break; + case 5: e.gridsize.reset(); break; + case 6: e.gridlayout.reset(); break; + case 7: e.fileright.reset(); break; + case 8: e.fileleft.reset(); break; + case 9: e.fileup.reset(); break; + case 10: e.filedown.reset(); break; + case 11: e.filefront.reset(); break; + case 12: e.fileback.reset(); break; + case 13: e.builtin.reset(); break; + case 14: e.rgb1.reset(); break; + case 15: e.rgb2.reset(); break; + case 16: e.mark.reset(); break; + case 17: e.markrgb.reset(); break; + case 18: e.random.reset(); break; + case 19: e.width.reset(); break; + case 20: e.height.reset(); break; + case 21: e.hflip.reset(); break; + case 22: e.vflip.reset(); break; + case 23: e.nchannel.reset(); break; + default: break; + } +} + +bool Present_Material(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.texrepeat.has_value(); + case 3: return e.texuniform.has_value(); + case 4: return e.emission.has_value(); + case 5: return e.specular.has_value(); + case 6: return e.shininess.has_value(); + case 7: return e.reflectance.has_value(); + case 8: return e.metallic.has_value(); + case 9: return e.roughness.has_value(); + case 10: return e.rgba.has_value(); + default: return false; + } +} +void Clear_Material(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.texrepeat.reset(); break; + case 3: e.texuniform.reset(); break; + case 4: e.emission.reset(); break; + case 5: e.specular.reset(); break; + case 6: e.shininess.reset(); break; + case 7: e.reflectance.reset(); break; + case 8: e.metallic.reset(); break; + case 9: e.roughness.reset(); break; + case 10: e.rgba.reset(); break; + default: break; + } +} + +bool Present_ModelAsset(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.file.has_value(); + case 2: return e.content_type.has_value(); + default: return false; + } +} +void Clear_ModelAsset(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.file.reset(); break; + case 2: e.content_type.reset(); break; + default: break; + } +} + +bool Present_Body(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.childclass.has_value(); + case 2: return e.pos.has_value(); + case 3: return e.quat.has_value(); + case 4: return e.mocap.has_value(); + case 5: return e.gravcomp.has_value(); + case 6: return e.sleep.has_value(); + case 7: return e.simple.has_value(); + case 8: return e.user.has_value(); + default: return false; + } +} +void Clear_Body(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.childclass.reset(); break; + case 2: e.pos.reset(); break; + case 3: e.quat.reset(); break; + case 4: e.mocap.reset(); break; + case 5: e.gravcomp.reset(); break; + case 6: e.sleep.reset(); break; + case 7: e.simple.reset(); break; + case 8: e.user.reset(); break; + default: break; + } +} + +bool Present_Inertial(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.quat.has_value(); + case 2: return true; + case 3: return e.diaginertia.has_value(); + default: return false; + } +} +void Clear_Inertial(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.quat.reset(); break; + case 3: e.diaginertia.reset(); break; + default: break; + } +} + +bool Present_Joint(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.type.has_value(); + case 3: return e.group.has_value(); + case 4: return e.pos.has_value(); + case 5: return e.axis.has_value(); + case 6: return e.springdamper.has_value(); + case 7: return e.limited.has_value(); + case 8: return e.actuatorfrclimited.has_value(); + case 9: return e.solreflimit.has_value(); + case 10: return e.solimplimit.has_value(); + case 11: return e.solreffriction.has_value(); + case 12: return e.solimpfriction.has_value(); + case 13: return e.stiffness.has_value(); + case 14: return e.range.has_value(); + case 15: return e.actuatorfrcrange.has_value(); + case 16: return e.actuatorgravcomp.has_value(); + case 17: return e.margin.has_value(); + case 18: return e.ref.has_value(); + case 19: return e.springref.has_value(); + case 20: return e.armature.has_value(); + case 21: return e.damping.has_value(); + case 22: return e.frictionloss.has_value(); + case 23: return e.user.has_value(); + default: return false; + } +} +void Clear_Joint(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.type.reset(); break; + case 3: e.group.reset(); break; + case 4: e.pos.reset(); break; + case 5: e.axis.reset(); break; + case 6: e.springdamper.reset(); break; + case 7: e.limited.reset(); break; + case 8: e.actuatorfrclimited.reset(); break; + case 9: e.solreflimit.reset(); break; + case 10: e.solimplimit.reset(); break; + case 11: e.solreffriction.reset(); break; + case 12: e.solimpfriction.reset(); break; + case 13: e.stiffness.reset(); break; + case 14: e.range.reset(); break; + case 15: e.actuatorfrcrange.reset(); break; + case 16: e.actuatorgravcomp.reset(); break; + case 17: e.margin.reset(); break; + case 18: e.ref.reset(); break; + case 19: e.springref.reset(); break; + case 20: e.armature.reset(); break; + case 21: e.damping.reset(); break; + case 22: e.frictionloss.reset(); break; + case 23: e.user.reset(); break; + default: break; + } +} + +bool Present_FreeJoint(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.group.has_value(); + case 2: return e.align.has_value(); + default: return false; + } +} +void Clear_FreeJoint(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.group.reset(); break; + case 2: e.align.reset(); break; + default: break; + } +} + +bool Present_Geom(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.type.has_value(); + case 3: return e.contype.has_value(); + case 4: return e.conaffinity.has_value(); + case 5: return e.condim.has_value(); + case 6: return e.group.has_value(); + case 7: return e.priority.has_value(); + case 8: return e.size.has_value(); + case 9: return e.material.has_value(); + case 10: return e.friction.has_value(); + case 11: return e.mass.has_value(); + case 12: return e.density.has_value(); + case 13: return e.shellinertia.has_value(); + case 14: return e.solmix.has_value(); + case 15: return e.solref.has_value(); + case 16: return e.solimp.has_value(); + case 17: return e.margin.has_value(); + case 18: return e.gap.has_value(); + case 19: return e.surfacevel.has_value(); + case 20: return e.adhesion.has_value(); + case 21: return e.fromto.has_value(); + case 22: return e.pos.has_value(); + case 23: return e.quat.has_value(); + case 24: return e.hfield.has_value(); + case 25: return e.mesh.has_value(); + case 26: return e.fitscale.has_value(); + case 27: return e.rgba.has_value(); + case 28: return e.fluidshape.has_value(); + case 29: return e.fluidcoef.has_value(); + case 30: return e.user.has_value(); + default: return false; + } +} +void Clear_Geom(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.type.reset(); break; + case 3: e.contype.reset(); break; + case 4: e.conaffinity.reset(); break; + case 5: e.condim.reset(); break; + case 6: e.group.reset(); break; + case 7: e.priority.reset(); break; + case 8: e.size.reset(); break; + case 9: e.material.reset(); break; + case 10: e.friction.reset(); break; + case 11: e.mass.reset(); break; + case 12: e.density.reset(); break; + case 13: e.shellinertia.reset(); break; + case 14: e.solmix.reset(); break; + case 15: e.solref.reset(); break; + case 16: e.solimp.reset(); break; + case 17: e.margin.reset(); break; + case 18: e.gap.reset(); break; + case 19: e.surfacevel.reset(); break; + case 20: e.adhesion.reset(); break; + case 21: e.fromto.reset(); break; + case 22: e.pos.reset(); break; + case 23: e.quat.reset(); break; + case 24: e.hfield.reset(); break; + case 25: e.mesh.reset(); break; + case 26: e.fitscale.reset(); break; + case 27: e.rgba.reset(); break; + case 28: e.fluidshape.reset(); break; + case 29: e.fluidcoef.reset(); break; + case 30: e.user.reset(); break; + default: break; + } +} + +bool Present_Attach(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.model.has_value(); + case 1: return e.body.has_value(); + case 2: return e.frame.has_value(); + case 3: return true; + default: return false; + } +} +void Clear_Attach(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.model.reset(); break; + case 1: e.body.reset(); break; + case 2: e.frame.reset(); break; + default: break; + } +} + +bool Present_Site(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.type.has_value(); + case 3: return e.group.has_value(); + case 4: return e.pos.has_value(); + case 5: return e.quat.has_value(); + case 6: return e.material.has_value(); + case 7: return e.size.has_value(); + case 8: return e.fromto.has_value(); + case 9: return e.rgba.has_value(); + case 10: return e.user.has_value(); + default: return false; + } +} +void Clear_Site(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.type.reset(); break; + case 3: e.group.reset(); break; + case 4: e.pos.reset(); break; + case 5: e.quat.reset(); break; + case 6: e.material.reset(); break; + case 7: e.size.reset(); break; + case 8: e.fromto.reset(); break; + case 9: e.rgba.reset(); break; + case 10: e.user.reset(); break; + default: break; + } +} + +bool Present_Camera(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.projection.has_value(); + case 3: return e.fovy.has_value(); + case 4: return e.ipd.has_value(); + case 5: return e.resolution.has_value(); + case 6: return e.output.has_value(); + case 7: return e.pos.has_value(); + case 8: return e.quat.has_value(); + case 9: return e.mode.has_value(); + case 10: return e.target.has_value(); + case 11: return e.focal.has_value(); + case 12: return e.focalpixel.has_value(); + case 13: return e.principal.has_value(); + case 14: return e.principalpixel.has_value(); + case 15: return e.sensorsize.has_value(); + case 16: return e.user.has_value(); + default: return false; + } +} +void Clear_Camera(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.projection.reset(); break; + case 3: e.fovy.reset(); break; + case 4: e.ipd.reset(); break; + case 5: e.resolution.reset(); break; + case 6: e.output.reset(); break; + case 7: e.pos.reset(); break; + case 8: e.quat.reset(); break; + case 9: e.mode.reset(); break; + case 10: e.target.reset(); break; + case 11: e.focal.reset(); break; + case 12: e.focalpixel.reset(); break; + case 13: e.principal.reset(); break; + case 14: e.principalpixel.reset(); break; + case 15: e.sensorsize.reset(); break; + case 16: e.user.reset(); break; + default: break; + } +} + +bool Present_Light(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.type.has_value(); + case 3: return e.castshadow.has_value(); + case 4: return e.active.has_value(); + case 5: return e.pos.has_value(); + case 6: return e.dir.has_value(); + case 7: return e.bulbradius.has_value(); + case 8: return e.intensity.has_value(); + case 9: return e.range.has_value(); + case 10: return e.attenuation.has_value(); + case 11: return e.cutoff.has_value(); + case 12: return e.exponent.has_value(); + case 13: return e.ambient.has_value(); + case 14: return e.diffuse.has_value(); + case 15: return e.specular.has_value(); + case 16: return e.mode.has_value(); + case 17: return e.target.has_value(); + case 18: return e.texture.has_value(); + default: return false; + } +} +void Clear_Light(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.type.reset(); break; + case 3: e.castshadow.reset(); break; + case 4: e.active.reset(); break; + case 5: e.pos.reset(); break; + case 6: e.dir.reset(); break; + case 7: e.bulbradius.reset(); break; + case 8: e.intensity.reset(); break; + case 9: e.range.reset(); break; + case 10: e.attenuation.reset(); break; + case 11: e.cutoff.reset(); break; + case 12: e.exponent.reset(); break; + case 13: e.ambient.reset(); break; + case 14: e.diffuse.reset(); break; + case 15: e.specular.reset(); break; + case 16: e.mode.reset(); break; + case 17: e.target.reset(); break; + case 18: e.texture.reset(); break; + default: break; + } +} + +bool Present_Composite(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.prefix.has_value(); + case 1: return true; + case 2: return e.count.has_value(); + case 3: return e.offset.has_value(); + case 4: return e.vertex.has_value(); + case 5: return e.initial.has_value(); + case 6: return e.curve.has_value(); + case 7: return e.size.has_value(); + case 8: return e.quat.has_value(); + default: return false; + } +} +void Clear_Composite(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.prefix.reset(); break; + case 2: e.count.reset(); break; + case 3: e.offset.reset(); break; + case 4: e.vertex.reset(); break; + case 5: e.initial.reset(); break; + case 6: e.curve.reset(); break; + case 7: e.size.reset(); break; + case 8: e.quat.reset(); break; + default: break; + } +} + +bool Present_CompositeJoint(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.group.has_value(); + case 2: return e.stiffness.has_value(); + case 3: return e.damping.has_value(); + case 4: return e.armature.has_value(); + case 5: return e.solreffix.has_value(); + case 6: return e.solimpfix.has_value(); + case 7: return e.type.has_value(); + case 8: return e.axis.has_value(); + case 9: return e.limited.has_value(); + case 10: return e.range.has_value(); + case 11: return e.margin.has_value(); + case 12: return e.solreflimit.has_value(); + case 13: return e.solimplimit.has_value(); + case 14: return e.frictionloss.has_value(); + case 15: return e.solreffriction.has_value(); + case 16: return e.solimpfriction.has_value(); + default: return false; + } +} +void Clear_CompositeJoint(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.group.reset(); break; + case 2: e.stiffness.reset(); break; + case 3: e.damping.reset(); break; + case 4: e.armature.reset(); break; + case 5: e.solreffix.reset(); break; + case 6: e.solimpfix.reset(); break; + case 7: e.type.reset(); break; + case 8: e.axis.reset(); break; + case 9: e.limited.reset(); break; + case 10: e.range.reset(); break; + case 11: e.margin.reset(); break; + case 12: e.solreflimit.reset(); break; + case 13: e.solimplimit.reset(); break; + case 14: e.frictionloss.reset(); break; + case 15: e.solreffriction.reset(); break; + case 16: e.solimpfriction.reset(); break; + default: break; + } +} + +bool Present_CompositeSkin(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.texcoord.has_value(); + case 1: return e.material.has_value(); + case 2: return e.group.has_value(); + case 3: return e.rgba.has_value(); + case 4: return e.inflate.has_value(); + case 5: return e.subgrid.has_value(); + default: return false; + } +} +void Clear_CompositeSkin(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.texcoord.reset(); break; + case 1: e.material.reset(); break; + case 2: e.group.reset(); break; + case 3: e.rgba.reset(); break; + case 4: e.inflate.reset(); break; + case 5: e.subgrid.reset(); break; + default: break; + } +} + +bool Present_CompositeGeom(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.type.has_value(); + case 1: return e.contype.has_value(); + case 2: return e.conaffinity.has_value(); + case 3: return e.condim.has_value(); + case 4: return e.group.has_value(); + case 5: return e.priority.has_value(); + case 6: return e.size.has_value(); + case 7: return e.material.has_value(); + case 8: return e.rgba.has_value(); + case 9: return e.friction.has_value(); + case 10: return e.mass.has_value(); + case 11: return e.density.has_value(); + case 12: return e.solmix.has_value(); + case 13: return e.solref.has_value(); + case 14: return e.solimp.has_value(); + case 15: return e.margin.has_value(); + case 16: return e.gap.has_value(); + case 17: return e.surfacevel.has_value(); + case 18: return e.adhesion.has_value(); + default: return false; + } +} +void Clear_CompositeGeom(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.type.reset(); break; + case 1: e.contype.reset(); break; + case 2: e.conaffinity.reset(); break; + case 3: e.condim.reset(); break; + case 4: e.group.reset(); break; + case 5: e.priority.reset(); break; + case 6: e.size.reset(); break; + case 7: e.material.reset(); break; + case 8: e.rgba.reset(); break; + case 9: e.friction.reset(); break; + case 10: e.mass.reset(); break; + case 11: e.density.reset(); break; + case 12: e.solmix.reset(); break; + case 13: e.solref.reset(); break; + case 14: e.solimp.reset(); break; + case 15: e.margin.reset(); break; + case 16: e.gap.reset(); break; + case 17: e.surfacevel.reset(); break; + case 18: e.adhesion.reset(); break; + default: break; + } +} + +bool Present_CompositeSite(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.group.has_value(); + case 1: return e.size.has_value(); + case 2: return e.material.has_value(); + case 3: return e.rgba.has_value(); + default: return false; + } +} +void Clear_CompositeSite(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.group.reset(); break; + case 1: e.size.reset(); break; + case 2: e.material.reset(); break; + case 3: e.rgba.reset(); break; + default: break; + } +} + +bool Present_Flexcomp(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.type.has_value(); + case 2: return e.group.has_value(); + case 3: return e.dim.has_value(); + case 4: return e.dof.has_value(); + case 5: return e.count.has_value(); + case 6: return e.cellcount.has_value(); + case 7: return e.spacing.has_value(); + case 8: return e.radius.has_value(); + case 9: return e.rigid.has_value(); + case 10: return e.mass.has_value(); + case 11: return e.inertiabox.has_value(); + case 12: return e.scale.has_value(); + case 13: return e.file.has_value(); + case 14: return e.point.has_value(); + case 15: return e.element.has_value(); + case 16: return e.texcoord.has_value(); + case 17: return e.material.has_value(); + case 18: return e.rgba.has_value(); + case 19: return e.flatskin.has_value(); + case 20: return e.pos.has_value(); + case 21: return e.quat.has_value(); + case 22: return e.axisangle.has_value(); + case 23: return e.xyaxes.has_value(); + case 24: return e.zaxis.has_value(); + case 25: return e.euler.has_value(); + case 26: return e.origin.has_value(); + default: return false; + } +} +void Clear_Flexcomp(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.type.reset(); break; + case 2: e.group.reset(); break; + case 3: e.dim.reset(); break; + case 4: e.dof.reset(); break; + case 5: e.count.reset(); break; + case 6: e.cellcount.reset(); break; + case 7: e.spacing.reset(); break; + case 8: e.radius.reset(); break; + case 9: e.rigid.reset(); break; + case 10: e.mass.reset(); break; + case 11: e.inertiabox.reset(); break; + case 12: e.scale.reset(); break; + case 13: e.file.reset(); break; + case 14: e.point.reset(); break; + case 15: e.element.reset(); break; + case 16: e.texcoord.reset(); break; + case 17: e.material.reset(); break; + case 18: e.rgba.reset(); break; + case 19: e.flatskin.reset(); break; + case 20: e.pos.reset(); break; + case 21: e.quat.reset(); break; + case 22: e.axisangle.reset(); break; + case 23: e.xyaxes.reset(); break; + case 24: e.zaxis.reset(); break; + case 25: e.euler.reset(); break; + case 26: e.origin.reset(); break; + default: break; + } +} + +bool Present_FlexcompEdge(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.equality.has_value(); + case 1: return e.solref.has_value(); + case 2: return e.solimp.has_value(); + case 3: return e.stiffness.has_value(); + case 4: return e.damping.has_value(); + default: return false; + } +} +void Clear_FlexcompEdge(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.equality.reset(); break; + case 1: e.solref.reset(); break; + case 2: e.solimp.reset(); break; + case 3: e.stiffness.reset(); break; + case 4: e.damping.reset(); break; + default: break; + } +} + +bool Present_FlexElasticity(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.young.has_value(); + case 1: return e.poisson.has_value(); + case 2: return e.damping.has_value(); + case 3: return e.thickness.has_value(); + case 4: return e.elastic2d.has_value(); + default: return false; + } +} +void Clear_FlexElasticity(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.young.reset(); break; + case 1: e.poisson.reset(); break; + case 2: e.damping.reset(); break; + case 3: e.thickness.reset(); break; + case 4: e.elastic2d.reset(); break; + default: break; + } +} + +bool Present_FlexContact(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.contype.has_value(); + case 1: return e.conaffinity.has_value(); + case 2: return e.condim.has_value(); + case 3: return e.priority.has_value(); + case 4: return e.friction.has_value(); + case 5: return e.solmix.has_value(); + case 6: return e.solref.has_value(); + case 7: return e.solimp.has_value(); + case 8: return e.margin.has_value(); + case 9: return e.gap.has_value(); + case 10: return e.internal.has_value(); + case 11: return e.selfcollide.has_value(); + case 12: return e.activelayers.has_value(); + case 13: return e.passive.has_value(); + default: return false; + } +} +void Clear_FlexContact(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.contype.reset(); break; + case 1: e.conaffinity.reset(); break; + case 2: e.condim.reset(); break; + case 3: e.priority.reset(); break; + case 4: e.friction.reset(); break; + case 5: e.solmix.reset(); break; + case 6: e.solref.reset(); break; + case 7: e.solimp.reset(); break; + case 8: e.margin.reset(); break; + case 9: e.gap.reset(); break; + case 10: e.internal.reset(); break; + case 11: e.selfcollide.reset(); break; + case 12: e.activelayers.reset(); break; + case 13: e.passive.reset(); break; + default: break; + } +} + +bool Present_FlexcompPin(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.id.has_value(); + case 1: return e.range.has_value(); + case 2: return e.grid.has_value(); + case 3: return e.gridrange.has_value(); + default: return false; + } +} +void Clear_FlexcompPin(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.id.reset(); break; + case 1: e.range.reset(); break; + case 2: e.grid.reset(); break; + case 3: e.gridrange.reset(); break; + default: break; + } +} + +bool Present_Deformable(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Deformable(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Flex(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.group.has_value(); + case 2: return e.dim.has_value(); + case 3: return e.radius.has_value(); + case 4: return e.material.has_value(); + case 5: return e.rgba.has_value(); + case 6: return e.flatskin.has_value(); + case 7: return true; + case 8: return e.vertex.has_value(); + case 9: return true; + case 10: return e.texcoord.has_value(); + case 11: return e.elemtexcoord.has_value(); + case 12: return e.node.has_value(); + case 13: return e.cellcount.has_value(); + case 14: return e.dof.has_value(); + default: return false; + } +} +void Clear_Flex(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.group.reset(); break; + case 2: e.dim.reset(); break; + case 3: e.radius.reset(); break; + case 4: e.material.reset(); break; + case 5: e.rgba.reset(); break; + case 6: e.flatskin.reset(); break; + case 8: e.vertex.reset(); break; + case 10: e.texcoord.reset(); break; + case 11: e.elemtexcoord.reset(); break; + case 12: e.node.reset(); break; + case 13: e.cellcount.reset(); break; + case 14: e.dof.reset(); break; + default: break; + } +} + +bool Present_FlexEdge(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.stiffness.has_value(); + case 1: return e.damping.has_value(); + default: return false; + } +} +void Clear_FlexEdge(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.stiffness.reset(); break; + case 1: e.damping.reset(); break; + default: break; + } +} + +bool Present_Contact(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Contact(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Pair(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.geom1.has_value(); + case 3: return e.geom2.has_value(); + case 4: return e.condim.has_value(); + case 5: return e.friction.has_value(); + case 6: return e.solref.has_value(); + case 7: return e.solreffriction.has_value(); + case 8: return e.solimp.has_value(); + case 9: return e.gap.has_value(); + case 10: return e.margin.has_value(); + case 11: return e.adhesion.has_value(); + default: return false; + } +} +void Clear_Pair(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.geom1.reset(); break; + case 3: e.geom2.reset(); break; + case 4: e.condim.reset(); break; + case 5: e.friction.reset(); break; + case 6: e.solref.reset(); break; + case 7: e.solreffriction.reset(); break; + case 8: e.solimp.reset(); break; + case 9: e.gap.reset(); break; + case 10: e.margin.reset(); break; + case 11: e.adhesion.reset(); break; + default: break; + } +} + +bool Present_Exclude(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return true; + case 2: return true; + default: return false; + } +} +void Clear_Exclude(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + default: break; + } +} + +bool Present_Tendon(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Tendon(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Spatial(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.limited.has_value(); + case 4: return e.actuatorfrclimited.has_value(); + case 5: return e.range.has_value(); + case 6: return e.actuatorfrcrange.has_value(); + case 7: return e.solreflimit.has_value(); + case 8: return e.solimplimit.has_value(); + case 9: return e.solreffriction.has_value(); + case 10: return e.solimpfriction.has_value(); + case 11: return e.frictionloss.has_value(); + case 12: return e.springlength.has_value(); + case 13: return e.width.has_value(); + case 14: return e.material.has_value(); + case 15: return e.margin.has_value(); + case 16: return e.stiffness.has_value(); + case 17: return e.damping.has_value(); + case 18: return e.armature.has_value(); + case 19: return e.rgba.has_value(); + case 20: return e.user.has_value(); + default: return false; + } +} +void Clear_Spatial(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.limited.reset(); break; + case 4: e.actuatorfrclimited.reset(); break; + case 5: e.range.reset(); break; + case 6: e.actuatorfrcrange.reset(); break; + case 7: e.solreflimit.reset(); break; + case 8: e.solimplimit.reset(); break; + case 9: e.solreffriction.reset(); break; + case 10: e.solimpfriction.reset(); break; + case 11: e.frictionloss.reset(); break; + case 12: e.springlength.reset(); break; + case 13: e.width.reset(); break; + case 14: e.material.reset(); break; + case 15: e.margin.reset(); break; + case 16: e.stiffness.reset(); break; + case 17: e.damping.reset(); break; + case 18: e.armature.reset(); break; + case 19: e.rgba.reset(); break; + case 20: e.user.reset(); break; + default: break; + } +} + +bool Present_SpatialSite(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + default: return false; + } +} +void Clear_SpatialSite(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_SpatialGeom(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.sidesite.has_value(); + default: return false; + } +} +void Clear_SpatialGeom(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.sidesite.reset(); break; + default: break; + } +} + +bool Present_Pulley(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.divisor.has_value(); + default: return false; + } +} +void Clear_Pulley(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.divisor.reset(); break; + default: break; + } +} + +bool Present_Fixed(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.limited.has_value(); + case 4: return e.actuatorfrclimited.has_value(); + case 5: return e.range.has_value(); + case 6: return e.actuatorfrcrange.has_value(); + case 7: return e.solreflimit.has_value(); + case 8: return e.solimplimit.has_value(); + case 9: return e.solreffriction.has_value(); + case 10: return e.solimpfriction.has_value(); + case 11: return e.frictionloss.has_value(); + case 12: return e.springlength.has_value(); + case 13: return e.margin.has_value(); + case 14: return e.stiffness.has_value(); + case 15: return e.damping.has_value(); + case 16: return e.armature.has_value(); + case 17: return e.user.has_value(); + default: return false; + } +} +void Clear_Fixed(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.limited.reset(); break; + case 4: e.actuatorfrclimited.reset(); break; + case 5: e.range.reset(); break; + case 6: e.actuatorfrcrange.reset(); break; + case 7: e.solreflimit.reset(); break; + case 8: e.solimplimit.reset(); break; + case 9: e.solreffriction.reset(); break; + case 10: e.solimpfriction.reset(); break; + case 11: e.frictionloss.reset(); break; + case 12: e.springlength.reset(); break; + case 13: e.margin.reset(); break; + case 14: e.stiffness.reset(); break; + case 15: e.damping.reset(); break; + case 16: e.armature.reset(); break; + case 17: e.user.reset(); break; + default: break; + } +} + +bool Present_FixedJoint(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.coef.has_value(); + default: return false; + } +} +void Clear_FixedJoint(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.coef.reset(); break; + default: break; + } +} + +bool Present_Equality(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Equality(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Connect(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return e.body1.has_value(); + case 6: return e.body2.has_value(); + case 7: return e.anchor.has_value(); + case 8: return e.site1.has_value(); + case 9: return e.site2.has_value(); + default: return false; + } +} +void Clear_Connect(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + case 5: e.body1.reset(); break; + case 6: e.body2.reset(); break; + case 7: e.anchor.reset(); break; + case 8: e.site1.reset(); break; + case 9: e.site2.reset(); break; + default: break; + } +} + +bool Present_Weld(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return e.body1.has_value(); + case 6: return e.body2.has_value(); + case 7: return e.relpose.has_value(); + case 8: return e.anchor.has_value(); + case 9: return e.site1.has_value(); + case 10: return e.site2.has_value(); + case 11: return e.torquescale.has_value(); + default: return false; + } +} +void Clear_Weld(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + case 5: e.body1.reset(); break; + case 6: e.body2.reset(); break; + case 7: e.relpose.reset(); break; + case 8: e.anchor.reset(); break; + case 9: e.site1.reset(); break; + case 10: e.site2.reset(); break; + case 11: e.torquescale.reset(); break; + default: break; + } +} + +bool Present_EqualityJoint(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return true; + case 6: return e.joint2.has_value(); + case 7: return e.polycoef.has_value(); + default: return false; + } +} +void Clear_EqualityJoint(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + case 6: e.joint2.reset(); break; + case 7: e.polycoef.reset(); break; + default: break; + } +} + +bool Present_EqualityTendon(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return true; + case 6: return e.tendon2.has_value(); + case 7: return e.polycoef.has_value(); + default: return false; + } +} +void Clear_EqualityTendon(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + case 6: e.tendon2.reset(); break; + case 7: e.polycoef.reset(); break; + default: break; + } +} + +bool Present_EqualityFlex(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return true; + default: return false; + } +} +void Clear_EqualityFlex(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + default: break; + } +} + +bool Present_Flexvert(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return true; + default: return false; + } +} +void Clear_Flexvert(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + default: break; + } +} + +bool Present_Flexstrain(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.active.has_value(); + case 3: return e.solref.has_value(); + case 4: return e.solimp.has_value(); + case 5: return true; + case 6: return e.cell.has_value(); + default: return false; + } +} +void Clear_Flexstrain(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.active.reset(); break; + case 3: e.solref.reset(); break; + case 4: e.solimp.reset(); break; + case 6: e.cell.reset(); break; + default: break; + } +} + +bool Present_Actuator(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Actuator(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_ActuatorGeneral(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.actlimited.has_value(); + case 11: return e.forcerange.has_value(); + case 12: return e.actrange.has_value(); + case 13: return e.lengthrange.has_value(); + case 14: return e.gear.has_value(); + case 15: return e.damping.has_value(); + case 16: return e.armature.has_value(); + case 17: return e.cranklength.has_value(); + case 18: return e.joint.has_value(); + case 19: return e.jointinparent.has_value(); + case 20: return e.tendon.has_value(); + case 21: return e.slidersite.has_value(); + case 22: return e.cranksite.has_value(); + case 23: return e.site.has_value(); + case 24: return e.refsite.has_value(); + case 25: return e.body.has_value(); + case 26: return e.actdim.has_value(); + case 27: return e.input.has_value(); + case 28: return e.velrange.has_value(); + case 29: return e.ffrange.has_value(); + case 30: return e.dyntype.has_value(); + case 31: return e.gaintype.has_value(); + case 32: return e.biastype.has_value(); + case 33: return e.dynprm.has_value(); + case 34: return e.gainprm.has_value(); + case 35: return e.biasprm.has_value(); + case 36: return e.actearly.has_value(); + default: return false; + } +} +void Clear_ActuatorGeneral(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.actlimited.reset(); break; + case 11: e.forcerange.reset(); break; + case 12: e.actrange.reset(); break; + case 13: e.lengthrange.reset(); break; + case 14: e.gear.reset(); break; + case 15: e.damping.reset(); break; + case 16: e.armature.reset(); break; + case 17: e.cranklength.reset(); break; + case 18: e.joint.reset(); break; + case 19: e.jointinparent.reset(); break; + case 20: e.tendon.reset(); break; + case 21: e.slidersite.reset(); break; + case 22: e.cranksite.reset(); break; + case 23: e.site.reset(); break; + case 24: e.refsite.reset(); break; + case 25: e.body.reset(); break; + case 26: e.actdim.reset(); break; + case 27: e.input.reset(); break; + case 28: e.velrange.reset(); break; + case 29: e.ffrange.reset(); break; + case 30: e.dyntype.reset(); break; + case 31: e.gaintype.reset(); break; + case 32: e.biastype.reset(); break; + case 33: e.dynprm.reset(); break; + case 34: e.gainprm.reset(); break; + case 35: e.biasprm.reset(); break; + case 36: e.actearly.reset(); break; + default: break; + } +} + +bool Present_Motor(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.forcerange.has_value(); + case 11: return e.lengthrange.has_value(); + case 12: return e.gear.has_value(); + case 13: return e.damping.has_value(); + case 14: return e.armature.has_value(); + case 15: return e.cranklength.has_value(); + case 16: return e.joint.has_value(); + case 17: return e.jointinparent.has_value(); + case 18: return e.tendon.has_value(); + case 19: return e.slidersite.has_value(); + case 20: return e.cranksite.has_value(); + case 21: return e.site.has_value(); + case 22: return e.refsite.has_value(); + default: return false; + } +} +void Clear_Motor(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.forcerange.reset(); break; + case 11: e.lengthrange.reset(); break; + case 12: e.gear.reset(); break; + case 13: e.damping.reset(); break; + case 14: e.armature.reset(); break; + case 15: e.cranklength.reset(); break; + case 16: e.joint.reset(); break; + case 17: e.jointinparent.reset(); break; + case 18: e.tendon.reset(); break; + case 19: e.slidersite.reset(); break; + case 20: e.cranksite.reset(); break; + case 21: e.site.reset(); break; + case 22: e.refsite.reset(); break; + default: break; + } +} + +bool Present_Position(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.inheritrange.has_value(); + case 11: return e.forcerange.has_value(); + case 12: return e.lengthrange.has_value(); + case 13: return e.gear.has_value(); + case 14: return e.damping.has_value(); + case 15: return e.armature.has_value(); + case 16: return e.cranklength.has_value(); + case 17: return e.joint.has_value(); + case 18: return e.jointinparent.has_value(); + case 19: return e.tendon.has_value(); + case 20: return e.slidersite.has_value(); + case 21: return e.cranksite.has_value(); + case 22: return e.site.has_value(); + case 23: return e.refsite.has_value(); + case 24: return e.kp.has_value(); + case 25: return e.kv.has_value(); + case 26: return e.dampratio.has_value(); + case 27: return e.timeconst.has_value(); + default: return false; + } +} +void Clear_Position(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.inheritrange.reset(); break; + case 11: e.forcerange.reset(); break; + case 12: e.lengthrange.reset(); break; + case 13: e.gear.reset(); break; + case 14: e.damping.reset(); break; + case 15: e.armature.reset(); break; + case 16: e.cranklength.reset(); break; + case 17: e.joint.reset(); break; + case 18: e.jointinparent.reset(); break; + case 19: e.tendon.reset(); break; + case 20: e.slidersite.reset(); break; + case 21: e.cranksite.reset(); break; + case 22: e.site.reset(); break; + case 23: e.refsite.reset(); break; + case 24: e.kp.reset(); break; + case 25: e.kv.reset(); break; + case 26: e.dampratio.reset(); break; + case 27: e.timeconst.reset(); break; + default: break; + } +} + +bool Present_Velocity(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.forcerange.has_value(); + case 11: return e.lengthrange.has_value(); + case 12: return e.gear.has_value(); + case 13: return e.damping.has_value(); + case 14: return e.armature.has_value(); + case 15: return e.cranklength.has_value(); + case 16: return e.joint.has_value(); + case 17: return e.jointinparent.has_value(); + case 18: return e.tendon.has_value(); + case 19: return e.slidersite.has_value(); + case 20: return e.cranksite.has_value(); + case 21: return e.site.has_value(); + case 22: return e.refsite.has_value(); + case 23: return e.kv.has_value(); + default: return false; + } +} +void Clear_Velocity(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.forcerange.reset(); break; + case 11: e.lengthrange.reset(); break; + case 12: e.gear.reset(); break; + case 13: e.damping.reset(); break; + case 14: e.armature.reset(); break; + case 15: e.cranklength.reset(); break; + case 16: e.joint.reset(); break; + case 17: e.jointinparent.reset(); break; + case 18: e.tendon.reset(); break; + case 19: e.slidersite.reset(); break; + case 20: e.cranksite.reset(); break; + case 21: e.site.reset(); break; + case 22: e.refsite.reset(); break; + case 23: e.kv.reset(); break; + default: break; + } +} + +bool Present_IntVelocity(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.actlimited.has_value(); + case 11: return e.forcerange.has_value(); + case 12: return e.actrange.has_value(); + case 13: return e.inheritrange.has_value(); + case 14: return e.lengthrange.has_value(); + case 15: return e.gear.has_value(); + case 16: return e.damping.has_value(); + case 17: return e.armature.has_value(); + case 18: return e.cranklength.has_value(); + case 19: return e.joint.has_value(); + case 20: return e.jointinparent.has_value(); + case 21: return e.tendon.has_value(); + case 22: return e.slidersite.has_value(); + case 23: return e.cranksite.has_value(); + case 24: return e.site.has_value(); + case 25: return e.refsite.has_value(); + case 26: return e.kp.has_value(); + case 27: return e.kv.has_value(); + case 28: return e.dampratio.has_value(); + default: return false; + } +} +void Clear_IntVelocity(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.actlimited.reset(); break; + case 11: e.forcerange.reset(); break; + case 12: e.actrange.reset(); break; + case 13: e.inheritrange.reset(); break; + case 14: e.lengthrange.reset(); break; + case 15: e.gear.reset(); break; + case 16: e.damping.reset(); break; + case 17: e.armature.reset(); break; + case 18: e.cranklength.reset(); break; + case 19: e.joint.reset(); break; + case 20: e.jointinparent.reset(); break; + case 21: e.tendon.reset(); break; + case 22: e.slidersite.reset(); break; + case 23: e.cranksite.reset(); break; + case 24: e.site.reset(); break; + case 25: e.refsite.reset(); break; + case 26: e.kp.reset(); break; + case 27: e.kv.reset(); break; + case 28: e.dampratio.reset(); break; + default: break; + } +} + +bool Present_OrientationActuator(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.forcelimited.has_value(); + case 9: return e.forcerange.has_value(); + case 10: return e.joint.has_value(); + case 11: return e.site.has_value(); + case 12: return e.refsite.has_value(); + case 13: return e.kp.has_value(); + case 14: return e.kv.has_value(); + case 15: return e.dampratio.has_value(); + case 16: return e.input.has_value(); + default: return false; + } +} +void Clear_OrientationActuator(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.forcelimited.reset(); break; + case 9: e.forcerange.reset(); break; + case 10: e.joint.reset(); break; + case 11: e.site.reset(); break; + case 12: e.refsite.reset(); break; + case 13: e.kp.reset(); break; + case 14: e.kv.reset(); break; + case 15: e.dampratio.reset(); break; + case 16: e.input.reset(); break; + default: break; + } +} + +bool Present_Pid(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.posrange.has_value(); + case 11: return e.velrange.has_value(); + case 12: return e.ffrange.has_value(); + case 13: return e.forcerange.has_value(); + case 14: return e.inheritrange.has_value(); + case 15: return e.lengthrange.has_value(); + case 16: return e.gear.has_value(); + case 17: return e.damping.has_value(); + case 18: return e.armature.has_value(); + case 19: return e.cranklength.has_value(); + case 20: return e.joint.has_value(); + case 21: return e.jointinparent.has_value(); + case 22: return e.tendon.has_value(); + case 23: return e.slidersite.has_value(); + case 24: return e.cranksite.has_value(); + case 25: return e.site.has_value(); + case 26: return e.refsite.has_value(); + case 27: return e.kp.has_value(); + case 28: return e.kv.has_value(); + case 29: return e.dampratio.has_value(); + case 30: return e.ki.has_value(); + case 31: return e.imax.has_value(); + case 32: return e.slewmax.has_value(); + case 33: return e.input.has_value(); + default: return false; + } +} +void Clear_Pid(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.posrange.reset(); break; + case 11: e.velrange.reset(); break; + case 12: e.ffrange.reset(); break; + case 13: e.forcerange.reset(); break; + case 14: e.inheritrange.reset(); break; + case 15: e.lengthrange.reset(); break; + case 16: e.gear.reset(); break; + case 17: e.damping.reset(); break; + case 18: e.armature.reset(); break; + case 19: e.cranklength.reset(); break; + case 20: e.joint.reset(); break; + case 21: e.jointinparent.reset(); break; + case 22: e.tendon.reset(); break; + case 23: e.slidersite.reset(); break; + case 24: e.cranksite.reset(); break; + case 25: e.site.reset(); break; + case 26: e.refsite.reset(); break; + case 27: e.kp.reset(); break; + case 28: e.kv.reset(); break; + case 29: e.dampratio.reset(); break; + case 30: e.ki.reset(); break; + case 31: e.imax.reset(); break; + case 32: e.slewmax.reset(); break; + case 33: e.input.reset(); break; + default: break; + } +} + +bool Present_Damper(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.forcelimited.has_value(); + case 9: return e.forcerange.has_value(); + case 10: return e.lengthrange.has_value(); + case 11: return e.gear.has_value(); + case 12: return e.damping.has_value(); + case 13: return e.armature.has_value(); + case 14: return e.cranklength.has_value(); + case 15: return e.joint.has_value(); + case 16: return e.jointinparent.has_value(); + case 17: return e.tendon.has_value(); + case 18: return e.slidersite.has_value(); + case 19: return e.cranksite.has_value(); + case 20: return e.site.has_value(); + case 21: return e.refsite.has_value(); + case 22: return e.kv.has_value(); + default: return false; + } +} +void Clear_Damper(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.forcelimited.reset(); break; + case 9: e.forcerange.reset(); break; + case 10: e.lengthrange.reset(); break; + case 11: e.gear.reset(); break; + case 12: e.damping.reset(); break; + case 13: e.armature.reset(); break; + case 14: e.cranklength.reset(); break; + case 15: e.joint.reset(); break; + case 16: e.jointinparent.reset(); break; + case 17: e.tendon.reset(); break; + case 18: e.slidersite.reset(); break; + case 19: e.cranksite.reset(); break; + case 20: e.site.reset(); break; + case 21: e.refsite.reset(); break; + case 22: e.kv.reset(); break; + default: break; + } +} + +bool Present_Cylinder(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.forcerange.has_value(); + case 11: return e.lengthrange.has_value(); + case 12: return e.gear.has_value(); + case 13: return e.damping.has_value(); + case 14: return e.armature.has_value(); + case 15: return e.cranklength.has_value(); + case 16: return e.joint.has_value(); + case 17: return e.jointinparent.has_value(); + case 18: return e.tendon.has_value(); + case 19: return e.slidersite.has_value(); + case 20: return e.cranksite.has_value(); + case 21: return e.site.has_value(); + case 22: return e.refsite.has_value(); + case 23: return e.timeconst.has_value(); + case 24: return e.area.has_value(); + case 25: return e.bias.has_value(); + default: return false; + } +} +void Clear_Cylinder(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.forcerange.reset(); break; + case 11: e.lengthrange.reset(); break; + case 12: e.gear.reset(); break; + case 13: e.damping.reset(); break; + case 14: e.armature.reset(); break; + case 15: e.cranklength.reset(); break; + case 16: e.joint.reset(); break; + case 17: e.jointinparent.reset(); break; + case 18: e.tendon.reset(); break; + case 19: e.slidersite.reset(); break; + case 20: e.cranksite.reset(); break; + case 21: e.site.reset(); break; + case 22: e.refsite.reset(); break; + case 23: e.timeconst.reset(); break; + case 24: e.area.reset(); break; + case 25: e.bias.reset(); break; + default: break; + } +} + +bool Present_Muscle(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.forcelimited.has_value(); + case 10: return e.forcerange.has_value(); + case 11: return e.lengthrange.has_value(); + case 12: return e.gear.has_value(); + case 13: return e.damping.has_value(); + case 14: return e.armature.has_value(); + case 15: return e.cranklength.has_value(); + case 16: return e.joint.has_value(); + case 17: return e.jointinparent.has_value(); + case 18: return e.tendon.has_value(); + case 19: return e.slidersite.has_value(); + case 20: return e.cranksite.has_value(); + case 21: return e.timeconst.has_value(); + case 22: return e.tausmooth.has_value(); + case 23: return e.range.has_value(); + case 24: return e.force.has_value(); + case 25: return e.scale.has_value(); + case 26: return e.lmin.has_value(); + case 27: return e.lmax.has_value(); + case 28: return e.vmax.has_value(); + case 29: return e.fpmax.has_value(); + case 30: return e.fvmax.has_value(); + default: return false; + } +} +void Clear_Muscle(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.forcelimited.reset(); break; + case 10: e.forcerange.reset(); break; + case 11: e.lengthrange.reset(); break; + case 12: e.gear.reset(); break; + case 13: e.damping.reset(); break; + case 14: e.armature.reset(); break; + case 15: e.cranklength.reset(); break; + case 16: e.joint.reset(); break; + case 17: e.jointinparent.reset(); break; + case 18: e.tendon.reset(); break; + case 19: e.slidersite.reset(); break; + case 20: e.cranksite.reset(); break; + case 21: e.timeconst.reset(); break; + case 22: e.tausmooth.reset(); break; + case 23: e.range.reset(); break; + case 24: e.force.reset(); break; + case 25: e.scale.reset(); break; + case 26: e.lmin.reset(); break; + case 27: e.lmax.reset(); break; + case 28: e.vmax.reset(); break; + case 29: e.fpmax.reset(); break; + case 30: e.fvmax.reset(); break; + default: break; + } +} + +bool Present_Adhesion(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.forcelimited.has_value(); + case 9: return e.forcerange.has_value(); + case 10: return e.body.has_value(); + case 11: return e.gain.has_value(); + default: return false; + } +} +void Clear_Adhesion(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.forcelimited.reset(); break; + case 9: e.forcerange.reset(); break; + case 10: e.body.reset(); break; + case 11: e.gain.reset(); break; + default: break; + } +} + +bool Present_DcMotor(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.ctrllimited.has_value(); + case 9: return e.lengthrange.has_value(); + case 10: return e.gear.has_value(); + case 11: return e.damping.has_value(); + case 12: return e.armature.has_value(); + case 13: return e.cranklength.has_value(); + case 14: return e.joint.has_value(); + case 15: return e.jointinparent.has_value(); + case 16: return e.tendon.has_value(); + case 17: return e.slidersite.has_value(); + case 18: return e.cranksite.has_value(); + case 19: return e.site.has_value(); + case 20: return e.refsite.has_value(); + case 21: return e.motorconst.has_value(); + case 22: return e.resistance.has_value(); + case 23: return e.nominal.has_value(); + case 24: return e.saturation.has_value(); + case 25: return e.inductance.has_value(); + case 26: return e.cogging.has_value(); + case 27: return e.controller.has_value(); + case 28: return e.thermal.has_value(); + case 29: return e.lugre.has_value(); + case 30: return e.input.has_value(); + default: return false; + } +} +void Clear_DcMotor(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.ctrllimited.reset(); break; + case 9: e.lengthrange.reset(); break; + case 10: e.gear.reset(); break; + case 11: e.damping.reset(); break; + case 12: e.armature.reset(); break; + case 13: e.cranklength.reset(); break; + case 14: e.joint.reset(); break; + case 15: e.jointinparent.reset(); break; + case 16: e.tendon.reset(); break; + case 17: e.slidersite.reset(); break; + case 18: e.cranksite.reset(); break; + case 19: e.site.reset(); break; + case 20: e.refsite.reset(); break; + case 21: e.motorconst.reset(); break; + case 22: e.resistance.reset(); break; + case 23: e.nominal.reset(); break; + case 24: e.saturation.reset(); break; + case 25: e.inductance.reset(); break; + case 26: e.cogging.reset(); break; + case 27: e.controller.reset(); break; + case 28: e.thermal.reset(); break; + case 29: e.lugre.reset(); break; + case 30: e.input.reset(); break; + default: break; + } +} + +bool Present_ActuatorPlugin(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.dclass.has_value(); + case 2: return e.group.has_value(); + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.ctrlrange.has_value(); + case 7: return e.user.has_value(); + case 8: return e.plugin.has_value(); + case 9: return e.instance.has_value(); + case 10: return e.ctrllimited.has_value(); + case 11: return e.forcelimited.has_value(); + case 12: return e.actlimited.has_value(); + case 13: return e.forcerange.has_value(); + case 14: return e.actrange.has_value(); + case 15: return e.lengthrange.has_value(); + case 16: return e.gear.has_value(); + case 17: return e.damping.has_value(); + case 18: return e.armature.has_value(); + case 19: return e.cranklength.has_value(); + case 20: return e.joint.has_value(); + case 21: return e.jointinparent.has_value(); + case 22: return e.site.has_value(); + case 23: return e.actdim.has_value(); + case 24: return e.dyntype.has_value(); + case 25: return e.dynprm.has_value(); + case 26: return e.tendon.has_value(); + case 27: return e.cranksite.has_value(); + case 28: return e.slidersite.has_value(); + case 29: return e.actearly.has_value(); + default: return false; + } +} +void Clear_ActuatorPlugin(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.dclass.reset(); break; + case 2: e.group.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.ctrlrange.reset(); break; + case 7: e.user.reset(); break; + case 8: e.plugin.reset(); break; + case 9: e.instance.reset(); break; + case 10: e.ctrllimited.reset(); break; + case 11: e.forcelimited.reset(); break; + case 12: e.actlimited.reset(); break; + case 13: e.forcerange.reset(); break; + case 14: e.actrange.reset(); break; + case 15: e.lengthrange.reset(); break; + case 16: e.gear.reset(); break; + case 17: e.damping.reset(); break; + case 18: e.armature.reset(); break; + case 19: e.cranklength.reset(); break; + case 20: e.joint.reset(); break; + case 21: e.jointinparent.reset(); break; + case 22: e.site.reset(); break; + case 23: e.actdim.reset(); break; + case 24: e.dyntype.reset(); break; + case 25: e.dynprm.reset(); break; + case 26: e.tendon.reset(); break; + case 27: e.cranksite.reset(); break; + case 28: e.slidersite.reset(); break; + case 29: e.actearly.reset(); break; + default: break; + } +} + +bool Present_Sensor(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Sensor(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Touch(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Touch(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Accelerometer(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Accelerometer(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Velocimeter(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Velocimeter(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Gyro(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Gyro(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Force(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Force(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Torque(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Torque(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Magnetometer(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Magnetometer(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Camprojection(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + default: return false; + } +} +void Clear_Camprojection(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Rangefinder(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return e.site.has_value(); + case 9: return e.camera.has_value(); + case 10: return e.data.has_value(); + default: return false; + } +} +void Clear_Rangefinder(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 8: e.site.reset(); break; + case 9: e.camera.reset(); break; + case 10: e.data.reset(); break; + default: break; + } +} + +bool Present_Jointpos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointpos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Jointvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonpos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonpos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Actuatorpos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Actuatorpos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Actuatorvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Actuatorvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Actuatorfrc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Actuatorfrc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Jointactuatorfrc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointactuatorfrc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonactuatorfrc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonactuatorfrc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Ballquat(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Ballquat(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Ballangvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Ballangvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Jointlimitpos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointlimitpos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Jointlimitvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointlimitvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Jointlimitfrc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Jointlimitfrc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonlimitpos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonlimitpos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonlimitvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonlimitvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tendonlimitfrc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Tendonlimitfrc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Framepos(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Framepos(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Framequat(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Framequat(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Framexaxis(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Framexaxis(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Frameyaxis(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Frameyaxis(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Framezaxis(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Framezaxis(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Framelinvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Framelinvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Frameangvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return e.reftype.has_value(); + case 11: return e.refname.has_value(); + default: return false; + } +} +void Clear_Frameangvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 10: e.reftype.reset(); break; + case 11: e.refname.reset(); break; + default: break; + } +} + +bool Present_Framelinacc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + default: return false; + } +} +void Clear_Framelinacc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Frameangacc(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + default: return false; + } +} +void Clear_Frameangacc(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Subtreecom(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Subtreecom(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Subtreelinvel(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Subtreelinvel(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Subtreeangmom(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + default: return false; + } +} +void Clear_Subtreeangmom(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Insidesite(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return true; + case 9: return true; + case 10: return true; + default: return false; + } +} +void Clear_Insidesite(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Distance(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return e.geom1.has_value(); + case 9: return e.geom2.has_value(); + case 10: return e.body1.has_value(); + case 11: return e.body2.has_value(); + default: return false; + } +} +void Clear_Distance(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 8: e.geom1.reset(); break; + case 9: e.geom2.reset(); break; + case 10: e.body1.reset(); break; + case 11: e.body2.reset(); break; + default: break; + } +} + +bool Present_Normal(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return e.geom1.has_value(); + case 9: return e.geom2.has_value(); + case 10: return e.body1.has_value(); + case 11: return e.body2.has_value(); + default: return false; + } +} +void Clear_Normal(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 8: e.geom1.reset(); break; + case 9: e.geom2.reset(); break; + case 10: e.body1.reset(); break; + case 11: e.body2.reset(); break; + default: break; + } +} + +bool Present_Fromto(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return e.geom1.has_value(); + case 9: return e.geom2.has_value(); + case 10: return e.body1.has_value(); + case 11: return e.body2.has_value(); + default: return false; + } +} +void Clear_Fromto(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 8: e.geom1.reset(); break; + case 9: e.geom2.reset(); break; + case 10: e.body1.reset(); break; + case 11: e.body2.reset(); break; + default: break; + } +} + +bool Present_SensorContact(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + case 8: return e.geom1.has_value(); + case 9: return e.geom2.has_value(); + case 10: return e.body1.has_value(); + case 11: return e.body2.has_value(); + case 12: return e.subtree1.has_value(); + case 13: return e.subtree2.has_value(); + case 14: return e.site.has_value(); + case 15: return e.num.has_value(); + case 16: return e.data.has_value(); + case 17: return e.reduce.has_value(); + default: return false; + } +} +void Clear_SensorContact(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + case 8: e.geom1.reset(); break; + case 9: e.geom2.reset(); break; + case 10: e.body1.reset(); break; + case 11: e.body2.reset(); break; + case 12: e.subtree1.reset(); break; + case 13: e.subtree2.reset(); break; + case 14: e.site.reset(); break; + case 15: e.num.reset(); break; + case 16: e.data.reset(); break; + case 17: e.reduce.reset(); break; + default: break; + } +} + +bool Present_EPotential(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + default: return false; + } +} +void Clear_EPotential(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_EKinetic(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + default: return false; + } +} +void Clear_EKinetic(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Clock(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.nsample.has_value(); + case 2: return e.interp.has_value(); + case 3: return e.delay.has_value(); + case 4: return e.interval.has_value(); + case 5: return e.cutoff.has_value(); + case 6: return e.noise.has_value(); + case 7: return e.user.has_value(); + default: return false; + } +} +void Clear_Clock(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.nsample.reset(); break; + case 2: e.interp.reset(); break; + case 3: e.delay.reset(); break; + case 4: e.interval.reset(); break; + case 5: e.cutoff.reset(); break; + case 6: e.noise.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_Tactile(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return true; + case 2: return true; + case 3: return e.nsample.has_value(); + case 4: return e.interp.has_value(); + case 5: return e.delay.has_value(); + case 6: return e.interval.has_value(); + case 7: return e.user.has_value(); + default: return false; + } +} +void Clear_Tactile(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 3: e.nsample.reset(); break; + case 4: e.interp.reset(); break; + case 5: e.delay.reset(); break; + case 6: e.interval.reset(); break; + case 7: e.user.reset(); break; + default: break; + } +} + +bool Present_SensorUser(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.objtype.has_value(); + case 2: return e.objname.has_value(); + case 3: return e.datatype.has_value(); + case 4: return e.needstage.has_value(); + case 5: return e.dim.has_value(); + case 6: return e.cutoff.has_value(); + case 7: return e.noise.has_value(); + case 8: return e.user.has_value(); + default: return false; + } +} +void Clear_SensorUser(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.objtype.reset(); break; + case 2: e.objname.reset(); break; + case 3: e.datatype.reset(); break; + case 4: e.needstage.reset(); break; + case 5: e.dim.reset(); break; + case 6: e.cutoff.reset(); break; + case 7: e.noise.reset(); break; + case 8: e.user.reset(); break; + default: break; + } +} + +bool Present_SensorPlugin(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.plugin.has_value(); + case 2: return e.instance.has_value(); + case 3: return e.cutoff.has_value(); + case 4: return e.objtype.has_value(); + case 5: return e.objname.has_value(); + case 6: return e.reftype.has_value(); + case 7: return e.refname.has_value(); + case 8: return e.user.has_value(); + default: return false; + } +} +void Clear_SensorPlugin(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.plugin.reset(); break; + case 2: e.instance.reset(); break; + case 3: e.cutoff.reset(); break; + case 4: e.objtype.reset(); break; + case 5: e.objname.reset(); break; + case 6: e.reftype.reset(); break; + case 7: e.refname.reset(); break; + case 8: e.user.reset(); break; + default: break; + } +} + +bool Present_Custom(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Custom(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Numeric(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.size.has_value(); + case 2: return e.data.has_value(); + default: return false; + } +} +void Clear_Numeric(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.size.reset(); break; + case 2: e.data.reset(); break; + default: break; + } +} + +bool Present_Text(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return true; + default: return false; + } +} +void Clear_Text(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Tuple(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + default: return false; + } +} +void Clear_Tuple(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_TupleElement(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return true; + case 2: return e.prm.has_value(); + default: return false; + } +} +void Clear_TupleElement(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 2: e.prm.reset(); break; + default: break; + } +} + +bool Present_Keyframe(const void* p, int fid) { + (void)p; + (void)fid; + return false; +} +void Clear_Keyframe(void* p, int fid) { + (void)p; + (void)fid; +} + +bool Present_Key(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.time.has_value(); + case 2: return e.qpos.has_value(); + case 3: return e.qvel.has_value(); + case 4: return e.act.has_value(); + case 5: return e.mpos.has_value(); + case 6: return e.mquat.has_value(); + case 7: return e.ctrl.has_value(); + default: return false; + } +} +void Clear_Key(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.time.reset(); break; + case 2: e.qpos.reset(); break; + case 3: e.qvel.reset(); break; + case 4: e.act.reset(); break; + case 5: e.mpos.reset(); break; + case 6: e.mquat.reset(); break; + case 7: e.ctrl.reset(); break; + default: break; + } +} + +bool Present_Frame(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.name.has_value(); + case 1: return e.childclass.has_value(); + case 2: return e.pos.has_value(); + case 3: return e.quat.has_value(); + default: return false; + } +} +void Clear_Frame(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.name.reset(); break; + case 1: e.childclass.reset(); break; + case 2: e.pos.reset(); break; + case 3: e.quat.reset(); break; + default: break; + } +} + +bool Present_Replicate(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return true; + case 1: return e.offset.has_value(); + case 2: return e.euler.has_value(); + case 3: return e.sep.has_value(); + case 4: return e.childclass.has_value(); + default: return false; + } +} +void Clear_Replicate(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 1: e.offset.reset(); break; + case 2: e.euler.reset(); break; + case 3: e.sep.reset(); break; + case 4: e.childclass.reset(); break; + default: break; + } +} + +bool Present_EqualityDefault(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.active.has_value(); + case 1: return e.solref.has_value(); + case 2: return e.solimp.has_value(); + default: return false; + } +} +void Clear_EqualityDefault(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.active.reset(); break; + case 1: e.solref.reset(); break; + case 2: e.solimp.reset(); break; + default: break; + } +} + +bool Present_TendonDefault(const void* p, int fid) { + const auto& e = *static_cast(p); + (void)e; + switch (fid) { + case 0: return e.group.has_value(); + case 1: return e.limited.has_value(); + case 2: return e.range.has_value(); + case 3: return e.solreflimit.has_value(); + case 4: return e.solimplimit.has_value(); + case 5: return e.solreffriction.has_value(); + case 6: return e.solimpfriction.has_value(); + case 7: return e.frictionloss.has_value(); + case 8: return e.springlength.has_value(); + case 9: return e.width.has_value(); + case 10: return e.material.has_value(); + case 11: return e.margin.has_value(); + case 12: return e.stiffness.has_value(); + case 13: return e.damping.has_value(); + case 14: return e.rgba.has_value(); + case 15: return e.user.has_value(); + default: return false; + } +} +void Clear_TendonDefault(void* p, int fid) { + auto& e = *static_cast(p); + switch (fid) { + case 0: e.group.reset(); break; + case 1: e.limited.reset(); break; + case 2: e.range.reset(); break; + case 3: e.solreflimit.reset(); break; + case 4: e.solimplimit.reset(); break; + case 5: e.solreffriction.reset(); break; + case 6: e.solimpfriction.reset(); break; + case 7: e.frictionloss.reset(); break; + case 8: e.springlength.reset(); break; + case 9: e.width.reset(); break; + case 10: e.material.reset(); break; + case 11: e.margin.reset(); break; + case 12: e.stiffness.reset(); break; + case 13: e.damping.reset(); break; + case 14: e.rgba.reset(); break; + case 15: e.user.reset(); break; + default: break; + } +} + +constexpr ElementType kUnionMembers_ActuatorAny[] = { ElementType::ActuatorGeneral, ElementType::Motor, ElementType::Position, ElementType::Velocity, ElementType::IntVelocity, ElementType::OrientationActuator, ElementType::Pid, ElementType::Damper, ElementType::Cylinder, ElementType::Muscle, ElementType::Adhesion, ElementType::DcMotor, ElementType::ActuatorPlugin }; +constexpr ElementType kUnionMembers_BodyChildAny[] = { ElementType::Body, ElementType::Joint, ElementType::FreeJoint, ElementType::Geom, ElementType::Attach, ElementType::Site, ElementType::Camera, ElementType::Light, ElementType::PluginRef, ElementType::Composite, ElementType::Flexcomp, ElementType::Frame, ElementType::Replicate }; +constexpr ElementType kUnionMembers_EqualityAny[] = { ElementType::Connect, ElementType::Weld, ElementType::EqualityJoint, ElementType::EqualityTendon, ElementType::EqualityFlex, ElementType::Flexvert, ElementType::Flexstrain }; +constexpr ElementType kUnionMembers_FlexAny[] = { ElementType::Flex, ElementType::Flexcomp }; +constexpr ElementType kUnionMembers_JointAny[] = { ElementType::Joint, ElementType::FreeJoint }; +constexpr ElementType kUnionMembers_PathItemAny[] = { ElementType::SpatialSite, ElementType::SpatialGeom, ElementType::Pulley }; +constexpr ElementType kUnionMembers_SensorAny[] = { ElementType::Touch, ElementType::Accelerometer, ElementType::Velocimeter, ElementType::Gyro, ElementType::Force, ElementType::Torque, ElementType::Magnetometer, ElementType::Camprojection, ElementType::Rangefinder, ElementType::Jointpos, ElementType::Jointvel, ElementType::Tendonpos, ElementType::Tendonvel, ElementType::Actuatorpos, ElementType::Actuatorvel, ElementType::Actuatorfrc, ElementType::Jointactuatorfrc, ElementType::Tendonactuatorfrc, ElementType::Ballquat, ElementType::Ballangvel, ElementType::Jointlimitpos, ElementType::Jointlimitvel, ElementType::Jointlimitfrc, ElementType::Tendonlimitpos, ElementType::Tendonlimitvel, ElementType::Tendonlimitfrc, ElementType::Framepos, ElementType::Framequat, ElementType::Framexaxis, ElementType::Frameyaxis, ElementType::Framezaxis, ElementType::Framelinvel, ElementType::Frameangvel, ElementType::Framelinacc, ElementType::Frameangacc, ElementType::Subtreecom, ElementType::Subtreelinvel, ElementType::Subtreeangmom, ElementType::Insidesite, ElementType::Distance, ElementType::Normal, ElementType::Fromto, ElementType::SensorContact, ElementType::EPotential, ElementType::EKinetic, ElementType::Clock, ElementType::Tactile, ElementType::SensorUser, ElementType::SensorPlugin }; +constexpr ElementType kUnionMembers_TendonAny[] = { ElementType::Spatial, ElementType::Fixed }; + +constexpr ElementDescriptor kElements[] = { + {"Model", "mujoco", ElementType::Model, kFields_Model, 1, kChildren_Model, 17, nullptr, 0, &Present_Model, &Clear_Model}, + {"Compiler", "compiler", ElementType::Compiler, kFields_Compiler, 21, kChildren_Compiler, 1, nullptr, 0, &Present_Compiler, &Clear_Compiler}, + {"LengthRange", "lengthrange", ElementType::LengthRange, kFields_LengthRange, 10, nullptr, 0, nullptr, 0, &Present_LengthRange, &Clear_LengthRange}, + {"Option", "option", ElementType::Option, kFields_Option, 27, kChildren_Option, 1, nullptr, 0, &Present_Option, &Clear_Option}, + {"Flag", "flag", ElementType::Flag, kFields_Flag, 26, nullptr, 0, nullptr, 0, &Present_Flag, &Clear_Flag}, + {"Size", "size", ElementType::Size, kFields_Size, 14, nullptr, 0, kConstraints_Size, 2, &Present_Size, &Clear_Size}, + {"Statistic", "statistic", ElementType::Statistic, kFields_Statistic, 5, nullptr, 0, nullptr, 0, &Present_Statistic, &Clear_Statistic}, + {"Visual", "visual", ElementType::Visual, nullptr, 0, kChildren_Visual, 6, nullptr, 0, &Present_Visual, &Clear_Visual}, + {"VisualGlobal", "global", ElementType::VisualGlobal, kFields_VisualGlobal, 13, nullptr, 0, nullptr, 0, &Present_VisualGlobal, &Clear_VisualGlobal}, + {"VisualQuality", "quality", ElementType::VisualQuality, kFields_VisualQuality, 5, nullptr, 0, nullptr, 0, &Present_VisualQuality, &Clear_VisualQuality}, + {"VisualHeadlight", "headlight", ElementType::VisualHeadlight, kFields_VisualHeadlight, 4, nullptr, 0, nullptr, 0, &Present_VisualHeadlight, &Clear_VisualHeadlight}, + {"VisualMap", "map", ElementType::VisualMap, kFields_VisualMap, 13, nullptr, 0, nullptr, 0, &Present_VisualMap, &Clear_VisualMap}, + {"VisualScale", "scale", ElementType::VisualScale, kFields_VisualScale, 17, nullptr, 0, nullptr, 0, &Present_VisualScale, &Clear_VisualScale}, + {"VisualRgba", "rgba", ElementType::VisualRgba, kFields_VisualRgba, 25, nullptr, 0, nullptr, 0, &Present_VisualRgba, &Clear_VisualRgba}, + {"Default", "default", ElementType::Default, kFields_Default, 1, kChildren_Default, 23, nullptr, 0, &Present_Default, &Clear_Default}, + {"MaterialLayer", "layer", ElementType::MaterialLayer, kFields_MaterialLayer, 2, nullptr, 0, nullptr, 0, &Present_MaterialLayer, &Clear_MaterialLayer}, + {"Extension", "extension", ElementType::Extension, nullptr, 0, kChildren_Extension, 1, nullptr, 0, &Present_Extension, &Clear_Extension}, + {"PluginDef", "plugin", ElementType::PluginDef, kFields_PluginDef, 1, kChildren_PluginDef, 1, nullptr, 0, &Present_PluginDef, &Clear_PluginDef}, + {"PluginInstance", "instance", ElementType::PluginInstance, kFields_PluginInstance, 1, kChildren_PluginInstance, 1, nullptr, 0, &Present_PluginInstance, &Clear_PluginInstance}, + {"Config", "config", ElementType::Config, kFields_Config, 2, nullptr, 0, nullptr, 0, &Present_Config, &Clear_Config}, + {"Asset", "asset", ElementType::Asset, nullptr, 0, kChildren_Asset, 6, nullptr, 0, &Present_Asset, &Clear_Asset}, + {"Mesh", "mesh", ElementType::Mesh, kFields_Mesh, 17, kChildren_Mesh, 1, kConstraints_Mesh, 2, &Present_Mesh, &Clear_Mesh}, + {"PluginRef", "plugin", ElementType::PluginRef, kFields_PluginRef, 2, kChildren_PluginRef, 1, nullptr, 0, &Present_PluginRef, &Clear_PluginRef}, + {"Hfield", "hfield", ElementType::Hfield, kFields_Hfield, 7, nullptr, 0, nullptr, 0, &Present_Hfield, &Clear_Hfield}, + {"Skin", "skin", ElementType::Skin, kFields_Skin, 9, kChildren_Skin, 1, nullptr, 0, &Present_Skin, &Clear_Skin}, + {"SkinBone", "bone", ElementType::SkinBone, kFields_SkinBone, 5, nullptr, 0, nullptr, 0, &Present_SkinBone, &Clear_SkinBone}, + {"Texture", "texture", ElementType::Texture, kFields_Texture, 24, nullptr, 0, nullptr, 0, &Present_Texture, &Clear_Texture}, + {"Material", "material", ElementType::Material, kFields_Material, 11, kChildren_Material, 1, nullptr, 0, &Present_Material, &Clear_Material}, + {"ModelAsset", "model", ElementType::ModelAsset, kFields_ModelAsset, 3, nullptr, 0, nullptr, 0, &Present_ModelAsset, &Clear_ModelAsset}, + {"Body", "body", ElementType::Body, kFields_Body, 9, kChildren_Body, 2, nullptr, 0, &Present_Body, &Clear_Body}, + {"Inertial", "inertial", ElementType::Inertial, kFields_Inertial, 4, nullptr, 0, nullptr, 0, &Present_Inertial, &Clear_Inertial}, + {"Joint", "joint", ElementType::Joint, kFields_Joint, 24, nullptr, 0, nullptr, 0, &Present_Joint, &Clear_Joint}, + {"FreeJoint", "freejoint", ElementType::FreeJoint, kFields_FreeJoint, 3, nullptr, 0, nullptr, 0, &Present_FreeJoint, &Clear_FreeJoint}, + {"Geom", "geom", ElementType::Geom, kFields_Geom, 31, kChildren_Geom, 1, nullptr, 0, &Present_Geom, &Clear_Geom}, + {"Attach", "attach", ElementType::Attach, kFields_Attach, 4, nullptr, 0, kConstraints_Attach, 1, &Present_Attach, &Clear_Attach}, + {"Site", "site", ElementType::Site, kFields_Site, 11, nullptr, 0, nullptr, 0, &Present_Site, &Clear_Site}, + {"Camera", "camera", ElementType::Camera, kFields_Camera, 17, nullptr, 0, kConstraints_Camera, 1, &Present_Camera, &Clear_Camera}, + {"Light", "light", ElementType::Light, kFields_Light, 19, nullptr, 0, nullptr, 0, &Present_Light, &Clear_Light}, + {"Composite", "composite", ElementType::Composite, kFields_Composite, 9, kChildren_Composite, 5, nullptr, 0, &Present_Composite, &Clear_Composite}, + {"CompositeJoint", "joint", ElementType::CompositeJoint, kFields_CompositeJoint, 17, nullptr, 0, nullptr, 0, &Present_CompositeJoint, &Clear_CompositeJoint}, + {"CompositeSkin", "skin", ElementType::CompositeSkin, kFields_CompositeSkin, 6, nullptr, 0, nullptr, 0, &Present_CompositeSkin, &Clear_CompositeSkin}, + {"CompositeGeom", "geom", ElementType::CompositeGeom, kFields_CompositeGeom, 19, nullptr, 0, nullptr, 0, &Present_CompositeGeom, &Clear_CompositeGeom}, + {"CompositeSite", "site", ElementType::CompositeSite, kFields_CompositeSite, 4, nullptr, 0, nullptr, 0, &Present_CompositeSite, &Clear_CompositeSite}, + {"Flexcomp", "flexcomp", ElementType::Flexcomp, kFields_Flexcomp, 27, kChildren_Flexcomp, 5, nullptr, 0, &Present_Flexcomp, &Clear_Flexcomp}, + {"FlexcompEdge", "edge", ElementType::FlexcompEdge, kFields_FlexcompEdge, 5, nullptr, 0, nullptr, 0, &Present_FlexcompEdge, &Clear_FlexcompEdge}, + {"FlexElasticity", "elasticity", ElementType::FlexElasticity, kFields_FlexElasticity, 5, nullptr, 0, nullptr, 0, &Present_FlexElasticity, &Clear_FlexElasticity}, + {"FlexContact", "contact", ElementType::FlexContact, kFields_FlexContact, 14, nullptr, 0, nullptr, 0, &Present_FlexContact, &Clear_FlexContact}, + {"FlexcompPin", "pin", ElementType::FlexcompPin, kFields_FlexcompPin, 4, nullptr, 0, nullptr, 0, &Present_FlexcompPin, &Clear_FlexcompPin}, + {"Deformable", "deformable", ElementType::Deformable, nullptr, 0, kChildren_Deformable, 2, nullptr, 0, &Present_Deformable, &Clear_Deformable}, + {"Flex", "flex", ElementType::Flex, kFields_Flex, 15, kChildren_Flex, 3, nullptr, 0, &Present_Flex, &Clear_Flex}, + {"FlexEdge", "edge", ElementType::FlexEdge, kFields_FlexEdge, 2, nullptr, 0, nullptr, 0, &Present_FlexEdge, &Clear_FlexEdge}, + {"Contact", "contact", ElementType::Contact, nullptr, 0, kChildren_Contact, 2, nullptr, 0, &Present_Contact, &Clear_Contact}, + {"Pair", "pair", ElementType::Pair, kFields_Pair, 12, nullptr, 0, nullptr, 0, &Present_Pair, &Clear_Pair}, + {"Exclude", "exclude", ElementType::Exclude, kFields_Exclude, 3, nullptr, 0, nullptr, 0, &Present_Exclude, &Clear_Exclude}, + {"Tendon", "tendon", ElementType::Tendon, nullptr, 0, kChildren_Tendon, 1, nullptr, 0, &Present_Tendon, &Clear_Tendon}, + {"Spatial", "spatial", ElementType::Spatial, kFields_Spatial, 21, kChildren_Spatial, 1, nullptr, 0, &Present_Spatial, &Clear_Spatial}, + {"SpatialSite", "site", ElementType::SpatialSite, kFields_SpatialSite, 1, nullptr, 0, nullptr, 0, &Present_SpatialSite, &Clear_SpatialSite}, + {"SpatialGeom", "geom", ElementType::SpatialGeom, kFields_SpatialGeom, 2, nullptr, 0, nullptr, 0, &Present_SpatialGeom, &Clear_SpatialGeom}, + {"Pulley", "pulley", ElementType::Pulley, kFields_Pulley, 1, nullptr, 0, nullptr, 0, &Present_Pulley, &Clear_Pulley}, + {"Fixed", "fixed", ElementType::Fixed, kFields_Fixed, 18, kChildren_Fixed, 1, nullptr, 0, &Present_Fixed, &Clear_Fixed}, + {"FixedJoint", "joint", ElementType::FixedJoint, kFields_FixedJoint, 2, nullptr, 0, nullptr, 0, &Present_FixedJoint, &Clear_FixedJoint}, + {"Equality", "equality", ElementType::Equality, nullptr, 0, kChildren_Equality, 1, nullptr, 0, &Present_Equality, &Clear_Equality}, + {"Connect", "connect", ElementType::Connect, kFields_Connect, 10, nullptr, 0, kConstraints_Connect, 3, &Present_Connect, &Clear_Connect}, + {"Weld", "weld", ElementType::Weld, kFields_Weld, 12, nullptr, 0, kConstraints_Weld, 3, &Present_Weld, &Clear_Weld}, + {"EqualityJoint", "joint", ElementType::EqualityJoint, kFields_EqualityJoint, 8, nullptr, 0, nullptr, 0, &Present_EqualityJoint, &Clear_EqualityJoint}, + {"EqualityTendon", "tendon", ElementType::EqualityTendon, kFields_EqualityTendon, 8, nullptr, 0, nullptr, 0, &Present_EqualityTendon, &Clear_EqualityTendon}, + {"EqualityFlex", "flex", ElementType::EqualityFlex, kFields_EqualityFlex, 6, nullptr, 0, nullptr, 0, &Present_EqualityFlex, &Clear_EqualityFlex}, + {"Flexvert", "flexvert", ElementType::Flexvert, kFields_Flexvert, 6, nullptr, 0, nullptr, 0, &Present_Flexvert, &Clear_Flexvert}, + {"Flexstrain", "flexstrain", ElementType::Flexstrain, kFields_Flexstrain, 7, nullptr, 0, nullptr, 0, &Present_Flexstrain, &Clear_Flexstrain}, + {"Actuator", "actuator", ElementType::Actuator, nullptr, 0, kChildren_Actuator, 1, nullptr, 0, &Present_Actuator, &Clear_Actuator}, + {"ActuatorGeneral", "general", ElementType::ActuatorGeneral, kFields_ActuatorGeneral, 37, nullptr, 0, nullptr, 0, &Present_ActuatorGeneral, &Clear_ActuatorGeneral}, + {"Motor", "motor", ElementType::Motor, kFields_Motor, 23, nullptr, 0, nullptr, 0, &Present_Motor, &Clear_Motor}, + {"Position", "position", ElementType::Position, kFields_Position, 28, nullptr, 0, nullptr, 0, &Present_Position, &Clear_Position}, + {"Velocity", "velocity", ElementType::Velocity, kFields_Velocity, 24, nullptr, 0, nullptr, 0, &Present_Velocity, &Clear_Velocity}, + {"IntVelocity", "intvelocity", ElementType::IntVelocity, kFields_IntVelocity, 29, nullptr, 0, nullptr, 0, &Present_IntVelocity, &Clear_IntVelocity}, + {"OrientationActuator", "orientation", ElementType::OrientationActuator, kFields_OrientationActuator, 17, nullptr, 0, nullptr, 0, &Present_OrientationActuator, &Clear_OrientationActuator}, + {"Pid", "pid", ElementType::Pid, kFields_Pid, 34, nullptr, 0, nullptr, 0, &Present_Pid, &Clear_Pid}, + {"Damper", "damper", ElementType::Damper, kFields_Damper, 23, nullptr, 0, nullptr, 0, &Present_Damper, &Clear_Damper}, + {"Cylinder", "cylinder", ElementType::Cylinder, kFields_Cylinder, 26, nullptr, 0, nullptr, 0, &Present_Cylinder, &Clear_Cylinder}, + {"Muscle", "muscle", ElementType::Muscle, kFields_Muscle, 31, nullptr, 0, nullptr, 0, &Present_Muscle, &Clear_Muscle}, + {"Adhesion", "adhesion", ElementType::Adhesion, kFields_Adhesion, 12, nullptr, 0, nullptr, 0, &Present_Adhesion, &Clear_Adhesion}, + {"DcMotor", "dcmotor", ElementType::DcMotor, kFields_DcMotor, 31, nullptr, 0, nullptr, 0, &Present_DcMotor, &Clear_DcMotor}, + {"ActuatorPlugin", "plugin", ElementType::ActuatorPlugin, kFields_ActuatorPlugin, 30, kChildren_ActuatorPlugin, 1, nullptr, 0, &Present_ActuatorPlugin, &Clear_ActuatorPlugin}, + {"Sensor", "sensor", ElementType::Sensor, nullptr, 0, kChildren_Sensor, 1, nullptr, 0, &Present_Sensor, &Clear_Sensor}, + {"Touch", "touch", ElementType::Touch, kFields_Touch, 9, nullptr, 0, nullptr, 0, &Present_Touch, &Clear_Touch}, + {"Accelerometer", "accelerometer", ElementType::Accelerometer, kFields_Accelerometer, 9, nullptr, 0, nullptr, 0, &Present_Accelerometer, &Clear_Accelerometer}, + {"Velocimeter", "velocimeter", ElementType::Velocimeter, kFields_Velocimeter, 9, nullptr, 0, nullptr, 0, &Present_Velocimeter, &Clear_Velocimeter}, + {"Gyro", "gyro", ElementType::Gyro, kFields_Gyro, 9, nullptr, 0, nullptr, 0, &Present_Gyro, &Clear_Gyro}, + {"Force", "force", ElementType::Force, kFields_Force, 9, nullptr, 0, nullptr, 0, &Present_Force, &Clear_Force}, + {"Torque", "torque", ElementType::Torque, kFields_Torque, 9, nullptr, 0, nullptr, 0, &Present_Torque, &Clear_Torque}, + {"Magnetometer", "magnetometer", ElementType::Magnetometer, kFields_Magnetometer, 9, nullptr, 0, nullptr, 0, &Present_Magnetometer, &Clear_Magnetometer}, + {"Camprojection", "camprojection", ElementType::Camprojection, kFields_Camprojection, 10, nullptr, 0, nullptr, 0, &Present_Camprojection, &Clear_Camprojection}, + {"Rangefinder", "rangefinder", ElementType::Rangefinder, kFields_Rangefinder, 11, nullptr, 0, kConstraints_Rangefinder, 2, &Present_Rangefinder, &Clear_Rangefinder}, + {"Jointpos", "jointpos", ElementType::Jointpos, kFields_Jointpos, 9, nullptr, 0, nullptr, 0, &Present_Jointpos, &Clear_Jointpos}, + {"Jointvel", "jointvel", ElementType::Jointvel, kFields_Jointvel, 9, nullptr, 0, nullptr, 0, &Present_Jointvel, &Clear_Jointvel}, + {"Tendonpos", "tendonpos", ElementType::Tendonpos, kFields_Tendonpos, 9, nullptr, 0, nullptr, 0, &Present_Tendonpos, &Clear_Tendonpos}, + {"Tendonvel", "tendonvel", ElementType::Tendonvel, kFields_Tendonvel, 9, nullptr, 0, nullptr, 0, &Present_Tendonvel, &Clear_Tendonvel}, + {"Actuatorpos", "actuatorpos", ElementType::Actuatorpos, kFields_Actuatorpos, 9, nullptr, 0, nullptr, 0, &Present_Actuatorpos, &Clear_Actuatorpos}, + {"Actuatorvel", "actuatorvel", ElementType::Actuatorvel, kFields_Actuatorvel, 9, nullptr, 0, nullptr, 0, &Present_Actuatorvel, &Clear_Actuatorvel}, + {"Actuatorfrc", "actuatorfrc", ElementType::Actuatorfrc, kFields_Actuatorfrc, 9, nullptr, 0, nullptr, 0, &Present_Actuatorfrc, &Clear_Actuatorfrc}, + {"Jointactuatorfrc", "jointactuatorfrc", ElementType::Jointactuatorfrc, kFields_Jointactuatorfrc, 9, nullptr, 0, nullptr, 0, &Present_Jointactuatorfrc, &Clear_Jointactuatorfrc}, + {"Tendonactuatorfrc", "tendonactuatorfrc", ElementType::Tendonactuatorfrc, kFields_Tendonactuatorfrc, 9, nullptr, 0, nullptr, 0, &Present_Tendonactuatorfrc, &Clear_Tendonactuatorfrc}, + {"Ballquat", "ballquat", ElementType::Ballquat, kFields_Ballquat, 9, nullptr, 0, nullptr, 0, &Present_Ballquat, &Clear_Ballquat}, + {"Ballangvel", "ballangvel", ElementType::Ballangvel, kFields_Ballangvel, 9, nullptr, 0, nullptr, 0, &Present_Ballangvel, &Clear_Ballangvel}, + {"Jointlimitpos", "jointlimitpos", ElementType::Jointlimitpos, kFields_Jointlimitpos, 9, nullptr, 0, nullptr, 0, &Present_Jointlimitpos, &Clear_Jointlimitpos}, + {"Jointlimitvel", "jointlimitvel", ElementType::Jointlimitvel, kFields_Jointlimitvel, 9, nullptr, 0, nullptr, 0, &Present_Jointlimitvel, &Clear_Jointlimitvel}, + {"Jointlimitfrc", "jointlimitfrc", ElementType::Jointlimitfrc, kFields_Jointlimitfrc, 9, nullptr, 0, nullptr, 0, &Present_Jointlimitfrc, &Clear_Jointlimitfrc}, + {"Tendonlimitpos", "tendonlimitpos", ElementType::Tendonlimitpos, kFields_Tendonlimitpos, 9, nullptr, 0, nullptr, 0, &Present_Tendonlimitpos, &Clear_Tendonlimitpos}, + {"Tendonlimitvel", "tendonlimitvel", ElementType::Tendonlimitvel, kFields_Tendonlimitvel, 9, nullptr, 0, nullptr, 0, &Present_Tendonlimitvel, &Clear_Tendonlimitvel}, + {"Tendonlimitfrc", "tendonlimitfrc", ElementType::Tendonlimitfrc, kFields_Tendonlimitfrc, 9, nullptr, 0, nullptr, 0, &Present_Tendonlimitfrc, &Clear_Tendonlimitfrc}, + {"Framepos", "framepos", ElementType::Framepos, kFields_Framepos, 12, nullptr, 0, kConstraints_Framepos, 1, &Present_Framepos, &Clear_Framepos}, + {"Framequat", "framequat", ElementType::Framequat, kFields_Framequat, 12, nullptr, 0, kConstraints_Framequat, 1, &Present_Framequat, &Clear_Framequat}, + {"Framexaxis", "framexaxis", ElementType::Framexaxis, kFields_Framexaxis, 12, nullptr, 0, kConstraints_Framexaxis, 1, &Present_Framexaxis, &Clear_Framexaxis}, + {"Frameyaxis", "frameyaxis", ElementType::Frameyaxis, kFields_Frameyaxis, 12, nullptr, 0, kConstraints_Frameyaxis, 1, &Present_Frameyaxis, &Clear_Frameyaxis}, + {"Framezaxis", "framezaxis", ElementType::Framezaxis, kFields_Framezaxis, 12, nullptr, 0, kConstraints_Framezaxis, 1, &Present_Framezaxis, &Clear_Framezaxis}, + {"Framelinvel", "framelinvel", ElementType::Framelinvel, kFields_Framelinvel, 12, nullptr, 0, kConstraints_Framelinvel, 1, &Present_Framelinvel, &Clear_Framelinvel}, + {"Frameangvel", "frameangvel", ElementType::Frameangvel, kFields_Frameangvel, 12, nullptr, 0, kConstraints_Frameangvel, 1, &Present_Frameangvel, &Clear_Frameangvel}, + {"Framelinacc", "framelinacc", ElementType::Framelinacc, kFields_Framelinacc, 10, nullptr, 0, nullptr, 0, &Present_Framelinacc, &Clear_Framelinacc}, + {"Frameangacc", "frameangacc", ElementType::Frameangacc, kFields_Frameangacc, 10, nullptr, 0, nullptr, 0, &Present_Frameangacc, &Clear_Frameangacc}, + {"Subtreecom", "subtreecom", ElementType::Subtreecom, kFields_Subtreecom, 9, nullptr, 0, nullptr, 0, &Present_Subtreecom, &Clear_Subtreecom}, + {"Subtreelinvel", "subtreelinvel", ElementType::Subtreelinvel, kFields_Subtreelinvel, 9, nullptr, 0, nullptr, 0, &Present_Subtreelinvel, &Clear_Subtreelinvel}, + {"Subtreeangmom", "subtreeangmom", ElementType::Subtreeangmom, kFields_Subtreeangmom, 9, nullptr, 0, nullptr, 0, &Present_Subtreeangmom, &Clear_Subtreeangmom}, + {"Insidesite", "insidesite", ElementType::Insidesite, kFields_Insidesite, 11, nullptr, 0, nullptr, 0, &Present_Insidesite, &Clear_Insidesite}, + {"Distance", "distance", ElementType::Distance, kFields_Distance, 12, nullptr, 0, kConstraints_Distance, 4, &Present_Distance, &Clear_Distance}, + {"Normal", "normal", ElementType::Normal, kFields_Normal, 12, nullptr, 0, kConstraints_Normal, 4, &Present_Normal, &Clear_Normal}, + {"Fromto", "fromto", ElementType::Fromto, kFields_Fromto, 12, nullptr, 0, kConstraints_Fromto, 4, &Present_Fromto, &Clear_Fromto}, + {"SensorContact", "contact", ElementType::SensorContact, kFields_SensorContact, 18, nullptr, 0, kConstraints_SensorContact, 2, &Present_SensorContact, &Clear_SensorContact}, + {"EPotential", "e_potential", ElementType::EPotential, kFields_EPotential, 8, nullptr, 0, nullptr, 0, &Present_EPotential, &Clear_EPotential}, + {"EKinetic", "e_kinetic", ElementType::EKinetic, kFields_EKinetic, 8, nullptr, 0, nullptr, 0, &Present_EKinetic, &Clear_EKinetic}, + {"Clock", "clock", ElementType::Clock, kFields_Clock, 8, nullptr, 0, nullptr, 0, &Present_Clock, &Clear_Clock}, + {"Tactile", "tactile", ElementType::Tactile, kFields_Tactile, 8, nullptr, 0, nullptr, 0, &Present_Tactile, &Clear_Tactile}, + {"SensorUser", "user", ElementType::SensorUser, kFields_SensorUser, 9, nullptr, 0, kConstraints_SensorUser, 1, &Present_SensorUser, &Clear_SensorUser}, + {"SensorPlugin", "plugin", ElementType::SensorPlugin, kFields_SensorPlugin, 9, kChildren_SensorPlugin, 1, nullptr, 0, &Present_SensorPlugin, &Clear_SensorPlugin}, + {"Custom", "custom", ElementType::Custom, nullptr, 0, kChildren_Custom, 3, nullptr, 0, &Present_Custom, &Clear_Custom}, + {"Numeric", "numeric", ElementType::Numeric, kFields_Numeric, 3, nullptr, 0, nullptr, 0, &Present_Numeric, &Clear_Numeric}, + {"Text", "text", ElementType::Text, kFields_Text, 2, nullptr, 0, nullptr, 0, &Present_Text, &Clear_Text}, + {"Tuple", "tuple", ElementType::Tuple, kFields_Tuple, 1, kChildren_Tuple, 1, nullptr, 0, &Present_Tuple, &Clear_Tuple}, + {"TupleElement", "element", ElementType::TupleElement, kFields_TupleElement, 3, nullptr, 0, nullptr, 0, &Present_TupleElement, &Clear_TupleElement}, + {"Keyframe", "keyframe", ElementType::Keyframe, nullptr, 0, kChildren_Keyframe, 1, nullptr, 0, &Present_Keyframe, &Clear_Keyframe}, + {"Key", "key", ElementType::Key, kFields_Key, 8, nullptr, 0, nullptr, 0, &Present_Key, &Clear_Key}, + {"Frame", "frame", ElementType::Frame, kFields_Frame, 4, kChildren_Frame, 2, nullptr, 0, &Present_Frame, &Clear_Frame}, + {"Replicate", "replicate", ElementType::Replicate, kFields_Replicate, 5, kChildren_Replicate, 2, nullptr, 0, &Present_Replicate, &Clear_Replicate}, + {"EqualityDefault", "equality", ElementType::EqualityDefault, kFields_EqualityDefault, 3, nullptr, 0, nullptr, 0, &Present_EqualityDefault, &Clear_EqualityDefault}, + {"TendonDefault", "tendon", ElementType::TendonDefault, kFields_TendonDefault, 16, nullptr, 0, nullptr, 0, &Present_TendonDefault, &Clear_TendonDefault}, +}; + +constexpr UnionDescriptor kUnions[] = { + {"ActuatorAny", kUnionMembers_ActuatorAny, 13}, + {"BodyChildAny", kUnionMembers_BodyChildAny, 13}, + {"EqualityAny", kUnionMembers_EqualityAny, 7}, + {"FlexAny", kUnionMembers_FlexAny, 2}, + {"JointAny", kUnionMembers_JointAny, 2}, + {"PathItemAny", kUnionMembers_PathItemAny, 3}, + {"SensorAny", kUnionMembers_SensorAny, 49}, + {"TendonAny", kUnionMembers_TendonAny, 2}, +}; + +const std::unordered_map& +NameIndex() { + static const std::unordered_map + index = [] { + std::unordered_map m; + for (const auto& d : kElements) m.emplace(d.name, &d); + return m; + }(); + return index; +} + +} // namespace + +const ElementDescriptor& Describe(ElementType type) { + return kElements[static_cast(type)]; +} + +const ElementDescriptor* DescribeByName(std::string_view name) { + const auto& index = NameIndex(); + auto it = index.find(name); + return it == index.end() ? nullptr : it->second; +} + +std::size_t ElementCount() { return std::size(kElements); } + +const ElementDescriptor& ElementAt(std::size_t index) { + return kElements[index]; +} + +const UnionDescriptor& DescribeUnion(std::string_view name) { + for (const auto& u : kUnions) + if (u.name == name) return u; + return kUnions[0]; +} + +std::size_t UnionCount() { return std::size(kUnions); } + +const UnionDescriptor& UnionAt(std::size_t index) { + return kUnions[index]; +} + +} // namespace ps::mjcf::reflect diff --git a/protospec/lib/generated/reflect.h b/protospec/lib/generated/reflect.h new file mode 100644 index 00000000..a9310b06 --- /dev/null +++ b/protospec/lib/generated/reflect.h @@ -0,0 +1,290 @@ +// Generated by protospec_gen.emit — do not edit. +// +// Runtime reflection: static schema description usable without +// compile-time knowledge of the element structs. This is the contract +// downstream UE detail-panel mirroring consumes. +// +// Shape: every element has an ElementDescriptor carrying its name, XML +// tag, a contiguous FieldDescriptor[] (one per authored field, in +// flattened schema order -- the same order and ids the generated Visit +// uses) and a ChildDescriptor[] (element or union child lists). Each +// FieldDescriptor states the field's storage kind, element type name +// (enum/ref-target name for non-primitives), arity bounds, and +// whether it is optional / has an IDL default. Dynamic value access is +// the visitor's job (visit.h); reflection adds only presence + clear +// accessors (present/clear take the element by void* plus a field id). +// UnionDescriptor lists a union's member element types so a union child +// list can be walked generically. kFieldCount_ constants pin +// each table's length for the generated-code self-check. +#ifndef PROTOSPEC_GENERATED_REFLECT_H +#define PROTOSPEC_GENERATED_REFLECT_H + +#include +#include + +#include "types.h" + +namespace ps::mjcf::reflect { + +enum class FieldKind { + Bool, Int32, Uint64, Float, Double, String, Enum, Ref, +}; + +enum class ArityKind { Scalar, Fixed, Range, Unbounded }; + +enum class Cardinality { ZeroOrMore, ZeroOrOne, One }; + +struct FieldDescriptor { + std::string_view name; // IDL / IR field name + std::string_view xml; // MJCF attribute name + std::string_view type_name; // prim / enum / ref-target name + FieldKind kind; + ArityKind arity; + int arity_min; + int arity_max; + bool optional; + bool has_default; + std::string_view target_from; // dynamic ref: sibling field naming the + // target type ("" = not a dynamic ref) + std::string_view doc; // one-line field description (schema comment) +}; + +struct ChildDescriptor { + std::string_view name; // IR-side child-list field name + std::string_view target; // element name, or union name if is_union + bool is_union; + Cardinality card; +}; + +// A presence constraint over an element's fields, from the schema's +// exclusive / together / requires / oneof rows. A bundle is complete +// when every field it names is present; Exclusive admits at most one +// complete bundle, OneOf at least one, Together all-or-none, and +// Requires means the first bundle needs the second. +enum class ConstraintKind { Exclusive, Together, Requires, OneOf }; + +struct ConstraintBundle { + const int* fields; + std::size_t field_count; +}; + +struct ConstraintDescriptor { + ConstraintKind kind; + const ConstraintBundle* bundles; + std::size_t bundle_count; + std::string_view doc; +}; + +struct ElementDescriptor { + std::string_view name; + std::string_view xml; + ElementType type; + const FieldDescriptor* fields; + std::size_t field_count; + const ChildDescriptor* children; + std::size_t child_count; + const ConstraintDescriptor* constraints; + std::size_t constraint_count; + bool (*present)(const void* elem, int field_id); + void (*clear)(void* elem, int field_id); +}; + +struct UnionDescriptor { + std::string_view name; + const ElementType* members; + std::size_t member_count; +}; + +// Field-count constants (pin each generated table's length). +inline constexpr std::size_t kFieldCount_Model = 1; +inline constexpr std::size_t kFieldCount_Compiler = 21; +inline constexpr std::size_t kFieldCount_LengthRange = 10; +inline constexpr std::size_t kFieldCount_Option = 27; +inline constexpr std::size_t kFieldCount_Flag = 26; +inline constexpr std::size_t kFieldCount_Size = 14; +inline constexpr std::size_t kFieldCount_Statistic = 5; +inline constexpr std::size_t kFieldCount_Visual = 0; +inline constexpr std::size_t kFieldCount_VisualGlobal = 13; +inline constexpr std::size_t kFieldCount_VisualQuality = 5; +inline constexpr std::size_t kFieldCount_VisualHeadlight = 4; +inline constexpr std::size_t kFieldCount_VisualMap = 13; +inline constexpr std::size_t kFieldCount_VisualScale = 17; +inline constexpr std::size_t kFieldCount_VisualRgba = 25; +inline constexpr std::size_t kFieldCount_Default = 1; +inline constexpr std::size_t kFieldCount_MaterialLayer = 2; +inline constexpr std::size_t kFieldCount_Extension = 0; +inline constexpr std::size_t kFieldCount_PluginDef = 1; +inline constexpr std::size_t kFieldCount_PluginInstance = 1; +inline constexpr std::size_t kFieldCount_Config = 2; +inline constexpr std::size_t kFieldCount_Asset = 0; +inline constexpr std::size_t kFieldCount_Mesh = 17; +inline constexpr std::size_t kFieldCount_PluginRef = 2; +inline constexpr std::size_t kFieldCount_Hfield = 7; +inline constexpr std::size_t kFieldCount_Skin = 9; +inline constexpr std::size_t kFieldCount_SkinBone = 5; +inline constexpr std::size_t kFieldCount_Texture = 24; +inline constexpr std::size_t kFieldCount_Material = 11; +inline constexpr std::size_t kFieldCount_ModelAsset = 3; +inline constexpr std::size_t kFieldCount_Body = 9; +inline constexpr std::size_t kFieldCount_Inertial = 4; +inline constexpr std::size_t kFieldCount_Joint = 24; +inline constexpr std::size_t kFieldCount_FreeJoint = 3; +inline constexpr std::size_t kFieldCount_Geom = 31; +inline constexpr std::size_t kFieldCount_Attach = 4; +inline constexpr std::size_t kFieldCount_Site = 11; +inline constexpr std::size_t kFieldCount_Camera = 17; +inline constexpr std::size_t kFieldCount_Light = 19; +inline constexpr std::size_t kFieldCount_Composite = 9; +inline constexpr std::size_t kFieldCount_CompositeJoint = 17; +inline constexpr std::size_t kFieldCount_CompositeSkin = 6; +inline constexpr std::size_t kFieldCount_CompositeGeom = 19; +inline constexpr std::size_t kFieldCount_CompositeSite = 4; +inline constexpr std::size_t kFieldCount_Flexcomp = 27; +inline constexpr std::size_t kFieldCount_FlexcompEdge = 5; +inline constexpr std::size_t kFieldCount_FlexElasticity = 5; +inline constexpr std::size_t kFieldCount_FlexContact = 14; +inline constexpr std::size_t kFieldCount_FlexcompPin = 4; +inline constexpr std::size_t kFieldCount_Deformable = 0; +inline constexpr std::size_t kFieldCount_Flex = 15; +inline constexpr std::size_t kFieldCount_FlexEdge = 2; +inline constexpr std::size_t kFieldCount_Contact = 0; +inline constexpr std::size_t kFieldCount_Pair = 12; +inline constexpr std::size_t kFieldCount_Exclude = 3; +inline constexpr std::size_t kFieldCount_Tendon = 0; +inline constexpr std::size_t kFieldCount_Spatial = 21; +inline constexpr std::size_t kFieldCount_SpatialSite = 1; +inline constexpr std::size_t kFieldCount_SpatialGeom = 2; +inline constexpr std::size_t kFieldCount_Pulley = 1; +inline constexpr std::size_t kFieldCount_Fixed = 18; +inline constexpr std::size_t kFieldCount_FixedJoint = 2; +inline constexpr std::size_t kFieldCount_Equality = 0; +inline constexpr std::size_t kFieldCount_Connect = 10; +inline constexpr std::size_t kFieldCount_Weld = 12; +inline constexpr std::size_t kFieldCount_EqualityJoint = 8; +inline constexpr std::size_t kFieldCount_EqualityTendon = 8; +inline constexpr std::size_t kFieldCount_EqualityFlex = 6; +inline constexpr std::size_t kFieldCount_Flexvert = 6; +inline constexpr std::size_t kFieldCount_Flexstrain = 7; +inline constexpr std::size_t kFieldCount_Actuator = 0; +inline constexpr std::size_t kFieldCount_ActuatorGeneral = 37; +inline constexpr std::size_t kFieldCount_Motor = 23; +inline constexpr std::size_t kFieldCount_Position = 28; +inline constexpr std::size_t kFieldCount_Velocity = 24; +inline constexpr std::size_t kFieldCount_IntVelocity = 29; +inline constexpr std::size_t kFieldCount_OrientationActuator = 17; +inline constexpr std::size_t kFieldCount_Pid = 34; +inline constexpr std::size_t kFieldCount_Damper = 23; +inline constexpr std::size_t kFieldCount_Cylinder = 26; +inline constexpr std::size_t kFieldCount_Muscle = 31; +inline constexpr std::size_t kFieldCount_Adhesion = 12; +inline constexpr std::size_t kFieldCount_DcMotor = 31; +inline constexpr std::size_t kFieldCount_ActuatorPlugin = 30; +inline constexpr std::size_t kFieldCount_Sensor = 0; +inline constexpr std::size_t kFieldCount_Touch = 9; +inline constexpr std::size_t kFieldCount_Accelerometer = 9; +inline constexpr std::size_t kFieldCount_Velocimeter = 9; +inline constexpr std::size_t kFieldCount_Gyro = 9; +inline constexpr std::size_t kFieldCount_Force = 9; +inline constexpr std::size_t kFieldCount_Torque = 9; +inline constexpr std::size_t kFieldCount_Magnetometer = 9; +inline constexpr std::size_t kFieldCount_Camprojection = 10; +inline constexpr std::size_t kFieldCount_Rangefinder = 11; +inline constexpr std::size_t kFieldCount_Jointpos = 9; +inline constexpr std::size_t kFieldCount_Jointvel = 9; +inline constexpr std::size_t kFieldCount_Tendonpos = 9; +inline constexpr std::size_t kFieldCount_Tendonvel = 9; +inline constexpr std::size_t kFieldCount_Actuatorpos = 9; +inline constexpr std::size_t kFieldCount_Actuatorvel = 9; +inline constexpr std::size_t kFieldCount_Actuatorfrc = 9; +inline constexpr std::size_t kFieldCount_Jointactuatorfrc = 9; +inline constexpr std::size_t kFieldCount_Tendonactuatorfrc = 9; +inline constexpr std::size_t kFieldCount_Ballquat = 9; +inline constexpr std::size_t kFieldCount_Ballangvel = 9; +inline constexpr std::size_t kFieldCount_Jointlimitpos = 9; +inline constexpr std::size_t kFieldCount_Jointlimitvel = 9; +inline constexpr std::size_t kFieldCount_Jointlimitfrc = 9; +inline constexpr std::size_t kFieldCount_Tendonlimitpos = 9; +inline constexpr std::size_t kFieldCount_Tendonlimitvel = 9; +inline constexpr std::size_t kFieldCount_Tendonlimitfrc = 9; +inline constexpr std::size_t kFieldCount_Framepos = 12; +inline constexpr std::size_t kFieldCount_Framequat = 12; +inline constexpr std::size_t kFieldCount_Framexaxis = 12; +inline constexpr std::size_t kFieldCount_Frameyaxis = 12; +inline constexpr std::size_t kFieldCount_Framezaxis = 12; +inline constexpr std::size_t kFieldCount_Framelinvel = 12; +inline constexpr std::size_t kFieldCount_Frameangvel = 12; +inline constexpr std::size_t kFieldCount_Framelinacc = 10; +inline constexpr std::size_t kFieldCount_Frameangacc = 10; +inline constexpr std::size_t kFieldCount_Subtreecom = 9; +inline constexpr std::size_t kFieldCount_Subtreelinvel = 9; +inline constexpr std::size_t kFieldCount_Subtreeangmom = 9; +inline constexpr std::size_t kFieldCount_Insidesite = 11; +inline constexpr std::size_t kFieldCount_Distance = 12; +inline constexpr std::size_t kFieldCount_Normal = 12; +inline constexpr std::size_t kFieldCount_Fromto = 12; +inline constexpr std::size_t kFieldCount_SensorContact = 18; +inline constexpr std::size_t kFieldCount_EPotential = 8; +inline constexpr std::size_t kFieldCount_EKinetic = 8; +inline constexpr std::size_t kFieldCount_Clock = 8; +inline constexpr std::size_t kFieldCount_Tactile = 8; +inline constexpr std::size_t kFieldCount_SensorUser = 9; +inline constexpr std::size_t kFieldCount_SensorPlugin = 9; +inline constexpr std::size_t kFieldCount_Custom = 0; +inline constexpr std::size_t kFieldCount_Numeric = 3; +inline constexpr std::size_t kFieldCount_Text = 2; +inline constexpr std::size_t kFieldCount_Tuple = 1; +inline constexpr std::size_t kFieldCount_TupleElement = 3; +inline constexpr std::size_t kFieldCount_Keyframe = 0; +inline constexpr std::size_t kFieldCount_Key = 8; +inline constexpr std::size_t kFieldCount_Frame = 4; +inline constexpr std::size_t kFieldCount_Replicate = 5; +inline constexpr std::size_t kFieldCount_EqualityDefault = 3; +inline constexpr std::size_t kFieldCount_TendonDefault = 16; + +// The element types carries a SAME-TYPE class partial for: the +// families whose class element has the live element's own type, so the +// layered class merge applies field-wise. Read off 's child list, +// excluding the nested-class child and the distinct-partial families +// (EqualityDefault / TendonDefault), which the merge does not cover. +inline constexpr ElementType kDefaultFamilies[] = { + ElementType::Mesh, + ElementType::Material, + ElementType::Joint, + ElementType::Geom, + ElementType::Site, + ElementType::Camera, + ElementType::Light, + ElementType::Pair, + ElementType::ActuatorGeneral, + ElementType::Motor, + ElementType::Position, + ElementType::Velocity, + ElementType::IntVelocity, + ElementType::OrientationActuator, + ElementType::Pid, + ElementType::Damper, + ElementType::Cylinder, + ElementType::Muscle, + ElementType::Adhesion, + ElementType::DcMotor, +}; +inline constexpr std::size_t kDefaultFamilyCount = 20; + +constexpr bool IsDefaultFamily(ElementType t) { + for (ElementType f : kDefaultFamilies) + if (f == t) return true; + return false; +} + +const ElementDescriptor& Describe(ElementType type); +const ElementDescriptor* DescribeByName(std::string_view name); +std::size_t ElementCount(); +const ElementDescriptor& ElementAt(std::size_t index); + +const UnionDescriptor& DescribeUnion(std::string_view name); +std::size_t UnionCount(); +const UnionDescriptor& UnionAt(std::size_t index); + +} // namespace ps::mjcf::reflect + +#endif // PROTOSPEC_GENERATED_REFLECT_H diff --git a/protospec/lib/generated/types.cc b/protospec/lib/generated/types.cc new file mode 100644 index 00000000..a645fa84 --- /dev/null +++ b/protospec/lib/generated/types.cc @@ -0,0 +1,4704 @@ +// Generated by protospec_gen.emit — do not edit. +#include "types.h" + +#include +#include +#include + +namespace ps::mjcf { + +ActuatorAny Clone(const ActuatorAny& src) { + ActuatorAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const ActuatorAny& a, const ActuatorAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +BodyChildAny Clone(const BodyChildAny& src) { + BodyChildAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const BodyChildAny& a, const BodyChildAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +EqualityAny Clone(const EqualityAny& src) { + EqualityAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const EqualityAny& a, const EqualityAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +FlexAny Clone(const FlexAny& src) { + FlexAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const FlexAny& a, const FlexAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +JointAny Clone(const JointAny& src) { + JointAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const JointAny& a, const JointAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +PathItemAny Clone(const PathItemAny& src) { + PathItemAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const PathItemAny& a, const PathItemAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +SensorAny Clone(const SensorAny& src) { + SensorAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const SensorAny& a, const SensorAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +TendonAny Clone(const TendonAny& src) { + TendonAny out; + std::visit( + [&](const auto& p) { + if (p) out.node = Clone(*p); + }, + src.node); + return out; +} + +bool operator==(const TendonAny& a, const TendonAny& b) { + if (a.node.index() != b.node.index()) return false; + return std::visit( + [&](const auto& pa) { + const auto& pb = std::get>(b.node); + const bool ha = static_cast(pa); + const bool hb = static_cast(pb); + if (ha != hb) return false; + return !ha || (*pa == *pb); + }, + a.node); +} + +std::unique_ptr Clone(const Model& src) { + auto out = std::make_unique(); + out->loc = src.loc; + out->model = src.model; + out->compilers = ps::PtrVecClone(src.compilers); + out->options = ps::PtrVecClone(src.options); + out->sizes = ps::PtrVecClone(src.sizes); + out->statistics = ps::PtrVecClone(src.statistics); + out->visuals = ps::PtrVecClone(src.visuals); + out->defaults = ps::PtrVecClone(src.defaults); + out->extensions = ps::PtrVecClone(src.extensions); + out->assets = ps::PtrVecClone(src.assets); + out->worldbody = ps::PtrVecClone(src.worldbody); + out->deformables = ps::PtrVecClone(src.deformables); + out->contacts = ps::PtrVecClone(src.contacts); + out->tendons = ps::PtrVecClone(src.tendons); + out->equalities = ps::PtrVecClone(src.equalities); + out->actuators = ps::PtrVecClone(src.actuators); + out->sensors = ps::PtrVecClone(src.sensors); + out->customs = ps::PtrVecClone(src.customs); + out->keyframes = ps::PtrVecClone(src.keyframes); + return out; +} + +bool operator==(const Model& a, const Model& b) { + return a.model == b.model && + ps::PtrVecEq(a.compilers, b.compilers) && + ps::PtrVecEq(a.options, b.options) && + ps::PtrVecEq(a.sizes, b.sizes) && + ps::PtrVecEq(a.statistics, b.statistics) && + ps::PtrVecEq(a.visuals, b.visuals) && + ps::PtrVecEq(a.defaults, b.defaults) && + ps::PtrVecEq(a.extensions, b.extensions) && + ps::PtrVecEq(a.assets, b.assets) && + ps::PtrVecEq(a.worldbody, b.worldbody) && + ps::PtrVecEq(a.deformables, b.deformables) && + ps::PtrVecEq(a.contacts, b.contacts) && + ps::PtrVecEq(a.tendons, b.tendons) && + ps::PtrVecEq(a.equalities, b.equalities) && + ps::PtrVecEq(a.actuators, b.actuators) && + ps::PtrVecEq(a.sensors, b.sensors) && + ps::PtrVecEq(a.customs, b.customs) && + ps::PtrVecEq(a.keyframes, b.keyframes); +} + +std::unique_ptr Clone(const Compiler& src) { + auto out = std::make_unique(); + out->loc = src.loc; + out->autolimits = src.autolimits; + out->boundmass = src.boundmass; + out->boundinertia = src.boundinertia; + out->settotalmass = src.settotalmass; + out->balanceinertia = src.balanceinertia; + out->strippath = src.strippath; + out->coordinate = src.coordinate; + out->angle = src.angle; + out->fitaabb = src.fitaabb; + out->eulerseq = src.eulerseq; + out->meshdir = src.meshdir; + out->texturedir = src.texturedir; + out->discardvisual = src.discardvisual; + out->usethread = src.usethread; + out->fusestatic = src.fusestatic; + out->inertiafromgeom = src.inertiafromgeom; + out->inertiagrouprange = src.inertiagrouprange; + out->saveinertial = src.saveinertial; + out->assetdir = src.assetdir; + out->alignfree = src.alignfree; + out->conflict = src.conflict; + out->lengthRanges = ps::PtrVecClone(src.lengthRanges); + return out; +} + +bool operator==(const Compiler& a, const Compiler& b) { + return a.autolimits == b.autolimits && + a.boundmass == b.boundmass && + a.boundinertia == b.boundinertia && + a.settotalmass == b.settotalmass && + a.balanceinertia == b.balanceinertia && + a.strippath == b.strippath && + a.coordinate == b.coordinate && + a.angle == b.angle && + a.fitaabb == b.fitaabb && + a.eulerseq == b.eulerseq && + a.meshdir == b.meshdir && + a.texturedir == b.texturedir && + a.discardvisual == b.discardvisual && + a.usethread == b.usethread && + a.fusestatic == b.fusestatic && + a.inertiafromgeom == b.inertiafromgeom && + a.inertiagrouprange == b.inertiagrouprange && + a.saveinertial == b.saveinertial && + a.assetdir == b.assetdir && + a.alignfree == b.alignfree && + a.conflict == b.conflict && + ps::PtrVecEq(a.lengthRanges, b.lengthRanges); +} + +std::unique_ptr Clone(const LengthRange& src) { + auto out = std::make_unique(); + out->loc = src.loc; + out->mode = src.mode; + out->useexisting = src.useexisting; + out->uselimit = src.uselimit; + out->accel = src.accel; + out->maxforce = src.maxforce; + out->timeconst = src.timeconst; + out->timestep = src.timestep; + out->inttotal = src.inttotal; + out->interval = src.interval; + out->tolrange = src.tolrange; + return out; +} + +bool operator==(const LengthRange& a, const LengthRange& b) { + return a.mode == b.mode && + a.useexisting == b.useexisting && + a.uselimit == b.uselimit && + a.accel == b.accel && + a.maxforce == b.maxforce && + a.timeconst == b.timeconst && + a.timestep == b.timestep && + a.inttotal == b.inttotal && + a.interval == b.interval && + a.tolrange == b.tolrange; +} + +std::unique_ptr