From e3339918292af41d50951361b3f71474ed7688ab Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Wed, 1 Jul 2026 06:37:14 +0530 Subject: [PATCH 01/16] docs: add SolidWorks API reference for SW2GZ implementation Catalogs every SW COM API member the codebase calls (12 categories, active vs legacy-pipeline flagged) with file provenance and usage context; wires it into CLAUDE.md as required reading before new SW COM calls. --- CLAUDE.md | 13 ++ docs/reference/solidworks-api.md | 339 +++++++++++++++++++++++++++++++ 2 files changed, 352 insertions(+) create mode 100644 docs/reference/solidworks-api.md 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/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`. From 185bd84b6fc9f6b07d04b103356343ce76e51b5e Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Wed, 1 Jul 2026 10:59:55 +0530 Subject: [PATCH 02/16] feat(robot): minimal Create Robot wizard + exporter (v3 rebuild) Adds a working Create Robot PMP (Links step seeded from top-level components, all Fixed to the first/base link) and a matching Sw2gzRobotExporter that writes a real URDF + meshes, wired into both Export and Preview. Fixes a real, pre-existing bug found along the way: Component2.Transform2's 3x3 rotation block is column-major, not row-major as every call site assumed (verified against Component2.GetBox ground truth). This silently inverted rotation for any non-identity component in SolidWorksMeshTessellator and SolidWorksAssemblyWalker (used by World/Asset export too, not just Robot) -- fixed in both. Each link's mesh is un-baked into its own native part-local frame using a new IComponentPoses/SolidWorksComponentPoses (exact per-component rotation + translation, not an AABB approximation), so joint origin/rpy carry the real relative pose between parent and child instead of identity, while joint type stays hardcoded Fixed -- a mate-driven type/axis/limit detector was attempted and reverted after live testing showed it misclassifying joints; see memory robot-mode-dev for the postmortem. 464 -> 470 tests green (net of the reverted detector's own tests). --- SW2GZ/SW/SwAddin.cs | 21 +- SW2GZ/SW2GZ.csproj | 4 + .../SwSurface/Abstractions/IComponentPoses.cs | 20 + SW2GZ/SwSurface/SolidWorksAssemblyWalker.cs | 16 +- SW2GZ/SwSurface/SolidWorksComponentPoses.cs | 64 +++ SW2GZ/SwSurface/SolidWorksMeshTessellator.cs | 14 +- SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs | 477 ++++++++++++++++++ SW2GZ/URDFExport/Sw2gzDocToExportConfig.cs | 8 + SW2GZ/URDFExport/Sw2gzExportConfig.cs | 11 +- SW2GZ/URDFExport/Sw2gzModelExporter.cs | 12 +- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 312 ++++++++++++ Test/SW2GZ.Writers.Test.csproj | 4 + Test/URDFExport/Sw2gzRobotExporterTests.cs | 183 +++++++ 13 files changed, 1123 insertions(+), 23 deletions(-) create mode 100644 SW2GZ/SwSurface/Abstractions/IComponentPoses.cs create mode 100644 SW2GZ/SwSurface/SolidWorksComponentPoses.cs create mode 100644 SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs create mode 100644 SW2GZ/URDFExport/Sw2gzRobotExporter.cs create mode 100644 Test/URDFExport/Sw2gzRobotExporterTests.cs 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/Pmp/Sw2gzCreateRobotPmp.cs b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs new file mode 100644 index 0000000..2c808dd --- /dev/null +++ b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs @@ -0,0 +1,477 @@ +/* +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. Minimal v3 +rebuild cut: a flat list of links (one per top-level component), all fixed +to a single base link — no mate-driven joint detection yet (see +agent-progress/progress.md "Robot mode gutted for clean rebuild"). 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 (seeded from top-level components; first link = base, + every other link gets an implicit Fixed joint to it) + 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.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 IdLinksDescr = 11; + private const int IdLinksBtnBar = 12; + private const int IdLinksList = 13; + + 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 _reseedBtn; + private System.Windows.Forms.Button _removeLinkBtn; + private System.Windows.Forms.Button _clearLinksBtn; + + // PMP-native controls. + private PropertyManagerPageListbox _linksList; + private PropertyManagerPageLabel _reviewLinksLabel; + private PropertyManagerPageLabel _reviewBaseLabel; + private PropertyManagerPageLabel _reviewJointsLabel; + + 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(); + if (_liveDoc.Robot.Links.Count == 0) SeedLinksFromAssembly(); + 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); + grp.AddControl2(IdLinksDescr, + (short)swPropertyManagerPageControlType_e.swControlType_Label, + "One link per top-level component. The first link is the base " + + "link; every other link gets a Fixed joint to it. Reseed " + + "re-scans the assembly for new components without dropping " + + "existing links.", + (short)leftEdge, visibleEnabled, ""); + + _linksBar = NewBar(260, 32); + _reseedBtn = NewBarButton("Reseed", 70); + _removeLinkBtn = NewBarButton("Remove", 70); + _clearLinksBtn = NewBarButton("Clear all", 80); + _reseedBtn.Click += (s, e) => HandleReseed(); + _removeLinkBtn.Click += (s, e) => HandleRemoveLink(); + _clearLinksBtn.Click += (s, e) => HandleClearLinks(); + _linksBar.Controls.Add(_reseedBtn); + _linksBar.Controls.Add(_removeLinkBtn); + _linksBar.Controls.Add(_clearLinksBtn); + _linksBar.Resize += (s, e) => CenterRow(_linksBar, _reseedBtn, _removeLinkBtn, _clearLinksBtn); + CenterRow(_linksBar, _reseedBtn, _removeLinkBtn, _clearLinksBtn); + _linksBarHandle = (PropertyManagerPageWindowFromHandle)grp.AddControl2( + IdLinksBtnBar, + (short)swPropertyManagerPageControlType_e.swControlType_WindowFromHandle, + "", (short)leftEdge, visibleEnabled, "Reseed, remove, or clear links"); + _linksBarHandle.Height = 34; + _linksBarHandle.SetWindowHandlex64(_linksBar.Handle.ToInt64()); + + _linksList = (PropertyManagerPageListbox)grp.AddControl2( + IdLinksList, + (short)swPropertyManagerPageControlType_e.swControlType_Listbox, + "Links", (short)leftEdge, visibleEnabled, "Current robot links"); + ((IPropertyManagerPageListbox)_linksList).Height = 140; + + RefreshLinksList(); + return grp; + } + + 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 ─────────────────────────────────────────────────── + private void SeedLinksFromAssembly() + { + try + { + object[] comps = (object[])((AssemblyDoc)_modelDoc).GetComponents(true); + if (comps == null) return; + var existing = new HashSet( + _liveDoc.Robot.Links.SelectMany(l => l.ComponentIds), StringComparer.OrdinalIgnoreCase); + foreach (object o in comps) + { + var c = (Component2)o; + if (c.IsSuppressed()) continue; + if (string.IsNullOrEmpty(c.Name2)) continue; + if (existing.Contains(c.Name2)) continue; + string linkName = UniqueLinkName(RosNameSanitizer.Sanitize(c.Name2).Value); + _liveDoc.Robot.Links.Add(new LinkDef { Name = linkName, ComponentIds = { c.Name2 } }); + existing.Add(c.Name2); + } + RebuildParentsAndJoints(); + RefreshLinksList(); + } + catch (Exception e) { logger.Warn("SeedLinksFromAssembly failed", e); } + } + + private void HandleReseed() => SeedLinksFromAssembly(); + + private void HandleRemoveLink() + { + int idx = _linksList != null ? _linksList.CurrentSelection : -1; + if (idx < 0 || idx >= _liveDoc.Robot.Links.Count) return; + _liveDoc.Robot.Links.RemoveAt(idx); + RebuildParentsAndJoints(); + RefreshLinksList(); + } + + private void HandleClearLinks() + { + _liveDoc.Robot.Links.Clear(); + _liveDoc.Robot.Joints.Clear(); + RefreshLinksList(); + } + + // Every link after the first is Fixed to the first (the base link). + // Re-derived after every list mutation so Links/Joints never drift out + // of sync — there is no manual joint editing and no mate-driven + // detection in this cut (removed 2026-07-01 — detection was + // misclassifying joints; reverted to the last known-good, fully-Fixed + // baseline until it's rebuilt and verified live). + private void RebuildParentsAndJoints() + { + _liveDoc.Robot.Joints.Clear(); + if (_liveDoc.Robot.Links.Count == 0) return; + + LinkDef baseLink = _liveDoc.Robot.Links[0]; + baseLink.ParentName = string.Empty; + for (int i = 1; i < _liveDoc.Robot.Links.Count; i++) + { + LinkDef link = _liveDoc.Robot.Links[i]; + link.ParentName = baseLink.Name; + _liveDoc.Robot.Joints.Add(new JointDef + { + Name = baseLink.Name + "_to_" + link.Name, + ParentLink = baseLink.Name, + ChildLink = link.Name, + Type = UrdfJointType.Fixed, + }); + } + } + + private static string UniqueLinkName(string baseName) + { + // Caller already checked against ComponentIds for dedup; this only + // guards a same-named-different-instance edge case. + return baseName; + } + + private void RefreshLinksList() + { + if (_linksList == null) return; + _linksList.Clear(); + for (int i = 0; i < _liveDoc.Robot.Links.Count; i++) + { + LinkDef link = _liveDoc.Robot.Links[i]; + _linksList.AddItems(i == 0 ? link.Name + " (base)" : link.Name + " -> " + link.ParentName + " (fixed)"); + } + if (_liveDoc.Robot.Links.Count > 0) _linksList.CurrentSelection = 0; + } + + 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) { } + void IPropertyManagerPage2Handler9.OnGainedFocus(int Id) { } + void IPropertyManagerPage2Handler9.OnLostFocus(int Id) { } + 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) { } + 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..573176f --- /dev/null +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -0,0 +1,312 @@ +/* +Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md. + +Robot mode v3 — minimal validating cut for the clean rebuild (see +agent-progress/progress.md "Robot mode gutted for clean rebuild"). One link +per top-level component (config.RobotLinks[0] is the base link), every other +link Fixed to it — no mate-driven joint detection (removed 2026-07-01, was +misclassifying joints; reverted to this known-good baseline), but the joint +DOES carry the real relative pose (rotation + translation) between parent +and child, not an identity placeholder. + +Each link's mesh is expressed in that component's OWN native +part-local 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. 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); + + string baseLinkName = links[0].Name; + string baseCompName = links[0].ComponentIds?.FirstOrDefault(); + (Matrix3 baseR, Vector3 baseT) = TryGetPose(poses, baseCompName, issues, baseLinkName); + + foreach (LinkDef link in links) + { + string compName = link.ComponentIds?.FirstOrDefault(); + 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 '" + link.Name + "' — could not tessellate '" + compName + "': " + ex.Message, + "Sw2gzRobotExporter")); + meshWorld = null; + } + + if (meshWorld != null) + { + MeshData meshLocal; + if (link.Name == baseLinkName) + { + // Base = frame is identity by convention; only recenter. + meshLocal = Translate(meshWorld, -baseT); + } + else + { + (Matrix3 childR, Vector3 childT) = TryGetPose(poses, compName, issues, link.Name); + meshLocal = UnbakeToLocal(meshWorld, childR, childT); + + Matrix3 rJoint = baseR.Transpose() * childR; + Vector3 tJoint = baseR.Transpose().Mul(childT - baseT); + jointOrigins[link.Name] = tJoint; + jointRpys[link.Name] = rJoint.ToRpy(); + } + + string daeFile = link.Name + ".dae"; + DaeWriter.Write(meshLocal, Path.Combine(meshesDir, daeFile), withNormals: true); + meshFiles[link.Name] = daeFile; + } + + 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, 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); + } + } + + 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); + } + + 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; + + // 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/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..cd44739 --- /dev/null +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -0,0 +1,183 @@ +/* +Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md. +*/ +using System; +using System.Collections.Generic; +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 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_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")); + } + } +} From af33ca2f044a71ec0d0deaf5aef811426fa91b8b Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Wed, 1 Jul 2026 11:04:13 +0530 Subject: [PATCH 03/16] docs(progress): robot v3 wizard live + column-major transform fix --- agent-progress/progress.md | 60 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/agent-progress/progress.md b/agent-progress/progress.md index 81d3138..52696ea 100644 --- a/agent-progress/progress.md +++ b/agent-progress/progress.md @@ -1,11 +1,67 @@ # 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) +## ▶ CONTINUE HERE — Robot mode v3: minimal Create Robot wizard + exporter live (branch `feat/robot-mode-v3`, pushed, commit 185bd84) + +**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 From 5913648bd0f7c7ed713a7f73aa816ad95ad34551 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 08:55:32 +0530 Subject: [PATCH 04/16] docs(progress): revert failed link-hierarchy wizard attempt, DLL redeployed to last-known-good --- agent-progress/progress.md | 31 +++++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/agent-progress/progress.md b/agent-progress/progress.md index 52696ea..5b98c18 100644 --- a/agent-progress/progress.md +++ b/agent-progress/progress.md @@ -8,6 +8,37 @@ Addin compiles clean (SW closed for MSBuild; regasm MSB3216 is non-fatal). ## ▶ CONTINUE HERE — Robot mode v3: minimal Create Robot wizard + exporter live (branch `feat/robot-mode-v3`, pushed, commit 185bd84) +**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 From 43bde51325d2351537e17567f5faddf6aec0da3d Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 10:37:42 +0530 Subject: [PATCH 05/16] docs(spec): robot joint/link relative pose + multi-mesh links design Parent-relative joint origin (currently always root-relative), tree-based root detection, multi-mesh union per link, InertialAggregator Matrix3 overload to avoid new quaternion-conversion code. Approved via brainstorming. --- ...-07-02-robot-joint-relative-pose-design.md | 143 ++++++++++++++++++ 1 file changed, 143 insertions(+) create mode 100644 docs/superpowers/specs/2026-07-02-robot-joint-relative-pose-design.md 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. From 15956788e01d1c2ca3542b37c78682767df21297 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:01:46 +0530 Subject: [PATCH 06/16] feat(robot): Matrix3-parameterized InertialAggregator.Combine overloads Add Matrix3-parameterized twins of InertialAggregator.Combine to allow callers working entirely in Matrix3/Vector3 (e.g. Sw2gzRobotExporter) to avoid constructing a Quaternion just to aggregate inertia. Both overloads share CombineCore (same algorithm, proven direction Quaternion->Matrix3 only). Refactored CombineCore to de-duplicate inertia aggregation logic: - Combine(IReadOnlyList<(Props, Pose)>) converts pose to Matrix3, delegates - Combine(IReadOnlyList<(Props, Matrix3, Vector3)>) direct Matrix3 path - Combine(parts, anchorR, anchorT) rebase overload for link-local frame 475 tests green; added 6 new InertialAggregatorMatrixTests verifying byte-identical results for identity/non-identity rotations, null/empty handling, and anchor-based rebasing. --- SW2GZ/Build/InertialAggregator.cs | 83 +++++++++---- Test/Build/InertialAggregatorMatrixTests.cs | 123 ++++++++++++++++++++ Test/Build/InertialAggregatorTests.cs | 2 +- 3 files changed, 183 insertions(+), 25 deletions(-) create mode 100644 Test/Build/InertialAggregatorMatrixTests.cs 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/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); } From bbfc3ebe8230506a1c27d2e9af9c236d8f01fbab Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:13:17 +0530 Subject: [PATCH 07/16] fix(robot): joint origin relative to declared parent, root by tree structure --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 61 ++++++++++------- Test/URDFExport/Sw2gzRobotExporterTests.cs | 79 ++++++++++++++++++++++ 2 files changed, 117 insertions(+), 23 deletions(-) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index 573176f..5badf59 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -85,15 +85,33 @@ public static ValidationReport Export( var jointOrigins = new Dictionary(StringComparer.Ordinal); var jointRpys = new Dictionary(StringComparer.Ordinal); - string baseLinkName = links[0].Name; - string baseCompName = links[0].ComponentIds?.FirstOrDefault(); - (Matrix3 baseR, Vector3 baseT) = TryGetPose(poses, baseCompName, issues, baseLinkName); + // 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 { @@ -109,28 +127,27 @@ public static ValidationReport Export( if (meshWorld != null) { - MeshData meshLocal; - if (link.Name == baseLinkName) - { - // Base = frame is identity by convention; only recenter. - meshLocal = Translate(meshWorld, -baseT); - } - else - { - (Matrix3 childR, Vector3 childT) = TryGetPose(poses, compName, issues, link.Name); - meshLocal = UnbakeToLocal(meshWorld, childR, childT); - - Matrix3 rJoint = baseR.Transpose() * childR; - Vector3 tJoint = baseR.Transpose().Mul(childT - baseT); - jointOrigins[link.Name] = tJoint; - jointRpys[link.Name] = rJoint.ToRpy(); - } + 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); @@ -150,7 +167,7 @@ public static ValidationReport Export( } string urdfPath = Path.Combine(urdfDir, pkg + ".urdf.xacro"); - WriteUrdf(urdfPath, pkg, links, meshFiles, masses, jointOrigins, jointRpys, config.EmitWorldLink, swToRos); + WriteUrdf(urdfPath, pkg, baseLinkName, links, meshFiles, masses, jointOrigins, jointRpys, config.EmitWorldLink, swToRos); return new ValidationReport(issues); } @@ -192,7 +209,7 @@ private static MeshData UnbakeToLocal(MeshData mesh, Matrix3 r, Vector3 t) } private static void WriteUrdf( - string path, string pkg, List links, + string path, string pkg, string baseLinkName, List links, Dictionary meshFiles, Dictionary masses, Dictionary jointOrigins, Dictionary jointRpys, bool emitWorldLink, Matrix3 swToRos) @@ -203,8 +220,6 @@ private static void WriteUrdf( w.WriteStartElement("robot"); w.WriteAttributeString("name", pkg); - string baseLinkName = links[0].Name; - // 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 diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs index cd44739..6db1a03 100644 --- a/Test/URDFExport/Sw2gzRobotExporterTests.cs +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -142,6 +142,85 @@ public void Export_JointRotationIsRealRelativeRotation_NotIdentity() 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_NoLinks_Throws() { From 9a83c1d8babfdabee6ba8c8d1cb3d50852566f49 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:18:39 +0530 Subject: [PATCH 08/16] fix(robot): warn on unresolvable joint parent instead of silent zero origin --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 6 +++++ Test/URDFExport/Sw2gzRobotExporterTests.cs | 31 ++++++++++++++++++++++ 2 files changed, 37 insertions(+) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index 5badf59..d21f883 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -147,6 +147,12 @@ public static ValidationReport Export( 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")); + } try { diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs index 6db1a03..0ad3809 100644 --- a/Test/URDFExport/Sw2gzRobotExporterTests.cs +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -221,6 +221,37 @@ public void Export_RootDetectedByTreeStructure_NotListPosition() 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() { From 9f3f739707efa49daf5d4ed5745544d7d73a9fa2 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:23:16 +0530 Subject: [PATCH 09/16] feat(robot): union every assigned mesh component per link, not just the first --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 76 +++++++++++++--------- Test/URDFExport/Sw2gzRobotExporterTests.cs | 71 ++++++++++++++++++++ 2 files changed, 115 insertions(+), 32 deletions(-) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index d21f883..b223944 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -38,6 +38,7 @@ that rotation baked into its mesh rather than reflected in its own TF triad — */ using System; using System.Collections.Generic; +using System.Drawing; using System.Globalization; using System.IO; using System.Linq; @@ -112,25 +113,12 @@ public static ValidationReport Export( (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; - } + 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 (meshWorld != null) + if (meshLocal != 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; @@ -195,23 +183,47 @@ private static (Matrix3, Vector3) TryGetPose( } } - private static MeshData Translate(MeshData mesh, Vector3 t) + // 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) { - 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); - } + var verts = new List(); + var tris = new List(); + Color? color = null; + Matrix3 refRInv = refR.Transpose(); - // 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); + 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); } private static void WriteUrdf( diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs index 0ad3809..65edb5c 100644 --- a/Test/URDFExport/Sw2gzRobotExporterTests.cs +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -3,6 +3,7 @@ */ using System; using System.Collections.Generic; +using System.Globalization; using System.IO; using System.Linq; using System.Numerics; @@ -33,6 +34,14 @@ private sealed class FakeTess : IMeshTessellator 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 FakeMassProps : IMassProperties { public bool ThrowOnGet; @@ -289,5 +298,67 @@ public void Export_EmitWorldLink_AddsWorldJointWithRotation() 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); + } } } From 8b61a898dfa0a895bcf958bd5923104a4428e4a7 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:30:50 +0530 Subject: [PATCH 10/16] fix(robot): resolve Color namespace ambiguity from System.Drawing import --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index b223944..6b3364c 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -38,7 +38,6 @@ that rotation baked into its mesh rather than reflected in its own TF triad — */ using System; using System.Collections.Generic; -using System.Drawing; using System.Globalization; using System.IO; using System.Linq; @@ -196,7 +195,7 @@ private static MeshData UnionMeshInLocalFrame( { var verts = new List(); var tris = new List(); - Color? color = null; + System.Drawing.Color? color = null; Matrix3 refRInv = refR.Transpose(); foreach (string compName in componentIds ?? (IReadOnlyList)Array.Empty()) From fc9bf81466b0080881adeaae2bf455b146468b3f Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:37:13 +0530 Subject: [PATCH 11/16] feat(robot): combine mass/inertia of every assigned mesh component per link --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 60 ++++++++++++++++------ Test/URDFExport/Sw2gzRobotExporterTests.cs | 46 +++++++++++++++++ 2 files changed, 90 insertions(+), 16 deletions(-) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index 6b3364c..6a29fc7 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -141,22 +141,7 @@ public static ValidationReport Export( "Sw2gzRobotExporter")); } - 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")); - } + masses[link.Name] = CombineMass(massProps, poses, link.ComponentIds, linkR, linkT, issues, link.Name); } string urdfPath = Path.Combine(urdfDir, pkg + ".urdf.xacro"); @@ -225,6 +210,49 @@ private static MeshData UnionMeshInLocalFrame( 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, diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs index 65edb5c..15b5d3c 100644 --- a/Test/URDFExport/Sw2gzRobotExporterTests.cs +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -42,6 +42,14 @@ 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; @@ -360,5 +368,43 @@ public void Export_MultiComponentLink_UnionsAllMeshesInLinkReferenceFrame() 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); + } } } From 73a2551e7bd4bffa9fbcc00a2c9f4e59546aea99 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:44:06 +0530 Subject: [PATCH 12/16] test(robot): distinguish per-component pose lookup from shared-frame reuse in mass combine --- Test/URDFExport/Sw2gzRobotExporterTests.cs | 63 ++++++++++++++++++++++ 1 file changed, 63 insertions(+) diff --git a/Test/URDFExport/Sw2gzRobotExporterTests.cs b/Test/URDFExport/Sw2gzRobotExporterTests.cs index 15b5d3c..710a027 100644 --- a/Test/URDFExport/Sw2gzRobotExporterTests.cs +++ b/Test/URDFExport/Sw2gzRobotExporterTests.cs @@ -406,5 +406,68 @@ public void Export_MultiComponentLink_CombinesMassOfAllAssignedComponents() // 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."); + } } } From 9f3dc18bad500ad529ec45d7f2a9145622115de1 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:46:31 +0530 Subject: [PATCH 13/16] feat(robot): surface which mesh component is primary in the Links UI --- SW2GZ/UI/LinkTreeView.cs | 13 ++ SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs | 327 +++++++++++++++++++--------- 2 files changed, 240 insertions(+), 100 deletions(-) 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 index 2c808dd..bd0d9e4 100644 --- a/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs +++ b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs @@ -2,20 +2,23 @@ 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. Minimal v3 -rebuild cut: a flat list of links (one per top-level component), all fixed -to a single base link — no mate-driven joint detection yet (see -agent-progress/progress.md "Robot mode gutted for clean rebuild"). 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. +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 (seeded from top-level components; first link = base, - every other link gets an implicit Fixed joint to it) + 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 @@ -29,6 +32,7 @@ corrupts SW's PMP renderer. Nav clicks defer via BeginInvoke so the using SW2GZ.Build; using SW2GZ.Build.Model; using SW2GZ.Build.Urdf; +using SW2GZ.UI; using SW2GZ.URDFExport; using SW2GZ.Utilities; @@ -61,10 +65,18 @@ public sealed class Sw2gzCreateRobotPmp : PropertyManagerPage2Handler9 private const int IdNavBar = 3; // Step groups. - private const int IdLinksGroup = 10; - private const int IdLinksDescr = 11; - private const int IdLinksBtnBar = 12; - private const int IdLinksList = 13; + 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; @@ -84,16 +96,26 @@ public sealed class Sw2gzCreateRobotPmp : PropertyManagerPage2Handler9 // WinForms per-step action bar (Links step). private PropertyManagerPageWindowFromHandle _linksBarHandle; private System.Windows.Forms.Panel _linksBar; - private System.Windows.Forms.Button _reseedBtn; + private System.Windows.Forms.Button _addLinkBtn; private System.Windows.Forms.Button _removeLinkBtn; private System.Windows.Forms.Button _clearLinksBtn; // PMP-native controls. - private PropertyManagerPageListbox _linksList; + 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)); @@ -117,7 +139,6 @@ public Sw2gzCreateRobotPmp(SldWorks swApp, ModelDoc2 modelDoc, Sw2gzDoc liveDoc, } BuildPage(); - if (_liveDoc.Robot.Links.Count == 0) SeedLinksFromAssembly(); ShowStep(StepLinks); } @@ -212,43 +233,73 @@ private void BuildNavBar() private PropertyManagerPageGroup BuildLinksGroup(int grpOptions, int leftEdge, int visibleEnabled) { var grp = (PropertyManagerPageGroup)_page.AddGroupBox(IdLinksGroup, "Links", grpOptions); - grp.AddControl2(IdLinksDescr, - (short)swPropertyManagerPageControlType_e.swControlType_Label, - "One link per top-level component. The first link is the base " - + "link; every other link gets a Fixed joint to it. Reseed " - + "re-scans the assembly for new components without dropping " - + "existing links.", - (short)leftEdge, visibleEnabled, ""); + + 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); - _reseedBtn = NewBarButton("Reseed", 70); + _addLinkBtn = NewBarButton("Add link", 80); _removeLinkBtn = NewBarButton("Remove", 70); _clearLinksBtn = NewBarButton("Clear all", 80); - _reseedBtn.Click += (s, e) => HandleReseed(); - _removeLinkBtn.Click += (s, e) => HandleRemoveLink(); - _clearLinksBtn.Click += (s, e) => HandleClearLinks(); - _linksBar.Controls.Add(_reseedBtn); + _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, _reseedBtn, _removeLinkBtn, _clearLinksBtn); - CenterRow(_linksBar, _reseedBtn, _removeLinkBtn, _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, "Reseed, remove, or clear links"); + "", (short)leftEdge, visibleEnabled, "Add, remove, or clear links"); _linksBarHandle.Height = 34; _linksBarHandle.SetWindowHandlex64(_linksBar.Handle.ToInt64()); - _linksList = (PropertyManagerPageListbox)grp.AddControl2( - IdLinksList, - (short)swPropertyManagerPageControlType_e.swControlType_Listbox, - "Links", (short)leftEdge, visibleEnabled, "Current robot links"); - ((IPropertyManagerPageListbox)_linksList).Height = 140; + _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); + _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()); - RefreshLinksList(); 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); @@ -272,92 +323,152 @@ private PropertyManagerPageGroup BuildReviewGroup(int grpOptions, int leftEdge, } // ─── Action handlers ─────────────────────────────────────────────────── - private void SeedLinksFromAssembly() + // 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 { - object[] comps = (object[])((AssemblyDoc)_modelDoc).GetComponents(true); - if (comps == null) return; - var existing = new HashSet( - _liveDoc.Robot.Links.SelectMany(l => l.ComponentIds), StringComparer.OrdinalIgnoreCase); - foreach (object o in comps) + 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) { - var c = (Component2)o; - if (c.IsSuppressed()) continue; - if (string.IsNullOrEmpty(c.Name2)) continue; - if (existing.Contains(c.Name2)) continue; - string linkName = UniqueLinkName(RosNameSanitizer.Sanitize(c.Name2).Value); - _liveDoc.Robot.Links.Add(new LinkDef { Name = linkName, ComponentIds = { c.Name2 } }); - existing.Add(c.Name2); + name = "base_link"; } - RebuildParentsAndJoints(); - RefreshLinksList(); + 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("SeedLinksFromAssembly failed", e); } + catch (Exception e) { logger.Warn("HandleAddLink failed", e); } } - private void HandleReseed() => SeedLinksFromAssembly(); - private void HandleRemoveLink() { - int idx = _linksList != null ? _linksList.CurrentSelection : -1; - if (idx < 0 || idx >= _liveDoc.Robot.Links.Count) return; - _liveDoc.Robot.Links.RemoveAt(idx); - RebuildParentsAndJoints(); - RefreshLinksList(); + 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(); - RefreshLinksList(); + _linkTree.Rebuild(); + RefreshSelectedInfo(null); } - // Every link after the first is Fixed to the first (the base link). - // Re-derived after every list mutation so Links/Joints never drift out - // of sync — there is no manual joint editing and no mate-driven - // detection in this cut (removed 2026-07-01 — detection was - // misclassifying joints; reverted to the last known-good, fully-Fixed - // baseline until it's rebuilt and verified live). - private void RebuildParentsAndJoints() + // 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() { - _liveDoc.Robot.Joints.Clear(); - if (_liveDoc.Robot.Links.Count == 0) return; - - LinkDef baseLink = _liveDoc.Robot.Links[0]; - baseLink.ParentName = string.Empty; - for (int i = 1; i < _liveDoc.Robot.Links.Count; i++) + if (_linkNameBox == null || _liveDoc.Robot.Links.Count == 0) return; + ISelectionMgr selMgr = (ISelectionMgr)_modelDoc.SelectionManager; + if (selMgr == null) return; + if (selMgr.GetSelectedObjectCount2(MeshSelectionMark) != 1) { - LinkDef link = _liveDoc.Robot.Links[i]; - link.ParentName = baseLink.Name; - _liveDoc.Robot.Joints.Add(new JointDef - { - Name = baseLink.Name + "_to_" + link.Name, - ParentLink = baseLink.Name, - ChildLink = link.Name, - Type = UrdfJointType.Fixed, - }); + 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; + } + + private void RefreshSelectedInfo(LinkDef link) + { + if (_selectedInfoLabel == null) return; + _selectedInfoLabel.Caption = link == null + ? "Selected: (none)" + : "Selected: " + link.Name + " Mesh: " + DescribeMeshes(link.ComponentIds); } - private static string UniqueLinkName(string baseName) + // 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) { - // Caller already checked against ComponentIds for dedup; this only - // guards a same-named-different-instance edge case. - return baseName; + 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); } - private void RefreshLinksList() + // 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() { - if (_linksList == null) return; - _linksList.Clear(); - for (int i = 0; i < _liveDoc.Robot.Links.Count; i++) + _liveDoc.Robot.Joints.Clear(); + foreach (LinkDef link in _liveDoc.Robot.Links) { - LinkDef link = _liveDoc.Robot.Links[i]; - _linksList.AddItems(i == 0 ? link.Name + " (base)" : link.Name + " -> " + link.ParentName + " (fixed)"); + 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, + }); } - if (_liveDoc.Robot.Links.Count > 0) _linksList.CurrentSelection = 0; } private void RefreshReviewLabels() @@ -439,8 +550,20 @@ void IPropertyManagerPage2Handler9.OnClose(int Reason) void IPropertyManagerPage2Handler9.AfterClose() { if (_okay && _liveDoc != null) _onCommit(_liveDoc); _onClosed?.Invoke(); } void IPropertyManagerPage2Handler9.OnButtonPress(int Id) { } - void IPropertyManagerPage2Handler9.OnGainedFocus(int Id) { } - void IPropertyManagerPage2Handler9.OnLostFocus(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; @@ -450,7 +573,11 @@ void IPropertyManagerPage2Handler9.OnLostFocus(int Id) { } 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) { } + 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) { } From a67c4c81682ad66381042a2babbbd4e512074f2a Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 11:56:50 +0530 Subject: [PATCH 14/16] docs(robot): update Sw2gzRobotExporter header to describe multi-mesh/parent-relative behavior Header still described the pre-Tasks-2-4 flat/single-component behavior (root-relative joints, first-component-only mesh). Update to describe parent-relative joint origin via LinkHierarchy.Roots, multi-component mesh/mass union in a shared reference frame, and the base_link mass/mesh identity-convention asymmetry. --- SW2GZ/URDFExport/Sw2gzRobotExporter.cs | 48 +++++++++++++++++--------- 1 file changed, 32 insertions(+), 16 deletions(-) diff --git a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs index 6a29fc7..24a5086 100644 --- a/SW2GZ/URDFExport/Sw2gzRobotExporter.cs +++ b/SW2GZ/URDFExport/Sw2gzRobotExporter.cs @@ -1,18 +1,31 @@ /* Copyright (c) 2026 Aryan Arlikar. MIT License — see CONTRIBUTING.md. -Robot mode v3 — minimal validating cut for the clean rebuild (see -agent-progress/progress.md "Robot mode gutted for clean rebuild"). One link -per top-level component (config.RobotLinks[0] is the base link), every other -link Fixed to it — no mate-driven joint detection (removed 2026-07-01, was -misclassifying joints; reverted to this known-good baseline), but the joint -DOES carry the real relative pose (rotation + translation) between parent -and child, not an identity placeholder. - -Each link's mesh is expressed in that component's OWN native -part-local 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 +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) @@ -23,10 +36,13 @@ under the real joint chain and each link's TF frame reflects its true SW 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. 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. +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): From 05a667259095b26dadb0372e2bfda91f4938489b Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 12:01:59 +0530 Subject: [PATCH 15/16] docs(progress): robot joint/link relative pose + multi-mesh links, live-tested working 3-level chain (grandchild joint relative to real parent, not root) and multi-mesh link (both parts positioned correctly) confirmed working in SolidWorks on FULL_ARM.SLDASM. 481 tests green, both projects build clean. --- agent-progress/progress.md | 149 +- ...26-07-02-robot-joint-relative-pose-plan.md | 1282 +++++++++++++++++ 2 files changed, 1430 insertions(+), 1 deletion(-) create mode 100644 docs/superpowers/plans/2026-07-02-robot-joint-relative-pose-plan.md diff --git a/agent-progress/progress.md b/agent-progress/progress.md index 5b98c18..c33f688 100644 --- a/agent-progress/progress.md +++ b/agent-progress/progress.md @@ -6,7 +6,154 @@ 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 v3: minimal Create Robot wizard + exporter live (branch `feat/robot-mode-v3`, pushed, commit 185bd84) +## 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- 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" +``` From 7e55b0f8a831292513d54db934d1ec55b2270761 Mon Sep 17 00:00:00 2001 From: Aryan Arlikar Date: Thu, 2 Jul 2026 12:30:02 +0530 Subject: [PATCH 16/16] fix(robot): highlight selected link's mesh when clicked in the hierarchy Selecting a link in the tree only updated a text label; the Mesh box and 3D viewport never reflected the pick, staying empty regardless of tree selection. Reuses the legacy CommonSwOperations.SelectComponents helper (its own doc comment already described this exact use case, just never wired into the current wizard) to select the link's ComponentIds under the same mark the Mesh box listens to. --- SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs | 34 ++++++++++++++++++++++++++++- 1 file changed, 33 insertions(+), 1 deletion(-) diff --git a/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs index bd0d9e4..38bb3cf 100644 --- a/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs +++ b/SW2GZ/UI/Pmp/Sw2gzCreateRobotPmp.cs @@ -32,6 +32,7 @@ Joints rebuilt (Fixed) from each link's ParentName) using SW2GZ.Build; using SW2GZ.Build.Model; using SW2GZ.Build.Urdf; +using SW2GZ.SwSurface; using SW2GZ.UI; using SW2GZ.URDFExport; using SW2GZ.Utilities; @@ -281,7 +282,7 @@ private PropertyManagerPageGroup BuildLinksGroup(int grpOptions, int leftEdge, i 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); + _linkTree.ActiveLinkChanged += (s, link) => { RefreshSelectedInfo(link); HighlightLinkMesh(link); }; _linkTree.LinksChanged += (s, e) => RebuildJoints(); _linkTree.SetLinks(_liveDoc.Robot.Links); _treeHandle = (PropertyManagerPageWindowFromHandle)grp.AddControl2( @@ -428,6 +429,37 @@ private string LinkNameBoxValue() 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;