diff --git a/CLAUDE.md b/CLAUDE.md
index 8d5ae99..de8b08d 100644
--- a/CLAUDE.md
+++ b/CLAUDE.md
@@ -26,6 +26,19 @@ is just an at-a-glance cache.
See memory `sw2gz-build-deploy` (MSBuild path, `SolutionDir` param, regasm +
SolidWorks-lock gotchas).
+## SolidWorks API reference
+
+[`docs/reference/solidworks-api.md`](docs/reference/solidworks-api.md) —
+every SolidWorks COM API member this codebase calls, grouped by category
+(app/doc lifecycle, ribbon, PropertyManagerPage UI, assembly/component,
+mates, geometry/tessellation, mass properties, coordinate systems,
+selection, events, color, persistence), each with file provenance,
+usage context, and an **[active]**/**[legacy]** flag (legacy = inherited-
+upstream export pipeline, may not run on the current gutted robot-mode
+path). Read this before adding any new SW COM call — check whether the
+member is already used elsewhere first. Local offline API browser:
+`C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS\api\apihelp.chm`.
+
## Conventions
- **PowerShell + git.** git writes normal status (e.g. `Switched to branch`)
diff --git a/SW2GZ/Build/InertialAggregator.cs b/SW2GZ/Build/InertialAggregator.cs
index 75a3e5c..a3b45dd 100644
--- a/SW2GZ/Build/InertialAggregator.cs
+++ b/SW2GZ/Build/InertialAggregator.cs
@@ -11,7 +11,7 @@ public static class InertialAggregator
// Steps:
// 1) total mass
// 2) mass-weighted COM, with each part's local COM rotated into the assembly frame
- // via R_f = Matrix3.FromQuaternion(frame.Rotation) before translation
+ // via R_f before translation
// 3) for each part, transform its inertia tensor from part frame to assembly frame
// as I_a = R_f · I_part · R_fᵀ, then translate to the combined COM via the
// parallel-axis theorem, and sum.
@@ -23,19 +23,43 @@ public static class InertialAggregator
// link-local frame — use the (parts, linkAnchor) overload below for that.
public static MassProps Combine(IReadOnlyList<(MassProps Props, Pose Frame)> parts)
{
- if (parts == null) return new MassProps(0, Vector3.Zero, Matrix3.Identity);
- if (parts.Count == 0)
+ if (parts == null)
+ return new MassProps(0, Vector3.Zero, Matrix3.Identity);
+ var matrixParts = parts
+ .Select(p => (p.Props, Matrix3.FromQuaternion(p.Frame.Rotation), p.Frame.Position))
+ .ToList();
+ return CombineCore(matrixParts);
+ }
+
+ // Matrix3-parameterized twin of the overload above. Same algorithm,
+ // same result for an equivalent rotation — exists so callers that
+ // already work entirely in Matrix3/Vector3 (e.g. Sw2gzRobotExporter,
+ // which reads SolidWorks component poses as Matrix3 directly) never
+ // need to construct a Quaternion just to call in here. Deliberately
+ // NOT implemented by converting to Quaternion and delegating to the
+ // overload above — a Matrix3-to-Quaternion conversion is new
+ // coordinate-conversion code, exactly the category that has already
+ // produced two real bugs in this codebase (the Transform2.ArrayData
+ // column-major bug, the mate-classification bug). Both overloads
+ // instead share CombineCore; only the Quaternion overload ever
+ // converts (Quaternion -> Matrix3, an already-proven, already-used
+ // direction), never the reverse.
+ public static MassProps Combine(IReadOnlyList<(MassProps Props, Matrix3 Rotation, Vector3 Position)> parts)
+ {
+ return CombineCore(parts);
+ }
+
+ private static MassProps CombineCore(IReadOnlyList<(MassProps Props, Matrix3 R, Vector3 Position)> parts)
+ {
+ if (parts == null || parts.Count == 0)
return new MassProps(0, Vector3.Zero, Matrix3.Identity);
double totalMass = parts.Sum(p => p.Props.Mass);
if (totalMass <= 0)
return new MassProps(0, Vector3.Zero, Matrix3.Identity);
- // Cache R_f per part: FromQuaternion + the rotated COM offset are
- // needed in both the COM pass and the parallel-axis pass.
- var rotations = new Matrix3[parts.Count];
// Per-part rotated COM offset in assembly frame (double precision),
- // i.e. (f.Position + R_f * p.ComLocal). Reused for the parallel-axis d.
+ // i.e. (pos + R * p.ComLocal). Reused for the parallel-axis d.
var partComsX = new double[parts.Count];
var partComsY = new double[parts.Count];
var partComsZ = new double[parts.Count];
@@ -43,13 +67,11 @@ public static MassProps Combine(IReadOnlyList<(MassProps Props, Pose Frame)> par
double comX = 0.0, comY = 0.0, comZ = 0.0;
for (int i = 0; i < parts.Count; i++)
{
- var (p, f) = parts[i];
- var R_f = Matrix3.FromQuaternion(f.Rotation);
- rotations[i] = R_f;
+ var (p, R_f, pos) = parts[i];
var (rx, ry, rz) = R_f.Mul((double)p.ComLocal.X, p.ComLocal.Y, p.ComLocal.Z);
- double pcx = f.Position.X + rx;
- double pcy = f.Position.Y + ry;
- double pcz = f.Position.Z + rz;
+ double pcx = pos.X + rx;
+ double pcy = pos.Y + ry;
+ double pcz = pos.Z + rz;
partComsX[i] = pcx; partComsY[i] = pcy; partComsZ[i] = pcz;
double w = p.Mass / totalMass;
comX += w * pcx; comY += w * pcy; comZ += w * pcz;
@@ -60,11 +82,9 @@ public static MassProps Combine(IReadOnlyList<(MassProps Props, Pose Frame)> par
var I = new double[3, 3];
for (int i = 0; i < parts.Count; i++)
{
- var (p, _) = parts[i];
- var R_f = rotations[i];
+ var (p, R_f, _) = parts[i];
var I_rot = R_f * p.InertiaAtComLocal * R_f.Transpose();
- // Offset from assembly COM to this part's COM, in double precision.
double dx = partComsX[i] - comX;
double dy = partComsY[i] - comY;
double dz = partComsZ[i] - comZ;
@@ -108,20 +128,35 @@ public static MassProps Combine(
if (linkAnchor == null || linkAnchor == Pose.Identity) return assemblyFrame;
if (assemblyFrame.Mass <= 0) return assemblyFrame;
- // R = link anchor rotation (assembly → link is Rᵀ; world rotates by R
- // to land in link orientation only if we are going link → world).
- // We need a vector expressed in the LINK frame, so we apply Rᵀ.
Matrix3 R = Matrix3.FromQuaternion(linkAnchor.Rotation);
+ return RebaseCore(assemblyFrame, R, linkAnchor.Position);
+ }
+
+ // Matrix3-parameterized twin of the rebase overload above — see the
+ // Combine(parts) overload for why this exists instead of routing
+ // through Quaternion.
+ public static MassProps Combine(
+ IReadOnlyList<(MassProps Props, Matrix3 Rotation, Vector3 Position)> parts,
+ Matrix3 anchorR, Vector3 anchorT)
+ {
+ MassProps assemblyFrame = Combine(parts);
+ if (assemblyFrame.Mass <= 0) return assemblyFrame;
+ return RebaseCore(assemblyFrame, anchorR, anchorT);
+ }
+
+ private static MassProps RebaseCore(MassProps assemblyFrame, Matrix3 R, Vector3 anchorPosition)
+ {
+ // R = link anchor rotation. We need a vector expressed in the
+ // LINK frame, so we apply Rᵀ (R^-1 for an orthonormal rotation).
Matrix3 Rinv = R.Transpose();
- // COM offset in assembly coords, then rotate into link frame.
- double dx = assemblyFrame.ComLocal.X - linkAnchor.Position.X;
- double dy = assemblyFrame.ComLocal.Y - linkAnchor.Position.Y;
- double dz = assemblyFrame.ComLocal.Z - linkAnchor.Position.Z;
+ double dx = assemblyFrame.ComLocal.X - anchorPosition.X;
+ double dy = assemblyFrame.ComLocal.Y - anchorPosition.Y;
+ double dz = assemblyFrame.ComLocal.Z - anchorPosition.Z;
var (lx, ly, lz) = Rinv.Mul(dx, dy, dz);
var comLink = new Vector3((float)lx, (float)ly, (float)lz);
- // I_link = R^-1 · I_assembly · R (same tensor at the same point,
+ // I_link = R^-1 · I_assembly · R (same tensor at the same point,
// re-expressed in the rotated basis).
Matrix3 Ilink = Rinv * assemblyFrame.InertiaAtComLocal * R;
diff --git a/SW2GZ/SW/SwAddin.cs b/SW2GZ/SW/SwAddin.cs
index 01927df..6acd3ee 100644
--- a/SW2GZ/SW/SwAddin.cs
+++ b/SW2GZ/SW/SwAddin.cs
@@ -425,7 +425,7 @@ private bool TryGetActiveAssembly(out ModelDoc2 modeldoc)
// Sw2gzCreateWorldPmp / Sw2gzCreateAssetPmp). Held as fields — same
// COM-handler-rooting reason as _openPanel (the SW PMP handler
// interface is freed on AfterClose).
- // Robot mode v2 — robot Create wizard removed; OpenCreateRobot is a stub.
+ private SW2GZ.UI.Pmp.Sw2gzCreateRobotPmp _createRobotPmp;
private SW2GZ.UI.Pmp.Sw2gzCreateWorldPmp _createWorldPmp;
private SW2GZ.UI.Pmp.Sw2gzCreateAssetPmp _createAssetPmp;
// True while a Create wizard PMP is open. The mode pills read this and
@@ -523,11 +523,20 @@ public void OpenCreatePmp()
private void OpenCreateRobot(ModelDoc2 modelDoc, SW2GZ.URDFExport.Sw2gzDoc doc)
{
- // Robot mode v2 — the inherited robot implementation was removed for a
- // clean rebuild. The mode pill + Create/Edit Robot button stay; this
- // is a deliberate no-op stub. World and Asset modes are unaffected.
- MessageBox.Show("Robot mode is not implemented yet — coming in a future build.",
- "SW2GZ");
+ try
+ {
+ _wizardOpen = true;
+ _createRobotPmp = new SW2GZ.UI.Pmp.Sw2gzCreateRobotPmp(
+ (SldWorks)SwApp, modelDoc, doc,
+ d => PersistDoc(modelDoc, d), OnWizardClosed);
+ _createRobotPmp.Show();
+ }
+ catch (Exception e)
+ {
+ _wizardOpen = false;
+ logger.Error("OpenCreateRobot failed", e);
+ MessageBox.Show("Could not open Create Robot: " + e.Message);
+ }
}
private void OpenCreateWorld(ModelDoc2 modelDoc, SW2GZ.URDFExport.Sw2gzDoc doc)
diff --git a/SW2GZ/SW2GZ.csproj b/SW2GZ/SW2GZ.csproj
index 6da0ba4..9e02a40 100644
--- a/SW2GZ/SW2GZ.csproj
+++ b/SW2GZ/SW2GZ.csproj
@@ -503,10 +503,13 @@
+
+
+
@@ -544,6 +547,7 @@
+
diff --git a/SW2GZ/SwSurface/Abstractions/IComponentPoses.cs b/SW2GZ/SwSurface/Abstractions/IComponentPoses.cs
new file mode 100644
index 0000000..4af6919
--- /dev/null
+++ b/SW2GZ/SwSurface/Abstractions/IComponentPoses.cs
@@ -0,0 +1,20 @@
+/*
+Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md.
+
+Raw SolidWorks Component2.Transform2 pose (assembly-frame rotation +
+translation) for a given component, read via the verified COLUMN-major
+convention (see memory sw-mathtransform-column-major — confirmed against
+Component2.GetBox ground truth). Lets exporters compute the exact relative
+parent/child joint transform instead of approximating one from mesh bounding
+boxes.
+*/
+using System.Numerics;
+using SW2GZ.Math;
+
+namespace SW2GZ.SwSurface.Abstractions
+{
+ public interface IComponentPoses
+ {
+ (Matrix3 Rotation, Vector3 Translation) GetPose(string componentPathName);
+ }
+}
diff --git a/SW2GZ/SwSurface/SolidWorksAssemblyWalker.cs b/SW2GZ/SwSurface/SolidWorksAssemblyWalker.cs
index 7d13ff6..e85b93f 100644
--- a/SW2GZ/SwSurface/SolidWorksAssemblyWalker.cs
+++ b/SW2GZ/SwSurface/SolidWorksAssemblyWalker.cs
@@ -583,14 +583,18 @@ private static bool BothEntitiesPlanarFaces(Mate2 mate)
// PART-LOCAL point, returning the corresponding ASSEMBLY-frame point.
// Different from RotateByComponent above (which is for direction
// vectors and ignores translation).
+ // ArrayData's 3x3 rotation block is COLUMN-major (verified against
+ // Component2.GetBox ground truth — see memory
+ // sw-mathtransform-column-major); the naive row-major read silently
+ // inverts rotation for any non-identity component orientation.
private static Vector3 TransformByComponent(Component2 comp, Vector3 v)
{
MathTransform xform = comp.Transform2;
if (xform == null) return v;
if (!(xform.ArrayData is double[] d) || d.Length < 12) return v;
- float x = (float)(d[0] * v.X + d[1] * v.Y + d[2] * v.Z + d[9]);
- float y = (float)(d[3] * v.X + d[4] * v.Y + d[5] * v.Z + d[10]);
- float z = (float)(d[6] * v.X + d[7] * v.Y + d[8] * v.Z + d[11]);
+ float x = (float)(d[0] * v.X + d[3] * v.Y + d[6] * v.Z + d[9]);
+ float y = (float)(d[1] * v.X + d[4] * v.Y + d[7] * v.Z + d[10]);
+ float z = (float)(d[2] * v.X + d[5] * v.Y + d[8] * v.Z + d[11]);
return new Vector3(x, y, z);
}
@@ -601,9 +605,9 @@ private static Vector3 RotateByComponent(Component2 comp, Vector3 v)
MathTransform xform = comp.Transform2;
if (xform == null) return v;
if (!(xform.ArrayData is double[] d) || d.Length < 9) return v;
- float x = (float)(d[0] * v.X + d[1] * v.Y + d[2] * v.Z);
- float y = (float)(d[3] * v.X + d[4] * v.Y + d[5] * v.Z);
- float z = (float)(d[6] * v.X + d[7] * v.Y + d[8] * v.Z);
+ float x = (float)(d[0] * v.X + d[3] * v.Y + d[6] * v.Z);
+ float y = (float)(d[1] * v.X + d[4] * v.Y + d[7] * v.Z);
+ float z = (float)(d[2] * v.X + d[5] * v.Y + d[8] * v.Z);
return new Vector3(x, y, z);
}
diff --git a/SW2GZ/SwSurface/SolidWorksComponentPoses.cs b/SW2GZ/SwSurface/SolidWorksComponentPoses.cs
new file mode 100644
index 0000000..4c1346a
--- /dev/null
+++ b/SW2GZ/SwSurface/SolidWorksComponentPoses.cs
@@ -0,0 +1,64 @@
+/*
+Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md.
+
+Reads a component's raw Component2.Transform2 pose (assembly-frame rotation +
+translation), COLUMN-major (see memory sw-mathtransform-column-major).
+
+SW_INTEROP is defined when building SW2GZ.csproj (COM references); NOT
+defined when building the xunit test project, so the same source compiles
+in both (mirrors SolidWorksMassProperties / SolidWorksMeshTessellator).
+*/
+using System;
+using System.Numerics;
+using SW2GZ.Math;
+using SW2GZ.SwSurface.Abstractions;
+
+#if SW_INTEROP
+using SolidWorks.Interop.sldworks;
+#endif
+
+namespace SW2GZ.SwSurface
+{
+ public sealed class SolidWorksComponentPoses : IComponentPoses
+ {
+#if SW_INTEROP
+ private readonly AssemblyDoc _doc;
+#endif
+
+ // Skeleton ctor — preserves the NotImplementedException-when-unwired
+ // convention shared by SolidWorksMeshTessellator / SolidWorksMassProperties.
+ public SolidWorksComponentPoses() { }
+
+#if SW_INTEROP
+ public SolidWorksComponentPoses(AssemblyDoc doc) { _doc = doc; }
+#endif
+
+ public (Matrix3 Rotation, Vector3 Translation) GetPose(string componentPathName)
+ {
+#if SW_INTEROP
+ if (_doc == null)
+#endif
+ throw new NotImplementedException(
+ "SolidWorksComponentPoses.GetPose() requires an assembly doc — pass it via constructor.");
+
+#if SW_INTEROP
+ Component2 comp = SolidWorksMassProperties.FindComponent(
+ (object[])_doc.GetComponents(false), componentPathName);
+ if (comp == null)
+ throw new SW2GZ.Exceptions.Sw2gzExportException(
+ "Component path not found in active assembly: " + componentPathName);
+
+ MathTransform xform = comp.Transform2;
+ double[] d = xform?.ArrayData as double[];
+ if (d == null || d.Length < 12) return (Matrix3.Identity, Vector3.Zero);
+
+ var r = new Matrix3(
+ d[0], d[3], d[6],
+ d[1], d[4], d[7],
+ d[2], d[5], d[8]);
+ var t = new Vector3((float)d[9], (float)d[10], (float)d[11]);
+ return (r, t);
+#endif
+ }
+ }
+}
diff --git a/SW2GZ/SwSurface/SolidWorksMeshTessellator.cs b/SW2GZ/SwSurface/SolidWorksMeshTessellator.cs
index 9b5972d..00677d9 100644
--- a/SW2GZ/SwSurface/SolidWorksMeshTessellator.cs
+++ b/SW2GZ/SwSurface/SolidWorksMeshTessellator.cs
@@ -104,8 +104,12 @@ public MeshData Tessellate(string componentPathName, TessellationLod lod)
if (bodyObjs == null || bodyObjs.Length == 0) continue;
// Per-leaf assembly-frame transform. ArrayData layout:
- // [0..8] rotation 3x3 row-major, [9..11] translation,
- // [12] scale, [13..15] padding.
+ // [0..8] rotation 3x3 COLUMN-major, [9..11] translation,
+ // [12] scale, [13..15] padding. Verified against
+ // Component2.GetBox ground truth (see memory
+ // sw-mathtransform-column-major) — the naive row-major
+ // read silently inverts rotation for any non-identity
+ // component orientation.
MathTransform xform = leaf.Transform2;
double[] d = xform?.ArrayData as double[];
bool hasXf = d != null && d.Length >= 12;
@@ -147,9 +151,9 @@ public MeshData Tessellate(string componentPathName, TessellationLod lod)
// (GetVertexPoint returns part-local coords).
if (hasXf)
{
- double rx = d[0] * x + d[1] * y + d[2] * z;
- double ry = d[3] * x + d[4] * y + d[5] * z;
- double rz = d[6] * x + d[7] * y + d[8] * z;
+ double rx = d[0] * x + d[3] * y + d[6] * z;
+ double ry = d[1] * x + d[4] * y + d[7] * z;
+ double rz = d[2] * x + d[5] * y + d[8] * z;
x = rx * sc + d[9]; y = ry * sc + d[10]; z = rz * sc + d[11];
}
verts.Add(new System.Numerics.Vector3((float)x, (float)y, (float)z));
diff --git a/SW2GZ/UI/LinkTreeView.cs b/SW2GZ/UI/LinkTreeView.cs
index 89a7ab1..9638159 100644
--- a/SW2GZ/UI/LinkTreeView.cs
+++ b/SW2GZ/UI/LinkTreeView.cs
@@ -101,11 +101,23 @@ private TreeNode BuildNode(LinkDef link)
+ " [" + n + (n == 1 ? " part]" : " parts]");
var node = new TreeNode(label) { Tag = link };
if (n == 0) node.ForeColor = System.Drawing.Color.Firebrick; // unassigned = needs attention
+ node.ToolTipText = DescribePrimary(link);
foreach (LinkDef child in LinkHierarchy.ChildrenOf(links, link.Name))
node.Nodes.Add(BuildNode(child));
return node;
}
+ // The first ComponentIds entry defines this link's whole frame
+ // (mesh anchor, joint origin, inertial rebase) — surfaced on hover
+ // since the compact node label has no room for it.
+ private static string DescribePrimary(LinkDef link)
+ {
+ List ids = link.ComponentIds;
+ if (ids == null || ids.Count == 0) return "no mesh assigned";
+ if (ids.Count == 1) return ids[0];
+ return "primary: " + ids[0] + " | also: " + string.Join(", ", ids.GetRange(1, ids.Count - 1));
+ }
+
public void SelectByLinkName(string name)
{
foreach (TreeNode n in AllNodes(Nodes))
@@ -124,6 +136,7 @@ public void RefreshActiveNodeLabel()
n.Text = (link.Name ?? "")
+ (isRoot ? " (base)" : "")
+ " [" + parts + (parts == 1 ? " part]" : " parts]");
+ n.ToolTipText = DescribePrimary(link);
}
private static IEnumerable AllNodes(TreeNodeCollection nodes)
diff --git a/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs
new file mode 100644
index 0000000..38bb3cf
--- /dev/null
+++ b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs
@@ -0,0 +1,636 @@
+/*
+Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md.
+
+Sw2gzCreateRobotPmp — the "Create Robot" PropertyManagerPage opened from the
+mode-specific Create button when the active mode is Robot. Manual, URDF-
+hierarchy-shaped link building: no auto-seed, the user picks the mesh
+component(s) for each link (parts or sub-assemblies), names it, and picks
+its parent — the first link added is always the root, forced to
+"base_link" per ROS2/REP-105 convention. Every non-root link gets an
+implicit Fixed joint to its chosen parent (joint-type refinement is a later
+increment, see agent-progress/progress.md). Mirrors Sw2gzCreateWorldPmp's
+chrome exactly: a WinForms nav bar (Back/Next + step indicator, dark theme)
+embedded via WindowFromHandle, and a WinForms action-button bar per step.
+PMP swControlType_Button controls are avoided entirely — clicking one and
+mutating PMP state from inside OnButtonPress corrupts SW's PMP renderer.
+Nav clicks defer via BeginInvoke so the group-visibility flip runs off the
+click-handler reentrancy frame.
+
+Steps map to Sw2gzDoc.Robot:
+ 0 — Links (pick mesh -> name -> parent -> Add; first Add = base_link;
+ Joints rebuilt (Fixed) from each link's ParentName)
+ 1 — Review (counts; Next caption flips to "Finish")
+*/
+#if SW_INTEROP
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Runtime.InteropServices;
+using SolidWorks.Interop.sldworks;
+using SolidWorks.Interop.swconst;
+using SolidWorks.Interop.swpublished;
+using SW2GZ.Build;
+using SW2GZ.Build.Model;
+using SW2GZ.Build.Urdf;
+using SW2GZ.SwSurface;
+using SW2GZ.UI;
+using SW2GZ.URDFExport;
+using SW2GZ.Utilities;
+
+namespace SW2GZ.UI.Pmp
+{
+ [ComVisible(true)]
+ public sealed class Sw2gzCreateRobotPmp : PropertyManagerPage2Handler9
+ {
+ private static readonly log4net.ILog logger = Logger.GetLogger();
+
+ private readonly SldWorks _swApp;
+ private readonly ModelDoc2 _modelDoc;
+ private readonly Sw2gzDoc _liveDoc;
+ private readonly Sw2gzDoc _snapshot;
+ private readonly Action _onCommit;
+ private readonly Action _onClosed;
+ private readonly PropertyManagerPage2 _page;
+
+ private const int StepLinks = 0;
+ private const int StepReview = 1;
+ private static readonly string[] StepNames = { "Links", "Review" };
+ private const int StepCount = 2;
+
+ private bool _okay;
+ private int _currentStep = StepLinks;
+
+ // Header (Progress) group + nav bar.
+ private const int IdHeaderGroup = 1;
+ private const int IdHeaderLabel = 2;
+ private const int IdNavBar = 3;
+
+ // Step groups.
+ private const int IdLinksGroup = 10;
+ private const int IdMeshLabel = 11;
+ private const int IdMeshPicker = 12;
+ private const int IdLinkNameLabel = 13;
+ private const int IdLinkNameBox = 14;
+ private const int IdLinksBtnBar = 15;
+ private const int IdSelectedInfo = 16;
+ private const int IdLinksListLabel = 17;
+ private const int IdLinksTree = 18;
+
+ private const int MeshSelectionMark = 0x4C0;
+ private const string LinkNamePlaceholder = "e.g. wheel_link";
+
+ private const int IdReviewGroup = 20;
+ private const int IdReviewDescr = 21;
+ private const int IdReviewLinksLabel = 22;
+ private const int IdReviewBaseLabel = 23;
+ private const int IdReviewJointsLabel = 24;
+
+ private PropertyManagerPageGroup[] _stepGroups;
+
+ // WinForms nav bar (Back/Next + step indicator).
+ private PropertyManagerPageWindowFromHandle _navHandle;
+ private System.Windows.Forms.Panel _navBar;
+ private System.Windows.Forms.Button _backBtn;
+ private System.Windows.Forms.Button _nextBtn;
+ private System.Windows.Forms.Label _stepIndicator;
+
+ // WinForms per-step action bar (Links step).
+ private PropertyManagerPageWindowFromHandle _linksBarHandle;
+ private System.Windows.Forms.Panel _linksBar;
+ private System.Windows.Forms.Button _addLinkBtn;
+ private System.Windows.Forms.Button _removeLinkBtn;
+ private System.Windows.Forms.Button _clearLinksBtn;
+
+ // PMP-native controls.
+ private PropertyManagerPageSelectionbox _meshPicker;
+ private PropertyManagerPageTextbox _linkNameBox;
+ private PropertyManagerPageLabel _selectedInfoLabel;
+ private PropertyManagerPageLabel _reviewLinksLabel;
+ private PropertyManagerPageLabel _reviewBaseLabel;
+ private PropertyManagerPageLabel _reviewJointsLabel;
+
+ // WinForms tree (Links step) — drag-to-reparent hierarchy, embedded
+ // via WindowFromHandle like the nav/action bars. Reuses the pure
+ // SW2GZ.Build.LinkHierarchy helpers (already unit-tested) for
+ // roots/children/cycle-detection; operates directly on the live
+ // Robot.Links list.
+ private PropertyManagerPageWindowFromHandle _treeHandle;
+ private LinkTreeView _linkTree;
+
+ public Sw2gzCreateRobotPmp(SldWorks swApp, ModelDoc2 modelDoc, Sw2gzDoc liveDoc, Action onCommit, Action onClosed = null)
+ {
+ _swApp = swApp ?? throw new ArgumentNullException(nameof(swApp));
+ _modelDoc = modelDoc ?? throw new ArgumentNullException(nameof(modelDoc));
+ _liveDoc = liveDoc ?? throw new ArgumentNullException(nameof(liveDoc));
+ _onCommit = onCommit ?? (d => { });
+ _onClosed = onClosed;
+
+ _snapshot = Sw2gzDocSnapshot.Clone(liveDoc);
+
+ int errs = 0;
+ int opts = (int)swPropertyManagerPageOptions_e.swPropertyManagerOptions_OkayButton |
+ (int)swPropertyManagerPageOptions_e.swPropertyManagerOptions_CancelButton;
+ _page = (PropertyManagerPage2)swApp.CreatePropertyManagerPage(
+ "Create Robot", opts, this, ref errs);
+
+ if (_page == null)
+ {
+ logger.Error("Sw2gzCreateRobotPmp: CreatePropertyManagerPage failed (err=" + errs + ")");
+ return;
+ }
+
+ BuildPage();
+ ShowStep(StepLinks);
+ }
+
+ // ── Dark-theme palette (mirrors Sw2gzCreateWorldPmp) ──────────────────
+ private static readonly System.Drawing.Color DarkBarBg = System.Drawing.Color.FromArgb(53, 53, 53);
+ private static readonly System.Drawing.Color DarkBtnBg = System.Drawing.Color.FromArgb(70, 70, 72);
+ private static readonly System.Drawing.Color DarkBtnHover = System.Drawing.Color.FromArgb(95, 95, 98);
+ private static readonly System.Drawing.Color DarkFg = System.Drawing.Color.FromArgb(220, 220, 220);
+ private static readonly System.Drawing.Color DarkBtnBorder = System.Drawing.Color.FromArgb(100, 100, 102);
+
+ private static System.Windows.Forms.Panel NewBar(int width, int height) =>
+ new System.Windows.Forms.Panel { Width = width, Height = height, BackColor = DarkBarBg };
+
+ private static System.Windows.Forms.Button NewBarButton(string text, int width)
+ {
+ var b = new System.Windows.Forms.Button
+ {
+ Text = text, Width = width, Height = 26, Top = 3,
+ FlatStyle = System.Windows.Forms.FlatStyle.Flat,
+ BackColor = DarkBtnBg, ForeColor = DarkFg, UseVisualStyleBackColor = false,
+ };
+ b.FlatAppearance.BorderColor = DarkBtnBorder;
+ b.FlatAppearance.MouseOverBackColor = DarkBtnHover;
+ return b;
+ }
+
+ private static void CenterRow(System.Windows.Forms.Panel bar, params System.Windows.Forms.Button[] btns)
+ {
+ const int gap = 8;
+ int total = -gap;
+ foreach (var b in btns) total += b.Width + gap;
+ int x = System.Math.Max(0, (bar.Width - total) / 2);
+ foreach (var b in btns) { b.Left = x; x += b.Width + gap; }
+ }
+
+ private void BuildPage()
+ {
+ int leftEdge = (int)swPropertyManagerPageControlLeftAlign_e.swControlAlign_LeftEdge;
+ int visibleEnabled = (int)swAddControlOptions_e.swControlOptions_Enabled |
+ (int)swAddControlOptions_e.swControlOptions_Visible;
+ int grpOptions = (int)swAddGroupBoxOptions_e.swGroupBoxOptions_Visible +
+ (int)swAddGroupBoxOptions_e.swGroupBoxOptions_Expanded;
+
+ var header = (PropertyManagerPageGroup)_page.AddGroupBox(IdHeaderGroup, "Progress", grpOptions);
+ header.AddControl2(IdHeaderLabel,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "", (short)leftEdge, visibleEnabled, "");
+ BuildNavBar();
+ _navHandle = (PropertyManagerPageWindowFromHandle)header.AddControl2(
+ IdNavBar,
+ (short)swPropertyManagerPageControlType_e.swControlType_WindowFromHandle,
+ "", (short)leftEdge, visibleEnabled, "Step navigation");
+ _navHandle.Height = 58;
+ _navHandle.SetWindowHandlex64(_navBar.Handle.ToInt64());
+
+ _stepGroups = new PropertyManagerPageGroup[StepCount];
+ _stepGroups[StepLinks] = BuildLinksGroup(grpOptions, leftEdge, visibleEnabled);
+ _stepGroups[StepReview] = BuildReviewGroup(grpOptions, leftEdge, visibleEnabled);
+ }
+
+ private void BuildNavBar()
+ {
+ _navBar = NewBar(260, 56);
+ _stepIndicator = new System.Windows.Forms.Label
+ {
+ AutoSize = false, Width = 240, Height = 18, Top = 2, Left = 10,
+ TextAlign = System.Drawing.ContentAlignment.MiddleCenter,
+ ForeColor = System.Drawing.Color.White,
+ BackColor = System.Drawing.Color.FromArgb(48, 48, 48),
+ Font = new System.Drawing.Font("Segoe UI", 8.25f, System.Drawing.FontStyle.Bold),
+ Text = "",
+ };
+ _navBar.Controls.Add(_stepIndicator);
+ _backBtn = NewBarButton("◀", 50);
+ _nextBtn = NewBarButton("▶", 50);
+ _backBtn.Top = 24;
+ _nextBtn.Top = 24;
+ _backBtn.Click += (s, e) => _navBar.BeginInvoke((Action)(() =>
+ {
+ try { GoBack(); } catch (Exception ex) { logger.Error("GoBack threw", ex); }
+ }));
+ _nextBtn.Click += (s, e) => _navBar.BeginInvoke((Action)(() =>
+ {
+ try { GoNext(); } catch (Exception ex) { logger.Error("GoNext threw", ex); }
+ }));
+ _navBar.Controls.Add(_backBtn);
+ _navBar.Controls.Add(_nextBtn);
+ _navBar.Resize += (s, e) => CenterRow(_navBar, _backBtn, _nextBtn);
+ CenterRow(_navBar, _backBtn, _nextBtn);
+ }
+
+ private PropertyManagerPageGroup BuildLinksGroup(int grpOptions, int leftEdge, int visibleEnabled)
+ {
+ var grp = (PropertyManagerPageGroup)_page.AddGroupBox(IdLinksGroup, "Links", grpOptions);
+
+ AddFieldLabel(grp, IdMeshLabel, "Mesh", leftEdge, visibleEnabled);
+ _meshPicker = (PropertyManagerPageSelectionbox)grp.AddControl2(
+ IdMeshPicker,
+ (short)swPropertyManagerPageControlType_e.swControlType_Selectionbox,
+ "", (short)leftEdge, visibleEnabled,
+ "Pick one or more components in the viewport — parts or sub-assemblies");
+ _meshPicker.SingleEntityOnly = false;
+ _meshPicker.AllowMultipleSelectOfSameEntity = false;
+ _meshPicker.Height = 30;
+ _meshPicker.Mark = MeshSelectionMark;
+ _meshPicker.SetSelectionFilters((object)new swSelectType_e[] { swSelectType_e.swSelCOMPONENTS });
+
+ AddFieldLabel(grp, IdLinkNameLabel, "Link name", leftEdge, visibleEnabled);
+ _linkNameBox = (PropertyManagerPageTextbox)grp.AddControl2(
+ IdLinkNameBox,
+ (short)swPropertyManagerPageControlType_e.swControlType_Textbox,
+ "", (short)leftEdge, visibleEnabled,
+ "Auto-fills from a single part pick, editable. Ignored for the first/base link.");
+ SetLinkNamePlaceholder();
+
+ _linksBar = NewBar(260, 32);
+ _addLinkBtn = NewBarButton("Add link", 80);
+ _removeLinkBtn = NewBarButton("Remove", 70);
+ _clearLinksBtn = NewBarButton("Clear all", 80);
+ _addLinkBtn.Click += (s, e) => HandleAddLink();
+ _removeLinkBtn.Click += (s, e) => HandleRemoveLink();
+ _clearLinksBtn.Click += (s, e) => HandleClearLinks();
+ _linksBar.Controls.Add(_addLinkBtn);
+ _linksBar.Controls.Add(_removeLinkBtn);
+ _linksBar.Controls.Add(_clearLinksBtn);
+ _linksBar.Resize += (s, e) => CenterRow(_linksBar, _addLinkBtn, _removeLinkBtn, _clearLinksBtn);
+ CenterRow(_linksBar, _addLinkBtn, _removeLinkBtn, _clearLinksBtn);
+ _linksBarHandle = (PropertyManagerPageWindowFromHandle)grp.AddControl2(
+ IdLinksBtnBar,
+ (short)swPropertyManagerPageControlType_e.swControlType_WindowFromHandle,
+ "", (short)leftEdge, visibleEnabled, "Add, remove, or clear links");
+ _linksBarHandle.Height = 34;
+ _linksBarHandle.SetWindowHandlex64(_linksBar.Handle.ToInt64());
+
+ _selectedInfoLabel = (PropertyManagerPageLabel)grp.AddControl2(
+ IdSelectedInfo,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "Selected: (none)", (short)leftEdge, visibleEnabled, "");
+
+ AddFieldLabel(grp, IdLinksListLabel, "Hierarchy — drag to reparent, click to select",
+ leftEdge, visibleEnabled);
+ _linkTree = new LinkTreeView { Width = 260, Height = 220 };
+ _linkTree.ActiveLinkChanged += (s, link) => { RefreshSelectedInfo(link); HighlightLinkMesh(link); };
+ _linkTree.LinksChanged += (s, e) => RebuildJoints();
+ _linkTree.SetLinks(_liveDoc.Robot.Links);
+ _treeHandle = (PropertyManagerPageWindowFromHandle)grp.AddControl2(
+ IdLinksTree,
+ (short)swPropertyManagerPageControlType_e.swControlType_WindowFromHandle,
+ "", (short)leftEdge, visibleEnabled, "Drag a link onto another to re-parent it");
+ _treeHandle.Height = 220;
+ _treeHandle.SetWindowHandlex64(_linkTree.Handle.ToInt64());
+
+ return grp;
+ }
+
+ private static void AddFieldLabel(PropertyManagerPageGroup grp, int id, string text, int leftEdge, int visibleEnabled)
+ {
+ grp.AddControl2(id, (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ text, (short)leftEdge, visibleEnabled, "");
+ }
+
+ private PropertyManagerPageGroup BuildReviewGroup(int grpOptions, int leftEdge, int visibleEnabled)
+ {
+ var grp = (PropertyManagerPageGroup)_page.AddGroupBox(IdReviewGroup, "Review", grpOptions);
+ grp.AddControl2(IdReviewDescr,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "Review and Finish to commit. Cancel rolls back.",
+ (short)leftEdge, visibleEnabled, "");
+ _reviewLinksLabel = (PropertyManagerPageLabel)grp.AddControl2(
+ IdReviewLinksLabel,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "", (short)leftEdge, visibleEnabled, "");
+ _reviewBaseLabel = (PropertyManagerPageLabel)grp.AddControl2(
+ IdReviewBaseLabel,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "", (short)leftEdge, visibleEnabled, "");
+ _reviewJointsLabel = (PropertyManagerPageLabel)grp.AddControl2(
+ IdReviewJointsLabel,
+ (short)swPropertyManagerPageControlType_e.swControlType_Label,
+ "", (short)leftEdge, visibleEnabled, "");
+ return grp;
+ }
+
+ // ─── Action handlers ───────────────────────────────────────────────────
+ // Reads the mesh picker + name box and appends one link. The first
+ // link ever added is forced to root/base_link regardless of the name
+ // box — every URDF tree needs exactly one root, and REP-105 names it
+ // base_link, so there is nothing for the user to choose there. Every
+ // later link's parent is whichever node is selected in the hierarchy
+ // tree below (no separate parent picker — click to target; drag a
+ // node there afterward to move it under a different parent).
+ private void HandleAddLink()
+ {
+ try
+ {
+ ISelectionMgr selMgr = (ISelectionMgr)_modelDoc.SelectionManager;
+ if (selMgr == null) return;
+ int count = selMgr.GetSelectedObjectCount2(MeshSelectionMark);
+ if (count < 1) return;
+
+ var componentIds = new List();
+ for (int i = 1; i <= count; i++)
+ {
+ object selObj = selMgr.GetSelectedObject6(i, MeshSelectionMark);
+ if (selObj is Component2 c && !string.IsNullOrEmpty(c.Name2))
+ componentIds.Add(c.Name2);
+ }
+ if (componentIds.Count == 0) return;
+
+ bool isRoot = _liveDoc.Robot.Links.Count == 0;
+ string parentName = string.Empty;
+ string name;
+ if (isRoot)
+ {
+ name = "base_link";
+ }
+ else
+ {
+ LinkDef parent = _linkTree?.ActiveLink;
+ if (parent == null) return; // click a link in the tree to parent this one to it
+ parentName = parent.Name;
+ name = LinkNameBoxValue();
+ }
+ if (string.IsNullOrEmpty(name)) return;
+ if (_liveDoc.Robot.Links.Any(l => l.Name.Equals(name, StringComparison.OrdinalIgnoreCase))) return;
+
+ _liveDoc.Robot.Links.Add(new LinkDef { Name = name, ComponentIds = componentIds, ParentName = parentName });
+ RebuildJoints();
+ _linkTree.Rebuild();
+ _linkTree.SelectByLinkName(name); // chain the next Add onto what was just created
+ SetLinkNamePlaceholder();
+ _modelDoc.ClearSelection2(true);
+ }
+ catch (Exception e) { logger.Warn("HandleAddLink failed", e); }
+ }
+
+ private void HandleRemoveLink()
+ {
+ LinkDef link = _linkTree?.ActiveLink;
+ if (link == null) return;
+ if (_liveDoc.Robot.Links.Any(l => l.ParentName == link.Name))
+ {
+ _swApp.SendMsgToUser("Remove its child links first.");
+ return;
+ }
+ _liveDoc.Robot.Links.Remove(link);
+ RebuildJoints();
+ _linkTree.Rebuild();
+ }
+
+ private void HandleClearLinks()
+ {
+ _liveDoc.Robot.Links.Clear();
+ _liveDoc.Robot.Joints.Clear();
+ _linkTree.Rebuild();
+ RefreshSelectedInfo(null);
+ }
+
+ // Auto-fills the name box from a single-part mesh pick (sub-assembly
+ // or multi-component picks are ambiguous — leave it for the user to
+ // type). Root/base_link ignores the name box entirely, so skip while
+ // the tree is still empty.
+ private void AutoFillLinkName()
+ {
+ if (_linkNameBox == null || _liveDoc.Robot.Links.Count == 0) return;
+ ISelectionMgr selMgr = (ISelectionMgr)_modelDoc.SelectionManager;
+ if (selMgr == null) return;
+ if (selMgr.GetSelectedObjectCount2(MeshSelectionMark) != 1)
+ {
+ SetLinkNamePlaceholder();
+ return;
+ }
+ if (!(selMgr.GetSelectedObject6(1, MeshSelectionMark) is Component2 c)) return;
+ bool isSubAssembly = (c.GetModelDoc2() as ModelDoc2)?.GetType() == (int)swDocumentTypes_e.swDocASSEMBLY;
+ if (isSubAssembly) SetLinkNamePlaceholder();
+ else _linkNameBox.Text = RosNameSanitizer.Sanitize(c.Name2).Value;
+ }
+
+ private void SetLinkNamePlaceholder()
+ {
+ if (_linkNameBox != null) _linkNameBox.Text = LinkNamePlaceholder;
+ }
+
+ private string LinkNameBoxValue()
+ {
+ string t = (_linkNameBox?.Text ?? string.Empty).Trim();
+ return t == LinkNamePlaceholder ? string.Empty : t;
+ }
+
+ // Selects the clicked link's own mesh components in the SW model,
+ // tagged with the same MeshSelectionMark the Mesh box already
+ // listens to — so clicking a tree node highlights that link's
+ // geometry in the viewport AND populates the Mesh box, instead of
+ // the box staying empty regardless of tree selection. Reuses the
+ // legacy CommonSwOperations.SelectComponents helper (its own doc
+ // comment: "Helps highlight when the associated node is selected
+ // from the tree" — this exact use case, just never wired into the
+ // current wizard until now).
+ private void HighlightLinkMesh(LinkDef link)
+ {
+ try
+ {
+ if (link == null || link.ComponentIds == null || link.ComponentIds.Count == 0)
+ {
+ _modelDoc.ClearSelection2(true);
+ return;
+ }
+
+ object[] topLevel = (object[])((AssemblyDoc)_modelDoc).GetComponents(false);
+ var components = new List();
+ foreach (string compName in link.ComponentIds)
+ {
+ Component2 c = SolidWorksMassProperties.FindComponent(topLevel, compName);
+ if (c != null) components.Add(c);
+ }
+ CommonSwOperations.SelectComponents(_modelDoc, components, clearSelection: true, mark: MeshSelectionMark);
+ }
+ catch (Exception e) { logger.Warn("HighlightLinkMesh failed", e); }
+ }
+
+ private void RefreshSelectedInfo(LinkDef link)
+ {
+ if (_selectedInfoLabel == null) return;
+ _selectedInfoLabel.Caption = link == null
+ ? "Selected: (none)"
+ : "Selected: " + link.Name + " Mesh: " + DescribeMeshes(link.ComponentIds);
+ }
+
+ // The first component defines the link's own frame (mesh anchor,
+ // joint origin, inertial rebase — see
+ // docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md),
+ // so it's marked (primary) wherever the mesh list is shown —
+ // otherwise which one drives the frame is invisible.
+ private static string DescribeMeshes(List componentIds)
+ {
+ if (componentIds == null || componentIds.Count == 0) return "(none)";
+ var parts = new List(componentIds.Count);
+ for (int i = 0; i < componentIds.Count; i++)
+ parts.Add(i == 0 ? componentIds[i] + " (primary)" : componentIds[i]);
+ return string.Join(", ", parts);
+ }
+
+ // Joints are pure derived state: one Fixed joint per non-root link,
+ // straight off the ParentName the user picked at Add time. Re-run
+ // after every list mutation so Links/Joints never drift out of sync.
+ // Joint TYPE stays hardcoded Fixed in this cut (mate-driven detection
+ // was attempted and reverted — see memory `robot-mode-dev`).
+ private void RebuildJoints()
+ {
+ _liveDoc.Robot.Joints.Clear();
+ foreach (LinkDef link in _liveDoc.Robot.Links)
+ {
+ if (string.IsNullOrEmpty(link.ParentName)) continue;
+ _liveDoc.Robot.Joints.Add(new JointDef
+ {
+ Name = link.ParentName + "_to_" + link.Name,
+ ParentLink = link.ParentName,
+ ChildLink = link.Name,
+ Type = UrdfJointType.Fixed,
+ });
+ }
+ }
+
+ private void RefreshReviewLabels()
+ {
+ if (_reviewLinksLabel != null)
+ _reviewLinksLabel.Caption = "Links: " + _liveDoc.Robot.Links.Count;
+ if (_reviewBaseLabel != null)
+ _reviewBaseLabel.Caption = "Base link: " +
+ (_liveDoc.Robot.Links.Count > 0 ? _liveDoc.Robot.Links[0].Name : "(none)");
+ if (_reviewJointsLabel != null)
+ _reviewJointsLabel.Caption = "Joints (fixed): " + _liveDoc.Robot.Joints.Count;
+ }
+
+ // ─── Navigation ──────────────────────────────────────────────────────
+ private void ShowStep(int step)
+ {
+ if (step < 0) step = 0;
+ if (step > StepCount - 1) step = StepCount - 1;
+
+ _currentStep = step;
+ for (int i = 0; i < StepCount; i++)
+ {
+ try { _stepGroups[i].Visible = (i == _currentStep); }
+ catch (Exception ex) { logger.Error("Robot ShowStep group[" + i + "].Visible failed", ex); }
+ }
+
+ try { _stepIndicator.Text = "Step " + (_currentStep + 1) + " of " + StepCount + " — " + StepNames[_currentStep]; }
+ catch (Exception ex) { logger.Error("Robot ShowStep _stepIndicator.Text threw", ex); }
+ try { _backBtn.Enabled = _currentStep > 0; } catch (Exception ex) { logger.Error("Robot ShowStep back threw", ex); }
+ try
+ {
+ bool lastStep = _currentStep == StepCount - 1;
+ _nextBtn.Text = lastStep ? "Finish" : "▶";
+ _nextBtn.Width = lastStep ? 80 : 50;
+ CenterRow(_navBar, _backBtn, _nextBtn);
+ }
+ catch (Exception ex) { logger.Error("Robot ShowStep next threw", ex); }
+
+ if (_currentStep == StepReview) RefreshReviewLabels();
+ }
+
+ private void GoBack()
+ {
+ if (_currentStep > 0) ShowStep(_currentStep - 1);
+ }
+
+ private void GoNext()
+ {
+ if (_currentStep < StepCount - 1)
+ {
+ ShowStep(_currentStep + 1);
+ }
+ else
+ {
+ _okay = true;
+ _page.Close(true);
+ }
+ }
+
+ public void Show()
+ {
+ if (_page == null) { _swApp.SendMsgToUser("Could not open Create Robot."); return; }
+ _page.Show2(0);
+ }
+
+ void IPropertyManagerPage2Handler9.AfterActivation() { ShowStep(_currentStep); }
+
+ void IPropertyManagerPage2Handler9.OnClose(int Reason)
+ {
+ bool okay = Reason == (int)swPropertyManagerPageCloseReasons_e.swPropertyManagerPageClose_Okay;
+ _okay = _okay || okay;
+ if (!_okay)
+ {
+ Sw2gzDocSnapshot.Restore(_snapshot, _liveDoc);
+ logger.Info("Sw2gzCreateRobotPmp: cancel → snapshot restored");
+ }
+ }
+
+ void IPropertyManagerPage2Handler9.AfterClose() { if (_okay && _liveDoc != null) _onCommit(_liveDoc); _onClosed?.Invoke(); }
+
+ void IPropertyManagerPage2Handler9.OnButtonPress(int Id) { }
+
+ // Fake cue-banner placeholder: PMP-native Textbox has no real one, so
+ // swap the placeholder text for real empty content on focus and back
+ // on blur-while-still-empty.
+ void IPropertyManagerPage2Handler9.OnGainedFocus(int Id)
+ {
+ if (Id == IdLinkNameBox && _linkNameBox != null && _linkNameBox.Text == LinkNamePlaceholder)
+ _linkNameBox.Text = string.Empty;
+ }
+ void IPropertyManagerPage2Handler9.OnLostFocus(int Id)
+ {
+ if (Id == IdLinkNameBox && _linkNameBox != null && string.IsNullOrEmpty(_linkNameBox.Text))
+ SetLinkNamePlaceholder();
+ }
+ bool IPropertyManagerPage2Handler9.OnHelp() => true;
+ bool IPropertyManagerPage2Handler9.OnNextPage() => true;
+ bool IPropertyManagerPage2Handler9.OnPreviousPage() => true;
+ bool IPropertyManagerPage2Handler9.OnPreview() => true;
+ bool IPropertyManagerPage2Handler9.OnTabClicked(int Id) => true;
+ bool IPropertyManagerPage2Handler9.OnKeystroke(int Wparam, int Message, int Lparam, int Id) => false;
+ bool IPropertyManagerPage2Handler9.OnSubmitSelection(int Id, object Selection, int SelType, ref string ItemText) => true;
+ void IPropertyManagerPage2Handler9.OnTextboxChanged(int Id, string Text) { }
+ void IPropertyManagerPage2Handler9.OnSelectionboxFocusChanged(int Id) { }
+ void IPropertyManagerPage2Handler9.OnSelectionboxListChanged(int Id, int Count)
+ {
+ if (Id != IdMeshPicker) return;
+ try { AutoFillLinkName(); } catch (Exception e) { logger.Warn("AutoFillLinkName failed", e); }
+ }
+ void IPropertyManagerPage2Handler9.OnSelectionboxCalloutCreated(int Id) { }
+ void IPropertyManagerPage2Handler9.OnSelectionboxCalloutDestroyed(int Id) { }
+ void IPropertyManagerPage2Handler9.OnNumberboxChanged(int Id, double Value) { }
+ void IPropertyManagerPage2Handler9.OnNumberBoxTrackingCompleted(int Id, double Value) { }
+ void IPropertyManagerPage2Handler9.OnCheckboxCheck(int Id, bool Checked) { }
+ void IPropertyManagerPage2Handler9.OnComboboxEditChanged(int Id, string Text) { }
+ void IPropertyManagerPage2Handler9.OnComboboxSelectionChanged(int Id, int Item) { }
+ void IPropertyManagerPage2Handler9.OnListboxSelectionChanged(int Id, int Item) { }
+ void IPropertyManagerPage2Handler9.OnListboxRMBUp(int Id, int PosX, int PosY) { }
+ void IPropertyManagerPage2Handler9.OnGroupCheck(int Id, bool Checked) { }
+ void IPropertyManagerPage2Handler9.OnGroupExpand(int Id, bool Expanded) { }
+ void IPropertyManagerPage2Handler9.OnOptionCheck(int Id) { }
+ void IPropertyManagerPage2Handler9.OnPopupMenuItem(int Id) { }
+ void IPropertyManagerPage2Handler9.OnPopupMenuItemUpdate(int Id, ref int retval) { }
+ void IPropertyManagerPage2Handler9.OnSliderPositionChanged(int Id, double Value) { }
+ void IPropertyManagerPage2Handler9.OnSliderTrackingCompleted(int Id, double Value) { }
+ void IPropertyManagerPage2Handler9.OnRedo() { }
+ void IPropertyManagerPage2Handler9.OnUndo() { }
+ void IPropertyManagerPage2Handler9.OnWhatsNew() { }
+ int IPropertyManagerPage2Handler9.OnWindowFromHandleControlCreated(int Id, bool Status) => 0;
+ int IPropertyManagerPage2Handler9.OnActiveXControlCreated(int Id, bool Status) => 0;
+ }
+}
+#endif
diff --git a/SW2GZ/URDFExport/Sw2gzDocToExportConfig.cs b/SW2GZ/URDFExport/Sw2gzDocToExportConfig.cs
index 7e8b9df..38ec98a 100644
--- a/SW2GZ/URDFExport/Sw2gzDocToExportConfig.cs
+++ b/SW2GZ/URDFExport/Sw2gzDocToExportConfig.cs
@@ -41,6 +41,14 @@ public static Sw2gzExportConfig Bridge(Sw2gzDoc doc, ExportMetaInput meta)
License = meta?.License ?? string.Empty,
};
+ // Robot mode — carry the Create-Robot link/joint list through.
+ var robot = doc?.Robot;
+ if (robot != null)
+ {
+ cfg.RobotLinks = CloneLinks(robot.Links);
+ cfg.RobotJoints = CloneJoints(robot.Joints);
+ }
+
// World mode — carry the Create-World picks through to the exporter.
// (memory world-mode-dev: the first attempt failed here — Bridge
// dropped World config so the exporter saw empty picks.)
diff --git a/SW2GZ/URDFExport/Sw2gzExportConfig.cs b/SW2GZ/URDFExport/Sw2gzExportConfig.cs
index 2c2eb90..c2a75fd 100644
--- a/SW2GZ/URDFExport/Sw2gzExportConfig.cs
+++ b/SW2GZ/URDFExport/Sw2gzExportConfig.cs
@@ -53,7 +53,12 @@ public sealed class Sw2gzExportConfig
// Resume position — 0-based wizard step index reached at last save.
[DataMember] public int LastStep { get; set; }
- // Robot link/joint definitions removed for the v2 rebuild.
+ // Robot mode v3 — minimal flat list threaded from Sw2gzDoc.Robot via
+ // Sw2gzDocToExportConfig.Bridge. Links[0] is the base link; every other
+ // link's Fixed joint to it is derived by the wizard (Sw2gzCreateRobotPmp),
+ // not re-derived here. No mate-driven joint detection yet.
+ [DataMember] public List RobotLinks { get; set; } = new List();
+ [DataMember] public List RobotJoints { get; set; } = new List();
// World mode — picked components + physics from the Create-World wizard.
// Flat schema mirroring Sw2gzWorldConfig (the v2.1.0 in-memory model);
@@ -126,6 +131,8 @@ public sealed class Sw2gzExportConfig
[OnDeserializing]
private void OnDeserializing(StreamingContext context)
{
+ RobotLinks = new List();
+ RobotJoints = new List();
// World-mode defaults for checkpoints saved before these fields existed.
WorldGround = string.Empty;
WorldAssets = new List();
@@ -173,6 +180,8 @@ public Sw2gzExportConfig WithEmitWorldLink(bool emitWorldLink)
Email = this.Email,
License = this.License,
LastStep = this.LastStep,
+ RobotLinks = this.RobotLinks,
+ RobotJoints = this.RobotJoints,
WorldGround = this.WorldGround,
WorldAssets = this.WorldAssets,
WorldPhysicsEngine = this.WorldPhysicsEngine,
diff --git a/SW2GZ/URDFExport/Sw2gzModelExporter.cs b/SW2GZ/URDFExport/Sw2gzModelExporter.cs
index 10456b7..3b49279 100644
--- a/SW2GZ/URDFExport/Sw2gzModelExporter.cs
+++ b/SW2GZ/URDFExport/Sw2gzModelExporter.cs
@@ -58,11 +58,13 @@ internal static SW2GZ.Validate.ValidationReport RunCore(
return Sw2gzAssetExporter.Export(tess, config, outputDirOverride, rot);
}
- // Robot mode v2 — the inherited robot export pipeline was removed for
- // a clean rebuild. World and Asset returned above; reaching here means
- // a Robot-mode assembly doc, which is not implemented yet.
- throw new System.NotSupportedException(
- "Robot mode export is not implemented yet (removed for the v2 rebuild).");
+ // Robot mode v3 — minimal flat/fixed exporter (validating cut for the
+ // rebuild): one link per top-level component, all Fixed to the first,
+ // real relative joint pose (see Sw2gzRobotExporter for the math).
+ var massProps = new SolidWorksMassProperties(swApp, (AssemblyDoc)model);
+ var poses = new SolidWorksComponentPoses((AssemblyDoc)model);
+ var robotRot = SwToRosRotation.Build(config.SwUpAxis, config.SwForwardAxis);
+ return Sw2gzRobotExporter.Export(tess, massProps, poses, config, outputDirOverride, robotRot);
}
public static string WorkspacePath(string outputFolder, string packageName) =>
diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs
new file mode 100644
index 0000000..24a5086
--- /dev/null
+++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs
@@ -0,0 +1,388 @@
+/*
+Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md.
+
+Robot mode v3 — arbitrary user-built link tree (drag-to-reparent in the
+Links wizard), no mate-driven joint detection (removed 2026-07-01, was
+misclassifying joints; reverted to a known-good baseline) — every joint is
+still type="fixed", but DOES carry the real relative pose (rotation +
+translation) between parent and child, computed against each link's OWN
+declared ParentName, not always the root. Root is resolved by tree
+structure (LinkHierarchy.Roots — whichever link has no parent), not by
+list position, since re-rooting (LinkTreeView's "Set as base link") edits
+ParentName pointers without reordering Robot.Links.
+
+A link's mesh and mass/inertia are each a UNION of every component
+assigned to it (LinkDef.ComponentIds, wizard multi-select), not just the
+first — components are combined in the link's own reference frame, which
+is always its FIRST assigned component's pose. Mesh union: every
+component's tessellated mesh is un-baked into that one shared frame and
+concatenated. Mass union: every component's own MassProps + own pose feed
+InertialAggregator.Combine, rebased into the same shared frame — for a
+single-component link this is byte-identical to reading that component's
+MassProps directly (the rebase-by-anchor math cancels exactly when a
+part's own frame equals the anchor).
+
+Each link's mesh is expressed in that shared reference frame
+(un-baked from the tessellator's assembly-frame output using the same
+(R, t) pose read for the joint math), so the mesh renders correctly under
+the real joint chain and each link's TF frame reflects its true SW
+orientation — not just its true position.
+
+ p_world = R_link * p_local + t_link (tessellator bake)
+ p_local = R_link^T * (p_world - t_link) (un-bake for )
+ R_joint(parent->child) = R_parent^T * R_child
+ t_joint(parent->child) = R_parent^T * (t_child - t_parent)
+
+Base link is the one exception: its own frame is treated as identity (a
+common, valid URDF convention for the root link — nothing above it to be
+"relative" to), so its mesh is only re-centered (t_base subtracted), not
+un-rotated — mass combination does NOT get this same treatment (it rebases
+into the root's REAL pose, not forced identity; see CombineMass's own doc
+comment for why). This matches FULL_ARM's own base_link (already
+identity-rotated in SW) exactly; a base_link with a genuinely rotated
+native frame would show that rotation baked into its mesh rather than
+reflected in its own TF triad — a known simplification, not a bug, for
+this validating cut.
+
+Output layout (no ament package yet — deliberately out of scope for this
+validating cut):
+ /_ws/src//urdf/.urdf.xacro
+ /_ws/src//meshes/.dae
+
+COM-free (takes IMeshTessellator + IMassProperties + IComponentPoses) so it's
+unit-testable with fakes.
+*/
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.IO;
+using System.Linq;
+using System.Numerics;
+using SW2GZ.Build;
+using SW2GZ.Build.Model;
+using SW2GZ.Math;
+using SW2GZ.SwSurface.Abstractions;
+using SW2GZ.URDF;
+using SW2GZ.Validate;
+using SW2GZ.Write.Mesh;
+
+namespace SW2GZ.URDFExport
+{
+ public static class Sw2gzRobotExporter
+ {
+ public static ValidationReport Export(
+ IMeshTessellator tess, IMassProperties massProps, IComponentPoses poses,
+ Sw2gzExportConfig config, string outputDir, Matrix3 swToRos)
+ {
+ if (tess == null) throw new ArgumentNullException(nameof(tess));
+ if (massProps == null) throw new ArgumentNullException(nameof(massProps));
+ if (poses == null) throw new ArgumentNullException(nameof(poses));
+ if (config == null) throw new ArgumentNullException(nameof(config));
+ if (string.IsNullOrWhiteSpace(outputDir))
+ throw new SW2GZ.Exceptions.Sw2gzExportException(
+ "Output folder is empty — set a target directory in the Export dialog.");
+
+ List links = config.RobotLinks ?? new List();
+ if (links.Count == 0)
+ throw new SW2GZ.Exceptions.Sw2gzExportException(
+ "No links defined — open Create Robot and add at least one link.");
+
+ string pkg = PackageNameSanitizer.Sanitize(config.PackageName).Value;
+ string workspace = Path.Combine(outputDir, pkg + "_ws");
+ string root = Path.Combine(workspace, "src", pkg);
+ string urdfDir = Path.Combine(root, "urdf");
+ string meshesDir = Path.Combine(root, "meshes");
+ Directory.CreateDirectory(urdfDir);
+ Directory.CreateDirectory(meshesDir);
+
+ var issues = new List();
+ var meshFiles = new Dictionary(StringComparer.Ordinal);
+ var masses = new Dictionary(StringComparer.Ordinal);
+ var jointOrigins = new Dictionary(StringComparer.Ordinal);
+ var jointRpys = new Dictionary(StringComparer.Ordinal);
+
+ // Root = whichever link the TREE says has no parent, not
+ // links[0]. "Set as base link" (re-root, in LinkTreeView) edits
+ // ParentName pointers but never reorders Robot.Links, so list
+ // position [0] can silently stop being the real root.
+ LinkDef baseLink = LinkHierarchy.Roots(links).FirstOrDefault() ?? links[0];
+ string baseLinkName = baseLink.Name;
+
+ // Pass 1: every link's own reference pose (its first assigned
+ // component), read once up front. A child can be positioned
+ // before its parent in this list after a drag-drop reparent
+ // (reparenting only edits ParentName, never reorders Links), so
+ // pass 2 needs random access to ANY link's pose by name, not
+ // list order.
+ var linkPoses = new Dictionary(StringComparer.Ordinal);
+ foreach (LinkDef link in links)
+ {
+ string refComp = link.ComponentIds?.FirstOrDefault();
+ linkPoses[link.Name] = TryGetPose(poses, refComp, issues, link.Name);
+ }
+
+ foreach (LinkDef link in links)
+ {
+ string compName = link.ComponentIds?.FirstOrDefault();
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ (Matrix3 linkR, Vector3 linkT) = linkPoses[link.Name];
+
+ MeshData meshLocal = link.Name == baseLinkName
+ ? UnionMeshInLocalFrame(tess, link.ComponentIds, Matrix3.Identity, linkT, issues, link.Name)
+ : UnionMeshInLocalFrame(tess, link.ComponentIds, linkR, linkT, issues, link.Name);
+
+ if (meshLocal != null)
+ {
+ string daeFile = link.Name + ".dae";
+ DaeWriter.Write(meshLocal, Path.Combine(meshesDir, daeFile), withNormals: true);
+ meshFiles[link.Name] = daeFile;
+ }
+
+ // Joint origin relative to THIS link's own declared parent —
+ // not always the root. Same formula as before; only the
+ // source of "parent pose" changed.
+ if (!string.IsNullOrEmpty(link.ParentName) &&
+ linkPoses.TryGetValue(link.ParentName, out (Matrix3 R, Vector3 T) parentPose))
+ {
+ Matrix3 rJoint = parentPose.R.Transpose() * linkR;
+ Vector3 tJoint = parentPose.R.Transpose().Mul(linkT - parentPose.T);
+ jointOrigins[link.Name] = tJoint;
+ jointRpys[link.Name] = rJoint.ToRpy();
+ }
+ else if (!string.IsNullOrEmpty(link.ParentName))
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.PARENT",
+ "Link '" + link.Name + "' — parent '" + link.ParentName + "' not found, joint origin defaults to identity.",
+ "Sw2gzRobotExporter"));
+ }
+
+ masses[link.Name] = CombineMass(massProps, poses, link.ComponentIds, linkR, linkT, issues, link.Name);
+ }
+
+ string urdfPath = Path.Combine(urdfDir, pkg + ".urdf.xacro");
+ WriteUrdf(urdfPath, pkg, baseLinkName, links, meshFiles, masses, jointOrigins, jointRpys, config.EmitWorldLink, swToRos);
+
+ return new ValidationReport(issues);
+ }
+
+ private static (Matrix3, Vector3) TryGetPose(
+ IComponentPoses poses, string compName, List issues, string linkName)
+ {
+ if (string.IsNullOrWhiteSpace(compName)) return (Matrix3.Identity, Vector3.Zero);
+ try
+ {
+ return poses.GetPose(compName);
+ }
+ catch (Exception ex)
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.POSE",
+ "Link '" + linkName + "' — could not read pose for '" + compName + "', using identity: " + ex.Message,
+ "Sw2gzRobotExporter"));
+ return (Matrix3.Identity, Vector3.Zero);
+ }
+ }
+
+ // Every component assigned to a link gets tessellated and folded
+ // into ONE mesh, all expressed in the SAME reference frame (refR,
+ // refT) — not each component's own frame, which would scatter the
+ // pieces apart. Generalizes the old single-component un-bake to N
+ // components via the same vertex-offset + index-shift pattern
+ // SolidWorksMeshTessellator already uses internally to union
+ // multiple solid bodies within one component.
+ private static MeshData UnionMeshInLocalFrame(
+ IMeshTessellator tess, IReadOnlyList componentIds,
+ Matrix3 refR, Vector3 refT, List issues, string linkName)
+ {
+ var verts = new List();
+ var tris = new List();
+ System.Drawing.Color? color = null;
+ Matrix3 refRInv = refR.Transpose();
+
+ foreach (string compName in componentIds ?? (IReadOnlyList)Array.Empty())
+ {
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ MeshData meshWorld;
+ try
+ {
+ meshWorld = tess.Tessellate(compName, TessellationLod.Fine);
+ }
+ catch (Exception ex)
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MESH",
+ "Link '" + linkName + "' — could not tessellate '" + compName + "': " + ex.Message,
+ "Sw2gzRobotExporter"));
+ continue;
+ }
+ if (meshWorld?.Vertices == null || meshWorld.Vertices.Length == 0) continue;
+
+ color ??= meshWorld.MaterialColor;
+ int baseIdx = verts.Count;
+ foreach (Vector3 v in meshWorld.Vertices) verts.Add(refRInv.Mul(v - refT));
+ foreach (int idx in meshWorld.Triangles) tris.Add(baseIdx + idx);
+ }
+
+ return verts.Count == 0 ? null : new MeshData(verts.ToArray(), tris.ToArray(), color);
+ }
+
+ // Combines every assigned component's own mass/COM/inertia
+ // (parallel-axis, via InertialAggregator) into one MassProps rebased
+ // into the link's own reference frame (linkR/linkT — the SAME pose
+ // used for the joint and mesh math). For a single-component link
+ // this is byte-identical to using that component's raw MassProps
+ // directly: InertialAggregator's rebase exactly cancels when a
+ // part's own frame equals the anchor (see
+ // CombineWithLinkAnchor_SinglePartAtAnchor_RebasesBackToPartLocal /
+ // its Matrix3 twin). Mass/inertia physical accuracy beyond this is
+ // already out of scope for this validating cut (see the ponytail
+ // note this replaces) — this does not force the base_link
+ // identity-orientation convention the way mesh un-baking does,
+ // since that would only matter for a multi-component root with a
+ // non-identity native rotation, which isn't exercised yet.
+ private static MassProps CombineMass(
+ IMassProperties massProps, IComponentPoses poses, IReadOnlyList componentIds,
+ Matrix3 linkR, Vector3 linkT, List issues, string linkName)
+ {
+ var parts = new List<(MassProps Props, Matrix3 R, Vector3 T)>();
+ foreach (string compName in componentIds ?? (IReadOnlyList)Array.Empty())
+ {
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ MassProps mp;
+ try
+ {
+ mp = massProps.Get(compName);
+ }
+ catch (Exception ex)
+ {
+ mp = new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MASS",
+ "Link '" + linkName + "' — no material on '" + compName + "', using placeholder mass: " + ex.Message,
+ "Sw2gzRobotExporter"));
+ }
+ (Matrix3 compR, Vector3 compT) = TryGetPose(poses, compName, issues, linkName);
+ parts.Add((mp, compR, compT));
+ }
+
+ if (parts.Count == 0) return new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ return InertialAggregator.Combine(parts, linkR, linkT);
+ }
+
+ private static void WriteUrdf(
+ string path, string pkg, string baseLinkName, List links,
+ Dictionary meshFiles, Dictionary masses,
+ Dictionary jointOrigins, Dictionary jointRpys,
+ bool emitWorldLink, Matrix3 swToRos)
+ {
+ var uw = new URDFWriter(path);
+ System.Xml.XmlWriter w = uw.writer;
+ w.WriteStartDocument();
+ w.WriteStartElement("robot");
+ w.WriteAttributeString("name", pkg);
+
+ // Same mechanism Sw2gzModelPreviewer already uses for the browser
+ // preview: a synthetic world link + fixed joint carrying the SW→ROS
+ // rotation, only emitted when the caller opts in (preview forces
+ // this on; real exports honour the user's saved EmitWorldLink).
+ if (emitWorldLink)
+ {
+ w.WriteStartElement("link");
+ w.WriteAttributeString("name", "world");
+ w.WriteEndElement();
+
+ (double roll, double pitch, double yaw) = swToRos.ToRpy();
+ w.WriteStartElement("joint");
+ w.WriteAttributeString("name", "world_to_" + baseLinkName);
+ w.WriteAttributeString("type", "fixed");
+ w.WriteStartElement("parent"); w.WriteAttributeString("link", "world"); w.WriteEndElement();
+ w.WriteStartElement("child"); w.WriteAttributeString("link", baseLinkName); w.WriteEndElement();
+ w.WriteStartElement("origin");
+ w.WriteAttributeString("xyz", "0 0 0");
+ w.WriteAttributeString("rpy", Fmt(roll) + " " + Fmt(pitch) + " " + Fmt(yaw));
+ w.WriteEndElement();
+ w.WriteEndElement();
+ }
+
+ foreach (LinkDef link in links)
+ {
+ w.WriteStartElement("link");
+ w.WriteAttributeString("name", link.Name);
+
+ if (meshFiles.TryGetValue(link.Name, out string daeFile))
+ {
+ WriteVisualOrCollision(w, "visual", pkg, daeFile);
+ WriteVisualOrCollision(w, "collision", pkg, daeFile);
+ }
+
+ if (masses.TryGetValue(link.Name, out MassProps mp))
+ {
+ w.WriteStartElement("inertial");
+ // ponytail: COM held at the link origin rather than the SW
+ // mass-property centroid re-expressed in this link's local
+ // frame. Physical accuracy is out of scope for this
+ // validating cut (no actuation/physics sim runs on the
+ // export yet); revisit once Robot mode gets a real
+ // inertia pipeline.
+ w.WriteStartElement("origin");
+ w.WriteAttributeString("xyz", "0 0 0");
+ w.WriteEndElement();
+ w.WriteStartElement("mass");
+ w.WriteAttributeString("value", Fmt(mp.Mass));
+ w.WriteEndElement();
+ w.WriteStartElement("inertia");
+ w.WriteAttributeString("ixx", Fmt(mp.InertiaAtComLocal.M11));
+ w.WriteAttributeString("ixy", Fmt(mp.InertiaAtComLocal.M12));
+ w.WriteAttributeString("ixz", Fmt(mp.InertiaAtComLocal.M13));
+ w.WriteAttributeString("iyy", Fmt(mp.InertiaAtComLocal.M22));
+ w.WriteAttributeString("iyz", Fmt(mp.InertiaAtComLocal.M23));
+ w.WriteAttributeString("izz", Fmt(mp.InertiaAtComLocal.M33));
+ w.WriteEndElement();
+ w.WriteEndElement();
+ }
+
+ w.WriteEndElement(); // link
+ }
+
+ foreach (LinkDef link in links)
+ {
+ if (string.IsNullOrEmpty(link.ParentName)) continue;
+ Vector3 origin = jointOrigins.TryGetValue(link.Name, out var o) ? o : Vector3.Zero;
+ (double roll, double pitch, double yaw) = jointRpys.TryGetValue(link.Name, out var rpy)
+ ? rpy : (0.0, 0.0, 0.0);
+ w.WriteStartElement("joint");
+ w.WriteAttributeString("name", link.ParentName + "_to_" + link.Name);
+ w.WriteAttributeString("type", "fixed");
+ w.WriteStartElement("parent"); w.WriteAttributeString("link", link.ParentName); w.WriteEndElement();
+ w.WriteStartElement("child"); w.WriteAttributeString("link", link.Name); w.WriteEndElement();
+ w.WriteStartElement("origin");
+ w.WriteAttributeString("xyz", Fmt(origin.X) + " " + Fmt(origin.Y) + " " + Fmt(origin.Z));
+ w.WriteAttributeString("rpy", Fmt(roll) + " " + Fmt(pitch) + " " + Fmt(yaw));
+ w.WriteEndElement();
+ w.WriteEndElement();
+ }
+
+ w.WriteEndElement(); // robot
+ w.WriteEndDocument();
+ w.Flush();
+ w.Close();
+ }
+
+ private static void WriteVisualOrCollision(System.Xml.XmlWriter w, string tag, string pkg, string daeFile)
+ {
+ w.WriteStartElement(tag);
+ w.WriteStartElement("origin");
+ w.WriteAttributeString("xyz", "0 0 0");
+ w.WriteAttributeString("rpy", "0 0 0");
+ w.WriteEndElement();
+ w.WriteStartElement("geometry");
+ w.WriteStartElement("mesh");
+ w.WriteAttributeString("filename", "package://" + pkg + "/meshes/" + daeFile);
+ w.WriteEndElement();
+ w.WriteEndElement();
+ w.WriteEndElement();
+ }
+
+ private static string Fmt(double v) => v.ToString("0.######", CultureInfo.InvariantCulture);
+ }
+}
diff --git a/Test/Build/InertialAggregatorMatrixTests.cs b/Test/Build/InertialAggregatorMatrixTests.cs
new file mode 100644
index 0000000..e2f4546
--- /dev/null
+++ b/Test/Build/InertialAggregatorMatrixTests.cs
@@ -0,0 +1,123 @@
+using System.Collections.Generic;
+using System.Numerics;
+using SW2GZ.Build;
+using SW2GZ.Math;
+using Xunit;
+
+namespace SW2GZ.Build.Tests
+{
+ public class InertialAggregatorMatrixTests
+ {
+ private static Matrix3 RotZ(double radians)
+ {
+ double c = System.Math.Cos(radians), s = System.Math.Sin(radians);
+ return new Matrix3(c, -s, 0, s, c, 0, 0, 0, 1);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_MatchesQuaternionOverload_IdentityRotation()
+ {
+ var p = new MassProps(1.0, Vector3.Zero, Matrix3.Identity);
+ var posA = new Vector3(-1, 0, 0);
+ var posB = new Vector3(1, 0, 0);
+
+ var quaternionParts = new List<(MassProps, Pose)>
+ {
+ (p, new Pose(posA, Quaternion.Identity)),
+ (p, new Pose(posB, Quaternion.Identity)),
+ };
+ var matrixParts = new List<(MassProps, Matrix3, Vector3)>
+ {
+ (p, Matrix3.Identity, posA),
+ (p, Matrix3.Identity, posB),
+ };
+
+ MassProps viaQuaternion = InertialAggregator.Combine(quaternionParts);
+ MassProps viaMatrix3 = InertialAggregator.Combine(matrixParts);
+
+ Assert.Equal(2.0, viaMatrix3.Mass);
+ Assert.Equal(viaQuaternion.Mass, viaMatrix3.Mass);
+ Assert.Equal(viaQuaternion.ComLocal.X, viaMatrix3.ComLocal.X, 9);
+ Assert.Equal(viaQuaternion.ComLocal.Y, viaMatrix3.ComLocal.Y, 9);
+ Assert.Equal(viaQuaternion.ComLocal.Z, viaMatrix3.ComLocal.Z, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M11, viaMatrix3.InertiaAtComLocal.M11, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M22, viaMatrix3.InertiaAtComLocal.M22, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M33, viaMatrix3.InertiaAtComLocal.M33, 9);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_MatchesQuaternionOverload_NonIdentityRotation()
+ {
+ var inertia = new Matrix3(1.5, 0, 0, 0, 2.0, 0, 0, 0, 2.5);
+ var qA = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.4f);
+ var qB = Quaternion.CreateFromAxisAngle(Vector3.UnitY, -0.7f);
+ var posA = new Vector3(0.3f, 0.2f, -0.1f);
+ var posB = new Vector3(-0.2f, 0.5f, 0.4f);
+
+ var quaternionParts = new List<(MassProps, Pose)>
+ {
+ (new MassProps(1.0, Vector3.Zero, inertia), new Pose(posA, qA)),
+ (new MassProps(2.0, Vector3.Zero, inertia), new Pose(posB, qB)),
+ };
+ var matrixParts = new List<(MassProps, Matrix3, Vector3)>
+ {
+ (new MassProps(1.0, Vector3.Zero, inertia), Matrix3.FromQuaternion(qA), posA),
+ (new MassProps(2.0, Vector3.Zero, inertia), Matrix3.FromQuaternion(qB), posB),
+ };
+
+ MassProps viaQuaternion = InertialAggregator.Combine(quaternionParts);
+ MassProps viaMatrix3 = InertialAggregator.Combine(matrixParts);
+
+ Assert.Equal(viaQuaternion.Mass, viaMatrix3.Mass, 9);
+ Assert.Equal(viaQuaternion.ComLocal.X, viaMatrix3.ComLocal.X, 6);
+ Assert.Equal(viaQuaternion.ComLocal.Y, viaMatrix3.ComLocal.Y, 6);
+ Assert.Equal(viaQuaternion.ComLocal.Z, viaMatrix3.ComLocal.Z, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M11, viaMatrix3.InertiaAtComLocal.M11, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M22, viaMatrix3.InertiaAtComLocal.M22, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M33, viaMatrix3.InertiaAtComLocal.M33, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M12, viaMatrix3.InertiaAtComLocal.M12, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M13, viaMatrix3.InertiaAtComLocal.M13, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M23, viaMatrix3.InertiaAtComLocal.M23, 6);
+ }
+
+ [Fact]
+ public void CombineWithAnchor_Matrix3Overload_PartAtAnchor_RebasesBackToPartLocal()
+ {
+ // Mirrors InertialAggregatorTests.CombineWithLinkAnchor_SinglePartAtAnchor_RebasesBackToPartLocal
+ // for the Matrix3 overload: when a part's own frame equals the
+ // rebase anchor, the two transforms must cancel exactly,
+ // regardless of what that shared rotation actually is.
+ var partLocalCom = new Vector3(0f, 0f, 0.15f);
+ var partInertia = new Matrix3(0.003, 0, 0, 0, 0.003, 0, 0, 0, 0.0001);
+ var p = new MassProps(0.5, partLocalCom, partInertia);
+
+ Matrix3 anchorR = RotZ(0.5);
+ Vector3 anchorT = new Vector3(1.0f, -2.0f, 0.4f);
+
+ var parts = new List<(MassProps, Matrix3, Vector3)> { (p, anchorR, anchorT) };
+ MassProps rebased = InertialAggregator.Combine(parts, anchorR, anchorT);
+
+ Assert.Equal(0.5, rebased.Mass, 6);
+ Assert.Equal(partLocalCom.X, rebased.ComLocal.X, 5);
+ Assert.Equal(partLocalCom.Y, rebased.ComLocal.Y, 5);
+ Assert.Equal(partLocalCom.Z, rebased.ComLocal.Z, 5);
+ Assert.Equal(partInertia.M11, rebased.InertiaAtComLocal.M11, 5);
+ Assert.Equal(partInertia.M22, rebased.InertiaAtComLocal.M22, 5);
+ Assert.Equal(partInertia.M33, rebased.InertiaAtComLocal.M33, 5);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_Null_ReturnsIdentity()
+ {
+ var result = InertialAggregator.Combine((List<(MassProps, Matrix3, Vector3)>)null);
+ Assert.Equal(0.0, result.Mass);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_EmptyList_ReturnsIdentity()
+ {
+ var result = InertialAggregator.Combine(new List<(MassProps, Matrix3, Vector3)>());
+ Assert.Equal(0.0, result.Mass);
+ }
+ }
+}
diff --git a/Test/Build/InertialAggregatorTests.cs b/Test/Build/InertialAggregatorTests.cs
index efbe6db..1e9c839 100644
--- a/Test/Build/InertialAggregatorTests.cs
+++ b/Test/Build/InertialAggregatorTests.cs
@@ -11,7 +11,7 @@ public class InertialAggregatorTests
[Fact]
public void Combine_Null_ReturnsIdentity()
{
- var result = InertialAggregator.Combine(null);
+ var result = InertialAggregator.Combine((IReadOnlyList<(MassProps, Pose)>)null);
Assert.Equal(0.0, result.Mass);
}
diff --git a/Test/SW2GZ.Writers.Test.csproj b/Test/SW2GZ.Writers.Test.csproj
index 0645144..43b2147 100644
--- a/Test/SW2GZ.Writers.Test.csproj
+++ b/Test/SW2GZ.Writers.Test.csproj
@@ -34,6 +34,10 @@
+
+
+
+
diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs
new file mode 100644
index 0000000..710a027
--- /dev/null
+++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs
@@ -0,0 +1,473 @@
+/*
+Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md.
+*/
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.IO;
+using System.Linq;
+using System.Numerics;
+using System.Xml.Linq;
+using SW2GZ.Build;
+using SW2GZ.Build.Model;
+using SW2GZ.Math;
+using SW2GZ.SwSurface.Abstractions;
+using SW2GZ.URDFExport;
+using Xunit;
+
+namespace SW2GZ.Writers.Tests
+{
+ public class Sw2gzRobotExporterTests : IDisposable
+ {
+ private readonly string _dir;
+ public Sw2gzRobotExporterTests()
+ {
+ _dir = Path.Combine(Path.GetTempPath(), "sw2gz_robot_" + Guid.NewGuid().ToString("N"));
+ Directory.CreateDirectory(_dir);
+ }
+ public void Dispose() { try { Directory.Delete(_dir, true); } catch { } }
+
+ private sealed class FakeTess : IMeshTessellator
+ {
+ public MeshData Tessellate(string n, TessellationLod lod) => new MeshData(
+ new[] { new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 1, 0) },
+ new[] { 0, 1, 2 }, null);
+ }
+
+ private sealed class FakeMultiTess : IMeshTessellator
+ {
+ private readonly Dictionary _meshes;
+ public FakeMultiTess(Dictionary meshes) => _meshes = meshes;
+ public MeshData Tessellate(string n, TessellationLod lod) =>
+ _meshes.TryGetValue(n, out MeshData m) ? m : new MeshData(Array.Empty(), Array.Empty(), null);
+ }
+
+ private sealed class FakeMultiMassProps : IMassProperties
+ {
+ private readonly Dictionary _masses;
+ public FakeMultiMassProps(Dictionary masses) => _masses = masses;
+ public MassProps Get(string componentPathName) =>
+ _masses.TryGetValue(componentPathName, out MassProps m) ? m : new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ }
+
+ private sealed class FakeMassProps : IMassProperties
+ {
+ public bool ThrowOnGet;
+ public MassProps Get(string componentPathName)
+ {
+ if (ThrowOnGet) throw new InvalidOperationException("no material");
+ return new MassProps(2.5, Vector3.Zero, Matrix3.Identity);
+ }
+ }
+
+ // Per-component (rotation, translation), so tests can assert the
+ // exporter's real relative-joint-pose math against a known answer.
+ private sealed class FakePoses : IComponentPoses
+ {
+ private readonly Dictionary _poses;
+ public FakePoses(Dictionary poses = null) => _poses = poses;
+ public (Matrix3 Rotation, Vector3 Translation) GetPose(string componentPathName) =>
+ _poses != null && _poses.TryGetValue(componentPathName, out var p) ? p : (Matrix3.Identity, Vector3.Zero);
+ }
+
+ private static Matrix3 RotZ(double radians)
+ {
+ double c = System.Math.Cos(radians), s = System.Math.Sin(radians);
+ return new Matrix3(c, -s, 0, s, c, 0, 0, 0, 1);
+ }
+
+ private static List TwoLinks() => new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-1@asm" }, ParentName = "base_link" },
+ };
+
+ private Sw2gzExportConfig Cfg() => new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = TwoLinks(),
+ };
+
+ private XElement UrdfRoot() =>
+ XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+
+ [Fact]
+ public void Export_WritesUrdfWithLinksMeshesAndFixedJoint()
+ {
+ var rep = Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(), Cfg(), _dir, Matrix3.Identity);
+ Assert.False(rep.HasErrors);
+
+ string meshesDir = Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "meshes");
+ Assert.True(File.Exists(Path.Combine(meshesDir, "base_link.dae")));
+ Assert.True(File.Exists(Path.Combine(meshesDir, "arm_link.dae")));
+
+ XElement root = UrdfRoot();
+ Assert.Equal("robot", root.Name.LocalName);
+ Assert.Equal("my_robot", (string)root.Attribute("name"));
+
+ var linkNames = root.Elements("link").Select(l => (string)l.Attribute("name")).ToList();
+ Assert.Contains("base_link", linkNames);
+ Assert.Contains("arm_link", linkNames);
+
+ XElement joint = root.Elements("joint").Single();
+ Assert.Equal("base_link_to_arm_link", (string)joint.Attribute("name"));
+ Assert.Equal("fixed", (string)joint.Attribute("type"));
+ Assert.Equal("base_link", (string)joint.Element("parent").Attribute("link"));
+ Assert.Equal("arm_link", (string)joint.Element("child").Attribute("link"));
+
+ XElement armLink = root.Elements("link").Single(l => (string)l.Attribute("name") == "arm_link");
+ Assert.Equal("2.5", (string)armLink.Element("inertial").Element("mass").Attribute("value"));
+ }
+
+ [Fact]
+ public void Export_JointOriginIsRealTranslationDelta_NotIdentity()
+ {
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, new Vector3(10, 20, 30)),
+ ["arm-1@asm"] = (Matrix3.Identity, new Vector3(11, 22, 33)),
+ };
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), Cfg(), _dir, Matrix3.Identity);
+
+ XElement joint = UrdfRoot().Elements("joint").Single();
+ string[] xyz = ((string)joint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(1.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(2.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(3.0, double.Parse(xyz[2]), 3);
+ }
+
+ [Fact]
+ public void Export_JointRotationIsRealRelativeRotation_NotIdentity()
+ {
+ // Base identity, arm rotated 90 deg about Z in the assembly — the
+ // joint rpy must carry that ~90 deg yaw, not 0 0 0.
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-1@asm"] = (RotZ(System.Math.PI / 2), Vector3.Zero),
+ };
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), Cfg(), _dir, Matrix3.Identity);
+
+ XElement joint = UrdfRoot().Elements("joint").Single();
+ string[] rpy = ((string)joint.Element("origin").Attribute("rpy")).Split(' ');
+ Assert.Equal(0.0, double.Parse(rpy[0]), 3);
+ Assert.Equal(0.0, double.Parse(rpy[1]), 3);
+ Assert.Equal(System.Math.PI / 2, double.Parse(rpy[2]), 3);
+ }
+
+ [Fact]
+ public void Export_GrandchildJointOrigin_IsRelativeToItsOwnParent_NotRoot()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "mid_link", ComponentIds = { "mid-1@asm" }, ParentName = "base_link" },
+ new LinkDef { Name = "leaf_link", ComponentIds = { "leaf-1@asm" }, ParentName = "mid_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, new Vector3(0, 0, 0)),
+ ["mid-1@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["leaf-1@asm"] = (Matrix3.Identity, new Vector3(1, 5, 0)),
+ };
+
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement leafJoint = root.Elements("joint").Single(j => (string)j.Attribute("name") == "mid_link_to_leaf_link");
+ Assert.Equal("mid_link", (string)leafJoint.Element("parent").Attribute("link"));
+
+ // leaf is at (1,5,0), its real parent mid_link is at (1,0,0) — the
+ // relative offset is (0,5,0). If this were still computed relative
+ // to ROOT (0,0,0) instead of mid_link, it would wrongly read (1,5,0).
+ string[] xyz = ((string)leafJoint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(0.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(5.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[2]), 3);
+ }
+
+ [Fact]
+ public void Export_RootDetectedByTreeStructure_NotListPosition()
+ {
+ // Simulates a post-reroot doc: mid_link is now the actual root
+ // (ParentName == ""), but sits at list position [1], not [0] —
+ // exactly what LinkTreeView's "Set as base link" produces (it
+ // edits ParentName pointers, never reorders Robot.Links).
+ var links = new List
+ {
+ new LinkDef { Name = "leaf_link", ComponentIds = { "leaf-1@asm" }, ParentName = "mid_link" },
+ new LinkDef { Name = "mid_link", ComponentIds = { "mid-1@asm" }, ParentName = "" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["mid-1@asm"] = (Matrix3.Identity, new Vector3(5, 0, 0)),
+ ["leaf-1@asm"] = (Matrix3.Identity, new Vector3(5, 2, 0)),
+ };
+
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement joint = root.Elements("joint").Single();
+ Assert.Equal("mid_link", (string)joint.Element("parent").Attribute("link"));
+ Assert.Equal("leaf_link", (string)joint.Element("child").Attribute("link"));
+
+ // leaf (5,2,0) relative to its real parent mid_link (5,0,0) = (0,2,0).
+ // If root were still wrongly detected as leaf_link (list position
+ // [0]), this would never be computed at all (falls back to 0 0 0).
+ string[] xyz = ((string)joint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(0.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(2.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[2]), 3);
+ }
+
+ [Fact]
+ public void Export_DanglingParentReference_DoesNotCrash_AndWarns()
+ {
+ // arm_link's ParentName points at a link name that isn't in
+ // RobotLinks at all (e.g. the parent link was deleted but this
+ // sibling's ParentName pointer was left stale).
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-1@asm" }, ParentName = "deleted_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+
+ var rep = Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(), cfg, _dir, Matrix3.Identity);
+
+ Assert.True(rep.Warnings.Any(w => w.Code == "ROBOT.PARENT"));
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement joint = root.Elements("joint").Single(j => (string)j.Attribute("name") == "deleted_link_to_arm_link");
+ string[] xyz = ((string)joint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(0.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[2]), 3);
+ }
+
+ [Fact]
+ public void Export_NoLinks_Throws()
+ {
+ var cfg = Cfg(); cfg.RobotLinks = new List();
+ Assert.Throws(
+ () => Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(), cfg, _dir, Matrix3.Identity));
+ }
+
+ [Fact]
+ public void Export_MissingMaterial_FallsBackToPlaceholderMassAndWarns()
+ {
+ var rep = Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps { ThrowOnGet = true }, new FakePoses(), Cfg(), _dir, Matrix3.Identity);
+
+ XElement root = UrdfRoot();
+ XElement armLink = root.Elements("link").Single(l => (string)l.Attribute("name") == "arm_link");
+ Assert.Equal("0.1", (string)armLink.Element("inertial").Element("mass").Attribute("value"));
+
+ Assert.True(rep.Warnings.Any());
+ }
+
+ [Fact]
+ public void Export_EmitWorldLink_AddsWorldJointWithRotation()
+ {
+ var cfg = Cfg(); cfg.EmitWorldLink = true;
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(), cfg, _dir,
+ SwToRosRotation.Build(SW2GZ.Build.Model.AxisDirection.PlusY, SW2GZ.Build.Model.AxisDirection.PlusZ));
+
+ XElement root = UrdfRoot();
+ Assert.Contains("world", root.Elements("link").Select(l => (string)l.Attribute("name")));
+ XElement worldJoint = root.Elements("joint").Single(j => (string)j.Attribute("name") == "world_to_base_link");
+ Assert.Equal("fixed", (string)worldJoint.Attribute("type"));
+ Assert.Equal("world", (string)worldJoint.Element("parent").Attribute("link"));
+ Assert.Equal("base_link", (string)worldJoint.Element("child").Attribute("link"));
+ }
+
+ [Fact]
+ public void Export_MultiComponentLink_UnionsAllMeshesInLinkReferenceFrame()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-a@asm", "arm-b@asm" }, ParentName = "base_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-a@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["arm-b@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ };
+ var meshA = new MeshData(
+ new[] { new Vector3(1, 0, 0), new Vector3(2, 0, 0), new Vector3(1, 1, 0) },
+ new[] { 0, 1, 2 }, null);
+ var meshB = new MeshData(
+ new[] { new Vector3(1, 0, 5), new Vector3(2, 0, 5), new Vector3(1, 1, 5) },
+ new[] { 0, 1, 2 }, null);
+ var tess = new FakeMultiTess(new Dictionary
+ {
+ ["base-1@asm"] = new MeshData(new[] { new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 1, 0) }, new[] { 0, 1, 2 }, null),
+ ["arm-a@asm"] = meshA,
+ ["arm-b@asm"] = meshB,
+ });
+
+ Sw2gzRobotExporter.Export(tess, new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ string daePath = Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "meshes", "arm_link.dae");
+ Assert.True(File.Exists(daePath));
+
+ XNamespace ns = "http://www.collada.org/2005/11/COLLADASchema";
+ XDocument dae = XDocument.Load(daePath);
+ XElement posArray = dae.Descendants(ns + "float_array")
+ .Single(e => (string)e.Attribute("id") == "g0-pos-array");
+ int floatCount = int.Parse((string)posArray.Attribute("count"));
+
+ // Both components' triangles survive the union: 3 verts each, 3
+ // floats per vert = 18 total (not 9 — which is what a
+ // "first component only" regression would silently produce).
+ Assert.Equal(18, floatCount);
+
+ // arm-b's vertices sit at z=5 in its own (identity-rotation,
+ // translation (1,0,0)) frame; arm_link's reference frame is
+ // arm-a's pose (also (1,0,0), identity) — so after un-baking,
+ // arm-b's local vertices should still carry that z=5 offset
+ // (proves it was folded into the SAME shared frame as arm-a,
+ // not silently dropped or mis-transformed).
+ string[] floats = posArray.Value.Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
+ var zValues = new List();
+ for (int i = 2; i < floats.Length; i += 3)
+ zValues.Add(double.Parse(floats[i], CultureInfo.InvariantCulture));
+ Assert.Contains(zValues, z => System.Math.Abs(z - 5.0) < 1e-3);
+ }
+
+ [Fact]
+ public void Export_MultiComponentLink_CombinesMassOfAllAssignedComponents()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-a@asm", "arm-b@asm" }, ParentName = "base_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-a@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["arm-b@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ };
+ var massProps = new FakeMultiMassProps(new Dictionary
+ {
+ ["base-1@asm"] = new MassProps(9.0, Vector3.Zero, Matrix3.Identity),
+ ["arm-a@asm"] = new MassProps(1.5, Vector3.Zero, Matrix3.Identity),
+ ["arm-b@asm"] = new MassProps(2.5, Vector3.Zero, Matrix3.Identity),
+ });
+
+ Sw2gzRobotExporter.Export(new FakeTess(), massProps, new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement armLink = root.Elements("link").Single(l => (string)l.Attribute("name") == "arm_link");
+ double mass = double.Parse((string)armLink.Element("inertial").Element("mass").Attribute("value"), CultureInfo.InvariantCulture);
+
+ // 1.5 + 2.5, not just arm-a's 1.5 (a "first component only"
+ // regression would report 1.5, silently dropping arm-b).
+ Assert.Equal(4.0, mass, 3);
+ }
+
+ [Fact]
+ public void Export_MultiComponentLink_UsesEachComponentsOwnPose_NotSharedLinkFrame()
+ {
+ // arm-a and arm-b sit at DIFFERENT positions (unlike
+ // Export_MultiComponentLink_CombinesMassOfAllAssignedComponents
+ // above, where both parts share arm_link's own reference pose
+ // and so can't distinguish a correct per-part TryGetPose call
+ // inside CombineMass from a bug that reused the shared
+ // linkR/linkT for every part — both produce the same answer
+ // when every part's pose already equals the link frame).
+ //
+ // is hardcoded to "0 0 0" in WriteUrdf (a
+ // separate, pre-existing simplification — COM is not written),
+ // and mass is pose-invariant, so the only pose-sensitive value
+ // that reaches the URDF is the combined inertia tensor: with
+ // both parts spread away from arm_link's own frame (arm-a's
+ // pose), the parallel-axis contribution is nonzero. A
+ // shared-frame bug would instead evaluate every part AT
+ // arm_link's own frame (d = 0 for every part), so parallel-axis
+ // contributes nothing and izz stays at the parts' own identity
+ // inertia (1.0) instead of growing with the offset.
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-a@asm", "arm-b@asm" }, ParentName = "base_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-a@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["arm-b@asm"] = (Matrix3.Identity, new Vector3(1, 4, 0)),
+ };
+ var unitInertia = new Matrix3(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ var massProps = new FakeMultiMassProps(new Dictionary
+ {
+ ["base-1@asm"] = new MassProps(9.0, Vector3.Zero, Matrix3.Identity),
+ ["arm-a@asm"] = new MassProps(1.0, Vector3.Zero, unitInertia),
+ ["arm-b@asm"] = new MassProps(1.0, Vector3.Zero, unitInertia),
+ });
+
+ Sw2gzRobotExporter.Export(new FakeTess(), massProps, new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement armLink = root.Elements("link").Single(l => (string)l.Attribute("name") == "arm_link");
+ double izz = double.Parse((string)armLink.Element("inertial").Element("inertia").Attribute("izz"), CultureInfo.InvariantCulture);
+
+ // arm-a at (1,0,0) == arm_link's own frame (d=0); arm-b at
+ // (1,4,0) is offset by (0,4,0) from the combined COM (0,2,0) in
+ // world space, i.e. d=2 on each side. izz picks up
+ // m*(dx^2+dy^2) = 1*(0+4) = 4 from EACH part's offset from the
+ // shared COM at (0,2,0): arm-a contributes 1*(0+4)=4, arm-b
+ // contributes 1*(0+4)=4, so combined izz = 1 + 1 + 4 + 4 = 10.
+ // A shared-frame bug (both parts evaluated at arm_link's own
+ // pose, d=0 for both) would instead report izz = 1 + 1 = 2.
+ Assert.True(izz > 5.0, "izz=" + izz + " — expected > 5 (parallel-axis from per-component pose); a shared-frame bug would report izz=2.");
+ }
+ }
+}
diff --git a/agent-progress/progress.md b/agent-progress/progress.md
index 81d3138..c33f688 100644
--- a/agent-progress/progress.md
+++ b/agent-progress/progress.md
@@ -1,11 +1,245 @@
# Progress
Current: world mode + asset mode + World GUI/camera (v2.4.0) + World Settings
-(v2.5.0) shipped on `main`. Tests: **797 green**.
+(v2.5.0) shipped on `main`. Tests: **797 green** (on `main`; the v3 rebuild
+branch below is on an older base, 470 green there).
Branch model: `main` is trunk; tags v2.1.0/v2.2.0/v2.3.1/v2.4.0/v2.5.0.
Addin compiles clean (SW closed for MSBuild; regasm MSB3216 is non-fatal).
-## ▶ CONTINUE HERE — Robot mode GUTTED for clean rebuild (branch `feat/robot-mode-v2`, pushed)
+## Done — Robot mode v3: joint/link relative pose + multi-mesh links, live-tested (branch `feat/robot-mode-v3`)
+
+**2026-07-02, closing out this session's robot-mode work.** Two pieces,
+both live-tested working on `FULL_ARM.SLDASM`:
+
+**1. Links step UI rebuilt** (`Sw2gzCreateRobotPmp.cs` + `SW2GZ.UI.LinkTreeView`):
+manual mesh-picker + name box + drag-to-reparent tree (reused, not
+rewritten — `LinkTreeView`/`LinkHierarchy` were pre-existing, previously
+unwired). No more auto-seed; user builds the link tree explicitly, first
+Add is always forced to `base_link`, every later Add's parent is whichever
+tree node is currently selected. First mesh component picked for a link is
+its "primary" (drives the link's whole frame) — marked `(primary)` in the
+selected-info label and the tree's hover tooltip.
+
+**2. Exporter math fixed to match: parent-relative joints + multi-mesh
+union** (`Sw2gzRobotExporter.cs`, `InertialAggregator.cs`) — spec/plan:
+[`docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md`](../docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md) /
+[`docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md`](../docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md).
+Built via subagent-driven-development, 6 tasks, each independently
+implemented + spec-compliance reviewed + code-quality reviewed (two tasks
+needed a fix-and-reverify round: Task 2 got a missing warning for
+dangling parent references, Task 3 hit a real `System.Drawing.Color` vs
+`SW2GZ.URDF.Color` namespace collision that broke the real add-in build
+even though the test project stayed green — caught only because a
+reviewer built the actual `SW2GZ.csproj`, not just `dotnet test`). A final
+holistic review across all 6 tasks combined traced the "first component
+defines the link's frame" invariant by hand across mesh union / joint
+origin / mass rebase / UI labels and found it consistent everywhere.
+- Joint origin is now relative to each link's own `ParentName`, not always
+ root (`Sw2gzRobotExporter.cs` two-pass: pass 1 reads every link's own
+ reference pose by name, pass 2 looks up the declared parent's pose —
+ needed since a child can sit before its parent in `Robot.Links` after a
+ drag-drop reparent).
+- Root detected via `LinkHierarchy.Roots` (tree structure), not
+ `links[0]` (list position) — needed because "Set as base link" re-roots
+ by flipping `ParentName` pointers without reordering the list.
+- Mesh and mass are now a union of every `ComponentIds` entry on a link
+ (was silently first-component-only), both combined into the link's
+ shared reference frame — `UnionMeshInLocalFrame` new helper for mesh;
+ `CombineMass` + `InertialAggregator`'s new `Matrix3`-parameterized
+ `Combine` overload for mass (added instead of writing a
+ `Matrix3`→`Quaternion` converter — that category of new coordinate-
+ conversion code has produced two real bugs in this codebase already;
+ see memory `sw-mathtransform-column-major`, `robot-mode-dev`).
+- Root link keeps its identity-orientation mesh convention (mesh only
+ translated, not un-rotated); mass combination does NOT get the same
+ treatment (rebases into the root's real pose) — documented asymmetry,
+ only matters for a multi-component root with non-identity native
+ rotation, not yet exercised.
+- Joint TYPE still hardcoded Fixed (unchanged, separate future increment).
+- 481 tests green (was 470 at session start), both `SW2GZ.csproj` and
+ `Test/SW2GZ.Writers.Test.csproj` build clean, deployed, **live-tested
+ by the user 2026-07-02: 3-level chain (grandchild joint relative to its
+ real non-root parent, not root) + multi-mesh link (both parts positioned
+ correctly, not just the first) — confirmed working.**
+
+## Prior — Robot mode v3: Links step now uses LinkTreeView, high-risk-flagged before live test
+
+**Swapped the flat listbox for `LinkTreeView` (2026-07-02, 4 more user fixes).**
+`Sw2gzCreateRobotPmp.cs`'s Links step now embeds the pre-existing
+`SW2GZ.UI.LinkTreeView` (WinForms `TreeView`, `WindowFromHandle`-embedded
+like the nav/button bars) instead of the flat `PropertyManagerPageListbox` +
+hand-rolled DFS renderer from the last two entries. Reused, not rewritten:
+`LinkHierarchy.cs` (pure, already unit-tested) for roots/children/cycle
+checks, `LinkTreeView.cs` (WinForms, already had drag-to-reparent +
+F2-rename + right-click "Set as base") for the widget itself — both were
+sitting unwired since the 2026-07-02 revert (see memory `robot-mode-dev`).
+Changes: drag a node onto another to reparent (cycle-guarded); clicking a
+node is now how you pick the next Add's parent (`_linkTree.ActiveLink`,
+replacing last session's click-a-listbox-row approach) and drives a new
+"Selected: name Mesh: a, b" info label; `_linkTree.LinksChanged` triggers
+`RebuildJoints()` so drag/rename/reroot edits stay joint-consistent same as
+Add/Remove. Link-name box got a fake placeholder ("e.g. wheel_link",
+swapped for real empty text on focus/blur via `OnGainedFocus`/
+`OnLostFocus`) since PMP-native Textbox has no real cue banner. 470 green,
+clean build, deployed 09:55:00.
+
+**⚠ Explicit risk flag, not just the usual boilerplate warning:** this is
+literally the same `LinkTreeView` drag-and-drop widget from the **2026-07-02
+FULLY REVERTED** link-hierarchy attempt earlier this same day — it passed
+every automated gate then too and still broke live in SW with no captured
+symptom. `LinkTreeView.cs`/`LinkHierarchy.cs` themselves were untouched by
+that attempt (only called into, never modified), so they're unchanged from
+what's in git history either way — but "unchanged" isn't "proven," it was
+never isolated and live-tested on its own, only ever shipped bundled with
+the mesh-funnel work that also broke. This is the **smallest possible
+wiring step** for that widget (just swap the list for the tree, nothing
+else piled on) — do the live check on THIS step alone before adding
+anything more to the Links UI. Test: open Create Robot, add base_link, add
+a child, drag the child onto nothing/itself (should reject), drag a second
+child onto the first child (should reparent + tree redraws), remove
+base_link while it has children (should block, same as before).
+
+**Polish pass on the manual link builder (2026-07-02, after user screenshot).**
+7 UI fixes on `Sw2gzCreateRobotPmp.cs`: dropped the paragraph description;
+added real `Label` controls above Mesh/Link-name/Hierarchy (captions on
+Selectionbox/Textbox don't render visibly in this PMP); link name
+auto-fills from a single-part mesh pick (blank for sub-assembly or
+multi-pick, `OnSelectionboxListChanged` on the mesh box); tips repurposed
+as hover placeholders (skipped: real WinForms cue-banner placeholder —
+these are native PMP controls, not WinForms, converting them is a bigger
+diff than the ask); **removed the Parent combobox** — clicking a row in the
+hierarchy list now sets the parent for the next Add (`_linkRows` maps
+display row -> `LinkDef`, since the tree is rendered depth-first from root
+and no longer matches `Robot.Links` storage order); hierarchy list is a
+real indented tree (`AppendLinkRow` recursive DFS) instead of a flat
+one-level "-> parent" line. Native PMP label/control fonts/colors are
+SW-theme-driven, not stylable beyond what's already dark-themed on the
+WinForms button bar. 470 green, clean build, deployed 09:42:17. **Still
+needs the same live SW check as the previous entry** — not yet done.
+
+**Links step UI rework (2026-07-02).** Old flat model auto-seeded one link
+per top-level component, all Fixed to link[0]. New model, per user spec:
+no auto-seed; user picks mesh component(s) (parts/sub-assemblies, native
+live `Selectionbox`) → names the link → picks parent from a combo of
+existing links → **Add link**. First Add is always forced to root/
+`base_link` (REP-105), every later link needs an explicit parent — no more
+implicit flat-to-base. `RebuildJoints()` derives Joints (still hardcoded
+Fixed — joint-type refinement is a separate future increment) straight
+from each `LinkDef.ParentName`, so Links/Joints can't drift. Remove blocks
+a link that still has children (must remove leaves first). `Sw2gzCreateRobotPmp.cs`
+only; backend (`LinkDef.ParentName`, exporter, `Sw2gzDocLinkTreeRoundTripTests`)
+already supported a real tree, unchanged. 470 green, clean build, deployed
+09:21:57. **Needs a live SW check** — this is exactly the class of change
+(PMP/COM wiring) that has silently broken live before despite green tests;
+open Create Robot on FULL_ARM.SLDASM and walk: pick a part → Add link (becomes
+base_link) → pick another part → name it → parent=base_link → Add → remove
+base_link (should be blocked, has a child) → remove the child → remove
+base_link (should now work).
+
+## Done — Robot mode v3: minimal Create Robot wizard + exporter live (branch `feat/robot-mode-v3`, pushed, commit 185bd84)
+
+**Re-applied (2026-07-02) — PillUpdate Create/Edit label-sync fix, isolated.**
+The 2026-07-02 full revert (`git reset --hard af33ca2`, see below) dropped a
+sound, independent fix alongside the faulty link-hierarchy work: `PillUpdate`
+in `SwAddin.cs` was checking the (possibly stale) cached doc's `IsLocked`
+before `HasSaved`, so deleting the saved Robot doc attribute from the
+FeatureManager tree never re-ran `MaybeDeferLabelSync` — the ribbon's
+Create/Edit label stayed stuck. Re-applied that exact fix only (cherry-picked
+by hand from reflog commit `eb18968`, not the tree/mesh-assignment work it
+shipped alongside): check `HasSaved` first, always call
+`MaybeDeferLabelSync`, and drop+refetch the cached doc via
+`Sw2gzDocStore.Reset` if it's stale-locked while nothing is actually saved.
+470 green (unchanged), clean Release build, redeployed
+(`C:\Program Files\SW2GZ\SW2GZ.dll` 09:02:45). **Needs a live re-check**
+(delete the saved Robot attribute from the tree, confirm the ribbon label
+flips back to "Create Robot" without a doc reopen) — this bug was never
+confirmed fixed live before the revert wiped it out the first time.
+
+**FAILED + FULLY REVERTED (2026-07-02) — link hierarchy tree + manual mesh
+assignment.** Attempted: rework the Links step from the flat one-mesh-per-
+link list into a drag-to-reparent hierarchy (reusing pre-existing, unwired
+`LinkHierarchy`/`LinkTreeView`) + a geometry "pick funnel" for manual multi-
+mesh assignment, plus a `PillUpdate` ribbon label-sync bugfix. Spec/plan
+written, implemented task-by-task via subagent-driven-development (fresh
+implementer + spec-compliance review + code-quality review per task,
+including one review→fix→re-review cycle), independently build- and
+test-verified (473 green, clean Release build) at every step. **Still
+faulty live in SolidWorks** per user report after deploy — no specific
+symptom captured before the user called for a full revert, so the failure
+mode is NOT diagnosed. `git reset --hard` back to `af33ca2` (this session's
+8 commits were local-only, never pushed, so the reset was clean); DLL
+rebuilt from the reverted tree and redeployed over the faulty one. Test
+suite back to the pre-session baseline (470 green). Spec/plan docs left on
+disk for reference (`docs/superpowers/specs/2026-07-01-robot-wizard-link-
+hierarchy-design.md`, `docs/superpowers/plans/2026-07-01-robot-wizard-link-
+hierarchy.md`) but treat both as **abandoned, not pending** — the code they
+describe no longer exists on this branch. **Lesson for next attempt:** this
+is the second time in a row (see the mate-driven-detection postmortem right
+below) that a robot-wizard change passed every automated gate (build, full
+test suite, multi-stage code review) and still broke live in SW — the gap
+is entirely in COM/PMP-UI behavior that isn't and can't be exercised by
+`dotnet test`. Next attempt should get an early live checkpoint (open the
+wizard in SW after the *first* small wiring step, before piling on 3 more
+tasks on top) rather than building the whole thing then discovering it's
+broken at the end. `LinkHierarchy`/`LinkTreeView` themselves were NOT
+touched by this attempt (only called into) and remain exactly as they were
+— still inert, unwired, still a plausible starting point for a retry, just
+proven not sufficient on their own to make the wizard work correctly.
+
+**Working now, live-tested against FULL_ARM.SLDASM:** Create Robot → Links
+step (seeded from top-level components, first = base_link, rest Fixed to
+it) → Finish → Preview / Export both produce a real URDF package with
+correct mesh geometry, orientation, and per-link placement. Joint TYPE is
+hardcoded Fixed for every link (mate-driven type detection was attempted and
+reverted — see below); the joint origin/rpy is real relative pose math, not
+a placeholder.
+
+**Big finding this session — a real, pre-existing, live bug, not a Robot-only
+issue:** `Component2.Transform2.ArrayData`'s 3x3 rotation block is
+**column-major**, not row-major as every existing call site assumed
+(verified empirically against `Component2.GetBox`, since a from-VBA
+`IMathPoint.MultiplyTransform` check turned out to silently no-op instead of
+throwing — see memory `sw-mathtransform-column-major`). This was silently
+inverting rotation for any non-identity-rotated component in
+`SolidWorksMeshTessellator` (mesh baking — used by **World and Asset export
+too**) and `SolidWorksAssemblyWalker` (`TransformByComponent`/
+`RotateByComponent`). **Both fixed.** Worth a live re-check of World/Asset
+exports containing rotated components next time either is touched, since
+this shipped wrong for an unknown amount of time before today.
+
+**New files:** `SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs` (wizard, mirrors
+`Sw2gzCreateWorldPmp`'s WinForms-nav-in-PMP chrome), `SW2GZ/URDFExport/
+Sw2gzRobotExporter.cs` (writes `_ws/src//urdf/.urdf.xacro` +
+`meshes/*.dae`, reuses `SolidWorksMeshTessellator`/`SolidWorksMassProperties`),
+`SW2GZ/SwSurface/Abstractions/IComponentPoses.cs` +
+`SolidWorksComponentPoses.cs` (exact per-component rotation+translation —
+lets the exporter un-bake each link's mesh into its own native part-local
+frame instead of an AABB-center approximation, so joint origin/rpy carry the
+real relative pose between parent and child).
+
+**Tried and reverted — mate-driven joint type/axis/limit auto-detection.**
+Built `SolidWorksMateJointDetector` (ported the pre-gut `AutoJointResolver`'s
+model from git history), wired into the wizard + exporter. Live-tested by
+the user: still showed Fixed-only / wrong behavior even after two fix
+passes, so fully reverted (file deleted, wiring removed) rather than ship
+something unverified. Data model (`JointDef.Type`/`AxisX-Z`/
+`LimitLower/Upper`) still holds the fields for whenever this is retried —
+full postmortem + what to do differently next time (build against a live
+SW test loop from the start, don't port old logic blind) is in memory
+`robot-mode-dev`.
+
+**Also unresolved, not root-caused:** user reported a stale Create/Edit
+ribbon label after deleting the saved Robot doc attribute — same symptom
+class as an old, already-fixed World-mode bug. Code audit of the whole sync
+chain (`SyncRibbonToActiveDoc`/`PillUpdate`/`RefreshTabForMode`) found it
+fully mode-generic with no Robot-specific gap; could not reproduce or
+isolate a concrete defect from static reading. Needs a live repro with the
+exact symptom (button text stuck, wrong wizard data, or a crash) before
+attempting a fix.
+
+**Test count:** 464 (this branch's baseline) → 470 green.
+
+## Done — Robot mode GUTTED for clean rebuild (branch `feat/robot-mode-v2`, pushed)
Robot mode's inherited implementation was buggy (coordinate tilt the Option-A
fix on `feat/robot-mode` didn't resolve live — that branch was reset, work in
diff --git a/docs/reference/solidworks-api.md b/docs/reference/solidworks-api.md
new file mode 100644
index 0000000..94f05fc
--- /dev/null
+++ b/docs/reference/solidworks-api.md
@@ -0,0 +1,339 @@
+# SolidWorks API Reference — SW2GZ Implementation
+
+**Scope:** every SolidWorks COM API member SW2GZ's own codebase calls,
+grouped by category, with exact file provenance, usage context, and a
+plain-English explanation of what the member does. This is the *proven*
+subset — every entry has at least one real call site in this repo as of
+2026-07-01. Not a copy of the official docs; use this first, fall back to
+the official reference (link below) for anything not covered here.
+
+Sources:
+- Local offline API browser (installed with SW):
+ `C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS\api\apihelp.chm`
+- Official API guide:
+ https://help.solidworks.com/2025/english/api/sldworksapiprogguide/GettingStarted/SolidWorks_API_Getting_Started_Overview.htm
+- Official object model reference:
+ https://help.solidworks.com/2025/english/api/sldworksapi/Welcome.htm
+ (swap the year in the URL to match the installed SW version)
+- Interop assemblies referenced by `SW2GZ\SW2GZ.csproj`:
+ `C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS\api\redist\SolidWorks.Interop.{sldworks,swconst,swpublished,swcommands}.dll`
+- Upstream lineage for inherited files: [`ros/solidworks_urdf_exporter`](https://github.com/ros/solidworks_urdf_exporter)
+
+**Provenance flag used below:**
+- **[active]** — called from SW2GZ's current, non-gutted code path.
+- **[legacy]** — called only from inherited-upstream files
+ (`ExportHelperExtension.cs`, `ConfigurationSerialization.cs`,
+ `AssemblyExportForm.cs`, `ExportPropertyManager(Extension).cs`,
+ `CommonSwOperations.cs`) that predate the robot-mode v2 gut
+ (see `agent-progress/progress.md`). Still real, working COM calls — just
+ not necessarily wired into the current export pipeline. Treat as a
+ reference for *how it was done before*, not a guarantee it still runs.
+
+---
+
+## 1. App / Doc lifecycle
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `ISwAddin.ConnectToSW` | `SW\SwAddin.cs` **[active]** | entry point when SW loads the add-in; stores `SwApp`, wires `CmdMgr` + events | SW calls this to hand the add-in its `SldWorks` app object + connection cookie |
+| `ISwAddin.DisconnectFromSW` | `SW\SwAddin.cs` **[active]** | tears down command group + event handlers, releases COM refs | SW calls this on add-in unload |
+| `ISldWorks.SetAddinCallbackInfo` | `SW\SwAddin.cs` **[active]** | registers this add-in instance + cookie | tells SW which object to invoke for this add-in's UI callbacks |
+| `ISldWorks.GetCommandManager` | `SW\SwAddin.cs` **[active]** | obtains `ICommandManager` | returns the ribbon/toolbar/menu builder object |
+| `ISldWorks.ActiveDoc` | `SwAddin.cs`, `EventHandling.cs`, `ExportHelper.cs`, `ExportPropertyManager.cs`, `AssemblyExportForm.cs` **[active + legacy]** | read repeatedly | property returning the currently active open document |
+| `ISldWorks.GetFirstDocument` / `IModelDoc2.GetNext` | `SW\SwAddin.cs` **[active]** | iterates open docs to wire per-doc event handlers | walks the open-document linked list |
+| `IModelDoc2.GetType` | `SwAddin.cs`, `EventHandling.cs`, `ExportHelperExtension.cs`, `Sw2gzModelExporter.cs`, `Sw2gzModelPreviewer.cs`, `SolidWorksAssemblyWalker.cs`, `Sw2gzRibbonRegistrar.cs` **[active]** | branches Part/Assembly/Drawing handling and ribbon gating | returns integer document-type code (`swDocumentTypes_e`) |
+| `IModelDoc2.GetTitle` | `SwAddin.cs`, `ExportHelper.cs`, `ExportHelperExtension.cs`, `ExportPropertyManager.cs`, `Sw2gzDocStore.cs`, `Sw2gzExportWizardForm.cs` **[active + legacy]** | default ROS package/link name source | returns document's window/title-bar name |
+| `IModelDoc2.GetPathName` | `Sw2gzDocStore.cs`, `CommonSwOperations.cs`, `Sw2gzExportWizardForm.cs` **[active + legacy]** | keys the doc-config cache; logs component source path | returns full saved file-system path |
+| `IModelDocExtension.ActiveCommandTab` (set) | `SW\SwAddin.cs` **[active]** | reactivates the "SW2GZ" ribbon tab after mode switches | gets/sets which CommandManager tab is currently shown |
+| `IModelDoc2.GetFirstModelView` / `IModelView.GetNext` | `SW\EventHandling.cs` **[active]** | enumerates a doc's model views for event wiring | first/next graphics view of a document |
+| `ISldWorks.SendMsgToUser2` | `SwAddin.cs`, all `UI\Pmp\*.cs` **[active]** | informational popups for gating failures / page-create failure | displays a SW-styled message box |
+| `ISldWorks.GetUserProgressBar` | `URDFExport\ExportHelper.cs` **[active]** | progress-bar handle for long exports | returns SW's built-in progress-bar UI object |
+| `ISldWorks.GetMathUtility` | `URDFExport\ExportHelper.cs` **[active]** | cached once as `swMath`, used for matrix ops | returns SW's vector/matrix/transform math utility |
+| `Get/SetUserPreferenceToggle` | `URDFExport\ExportHelper.cs` **[active]** | saves/restores STL export toggles (binary/preview/positive-translate/one-file) around export | gets/sets a boolean SW app preference |
+| `Get/SetUserPreferenceIntegerValue` | `URDFExport\ExportHelper.cs` **[active]** | saves/restores STL units + quality constants | gets/sets an integer SW app preference |
+| `Get/SetUserPreferenceDoubleValue` | `URDFExport\ExportHelper.cs` **[active]** | saves/restores component-hide/view-transition speed | gets/sets a double SW app preference |
+| `ISldWorks.DefineAttribute` | `URDFExport\Sw2gzConfigSerialization.cs`, `URDFExport\Sw2gzDocSerialization.cs`, `AssemblyExportForm.cs`, `ConfigurationSerialization.cs` **[active + legacy]** | defines a custom `AttributeDef` type used to persist SW2GZ's whole doc-model on the SLDASM file | begins defining a new custom document-attribute type |
+| `AttributeDef.AddParameter` | same files **[active + legacy]** | adds `data`(string)/`date`(string)/`version`(double) fields | adds a named/typed field to an attribute definition |
+| `AttributeDef.Register` | same files **[active + legacy]** | finalizes definition before instancing | registers the attribute type with SolidWorks |
+| `AttributeDef.CreateInstance5` | same files **[active + legacy]** | `(model, null template, name, options, swAllConfiguration)` | creates an attribute-feature instance on a model doc |
+| `SolidWorks.Interop.sldworks.Attribute.GetName` | same files **[active + legacy]** | compared against target attribute name while scanning features | reads the attribute feature's registered name |
+| `Attribute.GetParameter` | same files **[active + legacy]** | fetches `data`/`date`/`version` `Parameter` objects | opens one field of the hidden data tag |
+| `Parameter.GetStringValue` / `.GetDoubleValue` | same files **[active + legacy]** | reads serialized XML payload / numeric version | reads a string/double attribute parameter value |
+| `Parameter.SetStringValue2` / `.SetDoubleValue2` | same files **[active + legacy]** | writes with `swAllConfiguration` scope | writes a string/double attribute parameter value |
+| `Feature.Select2` | `Sw2gzDocSerialization.cs` **[active]** | selects the attribute feature, append flag + mark | selects the feature in the model so it can be deleted |
+| `ModelDoc2.EditDelete` | `Sw2gzDocSerialization.cs` **[active]** | deletes current selection | deletes the selected feature (removes an old attribute tag) |
+| `IModelDoc2.ConfigurationManager` → `.ActiveConfiguration` | `ExportHelperExtension.cs` **[legacy]** | reads active `Configuration` before applying "URDF Export" display state | exposes the doc's current active configuration |
+| `Configuration.GetDisplayStates` / `.ApplyDisplayState` | `ExportHelperExtension.cs` **[legacy]** | switches to a dedicated display state before geometry walk | lists/activates a configuration's named display states |
+| `IModelDoc2.MaterialPropertyValues` | see §7 Color/appearance | | |
+| `IModelDoc2.ClearSelection2` / `GraphicsRedraw2` | multiple, see §5/§8 | | |
+
+## 2. Ribbon / CommandManager
+
+All **[active]**, all in `UI\Ribbon\Sw2gzRibbonRegistrar.cs` + `SW\SwAddin.cs`.
+
+| API member | Usage | What it does |
+|---|---|---|
+| `ICommandManager.GetGroupDataFromRegistry` | probes cached registry IDs before forcing a fresh group | checks if SW has stale cached command-group data |
+| `ICommandManager.CreateCommandGroup2` | builds the "SW2GZ" command group, `ignorePrevious=true` | creates a new set of ribbon/toolbar commands |
+| `ICommandManager.RemoveCommandGroup` | `SwAddin.cs`, on disconnect | tears down a previously-registered command group |
+| `ICommandManager.GetCommandTab` | resolves live tab handle for assembly + part doc types by title | gets an existing ribbon tab for a document type |
+| `ICommandManager.AddCommandTab` | called for both assembly and part doc types | creates a new ribbon command tab |
+| `ICommandManager.RemoveCommandTab` | drops existing tab before full rebuild | removes a ribbon command tab |
+| `ICommandGroup.IconList` | set to sprite-strip PNG paths | per-button glyph images |
+| `ICommandGroup.MainIconList` | set to single-glyph cube icon set | the command-group's own toolbar glyph |
+| `ICommandGroup.AddCommandItem2` | called 18× with name/tip/callback/enable/image/userId | registers one clickable command button |
+| `ICommandGroup.HasToolbar` / `.HasMenu` | `HasToolbar=true`, `HasMenu=false` | toolbar vs menu display mode |
+| `ICommandGroup.Activate` | must run before `get_CommandID` resolves valid IDs | finalizes/registers the command group |
+| `ICommandGroup.get_CommandID` | called post-`Activate` for every `AddCommandItem2` index | resolves a button's internal command ID |
+| `CommandTab.AddCommandTabBox` | per cluster (mode-start/actions/mode-cluster/part boxes) | adds a grouped-button box to a ribbon tab |
+| `CommandTab.RemoveCommandTabBox` | wrapped in try/catch during `RefreshTabForMode` swap | removes a button box from a ribbon tab |
+| `ICommandTabBox.AddCommands` | takes parallel `cmdId[]` + `textType[]` arrays | populates a ribbon box with specific buttons + label styles |
+
+## 3. PropertyManagerPage (native wizard UI) — **[active]**
+
+Files: `UI\Pmp\Sw2gzCreateWorldPmp.cs`, `Sw2gzCreateAssetPmp.cs`,
+`Sw2gzWorldSensorsPmp.cs`, `Sw2gzWorldSettingsPmp.cs`, `Sw2gzStubPmp.cs`,
+plus legacy-pipeline `URDFExport\ExportPropertyManager.cs` /
+`GeometryPropertyManager.cs` **[legacy]**.
+
+### 3.1 Page scaffolding
+| API member | Usage | What it does |
+|---|---|---|
+| `SldWorks.CreatePropertyManagerPage` | every PMP class | creates a `PropertyManagerPage2` bound to an `IPropertyManagerPage2Handler9` COM callback object |
+| `PropertyManagerPage2.AddGroupBox` | builds each step/section group | adds a collapsible titled group box |
+| `PropertyManagerPage2.Show2` | every wizard's `Show()` wrapper, called with `0` | displays the page in SW's left dock |
+| `PropertyManagerPage2.Close` | `GoNext` on final step / OK-Cancel | programmatically closes the PMP as if OK/Cancel pressed |
+| `PropertyManagerPage2.SetCursor` | `ExportPropertyManager.cs` **[legacy]** | advances focus after RMB pick (`swPropertyManagerPageCursors_Advance`) |
+| `PropertyManagerPage2.SetFocus` | `ExportPropertyManager.cs` **[legacy]** | focuses the embedded `WindowFromHandle` tree control |
+
+### 3.2 Controls
+| Control type | Key members | What it does |
+|---|---|---|
+| `PropertyManagerPageGroup` | `.AddControl2(id, type, caption, align, options, tip)`, `.Visible` | adds a widget into a group / toggles a wizard "page" |
+| `PropertyManagerPageSelectionbox` | `.SingleEntityOnly`, `.Height`, `.Mark`, `.SetSelectionFilters`, `.AllowMultipleSelectOfSameEntity`, `.AllowSelectInMultipleBoxes`, `.SetSelectionFocus` | native viewport-pick control; `Mark` tags it so later `GetSelectedObjectCount2/6` calls can target this specific box |
+| `PropertyManagerPageListbox` | `.Height`, `.Clear`, `.AddItems`, `.CurrentSelection` | multi-row string list (used for World-mode asset list) |
+| `PropertyManagerPageCombobox` | `.AddItems`, `.CurrentSelection`, `.Style` (`swPropMgrPageComboBoxStyle_EditBoxReadOnly`), `.Height`, `.EditText`, `.get_ItemText`, `.Clear` | dropdown; `Style` forces read-only selection-only mode |
+| `PropertyManagerPageNumberbox` | `.SetRange2(unitType, min, max, resolution, ...)`, `.Value` | numeric spinner with range/precision/increment |
+| `PropertyManagerPageCheckbox` | `.Checked` | boolean toggle (static/friction/compute-inertia in SW2GZ) |
+| `PropertyManagerPageTextbox` | `.Text` | free-text field |
+| `PropertyManagerPageLabel` | `.Caption` | text label, updated live for status/step-description text |
+| `PropertyManagerPageWindowFromHandle` | `.SetWindowHandlex64(hwnd)`, `.Height` | embeds an arbitrary WinForms panel/TreeView HWND inside the native page — **this is how SW2GZ's actual wizard chrome (Back/Next nav bar, dark theme) works**, since native PMP buttons crash SW (see 3.4) |
+| `IPropertyManagerPageControl` (base, cast target) | `.Enabled`, `.Visible`, `.Width`, `.Tip` | generic control properties any typed control also exposes |
+
+### 3.3 `IPropertyManagerPage2Handler9` callback interface
+
+The mandatory COM callback contract every PMP class implements. In SW2GZ,
+almost all ~35 members are **intentional no-op stubs** — only a handful
+carry real logic:
+
+| Member | SW2GZ behavior |
+|---|---|
+| `AfterActivation` | fires once page is active; World/Asset PMPs call `ShowStep()` here |
+| `OnClose` | reads `Reason` (`swPropertyManagerPageClose_Okay`/`_Cancel`) to decide commit-vs-`Sw2gzDocSnapshot.Restore` rollback |
+| `AfterClose` | invokes the `onCommit`/`onClosed` continuation callback once, with the live doc if OK |
+| `OnButtonPress` | **intentionally no-op** — native PMP buttons corrupt SW's renderer (burned-in gotcha, see §3.4); all button logic lives in the WinForms nav bar instead |
+| `OnComboboxSelectionChanged` | no-op in most PMPs; real logic only in `GeometryPropertyManager.cs` **[legacy]** (drives `GoToLink`) |
+| `OnCheckboxCheck` | no-op — native checkboxes AV-crash SW (see §3.4); state read on `Close`/commit instead |
+| `OnSelectionboxListChanged` | wires `UpdateSelCount` / cursor-advance in the legacy pipeline |
+| `OnSubmitSelection` | returns `true` (accept all); `GeometryPropertyManager.cs` **[legacy]** validates entity type before accepting |
+| `OnTextboxChanged` | no-op in current PMPs; legacy pipeline live-syncs link name into tree node |
+| `OnNumberboxChanged` | no-op in current PMPs; legacy pipeline triggers `CreateNewNodes` on child-count change |
+| All others (`OnGainedFocus/OnLostFocus/OnHelp/OnNextPage/OnPreviousPage/OnPreview/OnTabClicked/OnKeystroke/OnSelectionboxFocusChanged/OnSelectionboxCalloutCreated/Destroyed/OnNumberBoxTrackingCompleted/OnComboboxEditChanged/OnListboxSelectionChanged/OnListboxRMBUp/OnGroupCheck/OnGroupExpand/OnOptionCheck/OnPopupMenuItem/OnPopupMenuItemUpdate/OnSliderPositionChanged/OnSliderTrackingCompleted/OnRedo/OnUndo/OnWhatsNew/OnWindowFromHandleControlCreated/OnActiveXControlCreated`) | mandatory interface stubs, unused — required because the interface must be fully implemented even for events SW2GZ never needs |
+
+### 3.4 Gotchas burned in (don't relitigate these)
+
+- **Native PMP buttons (`swControlType_Button`) corrupt SW's PMP
+ renderer** when the click handler mutates PMP state — buttons vanish,
+ multi-select glitches, theme breaks. Fix used everywhere: host the
+ wizard's actual Back/Next/action buttons in a WinForms panel embedded
+ via `PropertyManagerPageWindowFromHandle.SetWindowHandlex64`, deferred
+ with `BeginInvoke` to escape click-handler re-entrancy.
+- **Native PMP checkboxes AV-crash SW** on toggle even with an empty
+ `OnCheckboxCheck` handler — same WinForms-embedding fix.
+- **`internal` ComVisible classes are NOT exposed via CCW.** All PMP
+ classes must be `public sealed` or `CreatePropertyManagerPage`'s handler
+ param silently throws `InvalidCastException`.
+- **`AddGroupBox` needs `swGroupBoxOptions_Visible | swGroupBoxOptions_Expanded`** — passing `0` renders an empty collapsed shell.
+
+## 4. Assembly / Component structure
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `AssemblyDoc.GetComponents` | `SolidWorksAssemblyWalker.cs`, `SolidWorksMeshTessellator.cs`, `SolidWorksMassProperties.cs`, `Sw2gzCreateWorldPmp.cs`, `SwJointStateSampler.cs`, `CommonSwOperations.cs` **[active + legacy]** | root traversal (top-level or all, via `bool` arg) | lists the assembly's component instances |
+| `Component2.GetChildren` | walker, tessellator, mass-props **[active]** | recursion into sub-assembly contents | child `Component2[]` of a component |
+| `Component2.Name2` | everywhere **[active + legacy]** | sanitized link/leaf identifier, dedup key | instance-unique component name |
+| `Component2.GetParent` | walker, `ExportHelperExtension.cs` **[active + legacy]** | walks up to find top-level owning component | immediate parent `Component2`, null at top level |
+| `Component2.GetModelDoc2` / `.GetModelDoc` | tessellator, walker, `EventHandling.cs`, `ExportHelper.cs` **[active + legacy]** | checks doc type / opens underlying doc | `IModelDoc2` referenced by a component |
+| `Component2.GetBodies2` / `.GetBodies3` | `SolidWorksMeshTessellator.cs` **[active]**, `ExportHelperExtension.cs` **[legacy]** | filtered by `swSolidBody` | solid bodies owned by the component |
+| `Component2.IsSuppressed` | tessellator, `Sw2gzCreateWorldPmp.cs` **[active]** | filters suppressed comps out of auto-seeded asset list / mesh export | whether component is currently suppressed |
+| `Component2.IsHidden` | `CommonSwOperations.cs` **[legacy]** | builds hidden-component exclusion list before STL export | whether component is currently hidden |
+| `Component2.IsFixed` | `ExportHelperExtension.cs` **[legacy]** | DOF-probe prep | whether component is rigidly fixed |
+| `Component2.Transform2` → `MathTransform.ArrayData` | tessellator, joint sampler, walker, `MathOPS.cs` **[active]**; `ExportHelperExtension.cs` **[legacy]** | **the central geometric primitive** — maps part-local mesh/mate/axis data into assembly frame | component's placement transform (3×3 rotation + translation + scale, 16 doubles row-major) |
+| `Component2.GetMaterialPropertyValues2` | `SolidWorksMeshTessellator.cs` **[active]** | instance color override, preferred over part material | `double[9]`: R,G,B,Ambient,Diffuse,Specular,Shininess,Transparency,Emission |
+| `Component2.GetMates` | `ExportHelperExtension.cs` **[legacy]** | finds/suppresses limit mates before DOF probing | mate features referencing this component |
+| `Component2.GetID` | `ExportHelperExtension.cs` **[legacy]** | logging during fix/unfix toggling | numeric ID uniquely identifying the component instance |
+| `Component2.Select4` | walker, `CommonSwOperations.cs`, `AssemblyExportForm.cs`, `ExportPropertyManagerExtension.cs` **[active + legacy]** | highlights a link's owning component in viewport | adds/replaces component in current selection |
+| `Component2.GetBox` | `ExportHelperExtension.cs` **[legacy]** | clamps auto-generated joint origin inside bounding box | axis-aligned bounding-box corners |
+| `AssemblyDoc.ResolveAllLightWeightComponents` | `ExportPropertyManager.cs` **[legacy]** | pre-export prep, `ResolveAllLightWeightComponents(true)` | forces all lightweight components to fully resolve/load |
+| `AssemblyDoc.FixComponent` / `.UnfixComponent` | `ExportHelperExtension.cs` **[legacy]** | temporarily fixes parent chain to isolate one component's free DOF | locks/unlocks a component's degrees of freedom |
+| `Component2.GetRemainingDOFs` *(undocumented API)* | `ExportHelperExtension.cs` **[legacy]** | many out-params (`R1Status`, `RPoint1`, `RDir1`, ...) — auto-detects joint type/axis/origin from unconstrained DOF | computes the unconstrained degrees of freedom of a component |
+| `IPartDoc.GetBodies2` | `SolidWorksMeshTessellator.cs` **[active]** | solid bodies of a standalone part doc | bodies contained directly in a part (Asset-mode whole-part export) |
+
+## 5. Mates
+
+All `SolidWorksAssemblyWalker.cs` **[active]** unless noted.
+
+| API member | Usage | What it does |
+|---|---|---|
+| `IModelDoc2.FirstFeature` | entry point for feature-tree walk | first `Feature` in the model tree |
+| `Feature.GetNextFeature` | iterates top-level feature tree | next sibling feature |
+| `Feature.GetFirstSubFeature` / `.GetNextSubFeature` | iterates mate features nested under the `MateGroup` folder | first/next child feature under a parent |
+| `Feature.GetTypeName2` | identifies `"MateGroup"` | internal type-name string of a feature |
+| `Feature.GetSpecificFeature2` | casts generic `Feature` → `Mate2` | the type-specific object wrapped by a feature |
+| `Feature.Name` | compared against user-picked mate name | feature's display name |
+| `Mate2.Type` | primary signal for mate→joint-kind classification (`swMateType_e`: CONCENTRIC, COINCIDENT, DISTANCE, ANGLE, SLOT, LOCK) | the mate's type constant |
+| `Mate2.MaximumVariation` / `.MinimumVariation` | detects limit mates → derives joint limits | upper/lower travel range of a limit mate |
+| `Mate2.Flipped` | `ExportHelperExtension.cs` **[legacy]**, sign convention for joint limits | whether mate alignment is reversed |
+| `Mate2.GetMateEntityCount` | iterates coupled geometric references | count of `MateEntity2` in a mate |
+| `Mate2.MateEntity(i)` | fetches each entity | `MateEntity2` at index `i` |
+| `MateEntity2.ReferenceComponent` | identifies owning component for parent/child link resolution | the `Component2` owning a mate entity's reference |
+| `MateEntity2.Reference` | actual selectable geometry behind a mate entity | `Face2`/`Edge`/`Entity` referenced |
+| `MateEntity2.EntityParams` | generic origin+direction fallback when typed extraction fails | `double[6]`: origin + direction |
+| `Face2.GetSurface` | classifies mate reference faces plane vs cylinder | underlying `Surface` geometry object |
+| `Surface.IsPlane` / `.IsCylinder` | face classification | boolean type check |
+| `Surface.PlaneParams` | `double[6]`: normal + point | flat face's orientation |
+| `Surface.CylinderParams` | `double[7]`: origin + axis-direction + radius | cylinder's centerline location + direction — the primary axis-extraction path for concentric (revolute/continuous) joints |
+| `IEdge.GetCurveParams2` | `ExportHelperExtension.cs` **[legacy]**, fallback path | edge-midpoint mate reference when face extraction fails |
+| `Entity.Select4` | highlights the mate's reference geometry on screen | selects a generic `Entity` in the viewport |
+| `IModelDoc2.GraphicsRedraw2` | forces viewport redraw after highlighting mate geometry | redraws the 3D graphics view |
+| `Feature.Select` (Mate2-as-Feature) | `ExportHelperExtension.cs` **[legacy]** | selects a mate/feature in the tree |
+| `Feature.SetSuppression2` (Mate2-as-Feature) | `ExportHelperExtension.cs` **[legacy]**, around DOF probing | suppresses/unsuppresses a mate/feature |
+
+## 6. Geometry / tessellation
+
+All `SolidWorksMeshTessellator.cs` **[active]**.
+
+| API member | Usage | What it does |
+|---|---|---|
+| `Body2.GetTessellation(null)` | requests tessellation of all faces of a solid body | creates an `ITessellation` object for the body |
+| `ITessellation.NeedVertexNormal` / `.NeedFaceFacetMap` / `.NeedEdgeFinMap` | all set `false` to reduce overhead | flags controlling optional tessellation data generation |
+| `ITessellation.Tessellate()` | triggers computation, returns `bool` success | performs the triangulation algorithm |
+| `ITessellation.GetFacetCount()` | drives the triangle-emission loop | number of triangular facets produced |
+| `ITessellation.GetFacetFins(f)` | per-facet lookup | `int[3]` fin (edge) IDs for facet `f` |
+| `ITessellation.GetFinVertices(fin)` | per-fin lookup | `int[2]` vertex IDs for a fin |
+| `ITessellation.GetVertexPoint(v)` | part-local coords, baked into assembly frame via `Component2.Transform2` | `double[3]` XYZ of vertex `v` |
+
+## 7. Mass properties
+
+`SolidWorksMassProperties.cs` **[active]**, `ExportHelperExtension.cs` **[legacy]**.
+
+| API member | Usage | What it does |
+|---|---|---|
+| `ModelDoc2.Extension` → `ModelDocExtension.CreateMassProperty` | creates an `IMassProperty` calculator scoped to current selection/config | sets up mass-properties calculation |
+| `IMassProperty.Mass` | checked `≤0` to detect missing material | total mass in kg |
+| `IMassProperty.CenterOfMass` | `double[3]` | XYZ center-of-mass coordinates, baked into `Link.Inertial.Origin` |
+| `IMassProperty.GetMomentOfInertia(swMassPropertyMomentAboutCenterOfMass)` | `double[9]` | inertia tensor about a reference frame |
+| `MassProperty.SetCoordinateSystem(MathTransform)` | `ExportHelperExtension.cs` **[legacy]**, scopes calc to a joint's frame | sets the reference coordinate system for mass calculations |
+| `MassProperty.AddBodies(Body2[])` | `ExportHelperExtension.cs` **[legacy]**, restricts calc to per-link body subset | adds specific solid bodies to the mass-property calculation set |
+
+## 8. Coordinate systems / reference geometry
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `IModelDocExtension.GetCoordinateSystemTransformByName` | `ExportHelper.cs`, `ExportHelperExtension.cs` **[active + legacy]** | primary read path — resolves a link's joint-origin coordinate system by name | `MathTransform` of a named coordsys feature |
+| `RefAxis.GetRefAxisParams` | `ExportHelperExtension.cs` **[legacy]** | `{startX,Y,Z,endX,Y,Z}` | start/end point coordinates defining a reference axis |
+| `FeatureManager.GetFeatures` filtered by `GetTypeName2() == "CoordSys"/"RefAxis"` | `ExportHelperExtension.cs` **[legacy]** | discovery, feature-tree search incl. sub-components | enumerates named ref-geometry features |
+| `IMathTransform.ArrayData` | tessellator, walker, joint sampler, `CylinderTransform.cs`, `ExportHelperExtension.cs`, `MathOPS.cs` **[active + legacy]** | **the shared numeric primitive threading through the whole export pipeline** | 16-double row-major rotation+translation+scale+padding |
+| `IMathTransform.Multiply` | `ExportHelperExtension.cs` **[legacy]** | composes coordsys-local transform with component's `Transform2` | multiplies (composes) two transforms |
+| `IMathPoint.ArrayData` / `IMathVector.ArrayData` | `ExportHelperExtension.cs` **[legacy]** | reads out-params from `GetRemainingDOFs` directly | coordinate/vector components |
+| `IFeatureManager.InsertCoordinateSystem(false,false,false)` | `ExportHelperExtension.cs` **[legacy]**, authoring | creates a coordinate-system feature from 3 selected sketch points | inserts a new `Feature` |
+| `IModelDoc2.InsertAxis2(true)` | `ExportHelperExtension.cs` **[legacy]**, authoring | creates a reference axis from a selected sketch line | inserts a reference-axis feature |
+| `IModelDoc2.SketchManager` → `.Insert3DSketch(true)` / `.ActiveSketch` | `ExportHelperExtension.cs` **[legacy]** | opens/closes a 3D sketch for editing ("URDF Reference" construction geometry) | accesses 2D/3D sketch creation API |
+| `SketchManager.CreatePoint(x,y,z)` | `ExportHelperExtension.cs` **[legacy]** | returns `SketchPoint` | creates a 3D sketch point |
+| `SketchManager.CreateLine(x1,y1,z1,x2,y2,z2)` | `ExportHelperExtension.cs` **[legacy]** | returns `SketchSegment` | creates a line segment in the active sketch |
+| `SketchSegment.ConstructionGeometry` (set `true`) | `ExportHelperExtension.cs` **[legacy]** | marks a sketch line non-solid | construction geometry flag |
+| `SketchSegment.Width` | `ExportHelperExtension.cs` **[legacy]** | set to `2` | display line width |
+| `SketchSegment.Select4` / `SketchPoint.Select4` | `ExportHelperExtension.cs` **[legacy]** | selects sketch geometry via `SelectData.Mark` | selects using selection-mark data |
+| `IModelDocExtension.SelectByID2` | `ExportHelperExtension.cs`, `ExportPropertyManagerExtension.cs` **[legacy]** | selects named `COORDSYS`/`AXIS`/`SKETCH`/`ATTRIBUTE`/`EXTSKETCHPOINT` entities before authoring ops | selects a named entity by type string |
+| `FeatureManager.InsertFeatureTreeFolder2` / `.MoveToFolder` | `ExportPropertyManagerExtension.cs` **[legacy]** | organizes newly-created ref-geometry into a folder | creates/moves features into a named feature-tree folder |
+| `Feature.Name` (set) | `ExportHelperExtension.cs` **[legacy]** | names newly created coordsys/axis features | sets a feature's display name |
+
+## 9. Selection
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `IModelDoc2.SelectionManager` | `SwViewportSelectionService.cs`, `SolidWorksAssemblyWalker.cs`, `AssemblyExportForm.cs`, `CommonSwOperations.cs`, `GeometryPropertyManager.cs`, `Sw2gzCreateWorldPmp.cs`, `Sw2gzCreateAssetPmp.cs` **[active + legacy]** | gateway | returns `ISelectionMgr` |
+| `ISelectionMgr.GetSelectedObjectCount2(mark)` | same set | counts current selection filtered by mark (`-1` = all) | how many items are selected under a group |
+| `ISelectionMgr.GetSelectedObject6(index, mark)` | same set | retrieves the actual selected entity | raw COM object at index/mark |
+| `ISelectionMgr.GetSelectedObjectType3(index, mark)` | `SwViewportSelectionService.cs`, `GeometryPropertyManager.cs` **[active + legacy]** | distinguishes body/surface/component selection types | `swSelectType_e` of the object |
+| `ISelectionMgr.CreateSelectData()` | `AssemblyExportForm.cs`, `CommonSwOperations.cs`, `ExportHelperExtension.cs` **[legacy]** | builds `SelectData` for tagging selections | creates a selection-data options object |
+| `SelectData.Mark` (set) | same **[legacy]** | tags a selection batch for later filtered read-back | integer "selection group" tag |
+| `IModelDoc2.ClearSelection2(bool)` | everywhere **[active + legacy]** | reset before re-highlighting or fresh pick | clears current viewport selection |
+| `IModelDocExtension.SelectByID2` | see §8 | | |
+| `Entity.Select4(append, selectData)` | walker, `CommonSwOperations.cs`, `AssemblyExportForm.cs`, `ExportHelperExtension.cs` **[active + legacy]** | the viewport-highlight primitive used throughout | selects/appends an entity in the viewport |
+
+## 10. Events
+
+All `SW\EventHandling.cs` **[active]**, wired from `SW\SwAddin.cs`.
+
+| API member | SW2GZ handler | What it does |
+|---|---|---|
+| `DSldWorksEvents.ActiveDocChangeNotify` | `OnDocChange` → `SyncRibbonToActiveDoc` | fires when the active document switches |
+| `DSldWorksEvents.DocumentLoadNotify2` | `OnDocLoad` (no-op) | fires while a document is being loaded |
+| `DSldWorksEvents.FileNewNotify2` | `OnFileNew`, re-walks open docs to attach handlers | fires when a new document is created |
+| `DSldWorksEvents.ActiveModelDocChangeNotify` | `OnModelChange` (no-op) | fires when the active model document changes |
+| `DSldWorksEvents.FileOpenPostNotify` | re-attaches doc events + syncs ribbon | fires after a file finishes opening |
+| `DPartDocEvents.DestroyNotify` | `OnDestroy` in `PartEventHandler`, detaches all handlers | fires when a part document is closed |
+| `DPartDocEvents.NewSelectionNotify` | static `OnNewSelection` (no-op) | fires when part-doc selection changes |
+| `DAssemblyDocEvents.DestroyNotify` | `OnDestroy` in `AssemblyEventHandler` | fires when an assembly is closed |
+| `DAssemblyDocEvents.NewSelectionNotify` | static `OnNewSelection` (no-op) | fires when assembly selection changes |
+| `DAssemblyDocEvents.ComponentStateChangeNotify2` | gives old+new suppression state, routes to attach handlers when a component resolves | fires when a component's resolved/suppressed state changes |
+| `DAssemblyDocEvents.ComponentStateChangeNotify` | legacy variant, bound alongside v2 | older component-state-change notification |
+| `DAssemblyDocEvents.ComponentVisualPropertiesChangeNotify` | resolves component's ModelDoc2, treated as state change | fires when a component's visual/display properties change |
+| `DAssemblyDocEvents.ComponentDisplayStateChangeNotify` | treated as state change | fires when a component's display state changes |
+| `DDrawingDocEvents.DestroyNotify` | detaches handlers for closing drawing | fires when a drawing is closed |
+| `DDrawingDocEvents.NewSelectionNotify` | no-op | fires when drawing selection changes |
+| `DModelViewEvents.DestroyNotify2` | static `OnDestroy` in `DocView` (no-op) | fires when a graphics view is destroyed |
+| `DModelViewEvents.RepaintNotify` | static `OnRepaint` (no-op) | fires when a graphics view repaints |
+
+## 11. Color / appearance
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `Component2.GetMaterialPropertyValues2(swThisConfiguration, null)` | `SolidWorksMeshTessellator.cs` **[active]** | instance-level appearance override, preferred over part material | `double[9]` per-component color/material override |
+| `IModelDoc2.MaterialPropertyValues` | `SolidWorksMeshTessellator.cs` **[active]**, `ExportHelperExtension.cs` **[legacy]** | fallback when no instance override exists | `double[9]` part-doc base material array |
+
+Both arrays: `[R, G, B, Ambient, Diffuse, Specular, Shininess, Transparency, Emission]`.
+
+## 12. Persistence / utility
+
+| API member | File(s) | Usage | What it does |
+|---|---|---|---|
+| `ModelDocExtension.GetPersistReference3` | `CommonSwOperations.cs` **[legacy]** | converts a `Component2` reference into a durable PID | persistent byte-array ID for a model object |
+| `ModelDocExtension.GetObjectByPersistReference3` | `CommonSwOperations.cs` **[legacy]** | resolves a saved PID back into a live `Component2` | round-trips a persistent-reference ID back to a live COM object |
+| `swPersistReferencedObjectStates_e` | `CommonSwOperations.cs` **[legacy]** | branches logging when a saved PID fails to resolve | enumerates why a persistent-reference lookup failed |
+| `IModelDoc2.ShowComponent2` / `.HideComponent2` | `CommonSwOperations.cs` **[legacy]** | acts on current selection around per-link mesh export | shows/hides currently-selected components |
+| `swComponentSuppressionState_e` | `SW\EventHandling.cs` **[active]** | interprets `ComponentStateChangeNotify` event payload | enumerates resolved/suppressed/lightweight states |
+| `Marshal.ReleaseComObject` *(.NET interop, not SW API)* | `SolidWorksMeshTessellator.cs`, `SolidWorksMassProperties.cs`, `SolidWorksAssemblyWalker.cs` **[active]** | manually frees a COM object, always in `finally` | avoids leaking SolidWorks COM handles — copy this pattern for any new COM-touching code |
+
+---
+
+## Confirmed NOT used (checked, zero hits)
+
+Grepped for and came back empty — SW2GZ's problem domain never needed these:
+
+- `ISldWorks.OpenDoc6` / `CloseDoc` — SW2GZ only ever reads `ActiveDoc`, never opens/closes docs programmatically
+- `ISldWorks.EnableSelection`
+- `IRenderMaterial` / `GetRenderMaterial` / `SetMaterialPropertyValues` — modern PBR appearance API; SW2GZ uses the legacy `double[9]` property instead
+- `ICoordinateSystemFeatureData` — structured coordsys feature edit; SW2GZ reads via `GetCoordinateSystemTransformByName` only
+
+## Not touched at all (whole subsystems)
+
+Drawings/views, sheet metal, weldments, configuration authoring beyond
+`ActiveConfiguration`/display-state read, custom properties, equations,
+Toolbox, Routing, Costing, Simulation/Motion Study API, PDM. The
+corresponding interop DLLs (`SolidWorks.Interop.SWRoutingLib`,
+`.sldcostingapi`, `.sldtoolboxconfigureaddin`, `.swmotionstudy`,
+`.sustainability`, etc.) sit in
+`C:\Program Files\SOLIDWORKS Corp\SOLIDWORKS\api\redist\` unused — not
+referenced by `SW2GZ.csproj`.
diff --git a/docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md b/docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md
new file mode 100644
index 0000000..bcd11a1
--- /dev/null
+++ b/docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md
@@ -0,0 +1,1282 @@
+# Robot joint/link relative pose + multi-mesh links Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** Make the robot exporter's joint origin math parent-relative (it's
+currently always root-relative), detect the tree's root by structure instead
+of list position, and honor every mesh component a link has been assigned
+(currently only the first is used) — for both the mesh geometry and the mass
+properties.
+
+**Architecture:** `Sw2gzRobotExporter.Export` moves from a single pass that
+assumes list position `[0]` is the root and every link's parent is that
+root, to a two-pass approach: pass 1 reads every link's own reference pose
+once (order-independent); pass 2 uses that lookup to compute each link's
+joint relative to its *own* `ParentName`, and unions/combines every
+assigned mesh component and mass property into that link's reference
+frame. `InertialAggregator` gains a `Matrix3`-parameterized twin of its
+existing `Quaternion`-based `Combine` overloads (shared core, no new
+coordinate-conversion code) so the exporter never needs to leave
+`Matrix3`/`Vector3` space. Small UI addition in the wizard surfaces which
+assigned component is "primary" (defines the link's frame), reusing
+`LinkTreeView`'s already-wired-but-unused tooltip support.
+
+**Tech Stack:** C# / .NET Framework (add-in, `SW2GZ.csproj`) + .NET 8 (test
+project, `SW2GZ.Writers.Test.csproj`, xUnit). No new dependencies, no new
+files added to any `.csproj` (Test project is SDK-style — new test files
+under the project directory are auto-included).
+
+**Reference:** [`docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md`](../specs/2026-07-02-robot-joint-relative-pose-design.md)
+
+---
+
+### Task 1: `InertialAggregator` — Matrix3-parameterized `Combine` overloads
+
+**Files:**
+- Modify: `SW2GZ/Build/InertialAggregator.cs`
+- Test (new file, auto-included): `Test/Build/InertialAggregatorMatrixTests.cs`
+
+- [ ] **Step 1: Write the failing tests**
+
+Create `Test/Build/InertialAggregatorMatrixTests.cs`:
+
+```csharp
+using System.Collections.Generic;
+using System.Numerics;
+using SW2GZ.Build;
+using SW2GZ.Math;
+using Xunit;
+
+namespace SW2GZ.Build.Tests
+{
+ public class InertialAggregatorMatrixTests
+ {
+ private static Matrix3 RotZ(double radians)
+ {
+ double c = System.Math.Cos(radians), s = System.Math.Sin(radians);
+ return new Matrix3(c, -s, 0, s, c, 0, 0, 0, 1);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_MatchesQuaternionOverload_IdentityRotation()
+ {
+ var p = new MassProps(1.0, Vector3.Zero, Matrix3.Identity);
+ var posA = new Vector3(-1, 0, 0);
+ var posB = new Vector3(1, 0, 0);
+
+ var quaternionParts = new List<(MassProps, Pose)>
+ {
+ (p, new Pose(posA, Quaternion.Identity)),
+ (p, new Pose(posB, Quaternion.Identity)),
+ };
+ var matrixParts = new List<(MassProps, Matrix3, Vector3)>
+ {
+ (p, Matrix3.Identity, posA),
+ (p, Matrix3.Identity, posB),
+ };
+
+ MassProps viaQuaternion = InertialAggregator.Combine(quaternionParts);
+ MassProps viaMatrix3 = InertialAggregator.Combine(matrixParts);
+
+ Assert.Equal(2.0, viaMatrix3.Mass);
+ Assert.Equal(viaQuaternion.Mass, viaMatrix3.Mass);
+ Assert.Equal(viaQuaternion.ComLocal.X, viaMatrix3.ComLocal.X, 9);
+ Assert.Equal(viaQuaternion.ComLocal.Y, viaMatrix3.ComLocal.Y, 9);
+ Assert.Equal(viaQuaternion.ComLocal.Z, viaMatrix3.ComLocal.Z, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M11, viaMatrix3.InertiaAtComLocal.M11, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M22, viaMatrix3.InertiaAtComLocal.M22, 9);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M33, viaMatrix3.InertiaAtComLocal.M33, 9);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_MatchesQuaternionOverload_NonIdentityRotation()
+ {
+ var inertia = new Matrix3(1.5, 0, 0, 0, 2.0, 0, 0, 0, 2.5);
+ var qA = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, 0.4f);
+ var qB = Quaternion.CreateFromAxisAngle(Vector3.UnitY, -0.7f);
+ var posA = new Vector3(0.3f, 0.2f, -0.1f);
+ var posB = new Vector3(-0.2f, 0.5f, 0.4f);
+
+ var quaternionParts = new List<(MassProps, Pose)>
+ {
+ (new MassProps(1.0, Vector3.Zero, inertia), new Pose(posA, qA)),
+ (new MassProps(2.0, Vector3.Zero, inertia), new Pose(posB, qB)),
+ };
+ var matrixParts = new List<(MassProps, Matrix3, Vector3)>
+ {
+ (new MassProps(1.0, Vector3.Zero, inertia), Matrix3.FromQuaternion(qA), posA),
+ (new MassProps(2.0, Vector3.Zero, inertia), Matrix3.FromQuaternion(qB), posB),
+ };
+
+ MassProps viaQuaternion = InertialAggregator.Combine(quaternionParts);
+ MassProps viaMatrix3 = InertialAggregator.Combine(matrixParts);
+
+ Assert.Equal(viaQuaternion.Mass, viaMatrix3.Mass, 9);
+ Assert.Equal(viaQuaternion.ComLocal.X, viaMatrix3.ComLocal.X, 6);
+ Assert.Equal(viaQuaternion.ComLocal.Y, viaMatrix3.ComLocal.Y, 6);
+ Assert.Equal(viaQuaternion.ComLocal.Z, viaMatrix3.ComLocal.Z, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M11, viaMatrix3.InertiaAtComLocal.M11, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M22, viaMatrix3.InertiaAtComLocal.M22, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M33, viaMatrix3.InertiaAtComLocal.M33, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M12, viaMatrix3.InertiaAtComLocal.M12, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M13, viaMatrix3.InertiaAtComLocal.M13, 6);
+ Assert.Equal(viaQuaternion.InertiaAtComLocal.M23, viaMatrix3.InertiaAtComLocal.M23, 6);
+ }
+
+ [Fact]
+ public void CombineWithAnchor_Matrix3Overload_PartAtAnchor_RebasesBackToPartLocal()
+ {
+ // Mirrors InertialAggregatorTests.CombineWithLinkAnchor_SinglePartAtAnchor_RebasesBackToPartLocal
+ // for the Matrix3 overload: when a part's own frame equals the
+ // rebase anchor, the two transforms must cancel exactly,
+ // regardless of what that shared rotation actually is.
+ var partLocalCom = new Vector3(0f, 0f, 0.15f);
+ var partInertia = new Matrix3(0.003, 0, 0, 0, 0.003, 0, 0, 0, 0.0001);
+ var p = new MassProps(0.5, partLocalCom, partInertia);
+
+ Matrix3 anchorR = RotZ(0.5);
+ Vector3 anchorT = new Vector3(1.0f, -2.0f, 0.4f);
+
+ var parts = new List<(MassProps, Matrix3, Vector3)> { (p, anchorR, anchorT) };
+ MassProps rebased = InertialAggregator.Combine(parts, anchorR, anchorT);
+
+ Assert.Equal(0.5, rebased.Mass, 6);
+ Assert.Equal(partLocalCom.X, rebased.ComLocal.X, 5);
+ Assert.Equal(partLocalCom.Y, rebased.ComLocal.Y, 5);
+ Assert.Equal(partLocalCom.Z, rebased.ComLocal.Z, 5);
+ Assert.Equal(partInertia.M11, rebased.InertiaAtComLocal.M11, 5);
+ Assert.Equal(partInertia.M22, rebased.InertiaAtComLocal.M22, 5);
+ Assert.Equal(partInertia.M33, rebased.InertiaAtComLocal.M33, 5);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_Null_ReturnsIdentity()
+ {
+ var result = InertialAggregator.Combine((List<(MassProps, Matrix3, Vector3)>)null);
+ Assert.Equal(0.0, result.Mass);
+ }
+
+ [Fact]
+ public void Combine_Matrix3Overload_EmptyList_ReturnsIdentity()
+ {
+ var result = InertialAggregator.Combine(new List<(MassProps, Matrix3, Vector3)>());
+ Assert.Equal(0.0, result.Mass);
+ }
+ }
+}
+```
+
+- [ ] **Step 2: Run tests to verify they fail**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~InertialAggregatorMatrixTests"`
+Expected: build ERROR — `InertialAggregator.Combine` has no overload taking `(MassProps, Matrix3, Vector3)` tuples or `(parts, Matrix3, Vector3)`.
+
+- [ ] **Step 3: Implement the Matrix3 overloads**
+
+Replace the full contents of `SW2GZ/Build/InertialAggregator.cs` with:
+
+```csharp
+using System.Collections.Generic;
+using System.Linq;
+using System.Numerics;
+using SW2GZ.Math;
+
+namespace SW2GZ.Build
+{
+ public static class InertialAggregator
+ {
+ // Combine N parts at given poses into a single rigid-body MassProps at the assembly origin.
+ // Steps:
+ // 1) total mass
+ // 2) mass-weighted COM, with each part's local COM rotated into the assembly frame
+ // via R_f before translation
+ // 3) for each part, transform its inertia tensor from part frame to assembly frame
+ // as I_a = R_f · I_part · R_fᵀ, then translate to the combined COM via the
+ // parallel-axis theorem, and sum.
+ // If frame.Rotation is Quaternion.Identity, R_f is Identity and the result is
+ // byte-equivalent to the pre-P3 translation-only behavior.
+ //
+ // The returned MassProps reports COM and inertia in the ASSEMBLY frame
+ // (rotation + translation). URDF's block wants both in the
+ // link-local frame — use the (parts, linkAnchor) overload below for that.
+ public static MassProps Combine(IReadOnlyList<(MassProps Props, Pose Frame)> parts)
+ {
+ if (parts == null) return new MassProps(0, Vector3.Zero, Matrix3.Identity);
+ var matrixParts = parts
+ .Select(p => (p.Props, Matrix3.FromQuaternion(p.Frame.Rotation), p.Frame.Position))
+ .ToList();
+ return CombineCore(matrixParts);
+ }
+
+ // Matrix3-parameterized twin of the overload above. Same algorithm,
+ // same result for an equivalent rotation — exists so callers that
+ // already work entirely in Matrix3/Vector3 (e.g. Sw2gzRobotExporter,
+ // which reads SolidWorks component poses as Matrix3 directly) never
+ // need to construct a Quaternion just to call in here. Deliberately
+ // NOT implemented by converting to Quaternion and delegating to the
+ // overload above — a Matrix3-to-Quaternion conversion is new
+ // coordinate-conversion code, exactly the category that has already
+ // produced two real bugs in this codebase (the Transform2.ArrayData
+ // column-major bug, the mate-classification bug). Both overloads
+ // instead share CombineCore; only the Quaternion overload ever
+ // converts (Quaternion -> Matrix3, an already-proven, already-used
+ // direction), never the reverse.
+ public static MassProps Combine(IReadOnlyList<(MassProps Props, Matrix3 Rotation, Vector3 Position)> parts)
+ {
+ if (parts == null) return new MassProps(0, Vector3.Zero, Matrix3.Identity);
+ return CombineCore(parts);
+ }
+
+ private static MassProps CombineCore(IReadOnlyList<(MassProps Props, Matrix3 R, Vector3 Position)> parts)
+ {
+ if (parts.Count == 0)
+ return new MassProps(0, Vector3.Zero, Matrix3.Identity);
+
+ double totalMass = parts.Sum(p => p.Props.Mass);
+ if (totalMass <= 0)
+ return new MassProps(0, Vector3.Zero, Matrix3.Identity);
+
+ // Per-part rotated COM offset in assembly frame (double precision),
+ // i.e. (pos + R * p.ComLocal). Reused for the parallel-axis d.
+ var partComsX = new double[parts.Count];
+ var partComsY = new double[parts.Count];
+ var partComsZ = new double[parts.Count];
+
+ double comX = 0.0, comY = 0.0, comZ = 0.0;
+ for (int i = 0; i < parts.Count; i++)
+ {
+ var (p, R_f, pos) = parts[i];
+ var (rx, ry, rz) = R_f.Mul((double)p.ComLocal.X, p.ComLocal.Y, p.ComLocal.Z);
+ double pcx = pos.X + rx;
+ double pcy = pos.Y + ry;
+ double pcz = pos.Z + rz;
+ partComsX[i] = pcx; partComsY[i] = pcy; partComsZ[i] = pcz;
+ double w = p.Mass / totalMass;
+ comX += w * pcx; comY += w * pcy; comZ += w * pcz;
+ }
+ var com = new Vector3((float)comX, (float)comY, (float)comZ);
+
+ // Parallel-axis: I_parent = sum_i ( R_i I_i R_iᵀ + m_i * (||d_i||^2 * I - d_i * d_i^T) )
+ var I = new double[3, 3];
+ for (int i = 0; i < parts.Count; i++)
+ {
+ var (p, R_f, _) = parts[i];
+ var I_rot = R_f * p.InertiaAtComLocal * R_f.Transpose();
+
+ double dx = partComsX[i] - comX;
+ double dy = partComsY[i] - comY;
+ double dz = partComsZ[i] - comZ;
+ double d2 = dx * dx + dy * dy + dz * dz;
+
+ I[0, 0] += I_rot.M11 + p.Mass * (d2 - dx * dx);
+ I[0, 1] += I_rot.M12 + p.Mass * ( - dx * dy);
+ I[0, 2] += I_rot.M13 + p.Mass * ( - dx * dz);
+ I[1, 0] += I_rot.M21 + p.Mass * ( - dy * dx);
+ I[1, 1] += I_rot.M22 + p.Mass * (d2 - dy * dy);
+ I[1, 2] += I_rot.M23 + p.Mass * ( - dy * dz);
+ I[2, 0] += I_rot.M31 + p.Mass * ( - dz * dx);
+ I[2, 1] += I_rot.M32 + p.Mass * ( - dz * dy);
+ I[2, 2] += I_rot.M33 + p.Mass * (d2 - dz * dz);
+ }
+
+ return new MassProps(totalMass, com,
+ new Matrix3(I[0,0], I[0,1], I[0,2],
+ I[1,0], I[1,1], I[1,2],
+ I[2,0], I[2,1], I[2,2]));
+ }
+
+ // Combine + rebase into the link-local frame defined by `linkAnchor`
+ // (assembly-frame pose of the link's anchor part). URDF's
+ // wants COM and inertia expressed in the link's own frame; the base
+ // Combine() returns both in the assembly frame, which is wrong as soon
+ // as the link anchor is not at the assembly origin.
+ //
+ // Rebase math (R = R_linkAnchor):
+ // COM_link = R^-1 · (COM_assembly − linkAnchor.Position)
+ // I_link = R^-1 · I_assembly · R
+ // Mass is invariant.
+ //
+ // When linkAnchor == Pose.Identity, R = I and the result equals
+ // Combine(parts) byte-for-byte → existing goldens stay green.
+ public static MassProps Combine(
+ IReadOnlyList<(MassProps Props, Pose Frame)> parts,
+ Pose linkAnchor)
+ {
+ MassProps assemblyFrame = Combine(parts);
+ if (linkAnchor == null || linkAnchor == Pose.Identity) return assemblyFrame;
+ if (assemblyFrame.Mass <= 0) return assemblyFrame;
+
+ Matrix3 R = Matrix3.FromQuaternion(linkAnchor.Rotation);
+ return RebaseCore(assemblyFrame, R, linkAnchor.Position);
+ }
+
+ // Matrix3-parameterized twin of the rebase overload above — see the
+ // Combine(parts) overload for why this exists instead of routing
+ // through Quaternion.
+ public static MassProps Combine(
+ IReadOnlyList<(MassProps Props, Matrix3 Rotation, Vector3 Position)> parts,
+ Matrix3 anchorR, Vector3 anchorT)
+ {
+ MassProps assemblyFrame = Combine(parts);
+ if (assemblyFrame.Mass <= 0) return assemblyFrame;
+ return RebaseCore(assemblyFrame, anchorR, anchorT);
+ }
+
+ private static MassProps RebaseCore(MassProps assemblyFrame, Matrix3 R, Vector3 anchorPosition)
+ {
+ // R = link anchor rotation. We need a vector expressed in the
+ // LINK frame, so we apply Rᵀ (R^-1 for an orthonormal rotation).
+ Matrix3 Rinv = R.Transpose();
+
+ double dx = assemblyFrame.ComLocal.X - anchorPosition.X;
+ double dy = assemblyFrame.ComLocal.Y - anchorPosition.Y;
+ double dz = assemblyFrame.ComLocal.Z - anchorPosition.Z;
+ var (lx, ly, lz) = Rinv.Mul(dx, dy, dz);
+ var comLink = new Vector3((float)lx, (float)ly, (float)lz);
+
+ // I_link = R^-1 · I_assembly · R (same tensor at the same point,
+ // re-expressed in the rotated basis).
+ Matrix3 Ilink = Rinv * assemblyFrame.InertiaAtComLocal * R;
+
+ return new MassProps(assemblyFrame.Mass, comLink, Ilink);
+ }
+ }
+}
+```
+
+- [ ] **Step 4: Run the new tests and the full existing InertialAggregator suite**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~InertialAggregator"`
+Expected: PASS — all of `InertialAggregatorTests`, `InertialAggregatorRotationTests` (both existing, must stay green — they exercise the `Quaternion` overloads which now call `CombineCore`/`RebaseCore` but must produce byte-identical results), and the new `InertialAggregatorMatrixTests`.
+
+- [ ] **Step 5: Commit**
+
+```bash
+git add SW2GZ/Build/InertialAggregator.cs Test/Build/InertialAggregatorMatrixTests.cs
+git commit -m "feat(robot): Matrix3-parameterized InertialAggregator.Combine overloads"
+```
+
+---
+
+### Task 2: `Sw2gzRobotExporter` — parent-relative joint origin + tree-based root detection
+
+Mesh and mass logic are **not** touched in this task (still single-component,
+exactly as today) — this is deliberately the smallest possible first slice
+of the exporter change, isolating the joint-origin/root-detection fix so it
+can be verified on its own before Tasks 3/4 layer multi-mesh union and
+multi-part mass on top. (See `agent-progress/progress.md` — this exact
+class of change has broken live in SolidWorks before despite green tests;
+smallest-step-first is the standing lesson from that.)
+
+**Files:**
+- Modify: `SW2GZ/URDFExport/Sw2gzRobotExporter.cs`
+- Test: `Test/URDFExport/Sw2gzRobotExporterTests.cs`
+
+- [ ] **Step 1: Write the failing tests**
+
+Add these two `[Fact]` methods to the `Sw2gzRobotExporterTests` class in
+`Test/URDFExport/Sw2gzRobotExporterTests.cs` (add them after
+`Export_JointRotationIsRealRelativeRotation_NotIdentity`):
+
+```csharp
+ [Fact]
+ public void Export_GrandchildJointOrigin_IsRelativeToItsOwnParent_NotRoot()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "mid_link", ComponentIds = { "mid-1@asm" }, ParentName = "base_link" },
+ new LinkDef { Name = "leaf_link", ComponentIds = { "leaf-1@asm" }, ParentName = "mid_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, new Vector3(0, 0, 0)),
+ ["mid-1@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["leaf-1@asm"] = (Matrix3.Identity, new Vector3(1, 5, 0)),
+ };
+
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement leafJoint = root.Elements("joint").Single(j => (string)j.Attribute("name") == "mid_link_to_leaf_link");
+ Assert.Equal("mid_link", (string)leafJoint.Element("parent").Attribute("link"));
+
+ // leaf is at (1,5,0), its real parent mid_link is at (1,0,0) — the
+ // relative offset is (0,5,0). If this were still computed relative
+ // to ROOT (0,0,0) instead of mid_link, it would wrongly read (1,5,0).
+ string[] xyz = ((string)leafJoint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(0.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(5.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[2]), 3);
+ }
+
+ [Fact]
+ public void Export_RootDetectedByTreeStructure_NotListPosition()
+ {
+ // Simulates a post-reroot doc: mid_link is now the actual root
+ // (ParentName == ""), but sits at list position [1], not [0] —
+ // exactly what LinkTreeView's "Set as base link" produces (it
+ // edits ParentName pointers, never reorders Robot.Links).
+ var links = new List
+ {
+ new LinkDef { Name = "leaf_link", ComponentIds = { "leaf-1@asm" }, ParentName = "mid_link" },
+ new LinkDef { Name = "mid_link", ComponentIds = { "mid-1@asm" }, ParentName = "" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["mid-1@asm"] = (Matrix3.Identity, new Vector3(5, 0, 0)),
+ ["leaf-1@asm"] = (Matrix3.Identity, new Vector3(5, 2, 0)),
+ };
+
+ Sw2gzRobotExporter.Export(
+ new FakeTess(), new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement joint = root.Elements("joint").Single();
+ Assert.Equal("mid_link", (string)joint.Element("parent").Attribute("link"));
+ Assert.Equal("leaf_link", (string)joint.Element("child").Attribute("link"));
+
+ // leaf (5,2,0) relative to its real parent mid_link (5,0,0) = (0,2,0).
+ // If root were still wrongly detected as leaf_link (list position
+ // [0]), this would never be computed at all (falls back to 0 0 0).
+ string[] xyz = ((string)joint.Element("origin").Attribute("xyz")).Split(' ');
+ Assert.Equal(0.0, double.Parse(xyz[0]), 3);
+ Assert.Equal(2.0, double.Parse(xyz[1]), 3);
+ Assert.Equal(0.0, double.Parse(xyz[2]), 3);
+ }
+```
+
+- [ ] **Step 2: Run tests to verify they fail**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Export_GrandchildJointOrigin_IsRelativeToItsOwnParent_NotRoot|FullyQualifiedName~Export_RootDetectedByTreeStructure_NotListPosition"`
+Expected: FAIL — `Export_GrandchildJointOrigin_IsRelativeToItsOwnParent_NotRoot` fails because the current code computes `leaf_link`'s origin relative to `base_link` (root), giving `xyz="1 5 0"` not `"0 5 0"`. `Export_RootDetectedByTreeStructure_NotListPosition` fails because current code treats `leaf_link` (list position `[0]`) as the base, so `mid_link` never gets a computed origin and the single joint falls back to `xyz="0 0 0"` instead of `"0 2 0"`.
+
+- [ ] **Step 3: Implement the parent-relative fix + tree-based root detection**
+
+In `SW2GZ/URDFExport/Sw2gzRobotExporter.cs`, replace the `Export` method
+body (everything from `List links = config.RobotLinks` down to
+just before the closing brace, i.e. lines 69–155 of the current file) with:
+
+```csharp
+ List links = config.RobotLinks ?? new List();
+ if (links.Count == 0)
+ throw new SW2GZ.Exceptions.Sw2gzExportException(
+ "No links defined — open Create Robot and add at least one link.");
+
+ string pkg = PackageNameSanitizer.Sanitize(config.PackageName).Value;
+ string workspace = Path.Combine(outputDir, pkg + "_ws");
+ string root = Path.Combine(workspace, "src", pkg);
+ string urdfDir = Path.Combine(root, "urdf");
+ string meshesDir = Path.Combine(root, "meshes");
+ Directory.CreateDirectory(urdfDir);
+ Directory.CreateDirectory(meshesDir);
+
+ var issues = new List();
+ var meshFiles = new Dictionary(StringComparer.Ordinal);
+ var masses = new Dictionary(StringComparer.Ordinal);
+ var jointOrigins = new Dictionary(StringComparer.Ordinal);
+ var jointRpys = new Dictionary(StringComparer.Ordinal);
+
+ // Root = whichever link the TREE says has no parent, not
+ // links[0]. "Set as base link" (re-root, in LinkTreeView) edits
+ // ParentName pointers but never reorders Robot.Links, so list
+ // position [0] can silently stop being the real root.
+ LinkDef baseLink = LinkHierarchy.Roots(links).FirstOrDefault() ?? links[0];
+ string baseLinkName = baseLink.Name;
+
+ // Pass 1: every link's own reference pose (its first assigned
+ // component), read once up front. A child can be positioned
+ // before its parent in this list after a drag-drop reparent
+ // (reparenting only edits ParentName, never reorders Links), so
+ // pass 2 needs random access to ANY link's pose by name, not
+ // list order.
+ var linkPoses = new Dictionary(StringComparer.Ordinal);
+ foreach (LinkDef link in links)
+ {
+ string refComp = link.ComponentIds?.FirstOrDefault();
+ linkPoses[link.Name] = TryGetPose(poses, refComp, issues, link.Name);
+ }
+
+ foreach (LinkDef link in links)
+ {
+ string compName = link.ComponentIds?.FirstOrDefault();
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ (Matrix3 linkR, Vector3 linkT) = linkPoses[link.Name];
+
+ MeshData meshWorld;
+ try
+ {
+ meshWorld = tess.Tessellate(compName, TessellationLod.Fine);
+ }
+ catch (Exception ex)
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MESH",
+ "Link '" + link.Name + "' — could not tessellate '" + compName + "': " + ex.Message,
+ "Sw2gzRobotExporter"));
+ meshWorld = null;
+ }
+
+ if (meshWorld != null)
+ {
+ MeshData meshLocal = link.Name == baseLinkName
+ ? Translate(meshWorld, -linkT)
+ : UnbakeToLocal(meshWorld, linkR, linkT);
+
+ string daeFile = link.Name + ".dae";
+ DaeWriter.Write(meshLocal, Path.Combine(meshesDir, daeFile), withNormals: true);
+ meshFiles[link.Name] = daeFile;
+ }
+
+ // Joint origin relative to THIS link's own declared parent —
+ // not always the root. Same formula as before; only the
+ // source of "parent pose" changed.
+ if (!string.IsNullOrEmpty(link.ParentName) &&
+ linkPoses.TryGetValue(link.ParentName, out (Matrix3 R, Vector3 T) parentPose))
+ {
+ Matrix3 rJoint = parentPose.R.Transpose() * linkR;
+ Vector3 tJoint = parentPose.R.Transpose().Mul(linkT - parentPose.T);
+ jointOrigins[link.Name] = tJoint;
+ jointRpys[link.Name] = rJoint.ToRpy();
+ }
+
+ try
+ {
+ masses[link.Name] = massProps.Get(compName);
+ }
+ catch (Exception ex)
+ {
+ // ponytail: no SW material on this part → placeholder mass/
+ // inertia. Physical accuracy is out of scope for this
+ // validating cut (no actuation/physics sim runs on the
+ // export yet); revisit once Robot mode gets a real
+ // inertia pipeline.
+ masses[link.Name] = new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MASS",
+ "Link '" + link.Name + "' — no material on '" + compName + "', using placeholder mass: " + ex.Message,
+ "Sw2gzRobotExporter"));
+ }
+ }
+
+ string urdfPath = Path.Combine(urdfDir, pkg + ".urdf.xacro");
+ WriteUrdf(urdfPath, pkg, baseLinkName, links, meshFiles, masses, jointOrigins, jointRpys, config.EmitWorldLink, swToRos);
+
+ return new ValidationReport(issues);
+ }
+```
+
+Then update the `WriteUrdf` method signature and body — replace:
+
+```csharp
+ private static void WriteUrdf(
+ string path, string pkg, List links,
+ Dictionary meshFiles, Dictionary masses,
+ Dictionary jointOrigins, Dictionary jointRpys,
+ bool emitWorldLink, Matrix3 swToRos)
+ {
+ var uw = new URDFWriter(path);
+ System.Xml.XmlWriter w = uw.writer;
+ w.WriteStartDocument();
+ w.WriteStartElement("robot");
+ w.WriteAttributeString("name", pkg);
+
+ string baseLinkName = links[0].Name;
+```
+
+with:
+
+```csharp
+ private static void WriteUrdf(
+ string path, string pkg, string baseLinkName, List links,
+ Dictionary meshFiles, Dictionary masses,
+ Dictionary jointOrigins, Dictionary jointRpys,
+ bool emitWorldLink, Matrix3 swToRos)
+ {
+ var uw = new URDFWriter(path);
+ System.Xml.XmlWriter w = uw.writer;
+ w.WriteStartDocument();
+ w.WriteStartElement("robot");
+ w.WriteAttributeString("name", pkg);
+```
+
+(`baseLinkName` is now a parameter — the caller already resolved it via
+`LinkHierarchy.Roots`, so `WriteUrdf` must not independently recompute it
+from `links[0]`, which would silently reintroduce the exact bug this task
+fixes.)
+
+Also add `using SW2GZ.Build;` to the top of the file if it is not already
+present — it is (line 45 in the current file), so no change needed there.
+
+- [ ] **Step 4: Run the new tests and the full existing Sw2gzRobotExporterTests suite**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Sw2gzRobotExporterTests"`
+Expected: PASS — all of `Export_WritesUrdfWithLinksMeshesAndFixedJoint`,
+`Export_JointOriginIsRealTranslationDelta_NotIdentity`,
+`Export_JointRotationIsRealRelativeRotation_NotIdentity`,
+`Export_NoLinks_Throws`, `Export_MissingMaterial_FallsBackToPlaceholderMassAndWarns`,
+`Export_EmitWorldLink_AddsWorldJointWithRotation` (all existing — these are
+all 2-level trees, so parent-relative and root-relative give the same
+answer; they must stay green as the degenerate case), plus the two new
+tests from Step 1.
+
+- [ ] **Step 5: Commit**
+
+```bash
+git add SW2GZ/URDFExport/Sw2gzRobotExporter.cs Test/URDFExport/Sw2gzRobotExporterTests.cs
+git commit -m "fix(robot): joint origin relative to declared parent, root by tree structure"
+```
+
+---
+
+### Task 3: `Sw2gzRobotExporter` — multi-mesh union per link
+
+Every component assigned to a link (`LinkDef.ComponentIds`, already a list
+— the wizard's mesh picker already supports multi-select) currently has
+only its first entry tessellated; the rest are silently dropped from the
+export. This task unions all of them into one mesh per link.
+
+**Files:**
+- Modify: `SW2GZ/URDFExport/Sw2gzRobotExporter.cs`
+- Test: `Test/URDFExport/Sw2gzRobotExporterTests.cs`
+
+- [ ] **Step 1: Write the failing test**
+
+Add a `FakeMultiTess` test double and a new test to
+`Sw2gzRobotExporterTests` — add `FakeMultiTess` right after the existing
+`FakeTess` class:
+
+```csharp
+ private sealed class FakeMultiTess : IMeshTessellator
+ {
+ private readonly Dictionary _meshes;
+ public FakeMultiTess(Dictionary meshes) => _meshes = meshes;
+ public MeshData Tessellate(string n, TessellationLod lod) =>
+ _meshes.TryGetValue(n, out MeshData m) ? m : new MeshData(Array.Empty(), Array.Empty(), null);
+ }
+```
+
+Add this test after `Export_RootDetectedByTreeStructure_NotListPosition`
+(from Task 2). It needs `System.Globalization` for culture-invariant
+parsing — add `using System.Globalization;` to the file's using block:
+
+```csharp
+ [Fact]
+ public void Export_MultiComponentLink_UnionsAllMeshesInLinkReferenceFrame()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-a@asm", "arm-b@asm" }, ParentName = "base_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-a@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["arm-b@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ };
+ var meshA = new MeshData(
+ new[] { new Vector3(1, 0, 0), new Vector3(2, 0, 0), new Vector3(1, 1, 0) },
+ new[] { 0, 1, 2 }, null);
+ var meshB = new MeshData(
+ new[] { new Vector3(1, 0, 5), new Vector3(2, 0, 5), new Vector3(1, 1, 5) },
+ new[] { 0, 1, 2 }, null);
+ var tess = new FakeMultiTess(new Dictionary
+ {
+ ["base-1@asm"] = new MeshData(new[] { new Vector3(0, 0, 0), new Vector3(1, 0, 0), new Vector3(0, 1, 0) }, new[] { 0, 1, 2 }, null),
+ ["arm-a@asm"] = meshA,
+ ["arm-b@asm"] = meshB,
+ });
+
+ Sw2gzRobotExporter.Export(tess, new FakeMassProps(), new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ string daePath = Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "meshes", "arm_link.dae");
+ Assert.True(File.Exists(daePath));
+
+ XNamespace ns = "http://www.collada.org/2005/11/COLLADASchema";
+ XDocument dae = XDocument.Load(daePath);
+ XElement posArray = dae.Descendants(ns + "float_array")
+ .Single(e => (string)e.Attribute("id") == "g0-pos-array");
+ int floatCount = int.Parse((string)posArray.Attribute("count"));
+
+ // Both components' triangles survive the union: 3 verts each, 3
+ // floats per vert = 18 total (not 9 — which is what a
+ // "first component only" regression would silently produce).
+ Assert.Equal(18, floatCount);
+
+ // arm-b's vertices sit at z=5 in its own (identity-rotation,
+ // translation (1,0,0)) frame; arm_link's reference frame is
+ // arm-a's pose (also (1,0,0), identity) — so after un-baking,
+ // arm-b's local vertices should still carry that z=5 offset
+ // (proves it was folded into the SAME shared frame as arm-a,
+ // not silently dropped or mis-transformed).
+ string[] floats = posArray.Value.Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);
+ var zValues = new List();
+ for (int i = 2; i < floats.Length; i += 3)
+ zValues.Add(double.Parse(floats[i], CultureInfo.InvariantCulture));
+ Assert.Contains(zValues, z => System.Math.Abs(z - 5.0) < 1e-3);
+ }
+```
+
+- [ ] **Step 2: Run test to verify it fails**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Export_MultiComponentLink_UnionsAllMeshesInLinkReferenceFrame"`
+Expected: FAIL — `floatCount` is `9` (only `arm-a`'s 3 vertices), not `18`;
+`arm-b`'s mesh was never tessellated because `Export` still reads
+`ComponentIds.FirstOrDefault()`.
+
+- [ ] **Step 3: Implement the mesh-union helper and wire it in**
+
+In `SW2GZ/URDFExport/Sw2gzRobotExporter.cs`, add `using System.Drawing;` to
+the using block (needed for the `Color?` local in the new helper).
+
+Replace this block inside the `Export` loop:
+
+```csharp
+ if (meshWorld != null)
+ {
+ MeshData meshLocal = link.Name == baseLinkName
+ ? Translate(meshWorld, -linkT)
+ : UnbakeToLocal(meshWorld, linkR, linkT);
+
+ string daeFile = link.Name + ".dae";
+ DaeWriter.Write(meshLocal, Path.Combine(meshesDir, daeFile), withNormals: true);
+ meshFiles[link.Name] = daeFile;
+ }
+```
+
+and the tessellate-try/catch immediately above it:
+
+```csharp
+ MeshData meshWorld;
+ try
+ {
+ meshWorld = tess.Tessellate(compName, TessellationLod.Fine);
+ }
+ catch (Exception ex)
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MESH",
+ "Link '" + link.Name + "' — could not tessellate '" + compName + "': " + ex.Message,
+ "Sw2gzRobotExporter"));
+ meshWorld = null;
+ }
+```
+
+with a single call to the new union helper (root uses a forced-identity
+reference rotation — same "root's own rotation stays baked into the mesh,
+not represented as TF orientation" convention as before; every other link
+uses its real reference rotation):
+
+```csharp
+ MeshData meshLocal = link.Name == baseLinkName
+ ? UnionMeshInLocalFrame(tess, link.ComponentIds, Matrix3.Identity, linkT, issues, link.Name)
+ : UnionMeshInLocalFrame(tess, link.ComponentIds, linkR, linkT, issues, link.Name);
+
+ if (meshLocal != null)
+ {
+ string daeFile = link.Name + ".dae";
+ DaeWriter.Write(meshLocal, Path.Combine(meshesDir, daeFile), withNormals: true);
+ meshFiles[link.Name] = daeFile;
+ }
+```
+
+Delete the now-dead `Translate` and `UnbakeToLocal` private methods (both
+fully subsumed by `UnionMeshInLocalFrame` — a single-component link is just
+a 1-element union):
+
+```csharp
+ private static MeshData Translate(MeshData mesh, Vector3 t)
+ {
+ if (mesh?.Vertices == null || mesh.Vertices.Length == 0) return mesh;
+ var shifted = new Vector3[mesh.Vertices.Length];
+ for (int i = 0; i < shifted.Length; i++) shifted[i] = mesh.Vertices[i] + t;
+ return new MeshData(shifted, mesh.Triangles, mesh.MaterialColor);
+ }
+
+ // p_local = R^T * (p_world - t) — reverses the tessellator's bake so the
+ // mesh sits in the component's own native part frame.
+ private static MeshData UnbakeToLocal(MeshData mesh, Matrix3 r, Vector3 t)
+ {
+ if (mesh?.Vertices == null || mesh.Vertices.Length == 0) return mesh;
+ Matrix3 rInv = r.Transpose();
+ var local = new Vector3[mesh.Vertices.Length];
+ for (int i = 0; i < local.Length; i++) local[i] = rInv.Mul(mesh.Vertices[i] - t);
+ return new MeshData(local, mesh.Triangles, mesh.MaterialColor);
+ }
+```
+
+Add the new helper in their place:
+
+```csharp
+ // Every component assigned to a link gets tessellated and folded
+ // into ONE mesh, all expressed in the SAME reference frame (refR,
+ // refT) — not each component's own frame, which would scatter the
+ // pieces apart. Generalizes the old single-component un-bake to N
+ // components via the same vertex-offset + index-shift pattern
+ // SolidWorksMeshTessellator already uses internally to union
+ // multiple solid bodies within one component.
+ private static MeshData UnionMeshInLocalFrame(
+ IMeshTessellator tess, IReadOnlyList componentIds,
+ Matrix3 refR, Vector3 refT, List issues, string linkName)
+ {
+ var verts = new List();
+ var tris = new List();
+ Color? color = null;
+ Matrix3 refRInv = refR.Transpose();
+
+ foreach (string compName in componentIds ?? (IReadOnlyList)Array.Empty())
+ {
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ MeshData meshWorld;
+ try
+ {
+ meshWorld = tess.Tessellate(compName, TessellationLod.Fine);
+ }
+ catch (Exception ex)
+ {
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MESH",
+ "Link '" + linkName + "' — could not tessellate '" + compName + "': " + ex.Message,
+ "Sw2gzRobotExporter"));
+ continue;
+ }
+ if (meshWorld?.Vertices == null || meshWorld.Vertices.Length == 0) continue;
+
+ color ??= meshWorld.MaterialColor;
+ int baseIdx = verts.Count;
+ foreach (Vector3 v in meshWorld.Vertices) verts.Add(refRInv.Mul(v - refT));
+ foreach (int idx in meshWorld.Triangles) tris.Add(baseIdx + idx);
+ }
+
+ return verts.Count == 0 ? null : new MeshData(verts.ToArray(), tris.ToArray(), color);
+ }
+```
+
+- [ ] **Step 4: Run the new test and the full existing Sw2gzRobotExporterTests suite**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Sw2gzRobotExporterTests"`
+Expected: PASS — all existing tests (single-component links are a 1-element
+union, byte-identical output) plus the new multi-component test.
+
+- [ ] **Step 5: Commit**
+
+```bash
+git add SW2GZ/URDFExport/Sw2gzRobotExporter.cs Test/URDFExport/Sw2gzRobotExporterTests.cs
+git commit -m "feat(robot): union every assigned mesh component per link, not just the first"
+```
+
+---
+
+### Task 4: `Sw2gzRobotExporter` — multi-part mass/inertia combination
+
+**Files:**
+- Modify: `SW2GZ/URDFExport/Sw2gzRobotExporter.cs`
+- Test: `Test/URDFExport/Sw2gzRobotExporterTests.cs`
+
+- [ ] **Step 1: Write the failing test**
+
+Add a `FakeMultiMassProps` test double (after `FakeMultiTess` from Task 3)
+and a new test (after `Export_MultiComponentLink_UnionsAllMeshesInLinkReferenceFrame`
+from Task 3):
+
+```csharp
+ private sealed class FakeMultiMassProps : IMassProperties
+ {
+ private readonly Dictionary _masses;
+ public FakeMultiMassProps(Dictionary masses) => _masses = masses;
+ public MassProps Get(string componentPathName) =>
+ _masses.TryGetValue(componentPathName, out MassProps m) ? m : new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ }
+```
+
+```csharp
+ [Fact]
+ public void Export_MultiComponentLink_CombinesMassOfAllAssignedComponents()
+ {
+ var links = new List
+ {
+ new LinkDef { Name = "base_link", ComponentIds = { "base-1@asm" }, ParentName = "" },
+ new LinkDef { Name = "arm_link", ComponentIds = { "arm-a@asm", "arm-b@asm" }, ParentName = "base_link" },
+ };
+ var cfg = new Sw2gzExportConfig
+ {
+ Mode = SW2GZ.Ros2.ExportMode.RobotPackage,
+ PackageName = "my_robot",
+ RobotLinks = links,
+ };
+ var poses = new Dictionary
+ {
+ ["base-1@asm"] = (Matrix3.Identity, Vector3.Zero),
+ ["arm-a@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ ["arm-b@asm"] = (Matrix3.Identity, new Vector3(1, 0, 0)),
+ };
+ var massProps = new FakeMultiMassProps(new Dictionary
+ {
+ ["base-1@asm"] = new MassProps(9.0, Vector3.Zero, Matrix3.Identity),
+ ["arm-a@asm"] = new MassProps(1.5, Vector3.Zero, Matrix3.Identity),
+ ["arm-b@asm"] = new MassProps(2.5, Vector3.Zero, Matrix3.Identity),
+ });
+
+ Sw2gzRobotExporter.Export(new FakeTess(), massProps, new FakePoses(poses), cfg, _dir, Matrix3.Identity);
+
+ XElement root = XElement.Load(Path.Combine(_dir, "my_robot_ws", "src", "my_robot", "urdf", "my_robot.urdf.xacro"));
+ XElement armLink = root.Elements("link").Single(l => (string)l.Attribute("name") == "arm_link");
+ double mass = double.Parse((string)armLink.Element("inertial").Element("mass").Attribute("value"), CultureInfo.InvariantCulture);
+
+ // 1.5 + 2.5, not just arm-a's 1.5 (a "first component only"
+ // regression would report 1.5, silently dropping arm-b).
+ Assert.Equal(4.0, mass, 3);
+ }
+```
+
+- [ ] **Step 2: Run test to verify it fails**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Export_MultiComponentLink_CombinesMassOfAllAssignedComponents"`
+Expected: FAIL — reported mass is `1.5` (only `arm-a`), not `4.0`.
+
+- [ ] **Step 3: Implement the mass-combination helper and wire it in**
+
+Replace this block inside the `Export` loop:
+
+```csharp
+ try
+ {
+ masses[link.Name] = massProps.Get(compName);
+ }
+ catch (Exception ex)
+ {
+ // ponytail: no SW material on this part → placeholder mass/
+ // inertia. Physical accuracy is out of scope for this
+ // validating cut (no actuation/physics sim runs on the
+ // export yet); revisit once Robot mode gets a real
+ // inertia pipeline.
+ masses[link.Name] = new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MASS",
+ "Link '" + link.Name + "' — no material on '" + compName + "', using placeholder mass: " + ex.Message,
+ "Sw2gzRobotExporter"));
+ }
+```
+
+with:
+
+```csharp
+ masses[link.Name] = CombineMass(massProps, poses, link.ComponentIds, linkR, linkT, issues, link.Name);
+```
+
+Add the new helper next to `UnionMeshInLocalFrame`:
+
+```csharp
+ // Combines every assigned component's own mass/COM/inertia
+ // (parallel-axis, via InertialAggregator) into one MassProps rebased
+ // into the link's own reference frame (linkR/linkT — the SAME pose
+ // used for the joint and mesh math). For a single-component link
+ // this is byte-identical to using that component's raw MassProps
+ // directly: InertialAggregator's rebase exactly cancels when a
+ // part's own frame equals the anchor (see
+ // CombineWithLinkAnchor_SinglePartAtAnchor_RebasesBackToPartLocal /
+ // its Matrix3 twin). Mass/inertia physical accuracy beyond this is
+ // already out of scope for this validating cut (see the ponytail
+ // note this replaces) — this does not force the base_link
+ // identity-orientation convention the way mesh un-baking does,
+ // since that would only matter for a multi-component root with a
+ // non-identity native rotation, which isn't exercised yet.
+ private static MassProps CombineMass(
+ IMassProperties massProps, IComponentPoses poses, IReadOnlyList componentIds,
+ Matrix3 linkR, Vector3 linkT, List issues, string linkName)
+ {
+ var parts = new List<(MassProps Props, Matrix3 R, Vector3 T)>();
+ foreach (string compName in componentIds ?? (IReadOnlyList)Array.Empty())
+ {
+ if (string.IsNullOrWhiteSpace(compName)) continue;
+
+ MassProps mp;
+ try
+ {
+ mp = massProps.Get(compName);
+ }
+ catch (Exception ex)
+ {
+ mp = new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ issues.Add(new ValidationIssue(IssueSeverity.Warning, "ROBOT.MASS",
+ "Link '" + linkName + "' — no material on '" + compName + "', using placeholder mass: " + ex.Message,
+ "Sw2gzRobotExporter"));
+ }
+ (Matrix3 compR, Vector3 compT) = TryGetPose(poses, compName, issues, linkName);
+ parts.Add((mp, compR, compT));
+ }
+
+ if (parts.Count == 0) return new MassProps(0.1, Vector3.Zero, Matrix3.Identity);
+ return InertialAggregator.Combine(parts, linkR, linkT);
+ }
+```
+
+- [ ] **Step 4: Run the new test and the full existing Sw2gzRobotExporterTests suite**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --filter "FullyQualifiedName~Sw2gzRobotExporterTests"`
+Expected: PASS — including `Export_WritesUrdfWithLinksMeshesAndFixedJoint`
+(still asserts `arm_link`'s mass is exactly `"2.5"` — this is the
+single-component byte-identical regression check: `arm_link`'s one
+component's own pose equals its reference pose, so `InertialAggregator`'s
+rebase cancels exactly, mass stays exactly 2.5).
+
+- [ ] **Step 5: Run the full test suite (not just this file) to confirm no cross-file regressions**
+
+Run: `dotnet test Test/SW2GZ.Writers.Test.csproj -c Release`
+Expected: PASS — 470 baseline + all new tests from Tasks 1-4 (should be
+around 480).
+
+- [ ] **Step 6: Commit**
+
+```bash
+git add SW2GZ/URDFExport/Sw2gzRobotExporter.cs Test/URDFExport/Sw2gzRobotExporterTests.cs
+git commit -m "feat(robot): combine mass/inertia of every assigned mesh component per link"
+```
+
+---
+
+### Task 5: UI — surface which mesh component is "primary"
+
+The first `ComponentIds` entry now silently defines a link's whole frame
+(mesh anchor, joint origin, inertial rebase — Tasks 2-4). That needs to be
+visible in the wizard, not implicit.
+
+**Files:**
+- Modify: `SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs`
+- Modify: `SW2GZ/UI/LinkTreeView.cs`
+
+No test file — `Sw2gzCreateRobotPmp.cs` is `#if SW_INTEROP`-gated (COM,
+not unit-testable outside SolidWorks) and `LinkTreeView.cs` is explicitly
+"not source-linked into the net8 test project" per its own header comment.
+Verified by compiling + the live SW check in Task 6.
+
+- [ ] **Step 1: Mark the primary mesh in `Sw2gzCreateRobotPmp`'s selected-info label**
+
+In `SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs`, replace:
+
+```csharp
+ private void RefreshSelectedInfo(LinkDef link)
+ {
+ if (_selectedInfoLabel == null) return;
+ _selectedInfoLabel.Caption = link == null
+ ? "Selected: (none)"
+ : "Selected: " + link.Name + " Mesh: "
+ + (link.ComponentIds.Count == 0 ? "(none)" : string.Join(", ", link.ComponentIds));
+ }
+```
+
+with:
+
+```csharp
+ private void RefreshSelectedInfo(LinkDef link)
+ {
+ if (_selectedInfoLabel == null) return;
+ _selectedInfoLabel.Caption = link == null
+ ? "Selected: (none)"
+ : "Selected: " + link.Name + " Mesh: " + DescribeMeshes(link.ComponentIds);
+ }
+
+ // The first component defines the link's own frame (mesh anchor,
+ // joint origin, inertial rebase — see
+ // docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md),
+ // so it's marked (primary) wherever the mesh list is shown —
+ // otherwise which one drives the frame is invisible.
+ private static string DescribeMeshes(List componentIds)
+ {
+ if (componentIds == null || componentIds.Count == 0) return "(none)";
+ var parts = new List(componentIds.Count);
+ for (int i = 0; i < componentIds.Count; i++)
+ parts.Add(i == 0 ? componentIds[i] + " (primary)" : componentIds[i]);
+ return string.Join(", ", parts);
+ }
+```
+
+- [ ] **Step 2: Mark the primary mesh in `LinkTreeView`'s node tooltip**
+
+In `SW2GZ/UI/LinkTreeView.cs`, the constructor already sets
+`ShowNodeToolTips = true;` but no node ever gets a `ToolTipText` — this
+step finishes wiring that already-declared-but-unused feature instead of
+adding new UI surface.
+
+Replace:
+
+```csharp
+ private TreeNode BuildNode(LinkDef link)
+ {
+ bool isRoot = string.IsNullOrEmpty(link.ParentName);
+ int n = link.ComponentIds?.Count ?? 0;
+ // Links only — the component-name leaf duplicated the link name and added
+ // no information; show the part count as a suffix instead.
+ string label = (link.Name ?? "")
+ + (isRoot ? " (base)" : "")
+ + " [" + n + (n == 1 ? " part]" : " parts]");
+ var node = new TreeNode(label) { Tag = link };
+ if (n == 0) node.ForeColor = System.Drawing.Color.Firebrick; // unassigned = needs attention
+ foreach (LinkDef child in LinkHierarchy.ChildrenOf(links, link.Name))
+ node.Nodes.Add(BuildNode(child));
+ return node;
+ }
+```
+
+with:
+
+```csharp
+ private TreeNode BuildNode(LinkDef link)
+ {
+ bool isRoot = string.IsNullOrEmpty(link.ParentName);
+ int n = link.ComponentIds?.Count ?? 0;
+ // Links only — the component-name leaf duplicated the link name and added
+ // no information; show the part count as a suffix instead.
+ string label = (link.Name ?? "")
+ + (isRoot ? " (base)" : "")
+ + " [" + n + (n == 1 ? " part]" : " parts]");
+ var node = new TreeNode(label) { Tag = link };
+ if (n == 0) node.ForeColor = System.Drawing.Color.Firebrick; // unassigned = needs attention
+ node.ToolTipText = DescribePrimary(link);
+ foreach (LinkDef child in LinkHierarchy.ChildrenOf(links, link.Name))
+ node.Nodes.Add(BuildNode(child));
+ return node;
+ }
+
+ // The first ComponentIds entry defines this link's whole frame
+ // (mesh anchor, joint origin, inertial rebase) — surfaced on hover
+ // since the compact node label has no room for it.
+ private static string DescribePrimary(LinkDef link)
+ {
+ List ids = link.ComponentIds;
+ if (ids == null || ids.Count == 0) return "no mesh assigned";
+ if (ids.Count == 1) return ids[0];
+ return "primary: " + ids[0] + " | also: " + string.Join(", ", ids.GetRange(1, ids.Count - 1));
+ }
+```
+
+Then update `RefreshActiveNodeLabel` (keeps the tooltip in sync after an
+F2/inline rename, which rebuilds the label text without a full `Rebuild()`)
+— replace:
+
+```csharp
+ public void RefreshActiveNodeLabel()
+ {
+ TreeNode n = SelectedNode;
+ if (n == null || !(n.Tag is LinkDef link)) return;
+ bool isRoot = string.IsNullOrEmpty(link.ParentName);
+ int parts = link.ComponentIds?.Count ?? 0;
+ n.Text = (link.Name ?? "")
+ + (isRoot ? " (base)" : "")
+ + " [" + parts + (parts == 1 ? " part]" : " parts]");
+ }
+```
+
+with:
+
+```csharp
+ public void RefreshActiveNodeLabel()
+ {
+ TreeNode n = SelectedNode;
+ if (n == null || !(n.Tag is LinkDef link)) return;
+ bool isRoot = string.IsNullOrEmpty(link.ParentName);
+ int parts = link.ComponentIds?.Count ?? 0;
+ n.Text = (link.Name ?? "")
+ + (isRoot ? " (base)" : "")
+ + " [" + parts + (parts == 1 ? " part]" : " parts]");
+ n.ToolTipText = DescribePrimary(link);
+ }
+```
+
+- [ ] **Step 3: Commit**
+
+```bash
+git add SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs SW2GZ/UI/LinkTreeView.cs
+git commit -m "feat(robot): surface which mesh component is primary in the Links UI"
+```
+
+---
+
+### Task 6: Full verification — build, test, live SW check, deploy
+
+**Files:** none (build/test/deploy only)
+
+- [ ] **Step 1: Build the add-in**
+
+Run (PowerShell, SolidWorks closed):
+```powershell
+$msb = "C:\Program Files (x86)\Microsoft Visual Studio\2022\BuildTools\MSBuild\Current\Bin\MSBuild.exe"
+& $msb "C:\aryan\SW2GZ\SW2GZ\SW2GZ.csproj" /p:Configuration=Release /p:SolutionDir=C:\aryan\SW2GZ\ /t:Build /v:minimal /nologo /m
+```
+Expected: `SW2GZ -> C:\aryan\SW2GZ\SW2GZ\bin\Release\SW2GZ.dll`. The
+`MSB3216`/regasm access-denied line is a known non-fatal warning (see
+memory `sw2gz-build-deploy`) — the DLL still compiles.
+
+- [ ] **Step 2: Build the test project**
+
+```powershell
+& $msb "C:\aryan\SW2GZ\Test\SW2GZ.Writers.Test.csproj" /p:Configuration=Release /p:SolutionDir=C:\aryan\SW2GZ\ /t:Build /v:minimal /nologo /m
+```
+Expected: `SW2GZ.Writers.Test -> C:\aryan\SW2GZ\Test\bin\Release\net8.0\SW2GZ.Writers.Test.dll`, no errors.
+
+- [ ] **Step 3: Run the full test suite**
+
+```bash
+dotnet test Test/SW2GZ.Writers.Test.csproj -c Release --no-build
+```
+Expected: `Passed!` with 0 failures. Count should be the 470 baseline plus
+the new tests from Tasks 1–4 (5 in `InertialAggregatorMatrixTests` + 4 in
+`Sw2gzRobotExporterTests` = 9 new → ~479).
+
+- [ ] **Step 4: Deploy**
+
+Confirm SolidWorks is closed, then elevated-copy the fresh DLL:
+```powershell
+Get-Process SLDWORKS -ErrorAction SilentlyContinue # must return nothing
+Copy-Item "C:\aryan\SW2GZ\SW2GZ\bin\Release\SW2GZ.dll" "C:\Program Files\SW2GZ\SW2GZ.dll" -Force
+```
+(Needs admin — run via an elevated `Start-Process powershell -Verb RunAs`
+if the direct copy is denied.) Verify: `Get-Item "C:\Program Files\SW2GZ\SW2GZ.dll"`
+timestamp matches the fresh `bin\Release` build.
+
+- [ ] **Step 5: Live SW check on `FULL_ARM.SLDASM`**
+
+Open Create Robot. Build a 3-level chain: pick a part → Add link (becomes
+`base_link`) → pick another part → name it → click `base_link` in the tree
+→ Add link (child of base) → pick a third part → name it → click the
+*previous child* in the tree → Add link (grandchild). Then build a
+multi-mesh link: pick two parts at once in the Mesh selector → name → Add.
+Finish the wizard, Export (or Preview). Confirm in the output/preview:
+- The grandchild's mesh and joint sit at the correct relative position —
+ not offset as if computed relative to `base_link`.
+- The 2-part link's mesh shows BOTH parts, correctly positioned relative to
+ each other (not just the first one picked).
+- Removing/re-adding does not crash the PMP (same live-checkpoint standard
+ as the two prior UI-only sessions).
+
+- [ ] **Step 6: Update progress notes**
+
+Add a session entry to `agent-progress/progress.md` under the "CONTINUE
+HERE" section summarizing what shipped (parent-relative joints, tree-based
+root detection, multi-mesh union, multi-part mass combination, primary-mesh
+UI indicator), the test count, and the live-check result. Follow the same
+format as the two prior entries in that file from this session.
+
+- [ ] **Step 7: Commit the progress note**
+
+```bash
+git add agent-progress/progress.md
+git commit -m "docs(progress): parent-relative joints + multi-mesh links shipped, live-tested"
+```
diff --git a/docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md b/docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md
new file mode 100644
index 0000000..c86ad1e
--- /dev/null
+++ b/docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md
@@ -0,0 +1,143 @@
+# Robot joint/link relative pose + multi-mesh links
+
+**Date:** 2026-07-02
+**Mode:** Robot (v3)
+**Branch:** `feat/robot-mode-v3`
+**Status:** Approved via brainstorming dialogue (this doc), ready for
+implementation plan.
+
+## Problem
+
+The Links step wizard (`Sw2gzCreateRobotPmp.cs`) now lets a user build a real,
+arbitrary-depth, drag-to-reparent link tree, and lets a single link claim
+multiple mesh components (multi-select on the mesh picker, `LinkDef.
+ComponentIds` is already a list). The exporter (`Sw2gzRobotExporter.cs`)
+was never updated to match either capability:
+
+1. **Joint origin is always root-relative, not parent-relative.** The
+ relative-pose formula it uses is correct, but `baseR`/`baseT` (used as
+ "the parent" for every joint) are hardcoded to `links[0]`, never to the
+ link's own `ParentName`. Invisible under the old flat wizard (everything
+ was fixed straight to base, so parent == root for every link). Wrong now
+ that a grandchild's true parent can be a non-root link.
+2. **"Root" is detected by list position (`links[0]`), not tree structure.**
+ The tree's "Set as base link" (re-root) flips `ParentName` pointers
+ without reordering the `Links` list, so `links[0]` can silently stop
+ being the actual root after a reroot.
+3. **Only the first `ComponentIds` entry is used.** Mesh tessellation,
+ pose lookup, and mass-properties lookup all read
+ `link.ComponentIds.FirstOrDefault()` — every other component assigned to
+ that link is silently dropped from the export (mesh, mass, everything).
+
+## Design decisions (from brainstorming)
+
+- **Multi-mesh union is in scope for this pass** (user chose to bundle it
+ rather than defer — the parent-relative fix and the multi-mesh fix touch
+ the same per-link pose plumbing anyway).
+- **Root link keeps the identity-orientation convention.** `base_link`'s own
+ SW rotation stays baked into its mesh rather than represented as TF
+ orientation, same as today. Not touched by this pass.
+- **No new coordinate-conversion math.** `InertialAggregator.Combine` already
+ does correct mass-weighted-COM + parallel-axis inertia combination, but
+ takes `Pose` (quaternion-based); the exporter works entirely in `Matrix3`.
+ Writing a `Matrix3→Quaternion` converter to bridge them would be new,
+ security-adjacent-precision math in exactly the category that has already
+ produced two real bugs in this codebase (the `Transform2.ArrayData`
+ column-major bug, the mate-classification bug — see memory
+ `sw-mathtransform-column-major`, `robot-mode-dev`). Instead,
+ `InertialAggregator`'s core combination loop is extracted into a
+ `Matrix3`-parameterized overload; the existing `Quaternion` overload
+ becomes a thin wrapper over it (`Matrix3.FromQuaternion(f.Rotation)` was
+ already its first step). Zero new conversion code, zero duplicated
+ physics math, existing callers untouched.
+- **Joint type stays hardcoded Fixed.** Mate-driven type/axis detection is a
+ separate, previously-reverted effort (see memory `robot-mode-dev`) — not
+ reopened here.
+- **A link's "own frame" = its first `ComponentIds` entry**, consistently,
+ everywhere a link needs one frame: mesh un-bake anchor, joint origin
+ math, and inertial rebase anchor. Order = pick order in the wizard
+ (already implicit — no new UI concept).
+
+## Math
+
+Both link poses are read directly from `IComponentPoses.GetPose`, which
+already returns each component's pose in the assembly frame (verified
+column-major). No kinematic-chain walking is needed — every component's
+pose is already absolute, so a joint's relative pose is one direct
+computation between the child's reference component and the parent link's
+reference component, regardless of tree depth:
+
+```
+R_joint = R_parent^T · R_child
+t_joint = R_parent^T · (t_child - t_parent)
+```
+
+This is the exact formula already in `Sw2gzRobotExporter.cs:123-124` today —
+only the source of `R_parent/t_parent` changes (from "always `links[0]`'s
+pose" to "the pose of the link named in `link.ParentName`").
+
+Mesh un-baking for a multi-component link re-expresses every component's
+world-frame tessellation in the **link's own reference frame** (not each
+component's individual frame — that would scatter the pieces):
+
+```
+p_local_i = R_ref^T · (p_world_i - t_ref) for every component i on the link
+```
+
+Mass/inertia combination for a multi-component link: gather
+`(MassProps, R_i, t_i)` per component, run the new `Matrix3`-based
+`InertialAggregator.Combine` overload (mass-weighted COM + parallel-axis,
+same algorithm as today, no representation change), then rebase the
+combined result into the link's reference frame the same way the existing
+`Combine(parts, linkAnchor)` overload already does — just with `Matrix3`
+in place of `Pose`.
+
+## Scope (locked)
+
+**Backend:**
+- Parent-relative joint origin (existing formula, corrected parent lookup).
+- Root detection via `LinkHierarchy.Roots(links)` (pure, already
+ unit-tested), not `links[0]`.
+- Multi-mesh union per link: tessellate every `ComponentIds` entry, un-bake
+ each into the link's reference frame, concatenate with vertex-offset
+ bookkeeping (same pattern `SolidWorksMeshTessellator` already uses
+ internally for multi-body union, applied across components instead of
+ bodies).
+- Multi-part mass/inertia via a new `Matrix3`-parameterized
+ `InertialAggregator.Combine` overload; existing `Quaternion` overload
+ becomes a thin wrapper, byte-identical for existing callers.
+
+**UI (small, motivated by the backend change):**
+- Once a link's frame is defined by *which* assigned component is first,
+ that needs to be visible, not implicit. Add a `(primary)` marker on the
+ first mesh in:
+ - `Sw2gzCreateRobotPmp`'s "Selected: name Mesh: a, b" info label.
+ - `LinkTreeView`'s per-node label (which already shows a `[N parts]`
+ suffix — this sits next to it).
+
+**Out of scope:** joint type/axis/limit detection (Fixed only, unchanged),
+reordering or removing individual mesh entries within a link, changing
+which component is primary after the fact, real root orientation.
+
+## Code seams
+
+| Action | File | Change |
+|---|---|---|
+| EDIT | `SW2GZ/URDFExport/Sw2gzRobotExporter.cs` | Replace the single `baseR/baseT`-vs-everyone loop with: resolve root via `LinkHierarchy.Roots`; for each non-root link, look up its own `ParentName`'s reference pose (not root's) for the joint formula; replace the single-component tessellate/pose/mass calls with a loop over `ComponentIds`, unioning mesh + combining mass via the new helpers below. |
+| EDIT | `SW2GZ/Build/InertialAggregator.cs` | Extract the existing `Combine` loop body into a `Matrix3`-parameterized core; add `Combine(IReadOnlyList<(MassProps Props, Matrix3 Rotation, Vector3 Position)> parts)` (+ the `linkAnchor`-rebase overload in `Matrix3` form). Existing `Quaternion`-based overloads call the new core via `Matrix3.FromQuaternion` — byte-identical output, no behavior change for current callers (`SwJointStateSampler` and friends). |
+| EDIT (new private helper) | `SW2GZ/URDFExport/Sw2gzRobotExporter.cs` | Small private mesh-union method: given a link's `ComponentIds` + reference `(R, t)`, tessellate each, un-bake into the reference frame, concatenate with vertex-offset-adjusted triangle indices. Not extracted to a shared file — single caller, matches `SolidWorksMeshTessellator`'s existing private-helper precedent for this exact pattern. |
+| EDIT | `SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs` | `RefreshSelectedInfo` — mark the first `ComponentIds` entry `(primary)` in the mesh list string. |
+| EDIT | `SW2GZ/UI/LinkTreeView.cs` | `BuildNode`/`RefreshActiveNodeLabel` — same `(primary)` marker in the node label, next to the existing `[N parts]` suffix. |
+| TEST | `Test/URDFExport/Sw2gzRobotExporterTests.cs` (confirmed existing) | 3+-level chain: grandchild joint origin computed relative to its true (non-root) parent, not root. Reroot-then-export: root resolved by structure, not list order. Multi-mesh link: union mesh vertex/triangle count = sum of parts, correctly positioned in the reference frame. Existing single-mesh / 2-level-flat cases stay numerically identical (they're the degenerate case of the new logic). |
+| TEST | `Test/Build/InertialAggregatorTests.cs`, `Test/Build/InertialAggregatorRotationTests.cs` (both confirmed existing) | New `Matrix3` overload matches the existing `Quaternion` overload byte-for-byte when rotations are equivalent (cross-checked via `Matrix3.FromQuaternion` — test-only conversion, never production code). `InertialAggregatorRotationTests.cs` already covers non-identity-rotation cases for the `Quaternion` path — mirror the same cases against the new `Matrix3` overload. Multi-part combine + rebase math unchanged from what's already tested there. |
+
+## Definition of done
+
+- All new + existing tests green (470 baseline + new cases above).
+- Add-in compiles clean (`SW2GZ.csproj` + `Test/SW2GZ.Writers.Test.csproj`).
+- Live SW check on `FULL_ARM.SLDASM`: build a 3-level chain (base → mid →
+ leaf) and a link with 2+ assigned mesh components; confirm correct
+ placement/orientation in Preview (or RViz) — both the joint chain and the
+ unioned mesh land where they should, not just "doesn't crash."
+- DLL redeployed to `C:\Program Files\SW2GZ\SW2GZ.dll` after the live
+ check passes.