From 09d66c091285d8e8a5e379eb991f6ef313b03ece Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Sat, 18 Apr 2026 11:54:46 +0100 Subject: [PATCH 1/3] Add flexcomp import and spec registration UMjFlexcomp component parses MuJoCo XML into UE properties (type, dim, count, spacing, mass, elasticity, contact, pin, etc). RegisterToSpec serializes the attributes back to a standalone MJCF fragment, parses it via mj_parseXMLString, and attaches the resulting expanded sub-spec (flex + generated bodies + slider joints) into the parent body via mjs_addFrame + mjs_attach. MuJoCo's own parser handles the macro expansion, so all geometry types (grid, box, mesh, direct) and DOF modes (full, radial, trilinear, quadratic) work correctly. Mesh-type flexcomps: welds per-face-split UE render verts, writes an OBJ to the wrapper's VFS, references it by filename in the generated XML. Python preprocessor (clean_meshes.py) extended to convert flexcomp-referenced meshes to GLB during import. XML parser creates a child UStaticMeshComponent for mesh-type flexcomps by resolving the file attribute relative to the XML directory, preferring GLB over raw OBJ. 7 automated tests covering grid (1D/2D/3D) compilation, flex vertex count, pinning, import attribute parsing, and elasticity propagation. --- Scripts/clean_meshes.py | 14 + .../Components/Deformable/MjFlexcomp.cpp | 600 ++++++++++++++++++ .../Private/MuJoCo/Core/MjArticulation.cpp | 32 + .../MuJoCo/Components/Deformable/MjFlexcomp.h | 261 ++++++++ .../URLabEditor/Private/MujocoXmlParser.cpp | 90 +++ .../Private/Tests/MjFlexcompTests.cpp | 322 ++++++++++ docs/plan_flexcomp_support.md | 72 +++ 7 files changed, 1391 insertions(+) create mode 100644 Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp create mode 100644 Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h create mode 100644 Source/URLabEditor/Private/Tests/MjFlexcompTests.cpp create mode 100644 docs/plan_flexcomp_support.md diff --git a/Scripts/clean_meshes.py b/Scripts/clean_meshes.py index 6546816..989de41 100644 --- a/Scripts/clean_meshes.py +++ b/Scripts/clean_meshes.py @@ -132,6 +132,20 @@ def process_xml(xml_path: Path): # Collect all mesh elements mesh_elements = list(root.iter("mesh")) + + # Also convert meshes referenced by + for flexcomp in root.iter("flexcomp"): + file_attr = flexcomp.get("file") + if file_attr: + source_path = mesh_base / file_attr + if source_path.exists(): + output_glb = source_path.with_suffix(".glb") + if not output_glb.exists() or output_glb.stat().st_mtime < source_path.stat().st_mtime: + print(f"\n[flexcomp] Converting mesh: {source_path.name} -> {output_glb.name}") + convert_mesh(source_path, output_glb) + else: + print(f"\n[flexcomp] Mesh up to date: {output_glb.name}") + print(f"Found {len(mesh_elements)} mesh assets in XML\n") # Phase 1: Plan output filenames, detect conflicts diff --git a/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp b/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp new file mode 100644 index 0000000..d65d2be --- /dev/null +++ b/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp @@ -0,0 +1,600 @@ +// 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/Deformable/MjFlexcomp.h" +#include "MuJoCo/Core/Spec/MjSpecWrapper.h" +#include "MuJoCo/Utils/MjXmlUtils.h" +#include "MuJoCo/Utils/MjUtils.h" +#include "Utils/URLabLogging.h" +#include "XmlNode.h" +#include "Engine/StaticMesh.h" +#include "Components/StaticMeshComponent.h" +#include "StaticMeshResources.h" +#include "Misc/FileHelper.h" +#include "Misc/Paths.h" +#include "HAL/FileManager.h" + +UMjFlexcomp::UMjFlexcomp() +{ + PrimaryComponentTick.bCanEverTick = false; +} + +// ============================================================================ +// Import +// ============================================================================ + +void UMjFlexcomp::ImportFromXml(const FXmlNode* Node) +{ + if (!Node) return; + + MjXmlUtils::ReadAttrString(Node, TEXT("name"), MjName); + + // Type + FString TypeStr = Node->GetAttribute(TEXT("type")); + if (TypeStr == TEXT("grid")) Type = EMjFlexcompType::Grid; + else if (TypeStr == TEXT("box")) Type = EMjFlexcompType::Box; + else if (TypeStr == TEXT("cylinder")) Type = EMjFlexcompType::Cylinder; + else if (TypeStr == TEXT("ellipsoid")) Type = EMjFlexcompType::Ellipsoid; + else if (TypeStr == TEXT("square")) Type = EMjFlexcompType::Square; + else if (TypeStr == TEXT("disc")) Type = EMjFlexcompType::Disc; + else if (TypeStr == TEXT("circle")) Type = EMjFlexcompType::Circle; + else if (TypeStr == TEXT("mesh")) Type = EMjFlexcompType::Mesh; + else if (TypeStr == TEXT("direct")) Type = EMjFlexcompType::Direct; + + // Dim + FString DimStr = Node->GetAttribute(TEXT("dim")); + if (!DimStr.IsEmpty()) Dim = FCString::Atoi(*DimStr); + + // Dof + FString DofStr = Node->GetAttribute(TEXT("dof")); + if (DofStr == TEXT("full")) DofType = EMjFlexcompDof::Full; + else if (DofStr == TEXT("radial")) DofType = EMjFlexcompDof::Radial; + else if (DofStr == TEXT("trilinear")) DofType = EMjFlexcompDof::Trilinear; + else if (DofStr == TEXT("quadratic")) DofType = EMjFlexcompDof::Quadratic; + + // Count + FString CountStr = Node->GetAttribute(TEXT("count")); + if (!CountStr.IsEmpty()) + { + TArray Parts; + CountStr.ParseIntoArray(Parts, TEXT(" "), true); + if (Parts.Num() >= 1) Count.X = FCString::Atoi(*Parts[0]); + if (Parts.Num() >= 2) Count.Y = FCString::Atoi(*Parts[1]); + if (Parts.Num() >= 3) Count.Z = FCString::Atoi(*Parts[2]); + } + + // Spacing + FString SpacingStr = Node->GetAttribute(TEXT("spacing")); + if (!SpacingStr.IsEmpty()) + { + TArray Parts; + SpacingStr.ParseIntoArray(Parts, TEXT(" "), true); + if (Parts.Num() >= 1) Spacing.X = FCString::Atof(*Parts[0]); + if (Parts.Num() >= 2) Spacing.Y = FCString::Atof(*Parts[1]); + if (Parts.Num() >= 3) Spacing.Z = FCString::Atof(*Parts[2]); + } + + // Scale + FString ScaleStr = Node->GetAttribute(TEXT("scale")); + if (!ScaleStr.IsEmpty()) + { + TArray Parts; + ScaleStr.ParseIntoArray(Parts, TEXT(" "), true); + if (Parts.Num() >= 3) + { + Scale.X = FCString::Atof(*Parts[0]); + Scale.Y = FCString::Atof(*Parts[1]); + Scale.Z = FCString::Atof(*Parts[2]); + bOverride_Scale = true; + } + } + + // Scalars — set bOverride_X when the attribute is present + MjXmlUtils::ReadAttrFloat(Node, TEXT("mass"), Mass, bOverride_Mass); + MjXmlUtils::ReadAttrFloat(Node, TEXT("inertiabox"), InertiaBox, bOverride_InertiaBox); + MjXmlUtils::ReadAttrFloat(Node, TEXT("radius"), Radius, bOverride_Radius); + + // Rgba + FString RgbaStr = Node->GetAttribute(TEXT("rgba")); + if (!RgbaStr.IsEmpty()) + { + TArray Parts; + RgbaStr.ParseIntoArray(Parts, TEXT(" "), true); + if (Parts.Num() >= 4) + { + Rgba.R = FCString::Atof(*Parts[0]); + Rgba.G = FCString::Atof(*Parts[1]); + Rgba.B = FCString::Atof(*Parts[2]); + Rgba.A = FCString::Atof(*Parts[3]); + bOverride_Rgba = true; + } + } + + // Booleans + MjXmlUtils::ReadAttrString(Node, TEXT("file"), MeshFile); + + FString RigidStr = Node->GetAttribute(TEXT("rigid")); + if (!RigidStr.IsEmpty()) bRigid = RigidStr.ToBool(); + + FString FlatStr = Node->GetAttribute(TEXT("flatskin")); + if (!FlatStr.IsEmpty()) bFlatSkin = FlatStr.ToBool(); + + // Direct data + FString PointStr = Node->GetAttribute(TEXT("point")); + if (!PointStr.IsEmpty()) + { + TArray Parts; + PointStr.ParseIntoArray(Parts, TEXT(" "), true); + for (const FString& P : Parts) PointData.Add(FCString::Atod(*P)); + } + + FString ElemStr = Node->GetAttribute(TEXT("element")); + if (!ElemStr.IsEmpty()) + { + TArray Parts; + ElemStr.ParseIntoArray(Parts, TEXT(" "), true); + for (const FString& P : Parts) ElementData.Add(FCString::Atoi(*P)); + } + + // Pos/Quat (handled by USceneComponent transform) + FString PosStr = Node->GetAttribute(TEXT("pos")); + if (!PosStr.IsEmpty()) + { + FVector MjPos = MjXmlUtils::ParseVector(PosStr); + SetRelativeLocation(MjUtils::MjToUEPosition(&MjPos.X)); + } + + // Sub-elements + for (const FXmlNode* Child : Node->GetChildrenNodes()) + { + FString ChildTag = Child->GetTag(); + + if (ChildTag == TEXT("contact")) + { + FString ConTypeStr = Child->GetAttribute(TEXT("contype")); + if (!ConTypeStr.IsEmpty()) { ConType = FCString::Atoi(*ConTypeStr); bOverride_ConType = true; } + + FString ConAffStr = Child->GetAttribute(TEXT("conaffinity")); + if (!ConAffStr.IsEmpty()) { ConAffinity = FCString::Atoi(*ConAffStr); bOverride_ConAffinity = true; } + + FString ConDimStr = Child->GetAttribute(TEXT("condim")); + if (!ConDimStr.IsEmpty()) { ConDim = FCString::Atoi(*ConDimStr); bOverride_ConDim = true; } + + FString PriorityStr = Child->GetAttribute(TEXT("priority")); + if (!PriorityStr.IsEmpty()) { Priority = FCString::Atoi(*PriorityStr); bOverride_Priority = true; } + + MjXmlUtils::ReadAttrFloat(Child, TEXT("margin"), Margin, bOverride_Margin); + MjXmlUtils::ReadAttrFloat(Child, TEXT("gap"), Gap, bOverride_Gap); + + FString SelfStr = Child->GetAttribute(TEXT("selfcollide")); + if (!SelfStr.IsEmpty()) + { + if (SelfStr == TEXT("none")) SelfCollide = 0; + else if (SelfStr == TEXT("auto")) SelfCollide = 1; + else if (SelfStr == TEXT("all")) SelfCollide = 2; + bOverride_SelfCollide = true; + } + + FString IntStr = Child->GetAttribute(TEXT("internal")); + if (!IntStr.IsEmpty()) { bInternal = IntStr.ToBool(); bOverride_Internal = true; } + } + else if (ChildTag == TEXT("edge")) + { + MjXmlUtils::ReadAttrFloat(Child, TEXT("stiffness"), EdgeStiffness, bOverride_EdgeStiffness); + MjXmlUtils::ReadAttrFloat(Child, TEXT("damping"), EdgeDamping, bOverride_EdgeDamping); + } + else if (ChildTag == TEXT("elasticity")) + { + MjXmlUtils::ReadAttrFloat(Child, TEXT("young"), Young, bOverride_Young); + MjXmlUtils::ReadAttrFloat(Child, TEXT("poisson"), Poisson, bOverride_Poisson); + MjXmlUtils::ReadAttrFloat(Child, TEXT("damping"), Damping, bOverride_Damping); + MjXmlUtils::ReadAttrFloat(Child, TEXT("thickness"), Thickness, bOverride_Thickness); + + FString E2DStr = Child->GetAttribute(TEXT("elastic2d")); + if (!E2DStr.IsEmpty()) + { + if (E2DStr == TEXT("none")) Elastic2D = 0; + else if (E2DStr == TEXT("bend")) Elastic2D = 1; + else if (E2DStr == TEXT("stretch")) Elastic2D = 2; + else if (E2DStr == TEXT("both")) Elastic2D = 3; + bOverride_Elastic2D = true; + } + } + else if (ChildTag == TEXT("pin")) + { + FString IdStr = Child->GetAttribute(TEXT("id")); + if (!IdStr.IsEmpty()) + { + TArray Parts; + IdStr.ParseIntoArray(Parts, TEXT(" "), true); + for (const FString& P : Parts) PinIds.Add(FCString::Atoi(*P)); + } + + FString GridRangeStr = Child->GetAttribute(TEXT("gridrange")); + if (!GridRangeStr.IsEmpty()) + { + TArray Parts; + GridRangeStr.ParseIntoArray(Parts, TEXT(" "), true); + for (const FString& P : Parts) PinGridRange.Add(FCString::Atoi(*P)); + } + } + } + + UE_LOG(LogURLab, Log, TEXT("[MjFlexcomp] Imported '%s': type=%s dim=%d count=(%d,%d,%d)"), + *MjName, *TypeStr, Dim, Count.X, Count.Y, Count.Z); +} + +// ============================================================================ +// Mesh Export (for mesh-type flexcomp) +// ============================================================================ + +FString UMjFlexcomp::ExportMeshToVFS(FMujocoSpecWrapper& Wrapper) const +{ + TArray Children; + GetChildrenComponents(false, Children); + + UStaticMeshComponent* SMC = nullptr; + for (USceneComponent* Child : Children) + { + SMC = Cast(Child); + if (SMC && SMC->GetStaticMesh()) break; + SMC = nullptr; + } + + if (!SMC || !SMC->GetStaticMesh()) return FString(); + + UStaticMesh* Mesh = SMC->GetStaticMesh(); + const FStaticMeshLODResources& LOD = Mesh->GetRenderData()->LODResources[0]; + + // UE splits vertices per-face (for normals/UVs). MuJoCo needs welded + // vertices for flex. We build a local remap table and emit unique positions. + int32 NumRawVerts = LOD.VertexBuffers.PositionVertexBuffer.GetNumVertices(); + TArray UniquePositions; + TArray RawToWelded; + RawToWelded.SetNum(NumRawVerts); + + const float WeldTolerance = 1e-5f; + const int32 HashSize = 1 << 14; + const int32 HashMask = HashSize - 1; + TArray> HashBuckets; + HashBuckets.SetNum(HashSize); + + auto HashPos = [HashMask](const FVector3f& P) + { + uint32 H = (uint32)(P.X * 73856093.f) ^ (uint32)(P.Y * 19349663.f) ^ (uint32)(P.Z * 83492791.f); + return (int32)(H & HashMask); + }; + + for (int32 i = 0; i < NumRawVerts; i++) + { + FVector3f Pos = LOD.VertexBuffers.PositionVertexBuffer.VertexPosition(i); + int32 Bucket = HashPos(Pos); + int32 Found = INDEX_NONE; + for (int32 C : HashBuckets[Bucket]) + { + if (UniquePositions[C].Equals(Pos, WeldTolerance)) { Found = C; break; } + } + if (Found == INDEX_NONE) + { + Found = UniquePositions.Add(Pos); + HashBuckets[Bucket].Add(Found); + } + RawToWelded[i] = Found; + } + + FIndexArrayView Indices = LOD.IndexBuffer.GetArrayView(); + + // Write OBJ with welded unique positions: UE cm -> MuJoCo m, flip Y, reverse winding. + FString ObjContent; + ObjContent.Reserve(UniquePositions.Num() * 40); + for (const FVector3f& P : UniquePositions) + { + ObjContent += FString::Printf(TEXT("v %f %f %f\n"), + P.X / 100.0f, -P.Y / 100.0f, P.Z / 100.0f); + } + + for (int32 i = 0; i + 2 < Indices.Num(); i += 3) + { + int32 A = RawToWelded[Indices[i]]; + int32 B = RawToWelded[Indices[i + 1]]; + int32 C = RawToWelded[Indices[i + 2]]; + if (A == B || B == C || A == C) continue; + // OBJ is 1-indexed; swap last two to fix winding after Y-flip. + ObjContent += FString::Printf(TEXT("f %d %d %d\n"), A + 1, C + 1, B + 1); + } + + int32 NumVerts = UniquePositions.Num(); + + // Write OBJ to a temp file so MuJoCo's parser can load it + FString FlexName = MjName.IsEmpty() ? GetName() : MjName; + FString ObjFileName = FString::Printf(TEXT("flexcomp_%s.obj"), *FlexName); + FString TempDir = FPaths::Combine(FPaths::ProjectSavedDir(), TEXT("URLab/FlexcompMeshes")); + IFileManager::Get().MakeDirectory(*TempDir, true); + FString FullPath = FPaths::ConvertRelativePathToFull(FPaths::Combine(TempDir, ObjFileName)); + + if (!FFileHelper::SaveStringToFile(ObjContent, *FullPath)) + { + UE_LOG(LogURLab, Warning, TEXT("[MjFlexcomp] Failed to write OBJ to '%s'"), *FullPath); + return FString(); + } + + // Add to VFS so MuJoCo's XML parser can resolve file="" + FString Dir = FPaths::GetPath(FullPath); + FString FileName = FPaths::GetCleanFilename(FullPath); + int Result = mj_addFileVFS(Wrapper.VFS, TCHAR_TO_UTF8(*Dir), TCHAR_TO_UTF8(*FileName)); + if (Result != 0) + { + UE_LOG(LogURLab, Warning, TEXT("[MjFlexcomp] mj_addFileVFS returned %d for '%s'"), Result, *FullPath); + } + + UE_LOG(LogURLab, Log, TEXT("[MjFlexcomp] Exported mesh to VFS: %s (%d verts, %d tris)"), + *FileName, NumVerts, Indices.Num() / 3); + return FileName; +} + +// ============================================================================ +// XML Serialization +// ============================================================================ + +FString UMjFlexcomp::BuildFlexcompXml(const FString& MeshAssetName) const +{ + FString FlexName = MjName.IsEmpty() ? GetName() : MjName; + + const TCHAR* TypeStr = TEXT("grid"); + switch (Type) + { + case EMjFlexcompType::Grid: TypeStr = TEXT("grid"); break; + case EMjFlexcompType::Box: TypeStr = TEXT("box"); break; + case EMjFlexcompType::Cylinder: TypeStr = TEXT("cylinder"); break; + case EMjFlexcompType::Ellipsoid: TypeStr = TEXT("ellipsoid"); break; + case EMjFlexcompType::Square: TypeStr = TEXT("square"); break; + case EMjFlexcompType::Disc: TypeStr = TEXT("disc"); break; + case EMjFlexcompType::Circle: TypeStr = TEXT("circle"); break; + case EMjFlexcompType::Mesh: TypeStr = TEXT("mesh"); break; + case EMjFlexcompType::Direct: TypeStr = TEXT("direct"); break; + } + + const TCHAR* DofStr = TEXT("full"); + switch (DofType) + { + case EMjFlexcompDof::Full: DofStr = TEXT("full"); break; + case EMjFlexcompDof::Radial: DofStr = TEXT("radial"); break; + case EMjFlexcompDof::Trilinear: DofStr = TEXT("trilinear"); break; + case EMjFlexcompDof::Quadratic: DofStr = TEXT("quadratic"); break; + } + + // Structural attributes — always emitted + FString Attrs = FString::Printf(TEXT("name=\"%s\" type=\"%s\" dim=\"%d\" dof=\"%s\""), + *FlexName, TypeStr, Dim, DofStr); + + // Physics/visual attributes — only emit when user opted to override + if (bOverride_Mass) Attrs += FString::Printf(TEXT(" mass=\"%f\""), Mass); + if (bOverride_InertiaBox) Attrs += FString::Printf(TEXT(" inertiabox=\"%f\""), InertiaBox); + if (bOverride_Radius) Attrs += FString::Printf(TEXT(" radius=\"%f\""), Radius); + if (bOverride_Rgba) Attrs += FString::Printf(TEXT(" rgba=\"%f %f %f %f\""), Rgba.R, Rgba.G, Rgba.B, Rgba.A); + + if (Type == EMjFlexcompType::Grid || Type == EMjFlexcompType::Box || + Type == EMjFlexcompType::Cylinder || Type == EMjFlexcompType::Ellipsoid) + { + Attrs += FString::Printf(TEXT(" count=\"%d %d %d\" spacing=\"%f %f %f\""), + Count.X, Count.Y, Count.Z, Spacing.X, Spacing.Y, Spacing.Z); + } + + if (bOverride_Scale && !Scale.Equals(FVector::OneVector)) + { + Attrs += FString::Printf(TEXT(" scale=\"%f %f %f\""), Scale.X, Scale.Y, Scale.Z); + } + + if (bRigid) Attrs += TEXT(" rigid=\"true\""); + if (bFlatSkin) Attrs += TEXT(" flatskin=\"true\""); + + if (Type == EMjFlexcompType::Mesh && !MeshAssetName.IsEmpty()) + { + Attrs += FString::Printf(TEXT(" file=\"%s\""), *MeshAssetName); + } + + // Component's relative location in UE becomes flexcomp pos in MuJoCo coords + FVector UEPos = GetRelativeLocation(); + double MjPos[3]; + MjUtils::UEToMjPosition(UEPos, MjPos); + Attrs += FString::Printf(TEXT(" pos=\"%f %f %f\""), MjPos[0], MjPos[1], MjPos[2]); + + // Direct-type point/element data + FString DirectChildren; + if (Type == EMjFlexcompType::Direct) + { + if (PointData.Num() > 0) + { + Attrs += TEXT(" point=\""); + for (int32 i = 0; i < PointData.Num(); i++) + { + if (i > 0) Attrs += TEXT(" "); + Attrs += FString::Printf(TEXT("%f"), PointData[i]); + } + Attrs += TEXT("\""); + } + if (ElementData.Num() > 0) + { + Attrs += TEXT(" element=\""); + for (int32 i = 0; i < ElementData.Num(); i++) + { + if (i > 0) Attrs += TEXT(" "); + Attrs += FString::Printf(TEXT("%d"), ElementData[i]); + } + Attrs += TEXT("\""); + } + } + + // Sub-elements — emit each only when at least one attribute is overridden, + // and only emit the overridden attributes inside. + FString SubElements; + + // + { + FString ContactAttrs; + if (bOverride_ConType) ContactAttrs += FString::Printf(TEXT(" contype=\"%d\""), ConType); + if (bOverride_ConAffinity) ContactAttrs += FString::Printf(TEXT(" conaffinity=\"%d\""), ConAffinity); + if (bOverride_ConDim) ContactAttrs += FString::Printf(TEXT(" condim=\"%d\""), ConDim); + if (bOverride_Priority) ContactAttrs += FString::Printf(TEXT(" priority=\"%d\""), Priority); + if (bOverride_Margin) ContactAttrs += FString::Printf(TEXT(" margin=\"%f\""), Margin); + if (bOverride_Gap) ContactAttrs += FString::Printf(TEXT(" gap=\"%f\""), Gap); + if (bOverride_SelfCollide) + { + const TCHAR* SelfStr = TEXT("auto"); + if (SelfCollide == 0) SelfStr = TEXT("none"); + else if (SelfCollide == 1) SelfStr = TEXT("auto"); + else if (SelfCollide == 2) SelfStr = TEXT("all"); + ContactAttrs += FString::Printf(TEXT(" selfcollide=\"%s\""), SelfStr); + } + if (bOverride_Internal) + { + ContactAttrs += FString::Printf(TEXT(" internal=\"%s\""), bInternal ? TEXT("true") : TEXT("false")); + } + if (!ContactAttrs.IsEmpty()) + { + SubElements += FString::Printf(TEXT(""), *ContactAttrs); + } + } + + // + { + FString EdgeAttrs; + if (bOverride_EdgeStiffness) EdgeAttrs += FString::Printf(TEXT(" stiffness=\"%f\""), EdgeStiffness); + if (bOverride_EdgeDamping) EdgeAttrs += FString::Printf(TEXT(" damping=\"%f\""), EdgeDamping); + if (!EdgeAttrs.IsEmpty()) + { + SubElements += FString::Printf(TEXT(""), *EdgeAttrs); + } + } + + // + { + FString ElasticAttrs; + if (bOverride_Young) ElasticAttrs += FString::Printf(TEXT(" young=\"%f\""), Young); + if (bOverride_Poisson) ElasticAttrs += FString::Printf(TEXT(" poisson=\"%f\""), Poisson); + if (bOverride_Damping) ElasticAttrs += FString::Printf(TEXT(" damping=\"%f\""), Damping); + if (bOverride_Thickness) ElasticAttrs += FString::Printf(TEXT(" thickness=\"%f\""), Thickness); + if (bOverride_Elastic2D) + { + const TCHAR* E2DStr = TEXT("none"); + if (Elastic2D == 1) E2DStr = TEXT("bend"); + else if (Elastic2D == 2) E2DStr = TEXT("stretch"); + else if (Elastic2D == 3) E2DStr = TEXT("both"); + ElasticAttrs += FString::Printf(TEXT(" elastic2d=\"%s\""), E2DStr); + } + if (!ElasticAttrs.IsEmpty()) + { + SubElements += FString::Printf(TEXT(""), *ElasticAttrs); + } + } + + // + if (PinIds.Num() > 0 || PinGridRange.Num() >= 6) + { + SubElements += TEXT(" 0) + { + SubElements += TEXT(" id=\""); + for (int32 i = 0; i < PinIds.Num(); i++) + { + if (i > 0) SubElements += TEXT(" "); + SubElements += FString::FromInt(PinIds[i]); + } + SubElements += TEXT("\""); + } + if (PinGridRange.Num() >= 6) + { + SubElements += TEXT(" gridrange=\""); + for (int32 i = 0; i < PinGridRange.Num(); i++) + { + if (i > 0) SubElements += TEXT(" "); + SubElements += FString::FromInt(PinGridRange[i]); + } + SubElements += TEXT("\""); + } + SubElements += TEXT("/>"); + } + + return FString::Printf(TEXT("%s"), *Attrs, *SubElements); +} + +// ============================================================================ +// Spec Registration (Path 2: XML parse + attach) +// ============================================================================ + +void UMjFlexcomp::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody) +{ + if (!ParentBody) return; + if (bIsRegistered) return; + + FString FlexName = MjName.IsEmpty() ? GetName() : MjName; + + // 1. For mesh type, export child static mesh to an OBJ in the VFS + FString MeshAssetName; + if (Type == EMjFlexcompType::Mesh) + { + MeshAssetName = ExportMeshToVFS(Wrapper); + if (MeshAssetName.IsEmpty()) + { + UE_LOG(LogURLab, Warning, TEXT("[MjFlexcomp] '%s': mesh export failed"), *FlexName); + return; + } + } + + // 2. Build standalone MJCF containing just this flexcomp + FString FlexcompXml = BuildFlexcompXml(MeshAssetName); + FString FullXml = FString::Printf( + TEXT("%s"), *FlexcompXml); + + // 3. Parse into temp spec — MuJoCo expands the flexcomp macro + char ErrBuf[1000] = ""; + mjSpec* TempSpec = mj_parseXMLString(TCHAR_TO_UTF8(*FullXml), Wrapper.VFS, ErrBuf, sizeof(ErrBuf)); + if (!TempSpec) + { + UE_LOG(LogURLab, Error, TEXT("[MjFlexcomp] '%s': mj_parseXMLString failed: %hs"), + *FlexName, ErrBuf); + return; + } + + // 4. Attach temp spec's worldbody into our parent via a new frame + mjsFrame* AttachFrame = mjs_addFrame(ParentBody, nullptr); + mjsBody* TempWorld = mjs_findBody(TempSpec, "world"); + if (!TempWorld) + { + UE_LOG(LogURLab, Error, TEXT("[MjFlexcomp] '%s': temp spec has no worldbody"), *FlexName); + mj_deleteSpec(TempSpec); + return; + } + + mjsElement* Attached = mjs_attach(AttachFrame->element, TempWorld->element, "", ""); + if (!Attached) + { + UE_LOG(LogURLab, Error, TEXT("[MjFlexcomp] '%s': mjs_attach failed"), *FlexName); + } + else + { + bIsRegistered = true; + UE_LOG(LogURLab, Log, TEXT("[MjFlexcomp] '%s': attached via XML+parse+attach"), *FlexName); + } + + mj_deleteSpec(TempSpec); +} diff --git a/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp b/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp index db3897f..4f8db45 100644 --- a/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp +++ b/Source/URLab/Private/MuJoCo/Core/MjArticulation.cpp @@ -44,6 +44,7 @@ #include "MuJoCo/Components/Sensors/MjSensor.h" #include "MuJoCo/Components/Actuators/MjActuator.h" #include "MuJoCo/Components/Tendons/MjTendon.h" +#include "MuJoCo/Components/Deformable/MjFlexcomp.h" #include "MuJoCo/Components/Defaults/MjDefault.h" #include "MuJoCo/Components/Physics/MjContactPair.h" #include "MuJoCo/Components/Physics/MjContactExclude.h" @@ -295,6 +296,37 @@ void AMjArticulation::Setup(mjSpec* Spec, mjVFS* VFS) } } + // 3b. Register flexcomp components (can be worldbody children or body children) + TArray Flexcomps; + GetComponents(Flexcomps); + for (UMjFlexcomp* Flex : Flexcomps) + { + if (Flex && !Flex->bIsDefault) + { + // Find the parent body in the spec + mjsBody* ParentSpecBody = nullptr; + + USceneComponent* Parent = Flex->GetAttachParent(); + while (Parent) + { + if (UMjBody* Body = Cast(Parent)) + { + FString BodyName = Body->MjName.IsEmpty() ? Body->GetName() : Body->MjName; + ParentSpecBody = mjs_findBody(m_wrapper->Spec, TCHAR_TO_UTF8(*BodyName)); + if (ParentSpecBody) break; + } + Parent = Parent->GetAttachParent(); + } + + if (!ParentSpecBody) + { + ParentSpecBody = mjs_findBody(m_wrapper->Spec, "world"); + } + + Flex->RegisterToSpec(*m_wrapper, ParentSpecBody); + } + } + // 4. Add Tendons (into child spec, after bodies so joint names are set) TArray Tendons; GetComponents(Tendons); diff --git a/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h b/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h new file mode 100644 index 0000000..fbe4507 --- /dev/null +++ b/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h @@ -0,0 +1,261 @@ +// 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/MjComponent.h" +#include +#include +#include "MjFlexcomp.generated.h" + +UENUM(BlueprintType) +enum class EMjFlexcompType : uint8 +{ + Grid, + Box, + Cylinder, + Ellipsoid, + Square, + Disc, + Circle, + Mesh, + Direct +}; + +UENUM(BlueprintType) +enum class EMjFlexcompDof : uint8 +{ + Full, + Radial, + Trilinear, + Quadratic +}; + +/** + * @class UMjFlexcomp + * @brief Component representing a MuJoCo flexcomp deformable body. + * + * Flexcomp generates a deformable soft body: ropes (dim=1), cloth (dim=2), + * or volumetric (dim=3). At spec registration, we serialize the component's + * attributes back to a standalone MJCF fragment, let MuJoCo's + * parser expand the macro (handles geometry generation, pin resolution, + * body/joint creation for any DOF type), then attach the resulting sub-spec + * into the parent body via mjs_attach. + */ +UCLASS(ClassGroup=(Custom), meta=(BlueprintSpawnableComponent)) +class URLAB_API UMjFlexcomp : public UMjComponent +{ + GENERATED_BODY() + +public: + UMjFlexcomp(); + + // --- Core Properties --- + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + EMjFlexcompType Type = EMjFlexcompType::Grid; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + int32 Dim = 2; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + EMjFlexcompDof DofType = EMjFlexcompDof::Full; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + FIntVector Count = FIntVector(10, 10, 1); + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + FVector Spacing = FVector(0.05f, 0.05f, 0.05f); + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp", meta=(InlineEditConditionToggle)) + bool bOverride_Mass = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp", meta=(EditCondition="bOverride_Mass")) + float Mass = 1.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp", meta=(InlineEditConditionToggle)) + bool bOverride_InertiaBox = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp", meta=(EditCondition="bOverride_InertiaBox")) + float InertiaBox = 0.005f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp", meta=(InlineEditConditionToggle)) + bool bOverride_Radius = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp", meta=(EditCondition="bOverride_Radius")) + float Radius = 0.005f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp", meta=(InlineEditConditionToggle)) + bool bOverride_Rgba = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp", meta=(EditCondition="bOverride_Rgba")) + FLinearColor Rgba = FLinearColor(0.5f, 0.5f, 0.5f, 1.0f); + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp", meta=(InlineEditConditionToggle)) + bool bOverride_Scale = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp", meta=(EditCondition="bOverride_Scale")) + FVector Scale = FVector(1.0f); + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + bool bRigid = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + bool bFlatSkin = false; + + // --- Mesh Type --- + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp") + FString MeshFile; + + // --- Direct Type Data --- + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Direct") + TArray PointData; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Direct") + TArray ElementData; + + // --- Contact Properties --- + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_ConType = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_ConType")) + int32 ConType = 1; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_ConAffinity = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_ConAffinity")) + int32 ConAffinity = 1; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_ConDim = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_ConDim")) + int32 ConDim = 3; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_Priority = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_Priority")) + int32 Priority = 0; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_Margin = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_Margin")) + float Margin = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_Gap = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_Gap")) + float Gap = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_SelfCollide = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_SelfCollide")) + int32 SelfCollide = 0; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Contact", meta=(InlineEditConditionToggle)) + bool bOverride_Internal = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Contact", meta=(EditCondition="bOverride_Internal")) + bool bInternal = false; + + // --- Edge Properties --- + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Edge", meta=(InlineEditConditionToggle)) + bool bOverride_EdgeStiffness = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Edge", meta=(EditCondition="bOverride_EdgeStiffness")) + float EdgeStiffness = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Edge", meta=(InlineEditConditionToggle)) + bool bOverride_EdgeDamping = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Edge", meta=(EditCondition="bOverride_EdgeDamping")) + float EdgeDamping = 0.0f; + + // --- Elasticity Properties --- + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Elasticity", meta=(InlineEditConditionToggle)) + bool bOverride_Young = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Elasticity", meta=(EditCondition="bOverride_Young")) + float Young = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Elasticity", meta=(InlineEditConditionToggle)) + bool bOverride_Poisson = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Elasticity", meta=(EditCondition="bOverride_Poisson")) + float Poisson = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Elasticity", meta=(InlineEditConditionToggle)) + bool bOverride_Damping = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Elasticity", meta=(EditCondition="bOverride_Damping")) + float Damping = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Elasticity", meta=(InlineEditConditionToggle)) + bool bOverride_Thickness = false; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Elasticity", meta=(EditCondition="bOverride_Thickness")) + float Thickness = 0.0f; + + UPROPERTY(EditAnywhere, Category = "MuJoCo|Flexcomp|Elasticity", meta=(InlineEditConditionToggle)) + bool bOverride_Elastic2D = false; + + /** @brief 2D passive forces mode: 0=none, 1=bending, 2=stretching, 3=both. */ + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Elasticity", meta=(EditCondition="bOverride_Elastic2D")) + int32 Elastic2D = 0; + + // --- Pin Data --- + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Pin") + TArray PinIds; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Pin") + TArray PinGridRange; + + // --- Import/Spec --- + + void ImportFromXml(const class FXmlNode* Node); + + virtual void RegisterToSpec(class FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody = nullptr) override; + +private: + /** Set once RegisterToSpec succeeds — prevents double-registration from + * both the articulation-level loop and MjBody's child iteration. */ + bool bIsRegistered = false; + + /** Serializes this flexcomp's properties back to an MJCF fragment. */ + FString BuildFlexcompXml(const FString& MeshAssetName) const; + + /** Exports the child StaticMeshComponent's mesh to an OBJ file in the wrapper's VFS. + * Returns the filename used in the VFS, or empty string on failure. */ + FString ExportMeshToVFS(class FMujocoSpecWrapper& Wrapper) const; +}; diff --git a/Source/URLabEditor/Private/MujocoXmlParser.cpp b/Source/URLabEditor/Private/MujocoXmlParser.cpp index 7050afb..f8c4473 100644 --- a/Source/URLabEditor/Private/MujocoXmlParser.cpp +++ b/Source/URLabEditor/Private/MujocoXmlParser.cpp @@ -108,6 +108,7 @@ #include "MuJoCo/Components/Geometry/Primitives/MjCylinder.h" #include "MuJoCo/Components/Physics/MjInertial.h" #include "MuJoCo/Components/Constraints/MjEquality.h" +#include "MuJoCo/Components/Deformable/MjFlexcomp.h" #include "MuJoCo/Components/Keyframes/MjKeyframe.h" #include "Engine/SCS_Node.h" @@ -569,6 +570,95 @@ void UMujocoGenerationAction::ImportNodeRecursive(const FXmlNode* Node, USCS_Nod if (!NameAttr.IsEmpty()) JointComp->MjName = NameAttr; } } + // --- FLEXCOMP --- + else if (Tag.Equals(TEXT("flexcomp"))) + { + FString Name = Node->GetAttribute(TEXT("name")); + if (Name.IsEmpty()) Name = TEXT("AUTONAME_Flexcomp"); + + CreatedNode = BP->SimpleConstructionScript->CreateNode(UMjFlexcomp::StaticClass(), *Name); + UMjFlexcomp* FlexComp = Cast(CreatedNode->ComponentTemplate); + if (FlexComp) + { + FlexComp->ImportFromXml(Node); + + // For mesh type, import the mesh file and create a child UStaticMeshComponent + FString FlexMeshFile = Node->GetAttribute(TEXT("file")); + if (FlexComp->Type == EMjFlexcompType::Mesh && !FlexMeshFile.IsEmpty()) + { + FString MeshName = FPaths::GetBaseFilename(FlexMeshFile); + + // Flexcomp references the mesh file directly (not via ). + // The Python preprocessor converts it to GLB alongside the original. + // Try GLB first (preprocessed), then fall back to raw file. + FString MeshFilePath; + if (MeshAssets.Contains(MeshName)) + { + MeshFilePath = MeshAssets[MeshName]; + } + else + { + // Try GLB in meshdir, then XMLDir + FString GlbName = FPaths::GetBaseFilename(FlexMeshFile) + TEXT(".glb"); + FString GlbPath = FPaths::Combine(XMLDir, TEXT("asset"), GlbName); + if (FPaths::FileExists(GlbPath)) + { + MeshFilePath = GlbPath; + } + else + { + GlbPath = FPaths::Combine(XMLDir, GlbName); + if (FPaths::FileExists(GlbPath)) + { + MeshFilePath = GlbPath; + } + else + { + // Fall back to original file + MeshFilePath = FPaths::Combine(XMLDir, TEXT("asset"), FlexMeshFile); + if (!FPaths::FileExists(MeshFilePath)) + { + MeshFilePath = FPaths::Combine(XMLDir, FlexMeshFile); + } + } + } + } + + if (FPaths::FileExists(MeshFilePath)) + { + FString MeshImportPath = AssetImportPath + TEXT("/Meshes"); + UStaticMesh* NewMesh = ImportSingleMesh(MeshFilePath, MeshImportPath); + if (NewMesh) + { + FString VizNodeName = FString::Printf(TEXT("Viz_%s"), *MeshName); + USCS_Node* MeshNode = BP->SimpleConstructionScript->CreateNode(UStaticMeshComponent::StaticClass(), *VizNodeName); + CreatedNode->AddChildNode(MeshNode); + + UStaticMeshComponent* MeshTemplate = Cast(MeshNode->ComponentTemplate); + if (MeshTemplate) + { + MeshTemplate->SetStaticMesh(NewMesh); + MeshTemplate->SetCollisionEnabled(ECollisionEnabled::QueryOnly); + MeshTemplate->SetCollisionResponseToAllChannels(ECR_Overlap); + + if (MeshScales.Contains(MeshName)) + { + FVector MeshScale = MeshScales[MeshName]; + if (!MeshScale.Equals(FVector::OneVector)) + { + MeshTemplate->SetRelativeScale3D(MeshScale); + } + } + } + } + } + else + { + UE_LOG(LogURLabEditor, Warning, TEXT("[Flexcomp] Mesh file not found: %s"), *MeshFilePath); + } + } + } + } // --- SITE --- else if (Tag.Equals(TEXT("site"))) { diff --git a/Source/URLabEditor/Private/Tests/MjFlexcompTests.cpp b/Source/URLabEditor/Private/Tests/MjFlexcompTests.cpp new file mode 100644 index 0000000..b5019c1 --- /dev/null +++ b/Source/URLabEditor/Private/Tests/MjFlexcompTests.cpp @@ -0,0 +1,322 @@ +// 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 "MuJoCo/Components/Deformable/MjFlexcomp.h" +#include "mujoco/mujoco.h" + +// ============================================================================ +// URLab.Flexcomp.Grid2D_Compiles +// A 2D grid flexcomp should compile and produce a flex in the model. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompGrid2DCompiles, + "URLab.Flexcomp.Grid2D_Compiles", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompGrid2DCompiles::RunTest(const FString& Parameters) +{ + FMjUESession S; + bool bOk = S.Init([](FMjUESession& Session) + { + UMjFlexcomp* Flex = NewObject(Session.Robot, TEXT("TestFlex")); + Flex->Type = EMjFlexcompType::Grid; + Flex->Dim = 2; + Flex->Count = FIntVector(4, 4, 1); + Flex->Spacing = FVector(0.05, 0.05, 0.05); + Flex->Mass = 0.5f; + Flex->Radius = 0.005f; + Flex->MjName = TEXT("testgrid"); + Flex->RegisterComponent(); + Flex->AttachToComponent(Session.Body, FAttachmentTransformRules::KeepRelativeTransform); + }); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Manager->PhysicsEngine->m_model; + TestTrue(TEXT("Model should have at least 1 flex"), M->nflex >= 1); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.Grid1D_Compiles +// A 1D grid (rope) should compile. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompGrid1DCompiles, + "URLab.Flexcomp.Grid1D_Compiles", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompGrid1DCompiles::RunTest(const FString& Parameters) +{ + FMjUESession S; + bool bOk = S.Init([](FMjUESession& Session) + { + UMjFlexcomp* Flex = NewObject(Session.Robot, TEXT("TestRope")); + Flex->Type = EMjFlexcompType::Grid; + Flex->Dim = 1; + Flex->Count = FIntVector(8, 1, 1); + Flex->Spacing = FVector(0.1, 0.1, 0.1); + Flex->Mass = 0.2f; + Flex->Radius = 0.01f; + Flex->MjName = TEXT("testrope"); + Flex->RegisterComponent(); + Flex->AttachToComponent(Session.Body, FAttachmentTransformRules::KeepRelativeTransform); + }); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Manager->PhysicsEngine->m_model; + TestTrue(TEXT("Model should have at least 1 flex"), M->nflex >= 1); + TestTrue(TEXT("Flex dim should be 1"), M->flex_dim[0] == 1); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.Grid3D_Compiles +// A 3D grid (volumetric) should compile. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompGrid3DCompiles, + "URLab.Flexcomp.Grid3D_Compiles", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompGrid3DCompiles::RunTest(const FString& Parameters) +{ + FMjUESession S; + bool bOk = S.Init([](FMjUESession& Session) + { + UMjFlexcomp* Flex = NewObject(Session.Robot, TEXT("TestVol")); + Flex->Type = EMjFlexcompType::Grid; + Flex->Dim = 3; + Flex->Count = FIntVector(3, 3, 3); + Flex->Spacing = FVector(0.05, 0.05, 0.05); + Flex->Mass = 1.0f; + Flex->Radius = 0.005f; + Flex->MjName = TEXT("testvol"); + Flex->RegisterComponent(); + Flex->AttachToComponent(Session.Body, FAttachmentTransformRules::KeepRelativeTransform); + }); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Manager->PhysicsEngine->m_model; + TestTrue(TEXT("Model should have at least 1 flex"), M->nflex >= 1); + TestTrue(TEXT("Flex dim should be 3"), M->flex_dim[0] == 3); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.Grid2D_CorrectBodyCount +// 4x4 grid = 16 vertices = 16 child bodies (all unpinned). +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompGrid2DBodyCount, + "URLab.Flexcomp.Grid2D_CorrectBodyCount", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompGrid2DBodyCount::RunTest(const FString& Parameters) +{ + FMjUESession S; + bool bOk = S.Init([](FMjUESession& Session) + { + UMjFlexcomp* Flex = NewObject(Session.Robot, TEXT("TestFlex")); + Flex->Type = EMjFlexcompType::Grid; + Flex->Dim = 2; + Flex->Count = FIntVector(4, 4, 1); + Flex->Spacing = FVector(0.05, 0.05, 0.05); + Flex->Mass = 0.5f; + Flex->Radius = 0.005f; + Flex->MjName = TEXT("counttest"); + Flex->RegisterComponent(); + Flex->AttachToComponent(Session.Body, FAttachmentTransformRules::KeepRelativeTransform); + }); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Manager->PhysicsEngine->m_model; + // 4x4 grid = 16 flex vertices + TestTrue(TEXT("Should have at least 1 flex"), M->nflex >= 1); + TestEqual(TEXT("Flex should have 16 vertices"), (int32)M->flex_vertnum[0], 16); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.PinnedVertices +// Pinning vertices should reduce the number of created bodies. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompPinnedVertices, + "URLab.Flexcomp.PinnedVertices", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompPinnedVertices::RunTest(const FString& Parameters) +{ + FMjUESession S; + bool bOk = S.Init([](FMjUESession& Session) + { + UMjFlexcomp* Flex = NewObject(Session.Robot, TEXT("TestFlex")); + Flex->Type = EMjFlexcompType::Grid; + Flex->Dim = 1; + Flex->Count = FIntVector(5, 1, 1); + Flex->Spacing = FVector(0.1, 0.1, 0.1); + Flex->Mass = 0.2f; + Flex->Radius = 0.01f; + Flex->MjName = TEXT("pintest"); + Flex->PinIds = {0, 4}; // Pin first and last + Flex->RegisterComponent(); + Flex->AttachToComponent(Session.Body, FAttachmentTransformRules::KeepRelativeTransform); + }); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + mjModel* M = S.Manager->PhysicsEngine->m_model; + TestTrue(TEXT("Should have at least 1 flex"), M->nflex >= 1); + // 5 vertices in the flex regardless of pinning + TestEqual(TEXT("Flex should have 5 vertices"), (int32)M->flex_vertnum[0], 5); + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.Import_Grid2D +// Importing a flexcomp XML should create a UMjFlexcomp template. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompImportGrid2D, + "URLab.Flexcomp.Import_Grid2D", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompImportGrid2D::RunTest(const FString& Parameters) +{ + FMjXmlImportSession S; + bool bOk = S.Init(TEXT( + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + "")); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Import Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + UMjFlexcomp* Flex = S.FindTemplate(TEXT("myflex")); + TestNotNull(TEXT("Should find flexcomp template"), Flex); + + if (Flex) + { + TestEqual(TEXT("Type should be Grid"), Flex->Type, EMjFlexcompType::Grid); + TestEqual(TEXT("Dim should be 2"), Flex->Dim, 2); + TestEqual(TEXT("Count.X should be 3"), Flex->Count.X, 3); + TestEqual(TEXT("Count.Y should be 3"), Flex->Count.Y, 3); + TestTrue(TEXT("Young should be 100"), MjTestMath::NearlyEqual(Flex->Young, 100.0f)); + TestTrue(TEXT("Damping should be 0.5"), MjTestMath::NearlyEqual(Flex->Damping, 0.5f)); + TestEqual(TEXT("Should have 3 pin IDs"), Flex->PinIds.Num(), 3); + } + + S.Cleanup(); + return true; +} + +// ============================================================================ +// URLab.Flexcomp.Elasticity_Imported +// Elasticity sub-element attributes should be parsed. +// ============================================================================ +IMPLEMENT_SIMPLE_AUTOMATION_TEST(FMjFlexcompElasticityImported, + "URLab.Flexcomp.Elasticity_Imported", + EAutomationTestFlags::EditorContext | EAutomationTestFlags::ProductFilter) + +bool FMjFlexcompElasticityImported::RunTest(const FString& Parameters) +{ + FMjXmlImportSession S; + bool bOk = S.Init(TEXT( + "" + " " + " " + " " + " " + " " + " " + " " + " " + "")); + + if (!bOk) + { + AddError(FString::Printf(TEXT("Import Init failed: %s"), *S.LastError)); + S.Cleanup(); + return false; + } + + UMjFlexcomp* Flex = S.FindTemplate(TEXT("elastic")); + TestNotNull(TEXT("Should find flexcomp template"), Flex); + + if (Flex) + { + TestTrue(TEXT("Young should be 1000"), MjTestMath::NearlyEqual(Flex->Young, 1000.0f)); + TestTrue(TEXT("Poisson should be 0.3"), MjTestMath::NearlyEqual(Flex->Poisson, 0.3f)); + TestTrue(TEXT("Damping should be 0.01"), MjTestMath::NearlyEqual(Flex->Damping, 0.01f)); + } + + S.Cleanup(); + return true; +} diff --git a/docs/plan_flexcomp_support.md b/docs/plan_flexcomp_support.md new file mode 100644 index 0000000..9c86709 --- /dev/null +++ b/docs/plan_flexcomp_support.md @@ -0,0 +1,72 @@ +# Plan: Flexcomp / Deformable Body Support + +## Background + +MuJoCo's `` generates deformable soft bodies — ropes (1D), cloth (2D), and volumetric (3D). At compile time it expands into a `` element plus child bodies with slider joints (point masses). + +The plugin currently ignores `` during import. + +## Approach + +Implement `UMjFlexcomp` as a standard component following the same pattern as `UMjGeom`, `UMjJoint`, etc: + +- **`ImportFromXml`** — reads `` attributes into component properties (mirrors `OneFlexcomp` in MuJoCo's `xml_native_reader.cc`) +- **`RegisterToSpec`** — expands into spec elements via `mjs_addFlex` + `mjs_addBody` + `mjs_addJoint` (mirrors `mjCFlexcomp::Make` in `user_flexcomp.cc`) + +No XML passthrough, no special compilation path. Fits directly into the existing programmatic spec pipeline. + +## What `Make()` does (from `user_flexcomp.cc`) + +1. Generate vertices + elements based on type (`MakeGrid`, `MakeBox`, `MakeMesh`, etc.) +2. Process pin data → `pinned[]` boolean array +3. Call `mjs_addFlex` → set vertices, elements, texcoords, contact/physics properties +4. For each non-pinned vertex: `mjs_addBody` + slider joints (1 or 3 depending on doftype) +5. Set `nodebody` names on the flex to reference the created bodies + +## Phased Implementation + +### Phase 1: Core types (grid, box, direct) +Pure math geometry generation — no file I/O needed. + +- `UMjFlexcomp` component with UPROPERTY fields for all flexcomp attributes +- `ImportFromXml` parsing +- `RegisterToSpec` with `MakeGrid`/`MakeBox`/`MakeDirect` geometry generation +- Pin support (pinid, pinrange, pingrid) +- DOF types: full (3 joints), radial (1 joint) + +### Phase 2: Mesh type +- Extract `UStaticMesh` from child `UStaticMeshComponent` (same as MjGeom/MjMeshGeom pipeline) +- Export to OBJ temp file, add to MuJoCo VFS +- `MakeMesh` reads from VFS — surface triangulation (dim=2) or tetrahedralization (dim=3) handled by MuJoCo internally +- GMSH (`.msh`) files not supported + +### Phase 3: Runtime visualization +- After `mj_compile`, read `mjModel.flex_*` arrays +- Build `UProceduralMeshComponent` per flex +- Per-tick vertex position update from `mjData.flexvert_xpos` + +### Phase 4: Editor UX +- Detail panel, custom icon, Blueprint authoring support + +## Files + +| File | Change | +|------|--------| +| `Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h` | New component | +| `Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp` | Import + RegisterToSpec | +| `Source/URLabEditor/Private/MujocoXmlParser.cpp` | Parse `` in body/worldbody | + +## Key `mjsFlex` fields to populate + +``` +dim, radius, rgba, material, group, flatskin +contype, conaffinity, condim, priority, friction, solmix, solref, solimp, margin, gap +internal, selfcollide, activelayers, passive +edgestiffness, edgedamping, young, poisson, damping, thickness, elastic2d +nodebody, vertbody, node, vert, elem, texcoord +``` + +## Open Questions + +1. **Trilinear/quadratic DOF types** — these use a fixed number of DOFs (24/81) regardless of vertex count, with interpolation. More complex than full/radial. Defer? +2. **Edge equality constraints** — `Make()` optionally creates `mjs_addEquality` for edge length preservation. Need to check which attribute controls this. From a8e9bde1534e9326d2b54b5be92fdb2cfe5252a6 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Sat, 18 Apr 2026 11:56:01 +0100 Subject: [PATCH 2/3] Add flexcomp runtime visualization with UDynamicMeshComponent Each UMjFlexcomp creates a UDynamicMeshComponent on Bind that mirrors the source static mesh (positions, UVs, normals from the LOD render data) and hides the original static mesh. Every tick, the deformed vertex positions are pulled from mjData.flexvert_xpos, converted to UE local space, and pushed via FastNotifyPositionsUpdated. Raw UE vertex attributes (UVs, normals, material) are preserved via a RawToWelded remap table, so the source mesh's UV seams and smooth shading carry over to the deforming display. The source material is copied directly onto the dynamic mesh. Tangents are auto-calculated by GeometryFramework via MikkTSpace to avoid crashes from manually populated tangent overlays. Reads of flexvert_xpos are guarded by the physics engine's callback mutex to prevent torn reads while the async physics thread steps. Known limitation: TAA ghosts slightly on fast deformation because FLocalVertexFactory (used by UDynamicMeshComponent) has no previous- position vertex stream. For flex-heavy scenes, set FXAA via r.AntiAliasingMethod=1. --- .../Components/Deformable/MjFlexcomp.cpp | 180 +++++++++++++++++- .../MuJoCo/Components/Deformable/MjFlexcomp.h | 42 +++- Source/URLab/URLab.Build.cs | 5 +- 3 files changed, 214 insertions(+), 13 deletions(-) diff --git a/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp b/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp index d65d2be..448bf4f 100644 --- a/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp +++ b/Source/URLab/Private/MuJoCo/Components/Deformable/MjFlexcomp.cpp @@ -29,13 +29,22 @@ #include "Engine/StaticMesh.h" #include "Components/StaticMeshComponent.h" #include "StaticMeshResources.h" +#include "Components/DynamicMeshComponent.h" +#include "UDynamicMesh.h" +#include "DynamicMesh/DynamicMesh3.h" +#include "DynamicMesh/DynamicMeshAttributeSet.h" +#include "Materials/MaterialInstanceDynamic.h" +#include "MuJoCo/Core/AMjManager.h" +#include "MuJoCo/Core/MjPhysicsEngine.h" #include "Misc/FileHelper.h" #include "Misc/Paths.h" #include "HAL/FileManager.h" +#include "Interfaces/IPluginManager.h" UMjFlexcomp::UMjFlexcomp() { - PrimaryComponentTick.bCanEverTick = false; + PrimaryComponentTick.bCanEverTick = true; + PrimaryComponentTick.bStartWithTickEnabled = false; } // ============================================================================ @@ -247,7 +256,7 @@ void UMjFlexcomp::ImportFromXml(const FXmlNode* Node) // Mesh Export (for mesh-type flexcomp) // ============================================================================ -FString UMjFlexcomp::ExportMeshToVFS(FMujocoSpecWrapper& Wrapper) const +FString UMjFlexcomp::ExportMeshToVFS(FMujocoSpecWrapper& Wrapper) { TArray Children; GetChildrenComponents(false, Children); @@ -264,12 +273,14 @@ FString UMjFlexcomp::ExportMeshToVFS(FMujocoSpecWrapper& Wrapper) const UStaticMesh* Mesh = SMC->GetStaticMesh(); const FStaticMeshLODResources& LOD = Mesh->GetRenderData()->LODResources[0]; + const FStaticMeshVertexBuffer& VB = LOD.VertexBuffers.StaticMeshVertexBuffer; // UE splits vertices per-face (for normals/UVs). MuJoCo needs welded - // vertices for flex. We build a local remap table and emit unique positions. + // vertices for flex. We build a remap table so the visualization can keep + // UE's per-face UVs/tangents while physics uses welded positions. int32 NumRawVerts = LOD.VertexBuffers.PositionVertexBuffer.GetNumVertices(); + NumRenderVerts = NumRawVerts; TArray UniquePositions; - TArray RawToWelded; RawToWelded.SetNum(NumRawVerts); const float WeldTolerance = 1e-5f; @@ -598,3 +609,164 @@ void UMjFlexcomp::RegisterToSpec(FMujocoSpecWrapper& Wrapper, mjsBody* ParentBod mj_deleteSpec(TempSpec); } + +void UMjFlexcomp::Bind(mjModel* Model, mjData* Data, const FString& Prefix) +{ + Super::Bind(Model, Data, Prefix); + + FString FlexName = MjName.IsEmpty() ? GetName() : MjName; + FString PrefixedName = Prefix + FlexName; + + for (int i = 0; i < Model->nflex; i++) + { + const char* Name = mj_id2name(Model, mjOBJ_FLEX, i); + if (Name && FString(UTF8_TO_TCHAR(Name)) == PrefixedName) + { + FlexId = i; + FlexVertAdr = Model->flex_vertadr[i]; + FlexVertNum = Model->flex_vertnum[i]; + + // Cache shell/triangle indices once at Bind (static, no thread race) + int32 ShellNum = Model->flex_shellnum[i]; + int32 ShellDataAdr = Model->flex_shelldataadr[i]; + int32 FlexDim = Model->flex_dim[i]; + + UE_LOG(LogURLab, Log, TEXT("[MjFlexcomp] Bound '%s' to flex ID %d: %d flex verts, %d render verts"), + *FlexName, FlexId, FlexVertNum, NumRenderVerts); + + CreateProceduralMesh(); + SetComponentTickEnabled(true); + break; + } + } +} + +void UMjFlexcomp::CreateProceduralMesh() +{ + if (FlexId < 0 || !m_Model || !m_Data || NumRenderVerts == 0) return; + + AActor* Owner = GetOwner(); + if (!Owner) return; + + // Find source static mesh (we'll build the dynamic mesh from its render data) + TArray Children; + GetChildrenComponents(false, Children); + UStaticMeshComponent* SourceSMC = nullptr; + for (USceneComponent* Child : Children) + { + if (UStaticMeshComponent* SMC = Cast(Child)) + { + if (SMC->GetStaticMesh()) { SourceSMC = SMC; break; } + } + } + if (!SourceSMC) return; + + DynamicMesh = NewObject(Owner, TEXT("FlexMesh")); + DynamicMesh->SetupAttachment(Owner->GetRootComponent()); + DynamicMesh->RegisterComponent(); + DynamicMesh->SetCollisionEnabled(ECollisionEnabled::NoCollision); + // Let DynamicMesh compute tangents itself via MikkTSpace from normals + UVs. + DynamicMesh->SetTangentsType(EDynamicMeshComponentTangentsMode::AutoCalculated); + + // Reuse the source mesh's material + if (UMaterialInterface* SourceMat = SourceSMC->GetMaterial(0)) + { + DynamicMesh->SetMaterial(0, SourceMat); + } + SourceSMC->SetVisibility(false); + SourceSMC->SetHiddenInGame(true); + + // Build the dynamic mesh from the static mesh's render data (vertex + index + UV + normal + tangent) + const FStaticMeshLODResources& LOD = SourceSMC->GetStaticMesh()->GetRenderData()->LODResources[0]; + const FStaticMeshVertexBuffer& VB = LOD.VertexBuffers.StaticMeshVertexBuffer; + const FPositionVertexBuffer& PB = LOD.VertexBuffers.PositionVertexBuffer; + FIndexArrayView Indices = LOD.IndexBuffer.GetArrayView(); + bool bHasUVs = VB.GetNumTexCoords() > 0; + + DynamicMesh->EditMesh([&](UE::Geometry::FDynamicMesh3& Mesh) + { + Mesh.Clear(); + Mesh.EnableAttributes(); + Mesh.Attributes()->SetNumNormalLayers(1); + if (bHasUVs) Mesh.Attributes()->SetNumUVLayers(1); + + UE::Geometry::FDynamicMeshNormalOverlay* NormalOverlay = Mesh.Attributes()->PrimaryNormals(); + UE::Geometry::FDynamicMeshUVOverlay* UVOverlay = bHasUVs ? Mesh.Attributes()->PrimaryUV() : nullptr; + + // Add vertices (positions in UE local space; at t=0 these come from the static mesh directly) + for (int32 i = 0; i < NumRenderVerts; i++) + { + FVector3f P = PB.VertexPosition(i); + Mesh.AppendVertex(FVector3d(P.X, P.Y, P.Z)); + + FVector4f Nz = VB.VertexTangentZ(i); + NormalOverlay->AppendElement(FVector3f(Nz.X, Nz.Y, Nz.Z)); + if (UVOverlay) + { + FVector2f UV = VB.GetVertexUV(i, 0); + UVOverlay->AppendElement(FVector2f(UV.X, UV.Y)); + } + } + + // Add triangles — use raw UE indices and set overlays to same element indices + for (int32 i = 0; i + 2 < Indices.Num(); i += 3) + { + int32 A = Indices[i], B = Indices[i + 1], C = Indices[i + 2]; + if (A == B || B == C || A == C) continue; + int32 TriId = Mesh.AppendTriangle(A, B, C); + if (TriId >= 0) + { + NormalOverlay->SetTriangle(TriId, UE::Geometry::FIndex3i(A, B, C)); + if (UVOverlay) UVOverlay->SetTriangle(TriId, UE::Geometry::FIndex3i(A, B, C)); + } + } + }, EDynamicMeshComponentRenderUpdateMode::FullUpdate); + + UpdateProceduralMesh(); +} + +void UMjFlexcomp::UpdateProceduralMesh() +{ + if (!DynamicMesh || FlexId < 0 || !m_Data || NumRenderVerts == 0) return; + + FTransform ParentTransform = DynamicMesh->GetAttachParent() + ? DynamicMesh->GetAttachParent()->GetComponentTransform() + : FTransform::Identity; + + // Read welded flex positions (lock mutex to avoid torn reads from physics thread) + TArray WeldedPositions; + WeldedPositions.SetNum(FlexVertNum); + + AAMjManager* Manager = AAMjManager::GetManager(); + UMjPhysicsEngine* Engine = Manager ? Manager->PhysicsEngine : nullptr; + { + TOptional Lock; + if (Engine) Lock.Emplace(&Engine->CallbackMutex); + for (int32 i = 0; i < FlexVertNum; i++) + { + int32 Idx = (FlexVertAdr + i) * 3; + FVector WorldPos = MjUtils::MjToUEPosition(&m_Data->flexvert_xpos[Idx]); + WeldedPositions[i] = ParentTransform.InverseTransformPosition(WorldPos); + } + } + + DynamicMesh->EditMesh([&](UE::Geometry::FDynamicMesh3& Mesh) + { + for (int32 i = 0; i < NumRenderVerts; i++) + { + int32 W = RawToWelded[i]; + FVector P = (W >= 0 && W < FlexVertNum) ? WeldedPositions[W] : FVector::ZeroVector; + Mesh.SetVertex(i, FVector3d(P.X, P.Y, P.Z)); + } + }, EDynamicMeshComponentRenderUpdateMode::NoUpdate); + + DynamicMesh->FastNotifyPositionsUpdated(/*bNormals=*/false, /*bColors=*/false, /*bUVs=*/false); +} + +void UMjFlexcomp::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) +{ + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + UpdateProceduralMesh(); +} + + diff --git a/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h b/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h index fbe4507..da8528c 100644 --- a/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h +++ b/Source/URLab/Public/MuJoCo/Components/Deformable/MjFlexcomp.h @@ -28,6 +28,8 @@ #include #include "MjFlexcomp.generated.h" +class UDynamicMeshComponent; + UENUM(BlueprintType) enum class EMjFlexcompType : uint8 { @@ -56,11 +58,8 @@ enum class EMjFlexcompDof : uint8 * @brief Component representing a MuJoCo flexcomp deformable body. * * Flexcomp generates a deformable soft body: ropes (dim=1), cloth (dim=2), - * or volumetric (dim=3). At spec registration, we serialize the component's - * attributes back to a standalone MJCF fragment, let MuJoCo's - * parser expand the macro (handles geometry generation, pin resolution, - * body/joint creation for any DOF type), then attach the resulting sub-spec - * into the parent body via mjs_attach. + * or volumetric (dim=3). At spec registration, it expands into a flex element + * plus child bodies with slider joints for non-pinned vertices. */ UCLASS(ClassGroup=(Custom), meta=(BlueprintSpawnableComponent)) class URLAB_API UMjFlexcomp : public UMjComponent @@ -241,21 +240,48 @@ class URLAB_API UMjFlexcomp : public UMjComponent UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "MuJoCo|Flexcomp|Pin") TArray PinGridRange; - // --- Import/Spec --- + // --- Import/Export/Bind --- void ImportFromXml(const class FXmlNode* Node); virtual void RegisterToSpec(class FMujocoSpecWrapper& Wrapper, mjsBody* ParentBody = nullptr) override; + virtual void Bind(mjModel* Model, mjData* Data, const FString& Prefix = TEXT("")) override; + + virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + private: + int32 FlexId = -1; + int32 FlexVertAdr = 0; + int32 FlexVertNum = 0; + /** Set once RegisterToSpec succeeds — prevents double-registration from * both the articulation-level loop and MjBody's child iteration. */ bool bIsRegistered = false; + UPROPERTY() + UDynamicMeshComponent* DynamicMesh = nullptr; + + /** Remap from raw UE vertex index → welded MuJoCo flex vertex index. + * Used each tick: raw_pos[i] = flexvert_xpos[RawToWelded[i]]. */ + TArray RawToWelded; + + /** Number of raw UE vertices. */ + int32 NumRenderVerts = 0; + + void CreateProceduralMesh(); + void UpdateProceduralMesh(); + + /** Captures raw UE vertex data (UVs, tangents, indices) from the child + * static mesh for rendering, plus builds the welded remap for physics. */ + void CaptureRenderData(); + /** Serializes this flexcomp's properties back to an MJCF fragment. */ FString BuildFlexcompXml(const FString& MeshAssetName) const; /** Exports the child StaticMeshComponent's mesh to an OBJ file in the wrapper's VFS. - * Returns the filename used in the VFS, or empty string on failure. */ - FString ExportMeshToVFS(class FMujocoSpecWrapper& Wrapper) const; + * Returns the filename used in the VFS, or empty string on failure. + * Also populates RawToWelded, RenderUVs, RenderTangents, RenderTriangles for + * the procedural-mesh visualization path. */ + FString ExportMeshToVFS(class FMujocoSpecWrapper& Wrapper); }; diff --git a/Source/URLab/URLab.Build.cs b/Source/URLab/URLab.Build.cs index 7abbe36..b56414b 100644 --- a/Source/URLab/URLab.Build.cs +++ b/Source/URLab/URLab.Build.cs @@ -31,7 +31,10 @@ public URLab(ReadOnlyTargetRules Target) : base(Target) "RenderCore", "RHI", "UMG", - "AssetRegistry" + "AssetRegistry", + "ProceduralMeshComponent", + "GeometryFramework", + "GeometryCore" }); // Editor-only dependencies for DecomposeMesh and other #if WITH_EDITOR code From c1d030ad2ad2731a4a905b1b921e6f06f8ab2439 Mon Sep 17 00:00:00 2001 From: Jonathan Embley-Riches Date: Sat, 18 Apr 2026 12:24:27 +0100 Subject: [PATCH 3/3] Document flexcomp support Add user-facing guide covering authoring, import, visualization, DOF modes, and the TAA ghosting limitation with FXAA workaround. Update MJCF schema coverage with all supported flexcomp attributes and sub-elements, noting pingrid/pinrange as the remaining gaps. --- docs/guides/flexcomp.md | 69 ++++++++++++++++++++++++++++++++++++ docs/mjcf_schema_coverage.md | 36 +++++++++++++++++++ 2 files changed, 105 insertions(+) create mode 100644 docs/guides/flexcomp.md diff --git a/docs/guides/flexcomp.md b/docs/guides/flexcomp.md new file mode 100644 index 0000000..4dffacf --- /dev/null +++ b/docs/guides/flexcomp.md @@ -0,0 +1,69 @@ +# Flexcomp — Deformable Bodies + +Flexcomp is MuJoCo's macro for generating deformable soft bodies: ropes (1D), cloth (2D), and volumetric bodies (3D). URLab's `UMjFlexcomp` component wraps this feature so you can author, import, simulate, and visualize flex bodies inside Unreal. + +## What gets generated + +A single `` expands (at compile time) into: + +- A `` element (the actual deformable, with physics properties) +- Multiple child bodies — one per non-pinned vertex (for `dof="full"` / `"radial"`), or 8 corner nodes (for `dof="trilinear"`), or 27 corner nodes (for `dof="quadratic"`) +- Slider joints on each generated body + +URLab leverages MuJoCo's own macro expander — we serialize the component's attributes back to an `` XML fragment, parse it with `mj_parseXMLString`, and attach the resulting sub-spec into the parent body via `mjs_attach`. Every flex feature MuJoCo supports (grid/mesh/direct geometry, pins, elasticity, edge stiffness, contact filtering) works automatically. + +## Quick import + +The standard drag-and-drop MJCF import pipeline handles `` natively. Files that reference meshes (e.g. ``) are auto-converted to GLB by the Python preprocessor alongside other mesh assets. The imported Blueprint contains a `UMjFlexcomp` component with all attributes populated and, for `type="mesh"`, a child `UStaticMeshComponent` used as the rendering source. + +See the bunny example: +```xml + + + + +``` + +## Authoring in Blueprint + +Add a `UMjFlexcomp` component to an `AMjArticulation` Blueprint. Required properties: + +| Property | Default | Notes | +|---|---|---| +| `Type` | Grid | grid / box / cylinder / ellipsoid / square / disc / circle / mesh / direct | +| `Dim` | 2 | 1 = lines, 2 = triangles, 3 = tetrahedra | +| `DofType` | Full | full (3 joints/vert), radial (1 joint/vert along radius), trilinear (8 corner bodies), quadratic (27 corner bodies) | +| `Count` / `Spacing` | 10×10×1 / 0.05 | For grid/box/cylinder/ellipsoid types | +| `MeshFile` | *(empty)* | For `type=mesh`, set automatically on import. Also add a child `UStaticMeshComponent` with the mesh asset. | +| `PointData` / `ElementData` | empty | For `type=direct`, raw vertex positions and element indices | + +Optional physics/visual properties follow the same `bOverride_X` pattern used throughout the plugin — toggle to override MuJoCo defaults, leave off to inherit. Categories: Contact, Edge, Elasticity, Pin. + +## Visualization + +At runtime, `UMjFlexcomp::Bind` creates a `UDynamicMeshComponent` mirroring the source static mesh (geometry, UVs, normals copied from LOD 0). Each tick, deformed vertex positions are read from `mjData.flexvert_xpos`, mapped through the welded→raw remap table, and pushed via `FastNotifyPositionsUpdated`. The source static mesh is hidden; its material is reused on the dynamic mesh. + +**Known limitation — TAA ghosting.** Because UE's `FLocalVertexFactory` has no previous-position vertex stream and there is no public API to provide one, CPU-deformed meshes produce zero motion vectors. Temporal Anti-Aliasing then accumulates stale previous-frame samples, producing visible smearing/ghost trails on fast deformation. Workarounds: + +- Set **Project Settings → Rendering → Anti-Aliasing Method → FXAA** (or `r.AntiAliasingMethod=1` in DefaultEngine.ini) for flex-heavy scenes. +- Use slow, squishy simulations where the effect is subtle. +- A future proper fix requires either a custom vertex factory with a previous-position stream (~1000 LoC + forked shader) or a material-based WPO approach with paired current/previous position textures. + +## Supported DOF modes + +| DOF | Bodies generated | Use case | +|---|---|---| +| `full` | One body per non-pinned vertex, 3 slider joints each | High-fidelity soft body where every vertex moves independently | +| `radial` | One body per non-pinned vertex, 1 radial slider joint | Inflatable shapes (radial deformation only) | +| `trilinear` | 8 corner bodies, 3 sliders each (24 DOFs total) | Smooth bulk deformation of a whole shape; scales to high vertex counts | +| `quadratic` | 27 corner bodies, 3 sliders each (81 DOFs total) | Higher-order deformation; more detail than trilinear | + +Use `trilinear` for meshes with thousands of vertices. `full` DOF on a 2500-vertex bunny creates ~7500 joints, which is slow. + +## Known limitations & TODOs + +- **TAA ghosting** on the procedural visualization (see above). +- **`bRigid` flexcomps** are attached to the parent body as static surface; they don't deform. +- **Raw `` elements** (without the `` macro) aren't directly supported — authors should use `` to generate them. +- **Per-vertex velocity output** to UE's velocity buffer is not implemented — blocks proper motion blur and TSR on deforming geometry. diff --git a/docs/mjcf_schema_coverage.md b/docs/mjcf_schema_coverage.md index ad254f3..96d4981 100644 --- a/docs/mjcf_schema_coverage.md +++ b/docs/mjcf_schema_coverage.md @@ -22,6 +22,7 @@ Plugin version: UnrealRoboticsLab main branch, 2026-03-28 | keyframe | 6 | 0 | 0 | 6 | | contact | 2 | 0 | 0 | 2 | | asset | 3 | 0 | 1 | 4 | +| flexcomp | 22 | 0 | 2 | 24 | --- @@ -377,3 +378,38 @@ Equality common attributes: | material | SUPPORTED | Parsed into `FMuJoCoMaterialData`. Creates UE MaterialInstance with texture params | | texture | SUPPORTED | Imported from disk (PNG/JPG/BMP/TGA). Applied to material instances | | hfield | MISSING | Not parsed from ``. Geom type=hfield recognized but asset data not imported | + +## flexcomp + +URLab parses `` into a `UMjFlexcomp` component and, at spec registration, serializes back to an MJCF fragment that MuJoCo's own parser expands via `mjs_attach`. All geometry types and DOF modes work without plugin-side reimplementation. + +| Attribute | Status | Notes | +|-----------|--------|-------| +| name | SUPPORTED | Import: `ReadAttrString("name")` -> `MjName`. Export: written into generated `` | +| type | SUPPORTED | Import: string -> `EMjFlexcompType`. grid/box/cylinder/ellipsoid/square/disc/circle/mesh/direct | +| dim | SUPPORTED | Import: `ReadAttrInt`. 1D lines / 2D triangles / 3D tetrahedra | +| dof | SUPPORTED | Import: string -> `EMjFlexcompDof`. full/radial/trilinear/quadratic | +| count | SUPPORTED | Import: 3-int array -> `FIntVector`. Grid/box/cylinder/ellipsoid types | +| spacing | SUPPORTED | Import: 3-float array -> `FVector`. Same types as count | +| pos | SUPPORTED | Import: `ParseVector` -> component relative location. Export: `UEToMjPosition` | +| scale | SUPPORTED | Import: 3-float array + `bOverride_Scale`. Export: only emitted when overridden | +| mass | SUPPORTED | Import: `ReadAttrFloat` + `bOverride_Mass`. Default inherits MuJoCo | +| inertiabox | SUPPORTED | Import: `ReadAttrFloat` + `bOverride_InertiaBox` | +| radius | SUPPORTED | Import: `ReadAttrFloat` + `bOverride_Radius` | +| rgba | SUPPORTED | Import: 4-float array + `bOverride_Rgba` | +| rigid | SUPPORTED | Import: `ReadAttrBool` -> `bRigid` | +| flatskin | SUPPORTED | Import: `ReadAttrBool` -> `bFlatSkin` | +| file | SUPPORTED | type=mesh. Import: path resolved relative to XML dir; child `UStaticMeshComponent` created. Export: welded OBJ written to VFS | +| point | SUPPORTED | type=direct. Import: float array -> `PointData` | +| element | SUPPORTED | type=direct. Import: int array -> `ElementData` | +| `` | SUPPORTED | All attrs (contype/conaffinity/condim/priority/margin/gap/selfcollide/internal) via `bOverride_X` | +| `` | SUPPORTED | stiffness/damping via `bOverride_X` | +| `` | SUPPORTED | young/poisson/damping/thickness/elastic2d via `bOverride_X` | +| `` | SUPPORTED | id/gridrange attribute arrays | +| euler | SUPPORTED | Handled by UE component transform (set by parent body transform import) | +| pingrid | MISSING | Only `id` and `gridrange` pin selectors supported | +| pinrange | MISSING | Only `id` and `gridrange` pin selectors supported | + +### Flex element (compiled output) + +The compiled `` element is read at runtime for visualization (vertex positions, element indices). `UMjFlexcomp::Bind` resolves the flex by name from `mjModel.flex_*` arrays and caches `flex_vertadr`, `flex_vertnum`, and the triangle index list for per-tick updates via `mjData.flexvert_xpos`.