first commit
This commit is contained in:
@@ -0,0 +1,21 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2026 earthtojake
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
@@ -0,0 +1,104 @@
|
||||
---
|
||||
name: sdf
|
||||
description: SDFormat/SDF model and world generation, validation, and simulator handoff. Use for `.sdf` files, SDFormat XML, Python `gen_sdf()` sources, models, worlds, links, joints, poses, frames, inertials, visual/collision geometry, mesh URIs, sensors, lights, physics, plugins, includes, Gazebo, static SDF review, or simulator-specific metadata. Do not use for signed-distance-field geometry.
|
||||
---
|
||||
|
||||
# SDF
|
||||
|
||||
Provenance: maintained in [earthtojake/text-to-cad](https://github.com/earthtojake/text-to-cad).
|
||||
Use the installed local skill files as the runtime source of truth; the
|
||||
repository link is only for provenance and release review.
|
||||
|
||||
Use this skill when the deliverable is an SDFormat document or a Python `gen_sdf()` source. SDFormat describes simulator and world behavior: models, worlds, frames, poses, links, joints, inertials, visuals, collisions, sensors, lights, physics, plugins, includes, and simulator metadata.
|
||||
|
||||
This skill is for **SDFormat**, not signed-distance-field geometry.
|
||||
|
||||
## Core rules
|
||||
|
||||
1. Treat the Python file defining `gen_sdf()` as source of truth. Treat configured `.sdf` files as generated artifacts unless the user explicitly asks for direct XML editing.
|
||||
2. Identify the target consumer before editing: Gazebo/libsdformat version, another simulator, visualization-only tooling, model package, or world handoff.
|
||||
3. Decide document kind: model-level SDF, world-level SDF, or model-in-world. Prefer model-level SDF for reusable robot/object exports.
|
||||
4. Use SI units unless the target explicitly requires otherwise: meters, kilograms, seconds, radians.
|
||||
5. Prefer `version="1.12"` for new outputs unless the target consumer constrains the version.
|
||||
6. Establish the design ledger before writing poses, frames, joint axes, mesh scales, inertials, sensors, or plugins. Use `references/design-ledger.md` and `references/llm-guardrails.md`.
|
||||
7. Do not infer spatial transforms from visual impression alone. Derive poses, axes, scale, mass, inertia, and frame names from upstream source data, drawings, simulator documentation, measured values, or explicit assumptions.
|
||||
8. Prefer helper functions and named constants over large XML string literals. Hidden numbers are a common SDF failure mode.
|
||||
9. Generate only explicit targets with `scripts/sdf` or the repository's existing SDF launcher. Do not run directory-wide generation.
|
||||
10. Regenerate upstream geometry, mesh, robot-description, render, topology, or package assets with their owning workflows before regenerating SDF that references them.
|
||||
11. After generation, run available checks: bundled validation, optional `gz sdf --check`, simulator load, joint motion, and plugin/sensor startup.
|
||||
12. Report assumptions, skipped checks, unresolved resource paths, and target-specific compatibility risks.
|
||||
|
||||
## Scope
|
||||
|
||||
Use this skill for SDFormat outputs and generators. Do not use it for signed-distance-field modeling, raw geometry generation, planning semantics, or to paper over incorrect upstream robot/source data unless the task is explicitly simulator-only.
|
||||
|
||||
## CAD Viewer Handoff
|
||||
|
||||
After completing SDF work that creates or modifies a `.sdf`, you must ALWAYS hand the explicit file path to `$cad-viewer` when that skill is installed. `$cad-viewer` must start CAD Viewer if it is not already running and return link(s) to the relevant created or updated file(s); if `$cad-viewer` is unavailable or startup fails, report that instead of silently omitting the handoff.
|
||||
|
||||
## Workflow
|
||||
|
||||
1. Locate the `gen_sdf()` source and intended `.sdf` output.
|
||||
2. Read or create the design ledger.
|
||||
3. Read `references/frame-semantics.md` before editing any `<pose>`, `<frame>`, joint axis, `relative_to`, `expressed_in`, nested scope, sensor frame, or plugin frame.
|
||||
4. Edit the generator source, not generated XML.
|
||||
5. Use optional builder helpers when they make the generated structure clearer; raw ElementTree is still allowed.
|
||||
6. Regenerate the explicit target.
|
||||
7. Treat bundled validation as a guardrail, not simulator proof.
|
||||
8. Run target-consumer smoke tests when available.
|
||||
9. Report checks run, checks skipped, and assumptions. Static rendering does not execute SDF plugins or read file-authored motion metadata.
|
||||
|
||||
## Commands
|
||||
|
||||
Run with the project or workspace Python environment. Treat `python` in examples as an interpreter placeholder; if bare `python` is unavailable, substitute `python3`, a project virtualenv interpreter, or the configured interpreter path.
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/source.py
|
||||
python scripts/sdf path/to/source.py -o path/to/output.sdf
|
||||
python scripts/sdf path/to/a.py=out/a.sdf path/to/b.py=out/b.sdf
|
||||
```
|
||||
|
||||
Plain Python targets write sibling `.sdf` files beside their sources. `-o` / `--output` is valid only with one plain target. `SOURCE.py=OUTPUT.sdf` supports custom multi-target destinations.
|
||||
|
||||
If the runtime supports optional external checking:
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/source.py --gz-check auto
|
||||
python scripts/sdf path/to/source.py --gz-check required
|
||||
python scripts/sdf path/to/source.py --gz-check never
|
||||
```
|
||||
|
||||
`gz sdf --check` is optional target-consumer validation. It should be reported as skipped when unavailable unless explicitly required.
|
||||
|
||||
## Required report shape
|
||||
|
||||
When finishing an SDF task, include a compact report:
|
||||
|
||||
```text
|
||||
Generated: path/to/model.sdf from path/to/model.py
|
||||
Checks run:
|
||||
- bundled SDF validation: passed
|
||||
- gz sdf --check: skipped, gz not installed
|
||||
- simulator load: skipped, target simulator unavailable
|
||||
- viewer handoff: `$cad-viewer` link returned
|
||||
Assumptions:
|
||||
- Assumed mesh units are meters.
|
||||
- Assumed lidar frame is coincident with lidar_link.
|
||||
Risks:
|
||||
- Camera plugin filename was not verified in the target simulator environment.
|
||||
```
|
||||
|
||||
## References
|
||||
|
||||
- Generation command: `references/gen-sdf.md`
|
||||
- Generator contract: `references/generator-contract.md`
|
||||
- SDF workflow: `references/sdf-workflow.md`
|
||||
- Builder helpers: `references/builder-helpers.md`
|
||||
- LLM guardrails: `references/llm-guardrails.md`
|
||||
- Design ledger: `references/design-ledger.md`
|
||||
- Frame semantics: `references/frame-semantics.md`
|
||||
- Validation scope: `references/validation.md`
|
||||
- Smoke tests: `references/smoke-tests.md`
|
||||
- Interoperability notes: `references/interoperability.md`
|
||||
- Examples: `references/examples.md`
|
||||
- Runtime notes and current limitations: `references/implementation-notes.md`
|
||||
@@ -0,0 +1,4 @@
|
||||
interface:
|
||||
display_name: "SDF"
|
||||
short_description: "Generate and validate SDFormat robot models."
|
||||
default_prompt: "Use $sdf to generate, update, and validate SDFormat/SDF robot or simulator models from Python sources, handing generated SDFs to $cad-viewer when available."
|
||||
@@ -0,0 +1,136 @@
|
||||
# SDF builder helpers
|
||||
|
||||
Builder helpers are optional. They exist to reduce common LLM mistakes in XML construction, not to replace SDFormat or libsdformat.
|
||||
|
||||
## Why helpers exist
|
||||
|
||||
LLMs often make these mistakes when writing raw XML:
|
||||
|
||||
- wrong pose value length;
|
||||
- hidden degree/radian conversion;
|
||||
- missing `relative_to` or `expressed_in`;
|
||||
- zero or non-finite joint axes;
|
||||
- negative primitive dimensions;
|
||||
- mesh scale applied inconsistently;
|
||||
- inertial values copied from visuals;
|
||||
- plugin filenames invented from plausible names.
|
||||
|
||||
Helpers should make the common path explicit, typed-ish, and auditable while still returning ordinary `xml.etree.ElementTree.Element` nodes.
|
||||
|
||||
## Design constraints
|
||||
|
||||
- Standard library only.
|
||||
- No mandatory Gazebo, ROS, NumPy, lxml, CAD, or mesh dependency.
|
||||
- Return ElementTree elements.
|
||||
- Compose with raw ElementTree calls.
|
||||
- Validate local numeric shape and finiteness.
|
||||
- Do not attempt full SDFormat schema validation.
|
||||
- Do not silently invent frames, axes, inertials, plugins, or sensor parameters.
|
||||
|
||||
## Recommended helper surface
|
||||
|
||||
Names may vary to match the current runtime package, but the helper surface should stay small.
|
||||
|
||||
```python
|
||||
# XML basics
|
||||
text(parent, tag, value, attrib=None)
|
||||
fmt_float(value)
|
||||
fmt_vector(values)
|
||||
|
||||
# Poses and axes
|
||||
pose(parent, xyz=(0, 0, 0), rpy=(0, 0, 0), *, relative_to=None,
|
||||
rotation_format="euler_rpy", degrees=False)
|
||||
quat_pose(parent, xyz=(0, 0, 0), quat_xyzw=(0, 0, 0, 1), *, relative_to=None)
|
||||
axis(parent, xyz=(0, 0, 1), *, expressed_in=None)
|
||||
|
||||
# Document structure
|
||||
sdf_root(version="1.12")
|
||||
world(parent, name)
|
||||
model(parent, name, *, static=None, pose=None)
|
||||
frame(parent, name, *, attached_to=None, pose=None)
|
||||
link(parent, name, *, pose=None, inertial=None)
|
||||
joint(parent, name, joint_type, parent_link, child_link, *, pose=None,
|
||||
axis_xyz=None, axis_expressed_in=None, axis2_xyz=None, limits=None)
|
||||
|
||||
# Geometry
|
||||
visual(parent, name, *, pose=None)
|
||||
collision(parent, name, *, pose=None)
|
||||
box(parent, size_xyz)
|
||||
sphere(parent, radius)
|
||||
cylinder(parent, radius, length)
|
||||
capsule(parent, radius, length)
|
||||
mesh(parent, uri, *, scale=None)
|
||||
|
||||
# Physics and metadata
|
||||
inertial(parent, mass, inertia, *, pose=None)
|
||||
sensor(parent, name, sensor_type, *, pose=None, topic=None, update_rate=None)
|
||||
plugin(parent, name, filename, params=None)
|
||||
include(parent, uri, *, name=None, pose=None)
|
||||
```
|
||||
|
||||
## Numeric behavior
|
||||
|
||||
Helpers should reject:
|
||||
|
||||
- non-finite numbers;
|
||||
- vectors with the wrong length;
|
||||
- negative or zero primitive dimensions;
|
||||
- zero joint axes;
|
||||
- zero-mass inertials;
|
||||
- zero-norm quaternions.
|
||||
|
||||
Helpers may warn, but should not silently fix:
|
||||
|
||||
- non-unit axes;
|
||||
- non-normalized quaternions;
|
||||
- use of degrees;
|
||||
- missing `relative_to` on nontrivial poses.
|
||||
|
||||
## Example
|
||||
|
||||
```python
|
||||
from sdf.builder import axis, box, collision, joint, link, model, sdf_root, visual
|
||||
|
||||
BASE_SIZE_M = (0.4, 0.3, 0.1)
|
||||
LIFT_AXIS_Z = (0.0, 0.0, 1.0)
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = sdf_root("1.12")
|
||||
robot = model(sdf, "lift_fixture", static=False)
|
||||
|
||||
base = link(robot, "base_link")
|
||||
v = visual(base, "base_visual")
|
||||
box(v, BASE_SIZE_M)
|
||||
c = collision(base, "base_collision")
|
||||
box(c, BASE_SIZE_M)
|
||||
|
||||
carriage = link(robot, "carriage_link")
|
||||
j = joint(
|
||||
robot,
|
||||
"lift_joint",
|
||||
"prismatic",
|
||||
parent_link="base_link",
|
||||
child_link="carriage_link",
|
||||
axis_xyz=LIFT_AXIS_Z,
|
||||
axis_expressed_in="base_link",
|
||||
)
|
||||
|
||||
return {
|
||||
"xml": sdf,
|
||||
"assumptions": [
|
||||
{"code": "inertials_placeholder", "message": "Inertials omitted pending measured masses."}
|
||||
],
|
||||
}
|
||||
```
|
||||
|
||||
## When not to use helpers
|
||||
|
||||
Raw ElementTree is acceptable when:
|
||||
|
||||
- a target simulator needs unusual extension XML;
|
||||
- a plugin has arbitrary nested configuration;
|
||||
- an existing generator already has a clear internal abstraction;
|
||||
- the user asked for a minimal XML-only source.
|
||||
|
||||
Even then, keep named constants, ledger comments, and validation.
|
||||
@@ -0,0 +1,115 @@
|
||||
# SDF design ledger
|
||||
|
||||
Create or update this ledger before writing SDF XML. The ledger can live in the response, a project note, or comments in the generator source. The goal is to externalize spatial and simulator assumptions before they become hard-to-audit XML.
|
||||
|
||||
## Document
|
||||
|
||||
| Field | Value |
|
||||
|---|---|
|
||||
| Output path | |
|
||||
| Source file | |
|
||||
| SDF version | `1.12` unless constrained |
|
||||
| Document kind | model / world / model-in-world |
|
||||
| Target consumer | Gazebo / other simulator / visualization-only / model package |
|
||||
| Units | meters, kilograms, seconds, radians unless documented otherwise |
|
||||
| Coordinate convention | REP-103-like / simulator-specific / documented exception |
|
||||
| World support needed | yes/no; reason |
|
||||
| Optional external checks | `gz sdf --check`, simulator load, CAD Viewer, other |
|
||||
|
||||
## Model or world scope
|
||||
|
||||
| Item | Value |
|
||||
|---|---|
|
||||
| Model/world name | |
|
||||
| Static or dynamic | |
|
||||
| Canonical link, if relevant | |
|
||||
| Model/world pose | xyz + rpy/quaternion |
|
||||
| Model/world pose `relative_to` | |
|
||||
| Includes | URI + purpose |
|
||||
| World physics/lights/plugins | source and target simulator |
|
||||
|
||||
## Frames
|
||||
|
||||
| Frame | Scope | Attached to | Pose | Pose `relative_to` | Purpose | Source |
|
||||
|---|---|---|---|---|---|---|
|
||||
| | | | | | | |
|
||||
|
||||
Use named frames for clarity when multiple sensors, nested models, tool frames, plugin frames, or repeated transforms depend on the same relationship.
|
||||
|
||||
## Links
|
||||
|
||||
| Link | Physical / frame-like | Pose | Pose `relative_to` | Inertial source | Sensor/plugin attached | Notes |
|
||||
|---|---|---|---|---|---|---|
|
||||
| | | | | | | |
|
||||
|
||||
Physical dynamic links need inertials. Frame-like links may omit inertials only when documented.
|
||||
|
||||
## Joints
|
||||
|
||||
| Joint | Type | Parent | Child | Pose | Pose frame / `relative_to` | Axis | Axis frame / `expressed_in` | Limits | Positive motion | Source |
|
||||
|---|---|---|---|---|---|---|---|---|---|---|
|
||||
| | | | | | | | | | | |
|
||||
|
||||
For revolute and prismatic joints, record limit units: radians for revolute, meters for prismatic. Continuous joints should not be given artificial finite position limits unless a simulator-specific reason is documented.
|
||||
|
||||
## Geometry
|
||||
|
||||
| Owner | Visual/collision | Name | Geometry type | Pose | Pose `relative_to` | URI or dimensions | Mesh units | Scale | Source |
|
||||
|---|---|---|---|---|---|---|---|---|---|
|
||||
| | | | | | | | | | |
|
||||
|
||||
Collision geometry should be selected for simulation cost and stability, not just visual similarity.
|
||||
|
||||
## Inertials
|
||||
|
||||
| Link | Mass | COM pose | Inertia tensor | Method/source | Confidence |
|
||||
|---|---|---|---|---|---|
|
||||
| | | | | | |
|
||||
|
||||
Mark approximations clearly. Very small, zero, negative, or guessed inertias are high risk for simulation.
|
||||
|
||||
## Sensors and plugins
|
||||
|
||||
| Element | Parent | Pose/frame | Filename/type | Parameters | Source docs | Assumptions |
|
||||
|---|---|---|---|---|---|---|
|
||||
| | | | | | | |
|
||||
|
||||
Do not invent plugin filenames, topics, frame names, namespaces, controller parameters, or update rates. Derive them from simulator documentation or user-provided configuration.
|
||||
|
||||
## Mesh URI policy
|
||||
|
||||
| URI kind | Allowed? | Resolution expectation | Notes |
|
||||
|---|---|---|---|
|
||||
| Relative local path | | Relative to generated `.sdf` location | |
|
||||
| `file://` | | Absolute local file | |
|
||||
| `model://` | | Simulator model path | |
|
||||
| `package://` | | Simulator/ROS package environment | |
|
||||
| `fuel://`, `http://`, `https://` | | External resource | |
|
||||
|
||||
## Assumptions to report
|
||||
|
||||
List every guessed or inferred value:
|
||||
|
||||
- transform or pose;
|
||||
- axis sign or positive-motion convention;
|
||||
- mesh unit or scale;
|
||||
- mass, COM, or inertia;
|
||||
- target simulator behavior;
|
||||
- plugin parameter;
|
||||
- unresolved external URI;
|
||||
- skipped validation or smoke test.
|
||||
|
||||
If a value cannot be derived or safely assumed, generate a minimal placeholder only when the user asked for a placeholder, and label it as such.
|
||||
|
||||
## Compact response template
|
||||
|
||||
```text
|
||||
SDF source: path/to/source.py
|
||||
Generated target: path/to/output.sdf
|
||||
Target consumer: Gazebo Harmonic, SDF 1.12
|
||||
Bundled validation: passed with 2 warnings
|
||||
External checks: gz sdf --check skipped, gz not installed
|
||||
Assumptions:
|
||||
- Assumed mesh units are meters.
|
||||
- Assumed camera optical frame follows simulator plugin documentation.
|
||||
```
|
||||
@@ -0,0 +1,148 @@
|
||||
# SDF examples
|
||||
|
||||
These examples illustrate the intended style: explicit frames, source-relative assumptions, simple helpers, and structured assumptions.
|
||||
|
||||
## Minimal model with raw ElementTree
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def text(parent, tag, value, attrib=None):
|
||||
child = ET.SubElement(parent, tag, attrib or {})
|
||||
child.text = str(value)
|
||||
return child
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = ET.Element("sdf", {"version": "1.12"})
|
||||
model = ET.SubElement(sdf, "model", {"name": "calibration_box"})
|
||||
text(model, "pose", "0 0 0 0 0 0", {"relative_to": "world"})
|
||||
|
||||
link = ET.SubElement(model, "link", {"name": "body"})
|
||||
visual = ET.SubElement(link, "visual", {"name": "body_visual"})
|
||||
geometry = ET.SubElement(visual, "geometry")
|
||||
box = ET.SubElement(geometry, "box")
|
||||
text(box, "size", "0.1 0.1 0.1")
|
||||
|
||||
collision = ET.SubElement(link, "collision", {"name": "body_collision"})
|
||||
c_geometry = ET.SubElement(collision, "geometry")
|
||||
c_box = ET.SubElement(c_geometry, "box")
|
||||
text(c_box, "size", "0.1 0.1 0.1")
|
||||
|
||||
inertial = ET.SubElement(link, "inertial")
|
||||
text(inertial, "mass", "1.0")
|
||||
inertia = ET.SubElement(inertial, "inertia")
|
||||
text(inertia, "ixx", "0.0016666667")
|
||||
text(inertia, "iyy", "0.0016666667")
|
||||
text(inertia, "izz", "0.0016666667")
|
||||
text(inertia, "ixy", "0")
|
||||
text(inertia, "ixz", "0")
|
||||
text(inertia, "iyz", "0")
|
||||
|
||||
return {
|
||||
"xml": sdf,
|
||||
"assumptions": ["Box inertia uses a uniform-density primitive approximation."],
|
||||
}
|
||||
```
|
||||
|
||||
## Minimal world
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def text(parent, tag, value, attrib=None):
|
||||
child = ET.SubElement(parent, tag, attrib or {})
|
||||
child.text = str(value)
|
||||
return child
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = ET.Element("sdf", {"version": "1.12"})
|
||||
world = ET.SubElement(sdf, "world", {"name": "empty_lit_world"})
|
||||
|
||||
light = ET.SubElement(world, "light", {"name": "sun", "type": "directional"})
|
||||
text(light, "pose", "0 0 10 0 0 0", {"relative_to": "world"})
|
||||
text(light, "cast_shadows", "true")
|
||||
|
||||
return {
|
||||
"xml": sdf,
|
||||
"metadata": {"document_kind": "world"},
|
||||
"assumptions": ["World intentionally contains no inline model."],
|
||||
}
|
||||
```
|
||||
|
||||
## Model with builder helpers
|
||||
|
||||
```python
|
||||
from sdf.builder import (
|
||||
box,
|
||||
collision,
|
||||
inertial,
|
||||
joint,
|
||||
link,
|
||||
model,
|
||||
pose,
|
||||
sdf_root,
|
||||
visual,
|
||||
)
|
||||
|
||||
|
||||
BASE_MASS_KG = 2.0
|
||||
ARM_MASS_KG = 0.5
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = sdf_root("1.12")
|
||||
robot = model(sdf, "two_link_demo")
|
||||
pose(robot, relative_to="world")
|
||||
|
||||
base = link(robot, "base_link")
|
||||
box(visual(base, "base_visual"), (0.4, 0.3, 0.1))
|
||||
box(collision(base, "base_collision"), (0.4, 0.3, 0.1))
|
||||
inertial(base, BASE_MASS_KG, (0.02, 0.03, 0.04, 0, 0, 0))
|
||||
|
||||
arm = link(robot, "arm_link")
|
||||
box(visual(arm, "arm_visual"), (0.3, 0.05, 0.05))
|
||||
box(collision(arm, "arm_collision"), (0.3, 0.05, 0.05))
|
||||
inertial(arm, ARM_MASS_KG, (0.001, 0.004, 0.004, 0, 0, 0))
|
||||
|
||||
joint(
|
||||
robot,
|
||||
"shoulder_pan",
|
||||
"revolute",
|
||||
"base_link",
|
||||
"arm_link",
|
||||
axis_xyz=(0, 0, 1),
|
||||
axis_expressed_in="base_link",
|
||||
)
|
||||
|
||||
return {
|
||||
"xml": sdf,
|
||||
"assumptions": [
|
||||
"Primitive inertials are approximate placeholders.",
|
||||
"Positive shoulder_pan rotates counterclockwise when viewed from +Z."
|
||||
],
|
||||
}
|
||||
```
|
||||
|
||||
## Plugin block from documentation
|
||||
|
||||
When a plugin block is copied from target simulator documentation, preserve its explicit parameters and report the source:
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def add_known_plugin(parent):
|
||||
plugin = ET.SubElement(
|
||||
parent,
|
||||
"plugin",
|
||||
{"name": "example_control", "filename": "libexample_control.so"},
|
||||
)
|
||||
ET.SubElement(plugin, "namespace").text = "robot1"
|
||||
return plugin
|
||||
```
|
||||
|
||||
Do not invent plugin fields. If the documentation source is not available, mark the plugin as unverified in the generator envelope.
|
||||
@@ -0,0 +1,87 @@
|
||||
# SDF frame and pose semantics
|
||||
|
||||
Use this reference before editing any SDF `<pose>`, `<frame>`, `<joint>`, `<axis>`, `<visual>`, `<collision>`, sensor, or plugin placement.
|
||||
|
||||
## Core pose rules
|
||||
|
||||
A typical SDF pose is:
|
||||
|
||||
```xml
|
||||
<pose relative_to="some_frame">x y z roll pitch yaw</pose>
|
||||
```
|
||||
|
||||
or, when using quaternion rotation:
|
||||
|
||||
```xml
|
||||
<pose rotation_format="quat_xyzw" relative_to="some_frame">x y z qx qy qz qw</pose>
|
||||
```
|
||||
|
||||
Rules to keep in mind:
|
||||
|
||||
- The first three values are position.
|
||||
- With the default `rotation_format="euler_rpy"`, the pose has six values: `x y z roll pitch yaw`.
|
||||
- With `rotation_format="quat_xyzw"`, the pose has seven values: `x y z qx qy qz qw`.
|
||||
- Euler angles are radians by default. `degrees="true"` is valid SDF but should be avoided in generated sources unless the target explicitly requires it.
|
||||
- `relative_to` names the frame in which the pose is expressed.
|
||||
- If `relative_to` is omitted, SDF applies element-specific defaults, commonly the frame of the parent XML element. This may be valid but is easy to misread. Prefer explicit `relative_to` for nontrivial generated poses.
|
||||
- Nested scopes may use `::`, for example `outer_model::inner_model::sensor_frame`.
|
||||
|
||||
## Joint pose and axes
|
||||
|
||||
For SDF joints:
|
||||
|
||||
- `<parent>` names the parent frame or `world`.
|
||||
- `<child>` names the child frame; `world` is not valid as the child.
|
||||
- Joint pose defaults are easy to misinterpret. Use explicit `<pose relative_to="...">` when the joint frame is not obviously the child-link frame.
|
||||
- `<axis><xyz>...</xyz></axis>` is the unit axis vector.
|
||||
- An axis is expressed in the joint frame unless the axis `expressed_in` attribute specifies another frame.
|
||||
- `axis2` is used for multi-axis joints such as `revolute2` and `universal`.
|
||||
- Axis vectors should be finite, nonzero, and normalized.
|
||||
|
||||
Record the expected positive motion in the design ledger. Example: “positive shoulder_pan rotates the arm counterclockwise when viewed from +Z.”
|
||||
|
||||
## Visual and collision poses
|
||||
|
||||
A `<visual>` or `<collision>` pose places that geometry owner relative to its parent frame unless `relative_to` says otherwise. In ordinary model-level use, that parent is the link frame.
|
||||
|
||||
Do not use visual offsets to hide a wrong link or joint frame. If a mesh needs an offset because the mesh asset origin is not the link frame, record that fact in the geometry table.
|
||||
|
||||
## Named frames
|
||||
|
||||
Use `<frame>` when a reusable transform is meaningful:
|
||||
|
||||
```xml
|
||||
<frame name="camera_optical_frame" attached_to="camera_link">
|
||||
<pose relative_to="camera_link">0 0 0 -1.57079632679 0 -1.57079632679</pose>
|
||||
</frame>
|
||||
```
|
||||
|
||||
Frames are useful for sensors, plugins, tool frames, nested models, and repeated placement logic. They also make generated SDF more auditable.
|
||||
|
||||
`attached_to` and `relative_to` are different:
|
||||
|
||||
- `attached_to` says what the frame moves with.
|
||||
- `relative_to` says how the frame's pose numbers are represented.
|
||||
|
||||
The `attached_to` chain should not cycle and should eventually resolve to a link, model, world, joint, or another valid frame target.
|
||||
|
||||
## LLM guardrails
|
||||
|
||||
Do not infer any of the following from prose alone:
|
||||
|
||||
- sign of a joint axis;
|
||||
- frame in which an axis is expressed;
|
||||
- RPY order or units;
|
||||
- mesh origin convention;
|
||||
- `relative_to` frame;
|
||||
- nested-scope reference;
|
||||
- sensor optical-frame transform;
|
||||
- plugin frame/topic semantics.
|
||||
|
||||
When data is missing, either ask for the source data or write an explicitly labeled assumption.
|
||||
|
||||
## Useful official references
|
||||
|
||||
- SDFormat pose semantics: `https://sdformat.org/tutorials?tut=pose_frame_semantics`
|
||||
- SDFormat pose fields: `https://sdformat.org/spec/1.12/world/`
|
||||
- SDFormat joint element: `https://sdformat.org/spec/1.12/joint/`
|
||||
@@ -0,0 +1,55 @@
|
||||
# SDF generation command
|
||||
|
||||
Use the SDF launcher to regenerate explicit SDFormat outputs from Python sources with top-level `gen_sdf()` functions.
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/model.py
|
||||
python scripts/sdf path/to/model.py -o path/to/robot.sdf
|
||||
python scripts/sdf path/to/a.py=out/a.sdf path/to/b.py=out/b.sdf
|
||||
```
|
||||
|
||||
Plain Python targets write sibling `.sdf` files beside their sources. `-o` / `--output` is valid only with one plain target. Use `SOURCE.py=OUTPUT.sdf` pairs for custom multi-target destinations.
|
||||
|
||||
Relative source targets and CLI output paths resolve from the current working directory. When running from outside the skill directory, prefix the launcher path so source and target files still resolve from the intended workspace.
|
||||
|
||||
## What the command does
|
||||
|
||||
The tool should:
|
||||
|
||||
1. import the target Python source;
|
||||
2. call top-level zero-argument `gen_sdf()`;
|
||||
3. normalize the returned SDF XML or envelope;
|
||||
4. parse the generated XML;
|
||||
5. run bundled dependency-light validation;
|
||||
6. optionally run external `gz sdf --check` if requested and available;
|
||||
7. write the requested `.sdf` only after required checks pass;
|
||||
8. print structured warnings and assumptions.
|
||||
|
||||
The command does **not** regenerate geometry, meshes, GLB/topology outputs, render assets, robot-description files, planning metadata, or simulator resource packages. Regenerate those with their owning workflows before regenerating SDF that references them.
|
||||
|
||||
## Optional external check
|
||||
|
||||
If implemented, the recommended flag is:
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/model.py --gz-check auto
|
||||
python scripts/sdf path/to/model.py --gz-check required
|
||||
python scripts/sdf path/to/model.py --gz-check never
|
||||
python scripts/sdf path/to/model.py --strict
|
||||
```
|
||||
|
||||
- `auto`: run `gz sdf --check` when `gz` is available; otherwise report the check as skipped and continue.
|
||||
- `required`: fail if `gz` is unavailable or if `gz sdf --check` exits nonzero.
|
||||
- `never`: skip external checking.
|
||||
|
||||
If the external checker requires a path, write to a temporary file first. Do not overwrite the target until bundled validation and all required external checks pass.
|
||||
|
||||
## Failure behavior
|
||||
|
||||
If validation fails, the newly generated payload is not written. Existing output files may be stale; fix the Python source and regenerate.
|
||||
|
||||
Errors should be blocking. Warnings and assumptions should be reported but should not fail generation unless the user or CI explicitly requests strict behavior.
|
||||
|
||||
## Execution safety
|
||||
|
||||
The launcher imports generator modules. Top-level Python code in generator files may execute. Use this command only for trusted project sources. A future subprocess runner may reduce accidental side effects, but it cannot make untrusted Python safe.
|
||||
@@ -0,0 +1,151 @@
|
||||
# SDF generator contract
|
||||
|
||||
Use this reference when creating or editing Python sources that generate SDFormat/SDF files.
|
||||
|
||||
## Source of truth
|
||||
|
||||
The Python source that defines `gen_sdf()` is the source of truth. The configured `.sdf` output is generated and should not be hand-edited.
|
||||
|
||||
## Required shape
|
||||
|
||||
`gen_sdf()` must be a top-level zero-argument function. Prefer returning the root `xml.etree.ElementTree.Element` for a complete SDFormat document.
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = ET.Element("sdf", {"version": "1.12"})
|
||||
model = ET.SubElement(sdf, "model", {"name": "sample_robot"})
|
||||
ET.SubElement(model, "link", {"name": "base_link"})
|
||||
return sdf
|
||||
```
|
||||
|
||||
The root must be `<sdf>` and must include a non-empty `version` attribute. Default new outputs to `version="1.12"` unless the user or target simulator requires another version.
|
||||
|
||||
## Accepted return values
|
||||
|
||||
Preferred:
|
||||
|
||||
```python
|
||||
def gen_sdf():
|
||||
return sdf_root_element
|
||||
```
|
||||
|
||||
Accepted for compatibility:
|
||||
|
||||
```python
|
||||
def gen_sdf():
|
||||
return """<?xml version="1.0"?>
|
||||
<sdf version="1.12">...</sdf>
|
||||
"""
|
||||
```
|
||||
|
||||
Envelope form:
|
||||
|
||||
```python
|
||||
def gen_sdf():
|
||||
return {
|
||||
"xml": sdf_root_element,
|
||||
"assumptions": [
|
||||
{"code": "mesh_units", "message": "Assumed mesh units are meters."},
|
||||
"Assumed lidar frame is coincident with lidar_link.",
|
||||
],
|
||||
"warnings": [
|
||||
{"code": "plugin_unverified", "message": "Camera plugin filename was not verified in the target simulator."}
|
||||
],
|
||||
"metadata": {
|
||||
"target_consumer": "Gazebo Harmonic",
|
||||
"sdf_version": "1.12",
|
||||
},
|
||||
}
|
||||
```
|
||||
|
||||
Envelope rules:
|
||||
|
||||
- `xml` is required and may be an XML element or XML string.
|
||||
- `assumptions`, when present, must be a list of strings or dicts with `code`, `message`, and optional `source`.
|
||||
- `warnings`, when present, must be a list of strings or dicts with `code`, `message`, and optional `source`.
|
||||
- `metadata`, when present, must be a JSON-serializable dict with scalar values.
|
||||
- Output-path fields such as `sdf_output` are unsupported; select output paths with CLI targets.
|
||||
- Unsupported envelope fields should fail with a clear error.
|
||||
|
||||
Use envelope assumptions to make spatial, physical, simulator, and resource assumptions auditable. Do not hide guesses in XML literals.
|
||||
|
||||
## Output path
|
||||
|
||||
The generated `.sdf` output path is selected by the CLI:
|
||||
|
||||
- plain source target: sibling `.sdf` beside the source;
|
||||
- `-o` / `--output`: override for one plain target;
|
||||
- `SOURCE.py=OUTPUT.sdf`: per-target override for multi-target generation.
|
||||
|
||||
The generator should not write the output file itself.
|
||||
|
||||
## Source-relative asset paths
|
||||
|
||||
Do not rely on the shell working directory for mesh or resource paths inside generator code. Prefer source-relative constants:
|
||||
|
||||
```python
|
||||
from pathlib import Path
|
||||
|
||||
SOURCE_DIR = Path(__file__).resolve().parent
|
||||
MESH_DIR = SOURCE_DIR / "meshes"
|
||||
```
|
||||
|
||||
For model packages, prefer URI conventions understood by the target simulator, such as `model://...`, `package://...`, or stable relative paths from the generated `.sdf` location.
|
||||
|
||||
## Pose and frame discipline
|
||||
|
||||
Before emitting `<pose>`, `<frame>`, `<joint>`, `<axis>`, `<visual>`, `<collision>`, sensor, or plugin placement elements, fill out the design ledger:
|
||||
|
||||
- pose value;
|
||||
- pose frame or `relative_to`;
|
||||
- joint axis and `expressed_in` frame;
|
||||
- positive-motion convention;
|
||||
- mesh units and scale;
|
||||
- source of every value.
|
||||
|
||||
Use constants with names that expose assumptions:
|
||||
|
||||
```python
|
||||
ASSUMED_BASE_TO_LIDAR_Z_M = 0.18
|
||||
ASSUMED_LIDAR_YAW_RAD = 0.0
|
||||
```
|
||||
|
||||
Do not hide spatial guesses inside XML literals.
|
||||
|
||||
## Minimal model example with explicit pose intent
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def text(parent, tag, value, attrib=None):
|
||||
child = ET.SubElement(parent, tag, attrib or {})
|
||||
child.text = str(value)
|
||||
return child
|
||||
|
||||
|
||||
def pose(parent, xyz=(0, 0, 0), rpy=(0, 0, 0), *, relative_to=None):
|
||||
attrib = {"relative_to": relative_to} if relative_to else {}
|
||||
return text(parent, "pose", " ".join(str(v) for v in (*xyz, *rpy)), attrib)
|
||||
|
||||
|
||||
def gen_sdf():
|
||||
sdf = ET.Element("sdf", {"version": "1.12"})
|
||||
model = ET.SubElement(sdf, "model", {"name": "sample_robot"})
|
||||
pose(model, relative_to="world")
|
||||
|
||||
base = ET.SubElement(model, "link", {"name": "base_link"})
|
||||
visual = ET.SubElement(base, "visual", {"name": "base_visual"})
|
||||
geometry = ET.SubElement(visual, "geometry")
|
||||
box = ET.SubElement(geometry, "box")
|
||||
text(box, "size", "0.4 0.3 0.1")
|
||||
|
||||
return {
|
||||
"xml": sdf,
|
||||
"assumptions": ["base_link is coincident with the model frame."],
|
||||
"metadata": {"target_consumer": "visualization"},
|
||||
}
|
||||
```
|
||||
@@ -0,0 +1,56 @@
|
||||
# SDF implementation notes
|
||||
|
||||
These notes describe the intended runtime shape after the rewrite. Keep this file accurate when implementation changes.
|
||||
|
||||
## Implemented behavior
|
||||
|
||||
The SDF skill should:
|
||||
|
||||
- generate explicit targets only;
|
||||
- validate generated XML before writing;
|
||||
- preserve existing `gen_sdf()` return compatibility: XML element, XML string, or envelope dict with `xml`;
|
||||
- accept envelope `assumptions`, `warnings`, and `metadata` fields;
|
||||
- support `--strict` warning-as-failure behavior;
|
||||
- optionally run `gz sdf --check` through `--gz-check auto|required|never`;
|
||||
- provide optional stdlib-only authoring helpers;
|
||||
- parse SDF XML from files or in-memory strings;
|
||||
- resolve local mesh files relative to the generated output location;
|
||||
- accept external mesh URI schemes without local filesystem resolution;
|
||||
- allow pure world files when structurally valid.
|
||||
|
||||
## Intended bundled validation scope
|
||||
|
||||
The bundled validator should check common structural and numeric errors:
|
||||
|
||||
- root element and version;
|
||||
- document shape and pure world support;
|
||||
- required names and duplicate names in local scopes;
|
||||
- pose value counts, rotation formats, finite values, degrees usage, quaternion normalization, and local `relative_to` resolution;
|
||||
- named frame attachment references and cycles;
|
||||
- joint type set, parent/child references, axis/axis2 values, limits, and dynamics numbers;
|
||||
- visual/collision owner names and geometry presence;
|
||||
- primitive geometry dimensions and mesh URI/scale;
|
||||
- local mesh path existence;
|
||||
- inertial mass and inertia tensor plausibility;
|
||||
- sensor name/type/update-rate structure;
|
||||
- plugin name/filename structure.
|
||||
|
||||
## Remaining limitations
|
||||
|
||||
The bundled validator is still not a full libsdformat or simulator validator. It should not claim to fully validate:
|
||||
|
||||
- every version-specific SDFormat schema rule;
|
||||
- all nested-model frame semantics;
|
||||
- transform math or resolved poses;
|
||||
- mesh unit conventions;
|
||||
- arbitrary mesh inertia or collision quality;
|
||||
- simulator-specific physics settings;
|
||||
- plugin schemas and runtime availability;
|
||||
- sensor runtime behavior;
|
||||
- target simulator support for every element.
|
||||
|
||||
Use the design ledger, structured diagnostics, `gz sdf --check`, simulator load tests, and explicit reporting of skipped checks.
|
||||
|
||||
## Execution safety
|
||||
|
||||
The current launcher imports generator modules in-process. Generator Python files execute code and must be trusted project sources.
|
||||
@@ -0,0 +1,76 @@
|
||||
# SDF interoperability notes
|
||||
|
||||
Use this reference when SDF work touches upstream geometry, robot-description data, Gazebo/libsdformat, model packages, or CAD Viewer.
|
||||
|
||||
## Geometry assets
|
||||
|
||||
SDF should reference geometry and mesh assets; it should not regenerate them.
|
||||
|
||||
When SDF references generated meshes, record:
|
||||
|
||||
- source geometry file;
|
||||
- exported mesh path;
|
||||
- mesh unit convention;
|
||||
- mesh origin convention;
|
||||
- visual scale;
|
||||
- collision simplification decision.
|
||||
|
||||
Regenerate geometry and mesh artifacts with their owning workflow before regenerating SDF if geometry changed.
|
||||
|
||||
## Robot descriptions
|
||||
|
||||
Keep the simulator document aligned with the upstream robot-description source when one exists.
|
||||
|
||||
Upstream robot-description data usually owns:
|
||||
|
||||
- link and joint structure used by robot-state publishing;
|
||||
- physical joint limits;
|
||||
- inertials and visual/collision geometry when that source is authoritative;
|
||||
- control-related structure and runtime interfaces.
|
||||
|
||||
Use SDF for simulator/world concerns:
|
||||
|
||||
- simulator plugins;
|
||||
- sensors requiring simulator-specific XML;
|
||||
- surfaces/contact/friction;
|
||||
- lights, terrain, physics, and worlds;
|
||||
- nested models and includes;
|
||||
- simulator-specific metadata.
|
||||
|
||||
Do not use SDF to paper over a wrong upstream frame tree unless the task explicitly targets a simulator-only model.
|
||||
|
||||
## Planning metadata
|
||||
|
||||
SDF should not define planning groups, end-effectors, group states, or disabled-collision matrices. If the task becomes IK or path-planning work, use the planning metadata workflow that owns those semantics.
|
||||
|
||||
## CAD Viewer
|
||||
|
||||
CAD Viewer can review `.sdf` files visually through `$cad-viewer` and help catch gross placement or resource issues. It cannot prove simulator dynamics, inertial validity, plugin loading, sensor topics, or joint-axis semantics.
|
||||
|
||||
Pass explicit generated or modified `.sdf` paths to `$cad-viewer` whenever it is available, and return the live viewer link it prints.
|
||||
|
||||
CAD Viewer renders SDF as static structure plus direct inspection controls. It lists plugins, sensors, lights, includes, and nested models as metadata, but does not execute plugins or consume file-authored motion contracts.
|
||||
|
||||
## Gazebo / libsdformat
|
||||
|
||||
The bundled validator is a lightweight preflight check. Use the target simulator's parser and loader when compatibility matters.
|
||||
|
||||
Good checks include:
|
||||
|
||||
```bash
|
||||
gz sdf --check path/to/model.sdf
|
||||
```
|
||||
|
||||
and a real simulator load in the target environment.
|
||||
|
||||
## Model packages and URIs
|
||||
|
||||
SDF resource resolution is environment-dependent. Record which URI forms the target consumer can resolve:
|
||||
|
||||
- relative paths from the generated `.sdf` location;
|
||||
- `model://` paths under the simulator model path;
|
||||
- `package://` paths under ROS/package resolution;
|
||||
- `fuel://` resources;
|
||||
- `http://` or `https://` assets if external fetches are allowed.
|
||||
|
||||
The bundled validator can confirm local relative paths, but it cannot prove external simulator resource paths unless the target environment is available.
|
||||
@@ -0,0 +1,115 @@
|
||||
# LLM guardrails for SDF authoring
|
||||
|
||||
This skill assumes agents are useful at structuring SDFormat documents and weak at silently deriving precise spatial, physical, and simulator-specific values. The workflow should route those weaknesses into explicit ledgers, constants, helpers, validators, and smoke tests.
|
||||
|
||||
## What agents can usually do well
|
||||
|
||||
- organize an SDF model or world into links, joints, frames, visuals, collisions, sensors, plugins, and includes;
|
||||
- translate user intent into a plausible document structure;
|
||||
- maintain naming consistency when names are explicit;
|
||||
- write small Python generators and ElementTree transformations;
|
||||
- explain assumptions and create checklists;
|
||||
- preserve existing patterns when examples are nearby.
|
||||
|
||||
## What agents should not be trusted to infer silently
|
||||
|
||||
- exact link poses, frame transforms, or joint origins;
|
||||
- positive joint-axis directions from visual appearance;
|
||||
- mesh units, mesh scale, or coordinate-system conventions;
|
||||
- center of mass or inertia tensors from rendered shape alone;
|
||||
- plugin filenames, parameters, topics, namespaces, or sensor schemas;
|
||||
- whether a plugin is a simulator runtime plugin or a CAD Viewer visualization-only extension;
|
||||
- target simulator support for a given SDFormat version or extension;
|
||||
- whether collision geometry is stable for physics;
|
||||
- whether external URIs resolve in the deployment environment.
|
||||
|
||||
## Required mitigation pattern
|
||||
|
||||
For every spatial, physical, or simulator-specific value, use one of these sources:
|
||||
|
||||
1. user-provided requirement;
|
||||
2. upstream geometry, robot-description, planning-metadata, mesh manifest, or model package source;
|
||||
3. target simulator documentation;
|
||||
4. measured or calculated value with method stated;
|
||||
5. explicit assumption recorded in the generator envelope, design ledger, or final report.
|
||||
|
||||
Do not hide guessed values in raw XML.
|
||||
|
||||
## Placeholder policy
|
||||
|
||||
Placeholders are allowed only when the user asks for a scaffold, draft, or minimal example. Mark them as placeholders and keep them easy to replace.
|
||||
|
||||
Examples of acceptable placeholders:
|
||||
|
||||
```python
|
||||
return {
|
||||
"xml": sdf,
|
||||
"assumptions": [
|
||||
{
|
||||
"code": "placeholder_inertial",
|
||||
"message": "Inertial tensor uses a primitive approximation pending measured mass properties.",
|
||||
}
|
||||
],
|
||||
}
|
||||
```
|
||||
|
||||
Examples of unacceptable placeholders:
|
||||
|
||||
- invented plugin filenames;
|
||||
- adding CAD Viewer-only motion plugins to SDF files;
|
||||
- arbitrary inertia values on a dynamic robot without a warning;
|
||||
- guessed mesh scale that makes the visual look plausible;
|
||||
- silently flipping a joint axis to match an expected screenshot.
|
||||
|
||||
## Spatial reasoning checklist
|
||||
|
||||
Before generating or modifying SDF, answer these questions in the ledger or final report:
|
||||
|
||||
| Question | Required evidence |
|
||||
|---|---|
|
||||
| What frame is each pose expressed in? | `relative_to`, source file, or documented default |
|
||||
| What frame is each joint axis expressed in? | `expressed_in` or documented default |
|
||||
| What is positive motion for each non-fixed joint? | command/test expectation or upstream source |
|
||||
| Are mesh units and scales known? | manifest, CAD export config, or explicit assumption |
|
||||
| Are visual and collision poses intentionally different? | simulation reason or source geometry |
|
||||
| Are inertials measured, calculated, approximated, or omitted? | method and confidence |
|
||||
| Are plugin and sensor parameters copied from target docs? | target simulator/version and source |
|
||||
|
||||
## Code-generation style
|
||||
|
||||
Prefer this pattern:
|
||||
|
||||
```python
|
||||
BASE_TO_CAMERA_XYZ_M = (0.18, 0.0, 0.12)
|
||||
BASE_TO_CAMERA_RPY_RAD = (0.0, -0.2, 0.0)
|
||||
CAMERA_FRAME = "camera_frame"
|
||||
|
||||
# Source: project CAD frame export 2026-05-12. RPY radians.
|
||||
pose(camera_frame, BASE_TO_CAMERA_XYZ_M, BASE_TO_CAMERA_RPY_RAD, relative_to="base_link")
|
||||
```
|
||||
|
||||
Avoid this pattern:
|
||||
|
||||
```python
|
||||
ET.SubElement(camera, "pose").text = "0.18 0 .12 0 -11.5 0"
|
||||
```
|
||||
|
||||
The second version hides units, uses degrees without saying so, and makes the source of the transform impossible to audit.
|
||||
|
||||
## Validation expectations
|
||||
|
||||
The validator should catch cheap deterministic mistakes, but it cannot prove the design is physically or simulator-correct. After bundled validation, use optional external checks and simulator smoke tests when the task depends on simulator behavior.
|
||||
|
||||
Report skipped checks explicitly. A skipped check is not automatically a failure, but it is relevant risk information.
|
||||
|
||||
## Response behavior for agents
|
||||
|
||||
When finishing an SDF task, state:
|
||||
|
||||
- source generator path and generated target path;
|
||||
- checks run and their result;
|
||||
- checks skipped and why;
|
||||
- assumptions and placeholders;
|
||||
- risks that need simulator verification.
|
||||
|
||||
Do not simply say that the file is valid. Say which validator or smoke test passed.
|
||||
@@ -0,0 +1,79 @@
|
||||
# SDF workflow
|
||||
|
||||
Use this reference when editing SDF robot model structure, world structure, mesh references, simulator metadata, or generated SDF output.
|
||||
|
||||
## Edit loop
|
||||
|
||||
1. Find the Python source that defines `gen_sdf()`.
|
||||
2. Treat that source as authoritative. Do not hand-edit generated `.sdf` output unless explicitly instructed.
|
||||
3. Identify the target consumer and required SDFormat version.
|
||||
4. Decide whether the output is model-level, world-level, or model-in-world.
|
||||
5. Fill or update the design ledger before writing XML.
|
||||
6. For every pose and axis, state the frame in which it is expressed. Use `relative_to` / `expressed_in` where ambiguity would otherwise remain.
|
||||
7. Edit the generator source.
|
||||
8. Regenerate only the explicit target.
|
||||
9. Review bundled validation errors as structural guardrails, not exhaustive simulator proof.
|
||||
10. Hand generated or modified `.sdf` files to `$cad-viewer` for live viewer links when available.
|
||||
11. Run available smoke tests.
|
||||
12. Report assumptions and skipped checks.
|
||||
|
||||
## Model vs world
|
||||
|
||||
Use **model-level SDF** when exporting a reusable robot or object model that another world can include.
|
||||
|
||||
Use **world-level SDF** when the task includes:
|
||||
|
||||
- physics engine settings;
|
||||
- lights or scene setup;
|
||||
- terrain or ground plane;
|
||||
- multiple initial model placements;
|
||||
- world plugins;
|
||||
- includes of external model packages;
|
||||
- simulator scene setup.
|
||||
|
||||
Use **model-in-world SDF** when the task explicitly needs both an inline model and world-specific context.
|
||||
|
||||
The lightweight validator should allow pure world-only documents. A world-only document with lights, physics, actors, or includes can be valid SDFormat even when it contains no inline `<model>`.
|
||||
|
||||
## Mesh references
|
||||
|
||||
SDF mesh URIs should be stable from the generated `.sdf` file's perspective or use a simulator/package URI convention understood by the consumer.
|
||||
|
||||
Good URI choices include:
|
||||
|
||||
- relative paths beside the generated SDF when the model is self-contained;
|
||||
- `model://...` for simulator model packages;
|
||||
- `package://...` when the simulator environment resolves package roots;
|
||||
- `fuel://...`, `http://...`, or `https://...` only when the consumer is expected to fetch external assets.
|
||||
|
||||
Do not use generated SDF XML as the source of truth for mesh placement. Prefer deriving visual and collision mesh references from the same source data that owns mesh instance placement.
|
||||
|
||||
## Inertials and physics
|
||||
|
||||
For dynamic models, inertial data is simulation-critical. If inertials are estimated, record the approximation method. Do not copy visual origins into inertial origins unless that is physically justified.
|
||||
|
||||
Collision geometry should be selected for stable and fast physics, not visual fidelity. Use primitive or simplified collision geometry when possible.
|
||||
|
||||
## Plugins and sensors
|
||||
|
||||
For plugins and sensors, record:
|
||||
|
||||
- plugin filename or sensor type;
|
||||
- expected simulator distribution/version;
|
||||
- topics, frames, update rates, namespaces;
|
||||
- parameter source;
|
||||
- startup smoke test result.
|
||||
|
||||
Do not invent plugin parameters. Incorrect plugin XML can pass lightweight validation and still fail at simulator load time.
|
||||
|
||||
CAD Viewer reviews SDF files as static model/world structure through `$cad-viewer` links. Do not add Explorer-only motion plugins; use simulator-native controllers, plugins, or test harnesses for simulator behavior.
|
||||
|
||||
## Existing SDF inspection
|
||||
|
||||
When inspecting existing `.sdf` files, separate three questions:
|
||||
|
||||
1. Is the XML structurally valid enough for the bundled validator?
|
||||
2. Is it compatible with the target SDFormat/libsdformat/simulator version?
|
||||
3. Does it satisfy this project's packaging, mesh, and workflow policy?
|
||||
|
||||
Do not reject valid SDF solely because it violates a project preference unless the task or repository policy requires that preference.
|
||||
@@ -0,0 +1,98 @@
|
||||
# SDF smoke tests
|
||||
|
||||
Use smoke tests after generated SDF passes bundled validation. The goal is to catch simulator and spatial failures that dependency-light XML checks cannot detect.
|
||||
|
||||
## Recommended checks
|
||||
|
||||
### Bundled validation
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/model.py
|
||||
python scripts/sdf path/to/model.py --strict
|
||||
```
|
||||
|
||||
Bundled validation runs during explicit target generation. Use `--strict` when warnings should block handoff.
|
||||
|
||||
### SDFormat parser check
|
||||
|
||||
When Gazebo tooling is installed:
|
||||
|
||||
```bash
|
||||
gz sdf --check path/to/model.sdf
|
||||
```
|
||||
|
||||
or through the skill CLI:
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/model.py --gz-check auto
|
||||
```
|
||||
|
||||
Use the exact simulator environment that will consume the file when possible.
|
||||
|
||||
### Simulator load check
|
||||
|
||||
Load the model or world in the target simulator and check:
|
||||
|
||||
- no parser warnings or plugin load errors;
|
||||
- model appears at the intended pose;
|
||||
- visual and collision assets resolve;
|
||||
- collision geometry is not visibly offset from visuals;
|
||||
- dynamic model does not explode, fall through the floor, or produce invalid inertia warnings.
|
||||
|
||||
### Joint motion check
|
||||
|
||||
For each non-fixed joint:
|
||||
|
||||
- command a small positive motion;
|
||||
- confirm the moving child moves in the expected direction;
|
||||
- confirm limits stop motion where expected;
|
||||
- confirm continuous joints can rotate continuously if intended.
|
||||
|
||||
### CAD Viewer static review
|
||||
|
||||
After generating or modifying an `.sdf`, hand the explicit path to `$cad-viewer` for a live viewer link when available.
|
||||
|
||||
- confirm direct model links, joints, frames, visuals, and collisions are placed correctly;
|
||||
- confirm includes, plugins, sensors, lights, nested models, and unsupported geometry are listed as static metadata;
|
||||
- record any simulator-only behavior that CAD Viewer cannot execute.
|
||||
|
||||
### Sensor and plugin check
|
||||
|
||||
For each sensor or plugin:
|
||||
|
||||
- confirm plugin library loads;
|
||||
- confirm expected topics/services appear;
|
||||
- confirm frame names match the design ledger;
|
||||
- confirm update rate and namespace behavior;
|
||||
- capture one sample output if practical.
|
||||
|
||||
### Visual review
|
||||
|
||||
When CAD Viewer or an equivalent viewer is available through `$cad-viewer`, return the viewer link. Visual review is useful but insufficient: it can catch gross placement and mesh problems, but it cannot prove axis frames, inertials, dynamics, or plugin behavior.
|
||||
|
||||
## Report format
|
||||
|
||||
Use a compact report:
|
||||
|
||||
```text
|
||||
Checks run:
|
||||
- bundled SDF validation: passed
|
||||
- gz sdf --check: skipped, gz not installed
|
||||
- simulator load: passed in Gazebo Harmonic
|
||||
- joint motion: shoulder_pan positive motion verified; gripper joints skipped
|
||||
- plugin startup: camera plugin unresolved, requires target simulator package
|
||||
|
||||
Assumptions:
|
||||
- Assumed mesh units are meters.
|
||||
- Assumed lidar frame is coincident with lidar_link.
|
||||
```
|
||||
|
||||
## When to stop
|
||||
|
||||
Stop and fix the generator when:
|
||||
|
||||
- bundled validation has errors;
|
||||
- `gz sdf --check` fails under a required external-check policy;
|
||||
- the simulator reports invalid inertias or unresolved required assets;
|
||||
- a joint moves opposite from the documented positive direction;
|
||||
- plugin startup fails for a plugin required by the task.
|
||||
@@ -0,0 +1,154 @@
|
||||
# SDF validation
|
||||
|
||||
Generation validates every `gen_sdf()` result before writing. This validation is dependency-light and intended to catch common structural errors. It is not a replacement for libsdformat, Gazebo, or target-simulator validation.
|
||||
|
||||
## Validation model
|
||||
|
||||
The validator should produce structured diagnostics with severities:
|
||||
|
||||
- `error`: invalid or unsafe enough to block writing output;
|
||||
- `warning`: likely problem or unverified simulator behavior; output can be written unless `--strict` is used;
|
||||
- `info`: assumption, skipped check, or useful context.
|
||||
|
||||
`--strict` treats warnings as failures.
|
||||
|
||||
## Bundled checks
|
||||
|
||||
### Root and document shape
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- the root element is `<sdf>`;
|
||||
- the root has a non-empty `version` attribute;
|
||||
- the version looks like `major.minor`;
|
||||
- the document contains meaningful SDF content such as a model, world, actor, light, include, or plugin;
|
||||
- structurally valid pure world files are accepted even when they contain no inline model.
|
||||
|
||||
### Names and scopes
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- world names are non-empty and unique at root scope;
|
||||
- root model names are non-empty and unique;
|
||||
- model link, joint, frame, sensor, visual, and collision names are non-empty where required and unique within their owner scope;
|
||||
- duplicate names are reported with a path and scope.
|
||||
|
||||
### Poses
|
||||
|
||||
The runtime should check all `<pose>` elements:
|
||||
|
||||
- default `rotation_format="euler_rpy"` has exactly six finite values;
|
||||
- `rotation_format="quat_xyzw"` has exactly seven finite values;
|
||||
- unsupported `rotation_format` is an error;
|
||||
- quaternion values are approximately normalized;
|
||||
- `degrees="true"` is a warning unless strict mode is enabled;
|
||||
- nontrivial omitted `relative_to` is a warning;
|
||||
- `relative_to` resolves within local scope when possible;
|
||||
- nested `::` references have valid syntax and resolve when the local tree is available.
|
||||
|
||||
### Frames
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- `<frame name="...">` has a non-empty unique name in its scope;
|
||||
- `attached_to`, when present, resolves locally when possible;
|
||||
- frame attachment chains do not cycle;
|
||||
- unresolved nested or external frame references are reported as warnings when local validation cannot prove them invalid.
|
||||
|
||||
### Joints
|
||||
|
||||
Known SDF 1.12 joint types:
|
||||
|
||||
```text
|
||||
continuous, revolute, gearbox, revolute2, prismatic, ball, screw, universal, fixed
|
||||
```
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- joint type is non-empty and known;
|
||||
- `<parent>` and `<child>` text exists;
|
||||
- `world` is allowed as parent but not child;
|
||||
- unscoped parent/child references exist in the same model;
|
||||
- `axis` and `axis2` vectors are finite, nonzero, and normalized;
|
||||
- `axis2` is used only where the joint type supports a second axis;
|
||||
- `expressed_in` resolves when local resolution is possible;
|
||||
- limit and dynamics values are finite or documented infinities where SDFormat permits them;
|
||||
- finite lower limits do not exceed finite upper limits;
|
||||
- continuous joints with fake finite position limits produce a warning.
|
||||
|
||||
### Geometry and mesh URIs
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- each visual/collision owner has one geometry element;
|
||||
- each geometry has exactly one known primitive or mesh child when possible;
|
||||
- box size has 3 positive finite values;
|
||||
- cylinder radius and length are positive and finite;
|
||||
- sphere radius is positive and finite;
|
||||
- plane size has 2 positive finite values;
|
||||
- mesh URI values are non-empty;
|
||||
- mesh scale has 3 positive finite values when present;
|
||||
- local mesh references resolve relative to the generated `.sdf` location;
|
||||
- known external URI schemes such as `model://`, `package://`, `fuel://`, `http://`, and `https://` are accepted without local filesystem resolution.
|
||||
|
||||
### Inertials
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- mass is positive and finite;
|
||||
- inertial pose is valid when present;
|
||||
- inertia tensor components are finite;
|
||||
- inertia matrix is positive semidefinite within tolerance;
|
||||
- missing inertial data on dynamic physical links is at least a warning;
|
||||
- frame-like or static links can omit inertials when documented.
|
||||
|
||||
### Sensors and plugins
|
||||
|
||||
The runtime should check that:
|
||||
|
||||
- sensor names are non-empty and unique within owner scope;
|
||||
- sensor `type` is non-empty;
|
||||
- sensor `update_rate`, when present, is finite and non-negative;
|
||||
- sensor pose is valid;
|
||||
- plugin filename is non-empty;
|
||||
- plugin name, when present, is non-empty;
|
||||
- arbitrary simulator-specific plugin schemas are not invented by the validator.
|
||||
|
||||
Plugin filenames and parameters can pass bundled validation and still fail in the target simulator. Use smoke tests.
|
||||
|
||||
### CAD Viewer review
|
||||
|
||||
CAD Viewer treats SDF plugins, sensors, lights, includes, and nested models as static metadata. The bundled validator checks generic structure only; it does not validate Explorer-only motion contracts or execute simulator plugins.
|
||||
|
||||
After generated `.sdf` files are created or modified, hand explicit paths to `$cad-viewer` for live viewer links when available.
|
||||
|
||||
This plugin is for CAD Viewer visualization and review. It is not a Gazebo physics/controller plugin and should not be represented as simulator runtime behavior.
|
||||
|
||||
## External checks
|
||||
|
||||
When Gazebo tooling is available, run:
|
||||
|
||||
```bash
|
||||
gz sdf --check path/to/file.sdf
|
||||
```
|
||||
|
||||
The CLI option should be:
|
||||
|
||||
```bash
|
||||
python scripts/sdf path/to/source.py --gz-check auto
|
||||
```
|
||||
|
||||
External checks should be recorded in the diagnostics report. A skipped optional check is not a bundled-validation failure unless the user requested `--gz-check required`.
|
||||
|
||||
## SDF validity vs project policy
|
||||
|
||||
Separate these categories:
|
||||
|
||||
| Category | Examples |
|
||||
|---|---|
|
||||
| SDF structural validity | root `<sdf>`, version, legal element shape, non-empty names, references |
|
||||
| Numeric plausibility | finite poses, positive dimensions, positive mass, normalized axes, PSD inertia |
|
||||
| Simulator compatibility | libsdformat version, supported joint types, plugin availability, sensor support |
|
||||
| Project policy | mesh location, preferred URI style, STL/DAE preference, collision simplification, no unresolved external URIs |
|
||||
|
||||
Do not reject valid SDF merely because it violates a project policy unless the task or repository requires that policy. Prefer warnings and strict-mode controls.
|
||||
@@ -0,0 +1 @@
|
||||
--editable ./scripts/packages/cadpy_metadata
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=68"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "cadpy-metadata"
|
||||
version = "0.3.9"
|
||||
description = "Dependency-free metadata helpers for Python-generated CAD outputs."
|
||||
requires-python = ">=3.12"
|
||||
dependencies = []
|
||||
|
||||
[tool.setuptools.packages.find]
|
||||
where = ["src"]
|
||||
include = ["cadpy_metadata*"]
|
||||
|
||||
[tool.setuptools.package-data]
|
||||
cadpy_metadata = ["py.typed"]
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
"""Dependency-free metadata helpers for generated CAD-adjacent outputs."""
|
||||
|
||||
from cadpy_metadata.generator import (
|
||||
GenerationOutput,
|
||||
PythonSourceIdentity,
|
||||
python_source_identity,
|
||||
track_generation_run,
|
||||
xml_with_text_to_cad_metadata,
|
||||
)
|
||||
|
||||
__all__ = [
|
||||
"GenerationOutput",
|
||||
"PythonSourceIdentity",
|
||||
"python_source_identity",
|
||||
"track_generation_run",
|
||||
"xml_with_text_to_cad_metadata",
|
||||
]
|
||||
+246
@@ -0,0 +1,246 @@
|
||||
"""Metadata helpers for Python-generated CAD outputs."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import contextlib
|
||||
import hashlib
|
||||
import json
|
||||
import os
|
||||
import re
|
||||
import threading
|
||||
import time
|
||||
import uuid
|
||||
from dataclasses import dataclass
|
||||
from datetime import datetime, timezone
|
||||
from pathlib import Path
|
||||
from typing import Iterator, Sequence
|
||||
|
||||
|
||||
TEXT_TO_CAD_PREFIX = "cadpy:"
|
||||
GENERATION_STATUS_SCHEMA_VERSION = 1
|
||||
GENERATION_LOCK_SUFFIX = ".generation.lock.json"
|
||||
GENERATION_STATUS_HEARTBEAT_INTERVAL_SEC = 1.0
|
||||
_ACTIVE_RUN = threading.local()
|
||||
@dataclass(frozen=True)
|
||||
class PythonSourceIdentity:
|
||||
source_path: str
|
||||
source_hash: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GenerationOutput:
|
||||
path: Path
|
||||
kind: str
|
||||
|
||||
|
||||
def generation_lock_path(output_path: Path | str, run_id: str) -> Path:
|
||||
resolved_output = Path(output_path).expanduser().resolve()
|
||||
return resolved_output.parent / f".{resolved_output.name}.{_safe_lock_token(run_id)}{GENERATION_LOCK_SUFFIX}"
|
||||
|
||||
|
||||
def python_source_identity(script_path: Path) -> PythonSourceIdentity:
|
||||
resolved_script = script_path.expanduser().resolve()
|
||||
repo_root = Path.cwd().resolve()
|
||||
return PythonSourceIdentity(
|
||||
source_path=_manifest_path(resolved_script, repo_root),
|
||||
source_hash=_sha256_file(resolved_script),
|
||||
)
|
||||
|
||||
|
||||
def track_generation_run(
|
||||
*,
|
||||
source_path: Path | None,
|
||||
generator: str,
|
||||
outputs: Sequence[GenerationOutput | tuple[Path, str] | Path | str],
|
||||
repo_root: Path | None = None,
|
||||
) -> contextlib.AbstractContextManager[None]:
|
||||
tracker = _GenerationStatusTracker(
|
||||
repo_root=repo_root or Path.cwd(),
|
||||
source_path=source_path,
|
||||
generator=generator,
|
||||
outputs=outputs,
|
||||
)
|
||||
return tracker.run()
|
||||
|
||||
|
||||
class _GenerationStatusTracker:
|
||||
def __init__(
|
||||
self,
|
||||
*,
|
||||
repo_root: Path,
|
||||
source_path: Path | None,
|
||||
generator: str,
|
||||
outputs: Sequence[GenerationOutput | tuple[Path, str] | Path | str],
|
||||
) -> None:
|
||||
self.repo_root = repo_root.expanduser().resolve()
|
||||
self.source_path = source_path.expanduser().resolve() if source_path is not None else None
|
||||
self.generator = str(generator or "").strip()
|
||||
self.outputs = tuple(_normalize_generation_output(output) for output in outputs)
|
||||
self.run_id = f"{os.getpid()}-{uuid.uuid4().hex}"
|
||||
self.status_paths = tuple(
|
||||
dict.fromkeys(
|
||||
generation_lock_path(output.path, self.run_id)
|
||||
for output in self.outputs
|
||||
if output.path is not None
|
||||
)
|
||||
)
|
||||
self.status_path = self.status_paths[0] if self.status_paths else None
|
||||
self.started_at = _now_iso()
|
||||
self.stop = threading.Event()
|
||||
self.thread: threading.Thread | None = None
|
||||
|
||||
@contextlib.contextmanager
|
||||
def run(self) -> Iterator[None]:
|
||||
active_depth = int(getattr(_ACTIVE_RUN, "depth", 0) or 0)
|
||||
if active_depth > 0:
|
||||
yield
|
||||
return
|
||||
|
||||
_ACTIVE_RUN.depth = active_depth + 1
|
||||
self._start()
|
||||
try:
|
||||
yield
|
||||
finally:
|
||||
try:
|
||||
self._finish()
|
||||
finally:
|
||||
_ACTIVE_RUN.depth = active_depth
|
||||
|
||||
def _start(self) -> None:
|
||||
if not self.status_paths:
|
||||
return
|
||||
self._write_status()
|
||||
self.thread = threading.Thread(target=self._heartbeat, daemon=True)
|
||||
self.thread.start()
|
||||
|
||||
def _finish(self) -> None:
|
||||
self.stop.set()
|
||||
if self.thread is not None:
|
||||
self.thread.join(timeout=0.25)
|
||||
for status_path in self.status_paths:
|
||||
try:
|
||||
status_path.unlink()
|
||||
except FileNotFoundError:
|
||||
pass
|
||||
except OSError:
|
||||
self._write_status(status="finished", status_paths=(status_path,))
|
||||
|
||||
def _heartbeat(self) -> None:
|
||||
while not self.stop.wait(GENERATION_STATUS_HEARTBEAT_INTERVAL_SEC):
|
||||
self._write_status()
|
||||
|
||||
def _write_status(
|
||||
self,
|
||||
*,
|
||||
status: str = "running",
|
||||
status_paths: Sequence[Path] | None = None,
|
||||
) -> None:
|
||||
for status_path in tuple(status_paths or self.status_paths):
|
||||
try:
|
||||
status_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
tmp_path = status_path.with_name(f"{status_path.name}.{os.getpid()}.tmp")
|
||||
payload = self._status_payload(status=status, status_path=status_path)
|
||||
tmp_path.write_text(json.dumps(payload, sort_keys=True) + "\n", encoding="utf-8")
|
||||
tmp_path.replace(status_path)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
def _status_payload(self, *, status: str, status_path: Path) -> dict[str, object]:
|
||||
base_dir = status_path.parent
|
||||
return {
|
||||
"schemaVersion": GENERATION_STATUS_SCHEMA_VERSION,
|
||||
"id": self.run_id,
|
||||
"status": status,
|
||||
"pid": os.getpid(),
|
||||
"startedAt": self.started_at,
|
||||
"updatedAt": _now_iso(),
|
||||
"sourcePath": _display_generation_path(self.source_path, base_dir),
|
||||
"generator": self.generator,
|
||||
"outputs": [
|
||||
{
|
||||
"path": _display_generation_path(output.path, base_dir),
|
||||
"kind": output.kind,
|
||||
}
|
||||
for output in self.outputs
|
||||
if output.path is not None
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
def xml_with_text_to_cad_metadata(
|
||||
xml_text: str,
|
||||
identity: PythonSourceIdentity,
|
||||
*,
|
||||
output_path: Path | None = None,
|
||||
source_path: Path | None = None,
|
||||
) -> str:
|
||||
text = _strip_text_to_cad_metadata_comments(str(xml_text or ""))
|
||||
metadata_source_path = identity.source_path
|
||||
if output_path is not None and source_path is not None:
|
||||
metadata_source_path = _display_generation_path(source_path, Path(output_path).expanduser().resolve().parent)
|
||||
comment_block = "".join(
|
||||
f"<!-- {TEXT_TO_CAD_PREFIX}{key}={value} -->\n"
|
||||
for key, value in (
|
||||
("sourcePath", metadata_source_path),
|
||||
("sourceHash", identity.source_hash),
|
||||
)
|
||||
if value
|
||||
)
|
||||
declaration = re.match(r"\s*<\?xml[^>]*\?>\s*", text)
|
||||
if declaration:
|
||||
insert_at = declaration.end()
|
||||
return text[:insert_at] + comment_block + text[insert_at:]
|
||||
return comment_block + text
|
||||
|
||||
|
||||
def _normalize_generation_output(output: GenerationOutput | tuple[Path, str] | Path | str) -> GenerationOutput:
|
||||
if isinstance(output, GenerationOutput):
|
||||
return output
|
||||
if isinstance(output, tuple):
|
||||
output_path, kind = output
|
||||
return GenerationOutput(path=Path(output_path), kind=str(kind or "").strip())
|
||||
path = Path(output)
|
||||
return GenerationOutput(path=path, kind=path.suffix.lower().replace(".", "") or "output")
|
||||
|
||||
|
||||
def _safe_lock_token(value: object) -> str:
|
||||
token = "".join(
|
||||
character if character.isalnum() or character in {"-", "_"} else "_"
|
||||
for character in str(value or "").strip()
|
||||
).strip("._")
|
||||
return token or uuid.uuid4().hex
|
||||
|
||||
|
||||
def _display_generation_path(path: Path | None, repo_root: Path) -> str:
|
||||
if path is None:
|
||||
return ""
|
||||
resolved = path.expanduser().resolve()
|
||||
return os.path.relpath(resolved, start=repo_root.expanduser().resolve()).replace(os.sep, "/")
|
||||
|
||||
|
||||
def _now_iso() -> str:
|
||||
return datetime.fromtimestamp(time.time(), tz=timezone.utc).isoformat().replace("+00:00", "Z")
|
||||
|
||||
|
||||
def _strip_text_to_cad_metadata_comments(xml_text: str) -> str:
|
||||
return re.sub(
|
||||
r"\s*<!--\s*cadpy:source[A-Za-z]*=[\s\S]*?-->\s*",
|
||||
"\n",
|
||||
xml_text,
|
||||
).lstrip("\n")
|
||||
|
||||
|
||||
def _manifest_path(path: Path, repo_root: Path) -> str:
|
||||
resolved = path.resolve()
|
||||
try:
|
||||
return resolved.relative_to(repo_root).as_posix()
|
||||
except ValueError:
|
||||
return resolved.as_posix()
|
||||
|
||||
|
||||
def _sha256_file(path: Path) -> str:
|
||||
digest = hashlib.sha256()
|
||||
with path.open("rb") as handle:
|
||||
for chunk in iter(lambda: handle.read(1024 * 1024), b""):
|
||||
digest.update(chunk)
|
||||
return digest.hexdigest()
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
"""SDFormat/SDF generation and validation helpers."""
|
||||
@@ -0,0 +1,16 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
if __package__ in {None, ""}:
|
||||
scripts_dir = Path(__file__).resolve().parents[1]
|
||||
if str(scripts_dir) not in sys.path:
|
||||
sys.path.insert(0, str(scripts_dir))
|
||||
from sdf.cli import main
|
||||
else:
|
||||
from .cli import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,405 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Callable, Mapping, Sequence
|
||||
import math
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
Number = int | float
|
||||
PoseValues = Sequence[Number] | ET.Element | None
|
||||
|
||||
def fmt_float(value: object) -> str:
|
||||
number = _finite_float(value, "value")
|
||||
if number == 0:
|
||||
return "0"
|
||||
if 1e-4 <= abs(number) < 1e9:
|
||||
text_value = f"{number:.12f}".rstrip("0").rstrip(".")
|
||||
return text_value or "0"
|
||||
return f"{number:.12g}"
|
||||
|
||||
|
||||
def fmt_vector(values: Sequence[object]) -> str:
|
||||
return " ".join(fmt_float(value) for value in values)
|
||||
|
||||
|
||||
def text(parent: ET.Element, tag: str, value: object, attrib: Mapping[str, object] | None = None) -> ET.Element:
|
||||
child = ET.SubElement(parent, tag, _string_attrib(attrib))
|
||||
child.text = str(value)
|
||||
return child
|
||||
|
||||
|
||||
def sdf_root(version: str = "1.12") -> ET.Element:
|
||||
version_text = str(version or "").strip()
|
||||
if not version_text:
|
||||
raise ValueError("SDF version must be non-empty")
|
||||
return ET.Element("sdf", {"version": version_text})
|
||||
|
||||
|
||||
def world(parent: ET.Element, name: str) -> ET.Element:
|
||||
return ET.SubElement(parent, "world", {"name": _required_name(name, "world")})
|
||||
|
||||
|
||||
def model(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
static: bool | None = None,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
model_element = ET.SubElement(parent, "model", {"name": _required_name(name, "model")})
|
||||
if static is not None:
|
||||
text(model_element, "static", "true" if static else "false")
|
||||
_append_pose(model_element, pose)
|
||||
return model_element
|
||||
|
||||
|
||||
def frame(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
attached_to: str | None = None,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
attrib = {"name": _required_name(name, "frame")}
|
||||
if attached_to:
|
||||
attrib["attached_to"] = str(attached_to)
|
||||
frame_element = ET.SubElement(parent, "frame", attrib)
|
||||
_append_pose(frame_element, pose)
|
||||
return frame_element
|
||||
|
||||
|
||||
def link(parent: ET.Element, name: str, *, pose: PoseValues = None, inertial: Mapping[str, object] | None = None) -> ET.Element:
|
||||
link_element = ET.SubElement(parent, "link", {"name": _required_name(name, "link")})
|
||||
_append_pose(link_element, pose)
|
||||
if inertial:
|
||||
inertial_element = inertial_helper(
|
||||
link_element,
|
||||
inertial["mass"],
|
||||
inertial["inertia"],
|
||||
pose=inertial.get("pose"),
|
||||
)
|
||||
del inertial_element
|
||||
return link_element
|
||||
|
||||
|
||||
def joint(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
joint_type: str,
|
||||
parent_link: str,
|
||||
child_link: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
axis_xyz: Sequence[object] | None = None,
|
||||
axis_expressed_in: str | None = None,
|
||||
axis2_xyz: Sequence[object] | None = None,
|
||||
limits: Mapping[str, object] | None = None,
|
||||
) -> ET.Element:
|
||||
joint_element = ET.SubElement(
|
||||
parent,
|
||||
"joint",
|
||||
{
|
||||
"name": _required_name(name, "joint"),
|
||||
"type": _required_name(joint_type, "joint type"),
|
||||
},
|
||||
)
|
||||
text(joint_element, "parent", _required_name(parent_link, "joint parent"))
|
||||
text(joint_element, "child", _required_name(child_link, "joint child"))
|
||||
_append_pose(joint_element, pose)
|
||||
if axis_xyz is not None:
|
||||
axis_element = axis(joint_element, axis_xyz, expressed_in=axis_expressed_in)
|
||||
if limits:
|
||||
_limit(axis_element, limits)
|
||||
if axis2_xyz is not None:
|
||||
axis2_element = axis(joint_element, axis2_xyz, expressed_in=axis_expressed_in, tag="axis2")
|
||||
if limits and "axis2" in limits and isinstance(limits["axis2"], Mapping):
|
||||
_limit(axis2_element, limits["axis2"])
|
||||
return joint_element
|
||||
|
||||
|
||||
def pose(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 0),
|
||||
rpy: Sequence[object] = (0, 0, 0),
|
||||
*,
|
||||
relative_to: str | None = None,
|
||||
rotation_format: str = "euler_rpy",
|
||||
degrees: bool = False,
|
||||
) -> ET.Element:
|
||||
if rotation_format != "euler_rpy":
|
||||
raise ValueError("pose() supports rotation_format='euler_rpy'; use quat_pose() for quaternions")
|
||||
values = [*_vector(xyz, 3, "pose xyz"), *_vector(rpy, 3, "pose rpy")]
|
||||
attrib: dict[str, object] = {}
|
||||
if relative_to:
|
||||
attrib["relative_to"] = str(relative_to)
|
||||
if degrees:
|
||||
attrib["degrees"] = "true"
|
||||
return text(parent, "pose", fmt_vector(values), attrib)
|
||||
|
||||
|
||||
def quat_pose(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 0),
|
||||
quat_xyzw: Sequence[object] = (0, 0, 0, 1),
|
||||
*,
|
||||
relative_to: str | None = None,
|
||||
) -> ET.Element:
|
||||
quat = _vector(quat_xyzw, 4, "pose quaternion")
|
||||
if _norm(quat) == 0:
|
||||
raise ValueError("pose quaternion must be nonzero")
|
||||
attrib: dict[str, object] = {"rotation_format": "quat_xyzw"}
|
||||
if relative_to:
|
||||
attrib["relative_to"] = str(relative_to)
|
||||
return text(parent, "pose", fmt_vector([*_vector(xyz, 3, "pose xyz"), *quat]), attrib)
|
||||
|
||||
|
||||
def axis(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 1),
|
||||
*,
|
||||
expressed_in: str | None = None,
|
||||
tag: str = "axis",
|
||||
) -> ET.Element:
|
||||
values = _vector(xyz, 3, f"{tag} xyz")
|
||||
if _norm(values) == 0:
|
||||
raise ValueError(f"{tag} vector must be nonzero")
|
||||
axis_element = ET.SubElement(parent, tag)
|
||||
attrib = {"expressed_in": str(expressed_in)} if expressed_in else None
|
||||
text(axis_element, "xyz", fmt_vector(values), attrib)
|
||||
return axis_element
|
||||
|
||||
|
||||
def visual(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
geometry_builder: Callable[[ET.Element], object] | None = None,
|
||||
) -> ET.Element:
|
||||
owner = ET.SubElement(parent, "visual", {"name": _required_name(name, "visual")})
|
||||
_append_pose(owner, pose)
|
||||
_call_geometry_builder(owner, geometry_builder)
|
||||
return owner
|
||||
|
||||
|
||||
def collision(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
geometry_builder: Callable[[ET.Element], object] | None = None,
|
||||
) -> ET.Element:
|
||||
owner = ET.SubElement(parent, "collision", {"name": _required_name(name, "collision")})
|
||||
_append_pose(owner, pose)
|
||||
_call_geometry_builder(owner, geometry_builder)
|
||||
return owner
|
||||
|
||||
|
||||
def box(parent: ET.Element, size_xyz: Sequence[object]) -> ET.Element:
|
||||
values = _positive_vector(size_xyz, 3, "box size")
|
||||
box_element = ET.SubElement(_geometry_parent(parent), "box")
|
||||
text(box_element, "size", fmt_vector(values))
|
||||
return box_element
|
||||
|
||||
|
||||
def sphere(parent: ET.Element, radius: object) -> ET.Element:
|
||||
sphere_element = ET.SubElement(_geometry_parent(parent), "sphere")
|
||||
text(sphere_element, "radius", fmt_float(_positive_float(radius, "sphere radius")))
|
||||
return sphere_element
|
||||
|
||||
|
||||
def cylinder(parent: ET.Element, radius: object, length: object) -> ET.Element:
|
||||
cylinder_element = ET.SubElement(_geometry_parent(parent), "cylinder")
|
||||
text(cylinder_element, "radius", fmt_float(_positive_float(radius, "cylinder radius")))
|
||||
text(cylinder_element, "length", fmt_float(_positive_float(length, "cylinder length")))
|
||||
return cylinder_element
|
||||
|
||||
|
||||
def capsule(parent: ET.Element, radius: object, length: object) -> ET.Element:
|
||||
capsule_element = ET.SubElement(_geometry_parent(parent), "capsule")
|
||||
text(capsule_element, "radius", fmt_float(_positive_float(radius, "capsule radius")))
|
||||
text(capsule_element, "length", fmt_float(_positive_float(length, "capsule length")))
|
||||
return capsule_element
|
||||
|
||||
|
||||
def mesh(parent: ET.Element, uri: str, *, scale: Sequence[object] | None = None) -> ET.Element:
|
||||
uri_text = str(uri or "").strip()
|
||||
if not uri_text:
|
||||
raise ValueError("mesh uri must be non-empty")
|
||||
mesh_element = ET.SubElement(_geometry_parent(parent), "mesh")
|
||||
text(mesh_element, "uri", uri_text)
|
||||
if scale is not None:
|
||||
text(mesh_element, "scale", fmt_vector(_positive_vector(scale, 3, "mesh scale")))
|
||||
return mesh_element
|
||||
|
||||
|
||||
def inertial(parent: ET.Element, mass: object, inertia: Mapping[str, object] | Sequence[object], *, pose: PoseValues = None) -> ET.Element:
|
||||
return inertial_helper(parent, mass, inertia, pose=pose)
|
||||
|
||||
|
||||
def sensor(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
sensor_type: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
topic: str | None = None,
|
||||
update_rate: object | None = None,
|
||||
) -> ET.Element:
|
||||
sensor_element = ET.SubElement(
|
||||
parent,
|
||||
"sensor",
|
||||
{"name": _required_name(name, "sensor"), "type": _required_name(sensor_type, "sensor type")},
|
||||
)
|
||||
_append_pose(sensor_element, pose)
|
||||
if topic:
|
||||
text(sensor_element, "topic", topic)
|
||||
if update_rate is not None:
|
||||
rate = _finite_float(update_rate, "sensor update_rate")
|
||||
if rate < 0:
|
||||
raise ValueError("sensor update_rate must be non-negative")
|
||||
text(sensor_element, "update_rate", fmt_float(rate))
|
||||
return sensor_element
|
||||
|
||||
|
||||
def plugin(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
filename: str,
|
||||
params: Mapping[str, object] | None = None,
|
||||
**extra_params: object,
|
||||
) -> ET.Element:
|
||||
plugin_element = ET.SubElement(
|
||||
parent,
|
||||
"plugin",
|
||||
{
|
||||
"name": _required_name(name, "plugin"),
|
||||
"filename": _required_name(filename, "plugin filename"),
|
||||
},
|
||||
)
|
||||
for key, value in {**dict(params or {}), **extra_params}.items():
|
||||
text(plugin_element, str(key), value)
|
||||
return plugin_element
|
||||
|
||||
|
||||
def include(parent: ET.Element, uri: str, *, name: str | None = None, pose: PoseValues = None) -> ET.Element:
|
||||
include_element = ET.SubElement(parent, "include")
|
||||
text(include_element, "uri", _required_name(uri, "include uri"))
|
||||
if name:
|
||||
text(include_element, "name", name)
|
||||
_append_pose(include_element, pose)
|
||||
return include_element
|
||||
|
||||
|
||||
def inertial_helper(
|
||||
parent: ET.Element,
|
||||
mass: object,
|
||||
inertia: Mapping[str, object] | Sequence[object],
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
inertial_element = ET.SubElement(parent, "inertial")
|
||||
_append_pose(inertial_element, pose)
|
||||
text(inertial_element, "mass", fmt_float(_positive_float(mass, "inertial mass")))
|
||||
inertia_element = ET.SubElement(inertial_element, "inertia")
|
||||
for key, value in _inertia_items(inertia).items():
|
||||
text(inertia_element, key, fmt_float(_finite_float(value, f"inertia {key}")))
|
||||
return inertial_element
|
||||
|
||||
|
||||
def _limit(parent: ET.Element, values: Mapping[str, object]) -> ET.Element:
|
||||
limit_element = ET.SubElement(parent, "limit")
|
||||
for key in ("lower", "upper", "effort", "velocity"):
|
||||
if key in values:
|
||||
text(limit_element, key, fmt_float(_finite_float(values[key], f"limit {key}")))
|
||||
return limit_element
|
||||
|
||||
|
||||
def _append_pose(parent: ET.Element, value: PoseValues) -> None:
|
||||
if value is None:
|
||||
return
|
||||
if isinstance(value, ET.Element):
|
||||
parent.append(value)
|
||||
return
|
||||
if len(value) == 6:
|
||||
pose(parent, xyz=value[:3], rpy=value[3:])
|
||||
return
|
||||
if len(value) == 7:
|
||||
quat_pose(parent, xyz=value[:3], quat_xyzw=value[3:])
|
||||
return
|
||||
raise ValueError("pose values must contain 6 Euler values or 7 quaternion values")
|
||||
|
||||
|
||||
def _call_geometry_builder(
|
||||
owner: ET.Element,
|
||||
geometry_builder: Callable[[ET.Element], object] | None,
|
||||
) -> None:
|
||||
if geometry_builder is None:
|
||||
return
|
||||
geometry_builder(_geometry_parent(owner))
|
||||
|
||||
|
||||
def _geometry_parent(parent: ET.Element) -> ET.Element:
|
||||
if _local_name(parent.tag) == "geometry":
|
||||
return parent
|
||||
if _local_name(parent.tag) in {"visual", "collision"}:
|
||||
existing = next((child for child in list(parent) if _local_name(child.tag) == "geometry"), None)
|
||||
return existing if existing is not None else ET.SubElement(parent, "geometry")
|
||||
return ET.SubElement(parent, "geometry")
|
||||
|
||||
|
||||
def _inertia_items(inertia: Mapping[str, object] | Sequence[object]) -> dict[str, object]:
|
||||
keys = ("ixx", "iyy", "izz", "ixy", "ixz", "iyz")
|
||||
if isinstance(inertia, Mapping):
|
||||
missing = [key for key in keys if key not in inertia]
|
||||
if missing:
|
||||
raise ValueError(f"inertia is missing component(s): {', '.join(missing)}")
|
||||
return {key: inertia[key] for key in keys}
|
||||
if len(inertia) != 6:
|
||||
raise ValueError("inertia sequence must contain ixx, iyy, izz, ixy, ixz, iyz")
|
||||
return dict(zip(keys, inertia))
|
||||
|
||||
|
||||
def _vector(values: Sequence[object], expected_len: int, label: str) -> list[float]:
|
||||
if len(values) != expected_len:
|
||||
raise ValueError(f"{label} must contain {expected_len} values")
|
||||
return [_finite_float(value, label) for value in values]
|
||||
|
||||
|
||||
def _positive_vector(values: Sequence[object], expected_len: int, label: str) -> list[float]:
|
||||
return [_positive_float(value, label) for value in _vector(values, expected_len, label)]
|
||||
|
||||
|
||||
def _finite_float(value: object, label: str) -> float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise ValueError(f"{label} must be numeric") from exc
|
||||
if not math.isfinite(number):
|
||||
raise ValueError(f"{label} must be finite")
|
||||
return number
|
||||
|
||||
|
||||
def _positive_float(value: object, label: str) -> float:
|
||||
number = _finite_float(value, label)
|
||||
if number <= 0:
|
||||
raise ValueError(f"{label} must be positive")
|
||||
return number
|
||||
|
||||
|
||||
def _norm(values: Sequence[float]) -> float:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
|
||||
def _required_name(value: object, label: str) -> str:
|
||||
text_value = str(value or "").strip()
|
||||
if not text_value:
|
||||
raise ValueError(f"{label} must be non-empty")
|
||||
return text_value
|
||||
|
||||
|
||||
def _string_attrib(attrib: Mapping[str, object] | None) -> dict[str, str]:
|
||||
return {str(key): str(value) for key, value in dict(attrib or {}).items() if value is not None}
|
||||
|
||||
|
||||
def _local_name(tag: object) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
@@ -0,0 +1,387 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import importlib.util
|
||||
import inspect
|
||||
import sys
|
||||
from collections.abc import Sequence
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
SCRIPTS_DIR = Path(__file__).resolve().parents[1]
|
||||
if __package__ in {None, ""}:
|
||||
sys.path.insert(0, str(SCRIPTS_DIR))
|
||||
|
||||
PACKAGES_DIR = SCRIPTS_DIR / "packages"
|
||||
CADPY_METADATA_SRC_DIR = PACKAGES_DIR / "cadpy_metadata" / "src"
|
||||
if str(PACKAGES_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(PACKAGES_DIR))
|
||||
if str(CADPY_METADATA_SRC_DIR) not in sys.path:
|
||||
sys.path.insert(0, str(CADPY_METADATA_SRC_DIR))
|
||||
|
||||
from cadpy_metadata import (
|
||||
GenerationOutput,
|
||||
python_source_identity,
|
||||
track_generation_run,
|
||||
xml_with_text_to_cad_metadata,
|
||||
)
|
||||
from sdf.external import run_gz_sdf_check
|
||||
from sdf.findings import Finding, ValidationResult, format_findings
|
||||
from sdf.source import SdfSourceError
|
||||
from sdf.validation import validate_sdf_xml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _TargetSpec:
|
||||
source_path: Path
|
||||
output_path: Path
|
||||
|
||||
|
||||
def generate_sdf_targets(
|
||||
targets: Sequence[str],
|
||||
*,
|
||||
output: str | Path | None = None,
|
||||
gz_check: str = "auto",
|
||||
strict: bool = False,
|
||||
) -> int:
|
||||
target_specs = _resolve_target_specs(targets, output=output)
|
||||
_validate_unique_outputs(target_specs)
|
||||
for target_spec in target_specs:
|
||||
_generate_target(
|
||||
target_spec.source_path,
|
||||
output_path=target_spec.output_path,
|
||||
gz_check=gz_check,
|
||||
strict=strict,
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
def main(argv: Sequence[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
prog="sdf",
|
||||
description="Generate explicit SDFormat/SDF targets from Python sources.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"targets",
|
||||
nargs="+",
|
||||
help="Explicit Python source file or SOURCE.py=OUTPUT.sdf pair defining gen_sdf() to generate.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"-o",
|
||||
"--output",
|
||||
metavar="PATH",
|
||||
help="Write the generated SDF file to this path. Valid only with one plain Python target.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--gz-check",
|
||||
choices=("auto", "required", "never"),
|
||||
default="auto",
|
||||
help="Optionally run 'gz sdf --check' before writing output.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--strict",
|
||||
action="store_true",
|
||||
help="Treat bundled validation warnings and generator envelope warnings as failures.",
|
||||
)
|
||||
args = parser.parse_args(list(argv) if argv is not None else None)
|
||||
if args.output is not None:
|
||||
if _targets_include_output_pairs(args.targets):
|
||||
parser.error("--output cannot be combined with SOURCE=OUTPUT targets")
|
||||
if len(args.targets) != 1:
|
||||
parser.error("--output can only be used with exactly one target")
|
||||
return generate_sdf_targets(args.targets, output=args.output, gz_check=args.gz_check, strict=args.strict)
|
||||
|
||||
|
||||
def _resolve_target_specs(targets: Sequence[str], *, output: str | Path | None = None) -> list[_TargetSpec]:
|
||||
if output is not None and _targets_include_output_pairs(targets):
|
||||
raise ValueError("sdf --output cannot be combined with SOURCE=OUTPUT targets")
|
||||
if output is not None and len(targets) != 1:
|
||||
raise ValueError("sdf --output can only be used with exactly one target")
|
||||
|
||||
specs: list[_TargetSpec] = []
|
||||
for raw_target in targets:
|
||||
target_text = str(raw_target or "").strip()
|
||||
if "=" in target_text:
|
||||
raw_source, raw_output = target_text.split("=", 1)
|
||||
source_path = _resolve_source_path(raw_source)
|
||||
output_path = _resolve_cli_output_path(raw_output)
|
||||
else:
|
||||
source_path = _resolve_source_path(target_text)
|
||||
output_path = _resolve_cli_output_path(output) if output is not None else source_path.with_suffix(".sdf")
|
||||
specs.append(_TargetSpec(source_path=source_path, output_path=output_path))
|
||||
return specs
|
||||
|
||||
|
||||
def _resolve_source_path(raw_source: object) -> Path:
|
||||
value = str(raw_source or "").strip()
|
||||
if not value:
|
||||
raise ValueError("sdf target source must be a non-empty path")
|
||||
source_path = Path(value).expanduser()
|
||||
return source_path.resolve() if source_path.is_absolute() else (Path.cwd() / source_path).resolve()
|
||||
|
||||
|
||||
def _resolve_cli_output_path(raw_output: object) -> Path:
|
||||
value = str(raw_output or "").strip()
|
||||
if not value:
|
||||
raise ValueError("sdf output must be a non-empty path")
|
||||
if "\\" in value:
|
||||
raise ValueError("sdf output must use POSIX '/' separators")
|
||||
output_path = Path(value).expanduser()
|
||||
resolved = output_path.resolve() if output_path.is_absolute() else (Path.cwd() / output_path).resolve()
|
||||
if resolved.suffix.lower() != ".sdf":
|
||||
raise ValueError("sdf output must end in .sdf")
|
||||
return resolved
|
||||
|
||||
|
||||
def _targets_include_output_pairs(targets: Sequence[str]) -> bool:
|
||||
return any("=" in str(target or "") for target in targets)
|
||||
|
||||
|
||||
def _validate_unique_outputs(target_specs: Sequence[_TargetSpec]) -> None:
|
||||
seen: dict[Path, Path] = {}
|
||||
for target_spec in target_specs:
|
||||
output_path = target_spec.output_path.resolve()
|
||||
previous = seen.get(output_path)
|
||||
if previous is not None:
|
||||
raise ValueError(f"sdf output path is used more than once: {_display_path(target_spec.output_path)}")
|
||||
seen[output_path] = target_spec.output_path
|
||||
|
||||
|
||||
def _generate_target(script_path: Path, *, output_path: Path, gz_check: str, strict: bool) -> Path:
|
||||
script_path = script_path.resolve()
|
||||
if script_path.suffix.lower() != ".py":
|
||||
raise ValueError(f"{_display_path(script_path)} must be a Python source file")
|
||||
if not script_path.is_file():
|
||||
raise FileNotFoundError(f"Python source not found: {_display_path(script_path)}")
|
||||
|
||||
with track_generation_run(
|
||||
source_path=script_path,
|
||||
generator="gen_sdf",
|
||||
outputs=[GenerationOutput(output_path, "sdf")],
|
||||
):
|
||||
return _generate_target_inner(script_path, output_path=output_path, gz_check=gz_check, strict=strict)
|
||||
|
||||
|
||||
def _generate_target_inner(script_path: Path, *, output_path: Path, gz_check: str, strict: bool) -> Path:
|
||||
|
||||
module = _load_generator_module(script_path)
|
||||
generator = getattr(module, "gen_sdf", None)
|
||||
if not callable(generator):
|
||||
raise RuntimeError(f"{_display_path(script_path)} does not define callable gen_sdf()")
|
||||
if inspect.signature(generator).parameters:
|
||||
raise ValueError(f"{_display_path(script_path)} gen_sdf() must not accept arguments")
|
||||
|
||||
payload = _normalize_sdf_payload(generator(), script_path=script_path)
|
||||
xml_text = _payload_xml(payload, script_path=script_path)
|
||||
|
||||
validation = validate_sdf_xml(
|
||||
xml_text,
|
||||
source_path=output_path,
|
||||
base_dir=output_path.parent,
|
||||
metadata=payload.get("metadata") if isinstance(payload.get("metadata"), dict) else None,
|
||||
)
|
||||
validation.warnings.extend(_payload_warning_findings(payload, script_path=script_path))
|
||||
_raise_for_failed_generation(validation, strict=strict)
|
||||
|
||||
external = run_gz_sdf_check(xml_text, output_path=output_path, mode=gz_check)
|
||||
validation.extend(external)
|
||||
_raise_for_failed_generation(validation, strict=strict)
|
||||
|
||||
_write_sdf_payload(payload, output_path=output_path, script_path=script_path)
|
||||
_print_generation_report(payload, validation)
|
||||
if not output_path.exists():
|
||||
raise RuntimeError(f"{_display_path(script_path)} did not write {_display_path(output_path)}")
|
||||
return output_path
|
||||
|
||||
|
||||
def _load_generator_module(script_path: Path) -> object:
|
||||
module_name = (
|
||||
"_sdf_tool_"
|
||||
+ _display_path(script_path).replace("/", "_").replace("\\", "_").replace("-", "_").replace(".", "_")
|
||||
)
|
||||
module_spec = importlib.util.spec_from_file_location(module_name, script_path)
|
||||
if module_spec is None or module_spec.loader is None:
|
||||
raise RuntimeError(f"Failed to load generator module from {_display_path(script_path)}")
|
||||
|
||||
module = importlib.util.module_from_spec(module_spec)
|
||||
original_sys_path = list(sys.path)
|
||||
search_paths = [
|
||||
str(Path.cwd().resolve()),
|
||||
str(script_path.parent),
|
||||
]
|
||||
for candidate in reversed(search_paths):
|
||||
if candidate not in sys.path:
|
||||
sys.path.insert(0, candidate)
|
||||
|
||||
try:
|
||||
sys.modules[module_name] = module
|
||||
module_spec.loader.exec_module(module)
|
||||
finally:
|
||||
sys.path[:] = original_sys_path
|
||||
|
||||
return module
|
||||
|
||||
|
||||
def _normalize_sdf_payload(raw_payload: object, *, script_path: Path) -> dict[str, object]:
|
||||
if _is_xml_element(raw_payload):
|
||||
return {"xml": _serialize_xml_element(raw_payload)}
|
||||
if isinstance(raw_payload, str):
|
||||
return {"xml": raw_payload}
|
||||
if not isinstance(raw_payload, dict):
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() must return an SDF XML root element, XML string, "
|
||||
"or generator envelope dict"
|
||||
)
|
||||
allowed_fields = {"xml", "metadata", "assumptions", "warnings"}
|
||||
extra_fields = sorted(str(key) for key in raw_payload if key not in allowed_fields)
|
||||
if extra_fields:
|
||||
joined = ", ".join(extra_fields)
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope has unsupported field(s): {joined}")
|
||||
if "xml" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope must define 'xml'")
|
||||
payload = dict(raw_payload)
|
||||
payload["xml"] = _normalize_xml_value(payload["xml"], script_path=script_path, label="gen_sdf() envelope field 'xml'")
|
||||
payload["metadata"] = _normalize_metadata(payload.get("metadata"), script_path=script_path)
|
||||
payload["assumptions"] = _normalize_report_items(payload.get("assumptions"), field="assumptions", script_path=script_path)
|
||||
payload["warnings"] = _normalize_report_items(payload.get("warnings"), field="warnings", script_path=script_path)
|
||||
return payload
|
||||
|
||||
|
||||
def _write_sdf_payload(payload: dict[str, object], *, output_path: Path, script_path: Path) -> None:
|
||||
xml = _payload_xml(payload, script_path=script_path)
|
||||
output_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
xml = xml_with_text_to_cad_metadata(
|
||||
xml,
|
||||
python_source_identity(script_path),
|
||||
output_path=output_path,
|
||||
source_path=script_path,
|
||||
)
|
||||
text = xml if xml.endswith("\n") else xml + "\n"
|
||||
output_path.write_text(text, encoding="utf-8")
|
||||
print(f"Wrote SDF: {output_path}")
|
||||
|
||||
|
||||
def _payload_xml(payload: dict[str, object], *, script_path: Path) -> str:
|
||||
xml = payload.get("xml")
|
||||
if not isinstance(xml, str):
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() envelope field 'xml' must be a string, "
|
||||
f"got {type(xml).__name__}"
|
||||
)
|
||||
return xml
|
||||
|
||||
|
||||
def _normalize_xml_value(raw_xml: object, *, script_path: Path, label: str) -> str:
|
||||
if _is_xml_element(raw_xml):
|
||||
return _serialize_xml_element(raw_xml)
|
||||
if isinstance(raw_xml, str):
|
||||
return raw_xml
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} {label} must be an xml.etree.ElementTree.Element or string, "
|
||||
f"got {type(raw_xml).__name__}"
|
||||
)
|
||||
|
||||
|
||||
def _is_xml_element(value: object) -> bool:
|
||||
return isinstance(value, ET.Element)
|
||||
|
||||
|
||||
def _serialize_xml_element(root: ET.Element) -> str:
|
||||
ET.indent(root, space=" ")
|
||||
body = ET.tostring(root, encoding="unicode", short_empty_elements=True)
|
||||
return f'<?xml version="1.0"?>\n{body}'
|
||||
|
||||
|
||||
def _normalize_metadata(raw_metadata: object, *, script_path: Path) -> dict[str, object]:
|
||||
if raw_metadata is None:
|
||||
return {}
|
||||
if not isinstance(raw_metadata, dict):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field 'metadata' must be a dict")
|
||||
metadata: dict[str, object] = {}
|
||||
for key, value in raw_metadata.items():
|
||||
if not isinstance(value, (str, int, float, bool, type(None))):
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() metadata value for {key!r} must be a scalar"
|
||||
)
|
||||
metadata[str(key)] = value
|
||||
return metadata
|
||||
|
||||
|
||||
def _normalize_report_items(raw_items: object, *, field: str, script_path: Path) -> list[dict[str, str]]:
|
||||
if raw_items is None:
|
||||
return []
|
||||
if isinstance(raw_items, (str, bytes)) or not isinstance(raw_items, Sequence):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field '{field}' must be a list")
|
||||
normalized: list[dict[str, str]] = []
|
||||
default_code = "generator_assumption" if field == "assumptions" else "generator_warning"
|
||||
for item in raw_items:
|
||||
if isinstance(item, str):
|
||||
message = item.strip()
|
||||
code = default_code
|
||||
source = ""
|
||||
elif isinstance(item, dict):
|
||||
message = str(item.get("message") or "").strip()
|
||||
code = str(item.get("code") or default_code).strip()
|
||||
source = str(item.get("source") or "").strip()
|
||||
else:
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() envelope field '{field}' entries must be strings or dicts"
|
||||
)
|
||||
if not message:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field '{field}' has an empty message")
|
||||
normalized.append({"code": code, "message": message, "source": source})
|
||||
return normalized
|
||||
|
||||
|
||||
def _payload_warning_findings(payload: dict[str, object], *, script_path: Path) -> list[Finding]:
|
||||
warnings = payload.get("warnings")
|
||||
if not isinstance(warnings, list):
|
||||
return []
|
||||
findings: list[Finding] = []
|
||||
for item in warnings:
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
source = item.get("source")
|
||||
findings.append(
|
||||
Finding(
|
||||
severity="warning",
|
||||
code=str(item.get("code") or "generator_warning"),
|
||||
message=str(item.get("message") or ""),
|
||||
path=_display_path(script_path),
|
||||
hint=f"Source: {source}" if source else None,
|
||||
)
|
||||
)
|
||||
return findings
|
||||
|
||||
|
||||
def _raise_for_failed_generation(validation: ValidationResult, *, strict: bool) -> None:
|
||||
findings = validation.errors + (validation.warnings if strict else [])
|
||||
if findings:
|
||||
raise SdfSourceError(format_findings(findings))
|
||||
|
||||
|
||||
def _print_generation_report(payload: dict[str, object], validation: ValidationResult) -> None:
|
||||
assumptions = payload.get("assumptions")
|
||||
if isinstance(assumptions, list):
|
||||
for item in assumptions:
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
source = f" Source: {item['source']}." if item.get("source") else ""
|
||||
message = str(item["message"])
|
||||
ending = "" if message.endswith(".") else "."
|
||||
print(f"Assumption [{item['code']}]: {message}{ending}{source}")
|
||||
for finding in [*validation.warnings, *validation.infos]:
|
||||
print(finding.format())
|
||||
|
||||
|
||||
def _display_path(path: Path) -> str:
|
||||
resolved = path.resolve()
|
||||
try:
|
||||
return resolved.relative_to(Path.cwd().resolve()).as_posix()
|
||||
except ValueError:
|
||||
return resolved.as_posix()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,55 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
import shutil
|
||||
import subprocess
|
||||
import tempfile
|
||||
|
||||
from .findings import ValidationResult
|
||||
|
||||
GzCheckMode = str
|
||||
|
||||
|
||||
def run_gz_sdf_check(xml_text: str, *, output_path: Path, mode: GzCheckMode = "auto") -> ValidationResult:
|
||||
result = ValidationResult()
|
||||
normalized_mode = str(mode or "auto").strip().lower()
|
||||
if normalized_mode not in {"auto", "required", "never"}:
|
||||
raise ValueError("gz_check must be one of: auto, required, never")
|
||||
if normalized_mode == "never":
|
||||
result.add("info", "gz_check_skipped", "gz sdf --check skipped by request")
|
||||
return result
|
||||
|
||||
gz_path = shutil.which("gz")
|
||||
if gz_path is None:
|
||||
severity = "error" if normalized_mode == "required" else "warning"
|
||||
result.add(
|
||||
severity,
|
||||
"gz_check_unavailable",
|
||||
"gz sdf --check skipped because 'gz' is not on PATH",
|
||||
)
|
||||
return result
|
||||
|
||||
output_parent = output_path.resolve().parent
|
||||
output_parent.mkdir(parents=True, exist_ok=True)
|
||||
with tempfile.NamedTemporaryFile("w", encoding="utf-8", suffix=".sdf", dir=output_parent, delete=False) as handle:
|
||||
temp_path = Path(handle.name)
|
||||
handle.write(xml_text if xml_text.endswith("\n") else xml_text + "\n")
|
||||
try:
|
||||
completed = subprocess.run(
|
||||
[gz_path, "sdf", "--check", str(temp_path)],
|
||||
check=False,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
finally:
|
||||
temp_path.unlink(missing_ok=True)
|
||||
|
||||
if completed.returncode != 0:
|
||||
details = (completed.stderr or completed.stdout or "").strip()
|
||||
message = "gz sdf --check failed"
|
||||
if details:
|
||||
message = f"{message}: {details}"
|
||||
result.add("error", "gz_check_failed", message)
|
||||
else:
|
||||
result.add("info", "gz_check_passed", "gz sdf --check passed")
|
||||
return result
|
||||
@@ -0,0 +1,61 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Literal
|
||||
|
||||
Severity = Literal["error", "warning", "info"]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Finding:
|
||||
severity: Severity
|
||||
code: str
|
||||
message: str
|
||||
path: str | None = None
|
||||
hint: str | None = None
|
||||
|
||||
def format(self) -> str:
|
||||
location = f" at {self.path}" if self.path else ""
|
||||
hint = f" Hint: {self.hint}" if self.hint else ""
|
||||
return f"{self.severity}: {self.code}{location}: {self.message}{hint}"
|
||||
|
||||
|
||||
@dataclass
|
||||
class ValidationResult:
|
||||
errors: list[Finding] = field(default_factory=list)
|
||||
warnings: list[Finding] = field(default_factory=list)
|
||||
infos: list[Finding] = field(default_factory=list)
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return not self.errors
|
||||
|
||||
def add(
|
||||
self,
|
||||
severity: Severity,
|
||||
code: str,
|
||||
message: str,
|
||||
*,
|
||||
path: str | None = None,
|
||||
hint: str | None = None,
|
||||
) -> Finding:
|
||||
finding = Finding(severity=severity, code=code, message=message, path=path, hint=hint)
|
||||
if severity == "error":
|
||||
self.errors.append(finding)
|
||||
elif severity == "warning":
|
||||
self.warnings.append(finding)
|
||||
else:
|
||||
self.infos.append(finding)
|
||||
return finding
|
||||
|
||||
def extend(self, other: ValidationResult) -> None:
|
||||
self.errors.extend(other.errors)
|
||||
self.warnings.extend(other.warnings)
|
||||
self.infos.extend(other.infos)
|
||||
|
||||
def all_findings(self) -> list[Finding]:
|
||||
return [*self.errors, *self.warnings, *self.infos]
|
||||
|
||||
|
||||
def format_findings(findings: list[Finding]) -> str:
|
||||
return "\n".join(finding.format() for finding in findings)
|
||||
@@ -0,0 +1,309 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from urllib.parse import unquote, urlparse
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from .validation import raise_for_validation_errors, validate_sdf_root
|
||||
|
||||
SDF_SUFFIX = ".sdf"
|
||||
EXTERNAL_URI_SCHEMES = {"model", "package", "http", "https", "fuel"}
|
||||
|
||||
|
||||
class SdfSourceError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SdfJoint:
|
||||
name: str
|
||||
joint_type: str
|
||||
parent_link: str
|
||||
child_link: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SdfSource:
|
||||
file_ref: str
|
||||
source_path: Path
|
||||
version: str
|
||||
model_names: tuple[str, ...]
|
||||
world_names: tuple[str, ...]
|
||||
links: tuple[str, ...]
|
||||
joints: tuple[SdfJoint, ...]
|
||||
mesh_paths: tuple[Path, ...]
|
||||
visual_mesh_paths: tuple[Path, ...] = ()
|
||||
collision_mesh_paths: tuple[Path, ...] = ()
|
||||
|
||||
|
||||
def file_ref_from_sdf_path(sdf_path: Path) -> str:
|
||||
resolved = sdf_path.resolve()
|
||||
if resolved.suffix.lower() != SDF_SUFFIX:
|
||||
raise SdfSourceError(f"{resolved} is not an SDF source file")
|
||||
return _relative_to_repo(resolved)
|
||||
|
||||
|
||||
def read_sdf_source(sdf_path: Path) -> SdfSource:
|
||||
resolved_path = sdf_path.resolve()
|
||||
if resolved_path.suffix.lower() != SDF_SUFFIX:
|
||||
raise SdfSourceError(f"{resolved_path} is not an SDF source file")
|
||||
|
||||
try:
|
||||
root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ET.ParseError) as exc:
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as SDF XML") from exc
|
||||
return parse_sdf_root(root, source_path=resolved_path, base_dir=resolved_path.parent)
|
||||
|
||||
|
||||
def parse_sdf_xml(xml_text: str, *, source_path: Path, base_dir: Path | None = None) -> SdfSource:
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} could not be parsed as SDF XML") from exc
|
||||
return parse_sdf_root(root, source_path=source_path, base_dir=base_dir)
|
||||
|
||||
|
||||
def parse_sdf_root(root: ET.Element, *, source_path: Path, base_dir: Path | None = None) -> SdfSource:
|
||||
resolved_path = source_path.resolve()
|
||||
resolved_base_dir = Path(base_dir).resolve() if base_dir is not None else resolved_path.parent
|
||||
validation = validate_sdf_root(root, source_path=resolved_path, base_dir=resolved_base_dir)
|
||||
raise_for_validation_errors(validation)
|
||||
|
||||
if _local_name(root.tag) != "sdf":
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} root element must be <sdf>")
|
||||
version = str(root.attrib.get("version") or "").strip()
|
||||
if not version:
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} SDF version is required")
|
||||
|
||||
world_elements = _children(root, "world")
|
||||
world_names = [_required_name(world, source_path=resolved_path, label="world") for world in world_elements]
|
||||
_raise_on_duplicates(world_names, source_path=resolved_path, label="world")
|
||||
|
||||
model_elements = list(_children(root, "model"))
|
||||
for world in world_elements:
|
||||
model_elements.extend(_children(world, "model"))
|
||||
|
||||
model_names: list[str] = []
|
||||
links: list[str] = []
|
||||
joints: list[SdfJoint] = []
|
||||
visual_mesh_paths: list[Path] = []
|
||||
collision_mesh_paths: list[Path] = []
|
||||
|
||||
for model_element in model_elements:
|
||||
model_name = _required_name(model_element, source_path=resolved_path, label="model")
|
||||
model_names.append(model_name)
|
||||
model_links, model_joints, model_visual_meshes, model_collision_meshes = _read_model(
|
||||
model_element,
|
||||
source_path=resolved_path,
|
||||
base_dir=resolved_base_dir,
|
||||
model_name=model_name,
|
||||
)
|
||||
links.extend(model_links)
|
||||
joints.extend(model_joints)
|
||||
visual_mesh_paths.extend(model_visual_meshes)
|
||||
collision_mesh_paths.extend(model_collision_meshes)
|
||||
|
||||
return SdfSource(
|
||||
file_ref=file_ref_from_sdf_path(resolved_path),
|
||||
source_path=resolved_path,
|
||||
version=version,
|
||||
model_names=tuple(model_names),
|
||||
world_names=tuple(world_names),
|
||||
links=tuple(links),
|
||||
joints=tuple(joints),
|
||||
mesh_paths=tuple(visual_mesh_paths + collision_mesh_paths),
|
||||
visual_mesh_paths=tuple(visual_mesh_paths),
|
||||
collision_mesh_paths=tuple(collision_mesh_paths),
|
||||
)
|
||||
|
||||
|
||||
def _read_model(
|
||||
model_element: ET.Element,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path,
|
||||
model_name: str,
|
||||
) -> tuple[list[str], list[SdfJoint], list[Path], list[Path]]:
|
||||
link_names = [
|
||||
_required_name(link_element, source_path=source_path, label=f"model {model_name!r} link")
|
||||
for link_element in _children(model_element, "link")
|
||||
]
|
||||
_raise_on_duplicates(link_names, source_path=source_path, label=f"model {model_name!r} link")
|
||||
link_name_set = set(link_names)
|
||||
|
||||
visual_mesh_paths: list[Path] = []
|
||||
collision_mesh_paths: list[Path] = []
|
||||
for link_element in _children(model_element, "link"):
|
||||
visual_mesh_paths.extend(
|
||||
_geometry_mesh_paths(
|
||||
link_element,
|
||||
element_name="visual",
|
||||
source_path=source_path,
|
||||
base_dir=base_dir,
|
||||
)
|
||||
)
|
||||
collision_mesh_paths.extend(
|
||||
_geometry_mesh_paths(
|
||||
link_element,
|
||||
element_name="collision",
|
||||
source_path=source_path,
|
||||
base_dir=base_dir,
|
||||
)
|
||||
)
|
||||
|
||||
joint_names: list[str] = []
|
||||
joints: list[SdfJoint] = []
|
||||
for joint_element in _children(model_element, "joint"):
|
||||
joint_name = _required_name(joint_element, source_path=source_path, label=f"model {model_name!r} joint")
|
||||
joint_names.append(joint_name)
|
||||
joint_type = str(joint_element.attrib.get("type") or "").strip()
|
||||
if not joint_type:
|
||||
raise SdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} model {model_name!r} joint {joint_name!r} type is required"
|
||||
)
|
||||
parent_link = _required_child_text(
|
||||
joint_element,
|
||||
"parent",
|
||||
source_path=source_path,
|
||||
label=f"model {model_name!r} joint {joint_name!r} parent",
|
||||
)
|
||||
child_link = _required_child_text(
|
||||
joint_element,
|
||||
"child",
|
||||
source_path=source_path,
|
||||
label=f"model {model_name!r} joint {joint_name!r} child",
|
||||
)
|
||||
_validate_link_reference(
|
||||
parent_link,
|
||||
link_names=link_name_set,
|
||||
source_path=source_path,
|
||||
context=f"model {model_name!r} joint {joint_name!r} parent",
|
||||
)
|
||||
_validate_link_reference(
|
||||
child_link,
|
||||
link_names=link_name_set,
|
||||
source_path=source_path,
|
||||
context=f"model {model_name!r} joint {joint_name!r} child",
|
||||
allow_world=False,
|
||||
)
|
||||
joints.append(
|
||||
SdfJoint(
|
||||
name=joint_name,
|
||||
joint_type=joint_type,
|
||||
parent_link=parent_link,
|
||||
child_link=child_link,
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(joint_names, source_path=source_path, label=f"model {model_name!r} joint")
|
||||
|
||||
return link_names, joints, visual_mesh_paths, collision_mesh_paths
|
||||
|
||||
|
||||
def _children(parent: ET.Element, tag_name: str) -> list[ET.Element]:
|
||||
return [child for child in list(parent) if _local_name(child.tag) == tag_name]
|
||||
|
||||
|
||||
def _local_name(tag: str) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
|
||||
|
||||
def _required_name(element: ET.Element, *, source_path: Path, label: str) -> str:
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {label} name is required")
|
||||
return name
|
||||
|
||||
|
||||
def _required_child_text(
|
||||
parent: ET.Element,
|
||||
tag_name: str,
|
||||
*,
|
||||
source_path: Path,
|
||||
label: str,
|
||||
) -> str:
|
||||
element = next(iter(_children(parent, tag_name)), None)
|
||||
value = str(element.text if element is not None else "").strip()
|
||||
if not value:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {label} is required")
|
||||
return value
|
||||
|
||||
|
||||
def _validate_link_reference(
|
||||
link_ref: str,
|
||||
*,
|
||||
link_names: set[str],
|
||||
source_path: Path,
|
||||
context: str,
|
||||
allow_world: bool = True,
|
||||
) -> None:
|
||||
if allow_world and link_ref == "world":
|
||||
return
|
||||
if "::" in link_ref:
|
||||
return
|
||||
if link_ref not in link_names:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {context} references missing link {link_ref!r}")
|
||||
|
||||
|
||||
def _geometry_mesh_paths(
|
||||
link_element: ET.Element,
|
||||
*,
|
||||
element_name: str,
|
||||
source_path: Path,
|
||||
base_dir: Path,
|
||||
) -> list[Path]:
|
||||
mesh_paths: list[Path] = []
|
||||
for geometry_owner in _children(link_element, element_name):
|
||||
geometry_element = next(iter(_children(geometry_owner, "geometry")), None)
|
||||
if geometry_element is None:
|
||||
continue
|
||||
mesh_element = next(iter(_children(geometry_element, "mesh")), None)
|
||||
if mesh_element is None:
|
||||
continue
|
||||
uri = _required_child_text(
|
||||
mesh_element,
|
||||
"uri",
|
||||
source_path=source_path,
|
||||
label=f"{element_name} mesh uri",
|
||||
)
|
||||
mesh_path = _resolve_local_mesh_uri(uri, base_dir=base_dir)
|
||||
if mesh_path is not None:
|
||||
if not mesh_path.is_file():
|
||||
raise SdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} references missing mesh file: {uri!r}"
|
||||
)
|
||||
mesh_paths.append(mesh_path)
|
||||
return mesh_paths
|
||||
|
||||
|
||||
def _resolve_local_mesh_uri(uri: str, *, base_dir: Path) -> Path | None:
|
||||
parsed = urlparse(uri)
|
||||
if parsed.scheme in EXTERNAL_URI_SCHEMES:
|
||||
return None
|
||||
if parsed.scheme and parsed.scheme != "file":
|
||||
return None
|
||||
if parsed.scheme == "file":
|
||||
return Path(unquote(parsed.path)).resolve()
|
||||
return (base_dir / uri).resolve()
|
||||
|
||||
|
||||
def _raise_on_duplicates(values: list[str], *, source_path: Path, label: str) -> None:
|
||||
seen: set[str] = set()
|
||||
duplicates: set[str] = set()
|
||||
for value in values:
|
||||
if value in seen:
|
||||
duplicates.add(value)
|
||||
continue
|
||||
seen.add(value)
|
||||
if duplicates:
|
||||
duplicate_text = ", ".join(repr(item) for item in sorted(duplicates))
|
||||
raise SdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} {label} names contain duplicates {duplicate_text}"
|
||||
)
|
||||
|
||||
|
||||
def _relative_to_repo(path: Path) -> str:
|
||||
try:
|
||||
return path.resolve().relative_to(Path.cwd().resolve()).as_posix()
|
||||
except ValueError:
|
||||
return path.resolve().as_posix()
|
||||
@@ -0,0 +1,846 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Mapping
|
||||
import math
|
||||
from pathlib import Path
|
||||
import re
|
||||
from urllib.parse import unquote, urlparse
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from .findings import ValidationResult, format_findings
|
||||
|
||||
EXTERNAL_URI_SCHEMES = {"model", "package", "http", "https", "fuel"}
|
||||
COMMON_JOINT_TYPES = {
|
||||
"ball",
|
||||
"continuous",
|
||||
"fixed",
|
||||
"gearbox",
|
||||
"prismatic",
|
||||
"revolute",
|
||||
"revolute2",
|
||||
"screw",
|
||||
"universal",
|
||||
}
|
||||
SECOND_AXIS_JOINT_TYPES = {"revolute2", "universal"}
|
||||
BOOLEAN_VALUES = {"0", "1", "true", "false", "yes", "no", "on", "off"}
|
||||
TRUE_VALUES = {"1", "true", "yes", "on"}
|
||||
POSE_TOLERANCE = 1e-12
|
||||
UNIT_TOLERANCE = 1e-6
|
||||
PSD_TOLERANCE = 1e-9
|
||||
|
||||
|
||||
def validate_sdf_xml(
|
||||
xml_text: str,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path | None = None,
|
||||
metadata: Mapping[str, object] | None = None,
|
||||
) -> ValidationResult:
|
||||
result = ValidationResult()
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_xml",
|
||||
f"{_display_path(source_path)} could not be parsed as SDF XML: {exc}",
|
||||
path="/",
|
||||
)
|
||||
return result
|
||||
result.extend(validate_sdf_root(root, source_path=source_path, base_dir=base_dir, metadata=metadata))
|
||||
return result
|
||||
|
||||
|
||||
def validate_sdf_root(
|
||||
root: ET.Element,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path | None = None,
|
||||
metadata: Mapping[str, object] | None = None,
|
||||
) -> ValidationResult:
|
||||
del metadata
|
||||
result = ValidationResult()
|
||||
resolved_path = source_path.resolve()
|
||||
resolved_base_dir = Path(base_dir).resolve() if base_dir is not None else resolved_path.parent
|
||||
|
||||
if _local_name(root.tag) != "sdf":
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_root",
|
||||
f"{_display_path(resolved_path)} root element must be <sdf>",
|
||||
path="/",
|
||||
)
|
||||
return result
|
||||
|
||||
version = str(root.attrib.get("version") or "").strip()
|
||||
if not version:
|
||||
result.add(
|
||||
"error",
|
||||
"missing_version",
|
||||
f"{_display_path(resolved_path)} SDF version is required",
|
||||
path="/sdf",
|
||||
)
|
||||
elif not re.match(r"^\d+\.\d+(?:\.\d+)?$", version):
|
||||
result.add(
|
||||
"warning",
|
||||
"unusual_version",
|
||||
f"SDF version {version!r} does not look like major.minor",
|
||||
path="/sdf",
|
||||
)
|
||||
|
||||
meaningful_tags = {"model", "world", "actor", "light", "include", "plugin"}
|
||||
if not any(_local_name(child.tag) in meaningful_tags for child in list(root)):
|
||||
result.add(
|
||||
"error",
|
||||
"empty_document",
|
||||
f"{_display_path(resolved_path)} must define at least one <model>, <world>, or other SDF content",
|
||||
path="/sdf",
|
||||
)
|
||||
|
||||
_check_unique_named(_children(root, "world"), result, "/sdf", "world")
|
||||
_check_unique_named(_children(root, "model"), result, "/sdf", "model")
|
||||
|
||||
for include_element in _children(root, "include"):
|
||||
_validate_include(include_element, result, "/sdf/include")
|
||||
for plugin_element in _children(root, "plugin"):
|
||||
_validate_plugin(plugin_element, result, "/sdf/plugin")
|
||||
for world_element in _children(root, "world"):
|
||||
_validate_world(world_element, result, resolved_base_dir)
|
||||
for model_element in _children(root, "model"):
|
||||
_validate_model(model_element, result, resolved_base_dir, parent_targets={"world"})
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def raise_for_validation_errors(result: ValidationResult, *, strict: bool = False) -> None:
|
||||
findings = result.errors + (result.warnings if strict else [])
|
||||
if findings:
|
||||
from .source import SdfSourceError
|
||||
|
||||
raise SdfSourceError(format_findings(findings))
|
||||
|
||||
|
||||
def _validate_world(world_element: ET.Element, result: ValidationResult, base_dir: Path) -> None:
|
||||
world_name = _required_name(world_element, result, _path("world", world_element), "world")
|
||||
world_path = _path("world", world_element, fallback="/sdf/world")
|
||||
if not world_name:
|
||||
world_name = "world"
|
||||
targets = {"world", world_name}
|
||||
|
||||
_check_unique_named(_children(world_element, "model"), result, world_path, "model")
|
||||
_check_unique_named(_children(world_element, "frame"), result, world_path, "frame")
|
||||
|
||||
frames = _children(world_element, "frame")
|
||||
frame_names = _names(frames)
|
||||
targets.update(frame_names)
|
||||
for frame_element in frames:
|
||||
_validate_frame(frame_element, result, targets, world_path)
|
||||
_validate_frame_cycles(frames, result, world_path)
|
||||
|
||||
for pose_element in _children(world_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{world_path}/pose", targets)
|
||||
for include_element in _children(world_element, "include"):
|
||||
_validate_include(include_element, result, f"{world_path}/include")
|
||||
for plugin_element in _children(world_element, "plugin"):
|
||||
_validate_plugin(plugin_element, result, f"{world_path}/plugin")
|
||||
for model_element in _children(world_element, "model"):
|
||||
_validate_model(model_element, result, base_dir, parent_targets=targets)
|
||||
|
||||
|
||||
def _validate_model(
|
||||
model_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
*,
|
||||
parent_targets: set[str],
|
||||
) -> None:
|
||||
model_name = _required_name(model_element, result, _path("model", model_element), "model")
|
||||
model_path = _path("model", model_element, fallback="/sdf/model")
|
||||
if not model_name:
|
||||
model_name = "model"
|
||||
|
||||
link_elements = _children(model_element, "link")
|
||||
joint_elements = _children(model_element, "joint")
|
||||
frame_elements = _children(model_element, "frame")
|
||||
nested_model_elements = _children(model_element, "model")
|
||||
plugin_elements = _children(model_element, "plugin")
|
||||
|
||||
_check_unique_named(link_elements, result, model_path, "link")
|
||||
_check_unique_named(joint_elements, result, model_path, "joint")
|
||||
_check_unique_named(frame_elements, result, model_path, "frame")
|
||||
_check_unique_named(nested_model_elements, result, model_path, "model")
|
||||
|
||||
link_names = set(_names(link_elements))
|
||||
frame_names = set(_names(frame_elements))
|
||||
nested_model_names = set(_names(nested_model_elements))
|
||||
local_targets = {
|
||||
"world",
|
||||
"__model__",
|
||||
model_name,
|
||||
*parent_targets,
|
||||
*link_names,
|
||||
*frame_names,
|
||||
*nested_model_names,
|
||||
}
|
||||
|
||||
for pose_element in _children(model_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{model_path}/pose", local_targets)
|
||||
|
||||
for static_element in _children(model_element, "static"):
|
||||
_validate_boolean_text(static_element, result, f"{model_path}/static", "static")
|
||||
|
||||
for frame_element in frame_elements:
|
||||
_validate_frame(frame_element, result, local_targets, model_path)
|
||||
_validate_frame_cycles(frame_elements, result, model_path)
|
||||
|
||||
child_links = _joint_child_links(joint_elements)
|
||||
|
||||
for link_element in link_elements:
|
||||
_validate_link(link_element, result, base_dir, local_targets, model_path)
|
||||
for joint_element in joint_elements:
|
||||
_validate_joint(joint_element, result, link_names, local_targets, model_path)
|
||||
for include_element in _children(model_element, "include"):
|
||||
_validate_include(include_element, result, f"{model_path}/include")
|
||||
for plugin_element in plugin_elements:
|
||||
_validate_plugin(plugin_element, result, f"{model_path}/plugin")
|
||||
|
||||
if not _model_is_static(model_element):
|
||||
referenced_links = set(child_links)
|
||||
referenced_links.update(_joint_parent_links(joint_elements) - {"world"})
|
||||
for link_element in link_elements:
|
||||
name = _name(link_element)
|
||||
if (
|
||||
name
|
||||
and name in referenced_links
|
||||
and _children(link_element, "collision")
|
||||
and not _children(link_element, "inertial")
|
||||
):
|
||||
result.add(
|
||||
"warning",
|
||||
"missing_inertial",
|
||||
f"dynamic link {name!r} has collision geometry but no inertial data",
|
||||
path=f"{model_path}/link[@name='{name}']",
|
||||
)
|
||||
|
||||
for nested_model in nested_model_elements:
|
||||
_validate_model(nested_model, result, base_dir, parent_targets=local_targets)
|
||||
|
||||
|
||||
def _validate_link(
|
||||
link_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
targets: set[str],
|
||||
model_path: str,
|
||||
) -> None:
|
||||
link_name = _required_name(link_element, result, f"{model_path}/link", "link")
|
||||
link_path = f"{model_path}/link[@name='{link_name}']" if link_name else f"{model_path}/link"
|
||||
|
||||
for pose_element in _children(link_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{link_path}/pose", targets)
|
||||
|
||||
_check_unique_named(_children(link_element, "visual"), result, link_path, "visual")
|
||||
_check_unique_named(_children(link_element, "collision"), result, link_path, "collision")
|
||||
_check_unique_named(_children(link_element, "sensor"), result, link_path, "sensor")
|
||||
|
||||
visual_meshes: set[str] = set()
|
||||
for visual_element in _children(link_element, "visual"):
|
||||
visual_meshes.update(_validate_geometry_owner(visual_element, result, base_dir, targets, f"{link_path}/visual"))
|
||||
for collision_element in _children(link_element, "collision"):
|
||||
collision_meshes = _validate_geometry_owner(
|
||||
collision_element,
|
||||
result,
|
||||
base_dir,
|
||||
targets,
|
||||
f"{link_path}/collision",
|
||||
)
|
||||
for mesh_uri in collision_meshes:
|
||||
if mesh_uri in visual_meshes:
|
||||
result.add(
|
||||
"warning",
|
||||
"collision_reuses_visual_mesh",
|
||||
f"collision geometry reuses visual mesh URI {mesh_uri!r}",
|
||||
path=f"{link_path}/collision",
|
||||
hint="Use simplified collision geometry for physics when possible.",
|
||||
)
|
||||
for inertial_element in _children(link_element, "inertial"):
|
||||
_validate_inertial(inertial_element, result, targets, f"{link_path}/inertial")
|
||||
for sensor_element in _children(link_element, "sensor"):
|
||||
_validate_sensor(sensor_element, result, targets, f"{link_path}/sensor")
|
||||
|
||||
|
||||
def _validate_frame(frame_element: ET.Element, result: ValidationResult, targets: set[str], owner_path: str) -> None:
|
||||
frame_name = _required_name(frame_element, result, f"{owner_path}/frame", "frame")
|
||||
frame_path = f"{owner_path}/frame[@name='{frame_name}']" if frame_name else f"{owner_path}/frame"
|
||||
attached_to = str(frame_element.attrib.get("attached_to") or "").strip()
|
||||
if attached_to:
|
||||
_check_reference(attached_to, result, frame_path, "attached_to", targets)
|
||||
for pose_element in _children(frame_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{frame_path}/pose", targets)
|
||||
|
||||
|
||||
def _validate_joint(
|
||||
joint_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
link_names: set[str],
|
||||
targets: set[str],
|
||||
model_path: str,
|
||||
) -> None:
|
||||
joint_name = _required_name(joint_element, result, f"{model_path}/joint", "joint")
|
||||
joint_path = f"{model_path}/joint[@name='{joint_name}']" if joint_name else f"{model_path}/joint"
|
||||
joint_type = str(joint_element.attrib.get("type") or "").strip()
|
||||
if not joint_type:
|
||||
result.add("error", "missing_joint_type", f"{joint_path} type is required", path=joint_path)
|
||||
elif joint_type not in COMMON_JOINT_TYPES:
|
||||
result.add("error", "unknown_joint_type", f"unsupported SDF joint type {joint_type!r}", path=joint_path)
|
||||
|
||||
parent_link = _required_child_text(joint_element, "parent", result, f"{joint_path}/parent", "joint parent")
|
||||
child_link = _required_child_text(joint_element, "child", result, f"{joint_path}/child", "joint child")
|
||||
if parent_link:
|
||||
_validate_link_reference(parent_link, result, link_names, f"{joint_path}/parent", allow_world=True)
|
||||
if child_link:
|
||||
_validate_link_reference(child_link, result, link_names, f"{joint_path}/child", allow_world=False)
|
||||
|
||||
for pose_element in _children(joint_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{joint_path}/pose", targets)
|
||||
|
||||
axis_elements = _children(joint_element, "axis")
|
||||
axis2_elements = _children(joint_element, "axis2")
|
||||
if joint_type in {"continuous", "revolute", "prismatic"} and not axis_elements:
|
||||
result.add(
|
||||
"warning",
|
||||
"missing_joint_axis",
|
||||
f"{joint_type} joint {joint_name!r} does not declare an explicit axis",
|
||||
path=joint_path,
|
||||
)
|
||||
for axis_element in axis_elements:
|
||||
_validate_axis(axis_element, result, targets, f"{joint_path}/axis", joint_type=joint_type, axis_name="axis")
|
||||
for axis2_element in axis2_elements:
|
||||
if joint_type not in SECOND_AXIS_JOINT_TYPES:
|
||||
result.add(
|
||||
"error",
|
||||
"unsupported_axis2",
|
||||
f"joint type {joint_type!r} does not support axis2 in the lightweight validator",
|
||||
path=f"{joint_path}/axis2",
|
||||
)
|
||||
_validate_axis(axis2_element, result, targets, f"{joint_path}/axis2", joint_type=joint_type, axis_name="axis2")
|
||||
|
||||
|
||||
def _validate_axis(
|
||||
axis_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
targets: set[str],
|
||||
axis_path: str,
|
||||
*,
|
||||
joint_type: str,
|
||||
axis_name: str,
|
||||
) -> None:
|
||||
del axis_name
|
||||
xyz_element = _first_child(axis_element, "xyz")
|
||||
if xyz_element is None:
|
||||
result.add("error", "missing_axis_xyz", "joint axis must contain <xyz>", path=axis_path)
|
||||
else:
|
||||
values = _parse_number_text(str(xyz_element.text or ""), 3, result, f"{axis_path}/xyz", "axis xyz")
|
||||
if values is not None:
|
||||
norm = _norm(values)
|
||||
if norm == 0:
|
||||
result.add("error", "zero_axis", "joint axis vector must be nonzero", path=f"{axis_path}/xyz")
|
||||
elif abs(norm - 1.0) > UNIT_TOLERANCE:
|
||||
result.add(
|
||||
"warning",
|
||||
"non_unit_axis",
|
||||
f"joint axis vector length is {norm:.6g}, not 1",
|
||||
path=f"{axis_path}/xyz",
|
||||
)
|
||||
expressed_in = str(xyz_element.attrib.get("expressed_in") or axis_element.attrib.get("expressed_in") or "").strip()
|
||||
if expressed_in:
|
||||
_check_reference(expressed_in, result, f"{axis_path}/xyz", "expressed_in", targets)
|
||||
|
||||
for limit_element in _children(axis_element, "limit"):
|
||||
lower = _optional_number_child(limit_element, "lower", result, f"{axis_path}/limit/lower", allow_infinite=True)
|
||||
upper = _optional_number_child(limit_element, "upper", result, f"{axis_path}/limit/upper", allow_infinite=True)
|
||||
if lower is not None and upper is not None and lower > upper:
|
||||
result.add("error", "invalid_joint_limit", "joint lower limit exceeds upper limit", path=f"{axis_path}/limit")
|
||||
for tag in ("effort", "velocity", "stiffness", "dissipation"):
|
||||
_optional_number_child(limit_element, tag, result, f"{axis_path}/limit/{tag}", allow_infinite=True)
|
||||
if joint_type == "continuous" and (lower is not None or upper is not None):
|
||||
result.add(
|
||||
"warning",
|
||||
"continuous_joint_limits",
|
||||
"continuous joints usually should not use finite position limits",
|
||||
path=f"{axis_path}/limit",
|
||||
)
|
||||
|
||||
|
||||
def _validate_geometry_owner(
|
||||
owner: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
targets: set[str],
|
||||
owner_path: str,
|
||||
) -> set[str]:
|
||||
owner_name = _required_name(owner, result, owner_path, _local_name(owner.tag))
|
||||
path = f"{owner_path}[@name='{owner_name}']" if owner_name else owner_path
|
||||
for pose_element in _children(owner, "pose"):
|
||||
_validate_pose(pose_element, result, f"{path}/pose", targets)
|
||||
|
||||
geometry_elements = _children(owner, "geometry")
|
||||
if len(geometry_elements) != 1:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_geometry_count",
|
||||
f"{_local_name(owner.tag)} must contain exactly one <geometry>",
|
||||
path=path,
|
||||
)
|
||||
return set()
|
||||
|
||||
geometry = geometry_elements[0]
|
||||
known_children = [
|
||||
child for child in list(geometry) if _local_name(child.tag) in {"box", "sphere", "cylinder", "capsule", "plane", "mesh"}
|
||||
]
|
||||
if len(known_children) != 1:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_geometry_shape",
|
||||
"geometry must contain exactly one recognized primitive or mesh child",
|
||||
path=f"{path}/geometry",
|
||||
)
|
||||
return set()
|
||||
|
||||
shape = known_children[0]
|
||||
shape_name = _local_name(shape.tag)
|
||||
if shape_name == "box":
|
||||
_validate_positive_vector_child(shape, "size", 3, result, f"{path}/geometry/box/size")
|
||||
elif shape_name == "sphere":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/sphere/radius")
|
||||
elif shape_name == "cylinder":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/cylinder/radius")
|
||||
_validate_positive_number_child(shape, "length", result, f"{path}/geometry/cylinder/length")
|
||||
elif shape_name == "capsule":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/capsule/radius")
|
||||
_validate_positive_number_child(shape, "length", result, f"{path}/geometry/capsule/length")
|
||||
elif shape_name == "plane":
|
||||
_validate_positive_vector_child(shape, "size", 2, result, f"{path}/geometry/plane/size")
|
||||
elif shape_name == "mesh":
|
||||
uri = _required_child_text(shape, "uri", result, f"{path}/geometry/mesh/uri", "mesh uri")
|
||||
_validate_positive_vector_child(shape, "scale", 3, result, f"{path}/geometry/mesh/scale", required=False)
|
||||
if uri:
|
||||
_validate_mesh_uri(uri, result, base_dir, f"{path}/geometry/mesh/uri")
|
||||
return {uri}
|
||||
return set()
|
||||
|
||||
|
||||
def _validate_inertial(inertial_element: ET.Element, result: ValidationResult, targets: set[str], inertial_path: str) -> None:
|
||||
for pose_element in _children(inertial_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{inertial_path}/pose", targets)
|
||||
mass = _optional_number_child(inertial_element, "mass", result, f"{inertial_path}/mass", required=True)
|
||||
if mass is not None and mass <= 0:
|
||||
result.add("error", "invalid_mass", "inertial mass must be positive", path=f"{inertial_path}/mass")
|
||||
|
||||
inertia_element = _first_child(inertial_element, "inertia")
|
||||
if inertia_element is None:
|
||||
result.add("warning", "missing_inertia_matrix", "inertial element has mass but no inertia matrix", path=inertial_path)
|
||||
return
|
||||
components: dict[str, float] = {}
|
||||
for tag in ("ixx", "iyy", "izz", "ixy", "ixz", "iyz"):
|
||||
value = _optional_number_child(inertia_element, tag, result, f"{inertial_path}/inertia/{tag}", required=True)
|
||||
if value is not None:
|
||||
components[tag] = value
|
||||
if len(components) != 6:
|
||||
return
|
||||
if not _inertia_is_psd(components):
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_inertia_matrix",
|
||||
"inertia matrix must be positive semidefinite within tolerance",
|
||||
path=f"{inertial_path}/inertia",
|
||||
)
|
||||
|
||||
|
||||
def _validate_sensor(sensor_element: ET.Element, result: ValidationResult, targets: set[str], sensor_path: str) -> None:
|
||||
sensor_name = _required_name(sensor_element, result, sensor_path, "sensor")
|
||||
path = f"{sensor_path}[@name='{sensor_name}']" if sensor_name else sensor_path
|
||||
sensor_type = str(sensor_element.attrib.get("type") or "").strip()
|
||||
if not sensor_type:
|
||||
result.add("error", "missing_sensor_type", "sensor type is required", path=path)
|
||||
for pose_element in _children(sensor_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{path}/pose", targets)
|
||||
update_rate = _optional_number_child(sensor_element, "update_rate", result, f"{path}/update_rate")
|
||||
if update_rate is not None and update_rate < 0:
|
||||
result.add("error", "invalid_sensor_update_rate", "sensor update_rate must be non-negative", path=f"{path}/update_rate")
|
||||
|
||||
|
||||
def _validate_plugin(plugin_element: ET.Element, result: ValidationResult, plugin_path: str) -> None:
|
||||
filename = str(plugin_element.attrib.get("filename") or "").strip()
|
||||
if not filename:
|
||||
result.add("error", "missing_plugin_filename", "plugin filename is required", path=plugin_path)
|
||||
if "name" in plugin_element.attrib and not str(plugin_element.attrib.get("name") or "").strip():
|
||||
result.add("error", "missing_plugin_name", "plugin name is empty", path=plugin_path)
|
||||
elif "name" not in plugin_element.attrib:
|
||||
result.add("warning", "missing_plugin_name", "plugin name is omitted", path=plugin_path)
|
||||
|
||||
|
||||
def _validate_include(include_element: ET.Element, result: ValidationResult, include_path: str) -> None:
|
||||
_required_child_text(include_element, "uri", result, f"{include_path}/uri", "include uri")
|
||||
|
||||
|
||||
def _validate_pose(pose_element: ET.Element, result: ValidationResult, pose_path: str, targets: set[str]) -> None:
|
||||
rotation_format = str(pose_element.attrib.get("rotation_format") or "euler_rpy").strip()
|
||||
if rotation_format == "euler_rpy":
|
||||
values = _parse_number_text(str(pose_element.text or ""), 6, result, pose_path, "pose")
|
||||
elif rotation_format == "quat_xyzw":
|
||||
values = _parse_number_text(str(pose_element.text or ""), 7, result, pose_path, "pose")
|
||||
if values is not None:
|
||||
quat = values[3:]
|
||||
norm = _norm(quat)
|
||||
if norm == 0:
|
||||
result.add("error", "zero_quaternion", "quaternion pose has zero norm", path=pose_path)
|
||||
elif abs(norm - 1.0) > UNIT_TOLERANCE:
|
||||
result.add("warning", "non_unit_quaternion", f"quaternion pose norm is {norm:.6g}, not 1", path=pose_path)
|
||||
else:
|
||||
result.add("error", "unsupported_rotation_format", f"unsupported pose rotation_format {rotation_format!r}", path=pose_path)
|
||||
values = None
|
||||
|
||||
degrees = str(pose_element.attrib.get("degrees") or "").strip().lower()
|
||||
if degrees and degrees not in BOOLEAN_VALUES:
|
||||
result.add("error", "invalid_pose_degrees", "pose degrees attribute must be boolean-like", path=pose_path)
|
||||
elif degrees in TRUE_VALUES:
|
||||
result.add("warning", "pose_uses_degrees", "pose uses degrees=true; SDF defaults to radians", path=pose_path)
|
||||
|
||||
relative_to = str(pose_element.attrib.get("relative_to") or "").strip()
|
||||
if values is not None and _nontrivial_pose(values) and not relative_to:
|
||||
result.add(
|
||||
"warning",
|
||||
"pose_missing_relative_to",
|
||||
"nontrivial pose omits explicit relative_to",
|
||||
path=pose_path,
|
||||
)
|
||||
if relative_to:
|
||||
_check_reference(relative_to, result, pose_path, "relative_to", targets)
|
||||
|
||||
|
||||
def _validate_frame_cycles(frames: list[ET.Element], result: ValidationResult, owner_path: str) -> None:
|
||||
frame_names = set(_names(frames))
|
||||
edges: dict[str, str] = {}
|
||||
for frame in frames:
|
||||
name = _name(frame)
|
||||
target = str(frame.attrib.get("attached_to") or "").strip()
|
||||
if not target:
|
||||
pose_element = _first_child(frame, "pose")
|
||||
target = str(pose_element.attrib.get("relative_to") or "").strip() if pose_element is not None else ""
|
||||
if name and target in frame_names:
|
||||
edges[name] = target
|
||||
|
||||
visiting: set[str] = set()
|
||||
visited: set[str] = set()
|
||||
|
||||
def visit(frame_name: str, stack: list[str]) -> None:
|
||||
if frame_name in visited:
|
||||
return
|
||||
if frame_name in visiting:
|
||||
cycle = " -> ".join([*stack, frame_name])
|
||||
result.add("error", "frame_cycle", f"frame graph contains a cycle: {cycle}", path=f"{owner_path}/frame")
|
||||
return
|
||||
visiting.add(frame_name)
|
||||
target = edges.get(frame_name)
|
||||
if target:
|
||||
visit(target, [*stack, frame_name])
|
||||
visiting.remove(frame_name)
|
||||
visited.add(frame_name)
|
||||
|
||||
for frame_name in edges:
|
||||
visit(frame_name, [])
|
||||
|
||||
|
||||
def _validate_mesh_uri(uri: str, result: ValidationResult, base_dir: Path, uri_path: str) -> None:
|
||||
parsed = urlparse(uri)
|
||||
if parsed.scheme in EXTERNAL_URI_SCHEMES:
|
||||
return
|
||||
if parsed.scheme and parsed.scheme != "file":
|
||||
result.add(
|
||||
"warning",
|
||||
"unresolved_mesh_uri_scheme",
|
||||
f"mesh URI scheme {parsed.scheme!r} is not resolved by bundled validation",
|
||||
path=uri_path,
|
||||
)
|
||||
return
|
||||
mesh_path = Path(unquote(parsed.path)).resolve() if parsed.scheme == "file" else (base_dir / uri).resolve()
|
||||
if not mesh_path.is_file():
|
||||
result.add(
|
||||
"error",
|
||||
"missing_mesh_file",
|
||||
f"references missing mesh file: {uri!r}",
|
||||
path=uri_path,
|
||||
)
|
||||
|
||||
|
||||
def _validate_link_reference(
|
||||
link_ref: str,
|
||||
result: ValidationResult,
|
||||
link_names: set[str],
|
||||
path: str,
|
||||
*,
|
||||
allow_world: bool,
|
||||
) -> None:
|
||||
if link_ref == "world":
|
||||
if allow_world:
|
||||
return
|
||||
result.add("error", "invalid_joint_child", "joint child may not be world", path=path)
|
||||
return
|
||||
if "::" in link_ref:
|
||||
result.add(
|
||||
"warning",
|
||||
"scoped_link_reference_unchecked",
|
||||
f"scoped link reference {link_ref!r} was not fully resolved by bundled validation",
|
||||
path=path,
|
||||
)
|
||||
return
|
||||
if link_ref not in link_names:
|
||||
result.add("error", "missing_link_reference", f"references missing link {link_ref!r}", path=path)
|
||||
|
||||
|
||||
def _check_reference(ref: str, result: ValidationResult, path: str, attr: str, targets: set[str]) -> None:
|
||||
if "::" in ref:
|
||||
if any(part == "" for part in ref.split("::")):
|
||||
result.add("error", "invalid_scoped_reference", f"{attr} has malformed scoped reference {ref!r}", path=path)
|
||||
else:
|
||||
result.add(
|
||||
"warning",
|
||||
"scoped_reference_unchecked",
|
||||
f"{attr} scoped reference {ref!r} was not fully resolved by bundled validation",
|
||||
path=path,
|
||||
)
|
||||
return
|
||||
if ref not in targets:
|
||||
result.add("error", "unresolved_reference", f"{attr} references unknown frame or link {ref!r}", path=path)
|
||||
|
||||
|
||||
def _check_unique_named(elements: list[ET.Element], result: ValidationResult, scope_path: str, label: str) -> None:
|
||||
seen: set[str] = set()
|
||||
duplicates: set[str] = set()
|
||||
for element in elements:
|
||||
name = _required_name(element, result, f"{scope_path}/{label}", label)
|
||||
if not name:
|
||||
continue
|
||||
if name in seen:
|
||||
duplicates.add(name)
|
||||
seen.add(name)
|
||||
if duplicates:
|
||||
duplicate_text = ", ".join(repr(item) for item in sorted(duplicates))
|
||||
result.add(
|
||||
"error",
|
||||
"duplicate_names",
|
||||
f"{label} names contain duplicates {duplicate_text}",
|
||||
path=f"{scope_path}/{label}",
|
||||
)
|
||||
|
||||
|
||||
def _validate_positive_vector_child(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
expected_len: int,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
*,
|
||||
required: bool = True,
|
||||
) -> list[float] | None:
|
||||
child = _first_child(parent, tag)
|
||||
if child is None:
|
||||
if required:
|
||||
result.add("error", "missing_numeric_vector", f"{tag} is required", path=path)
|
||||
return None
|
||||
values = _parse_number_text(str(child.text or ""), expected_len, result, path, tag)
|
||||
if values is None:
|
||||
return None
|
||||
for value in values:
|
||||
if value <= 0:
|
||||
result.add("error", "invalid_dimension", f"{tag} values must be positive", path=path)
|
||||
break
|
||||
return values
|
||||
|
||||
|
||||
def _validate_positive_number_child(parent: ET.Element, tag: str, result: ValidationResult, path: str) -> float | None:
|
||||
value = _optional_number_child(parent, tag, result, path, required=True)
|
||||
if value is not None and value <= 0:
|
||||
result.add("error", "invalid_dimension", f"{tag} must be positive", path=path)
|
||||
return value
|
||||
|
||||
|
||||
def _optional_number_child(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
*,
|
||||
required: bool = False,
|
||||
allow_infinite: bool = False,
|
||||
) -> float | None:
|
||||
child = _first_child(parent, tag)
|
||||
if child is None or str(child.text or "").strip() == "":
|
||||
if required:
|
||||
result.add("error", "missing_number", f"{tag} is required", path=path)
|
||||
return None
|
||||
return _parse_optional_number(str(child.text or ""), result, path, tag, required=required, allow_infinite=allow_infinite)
|
||||
|
||||
|
||||
def _parse_optional_number(
|
||||
value: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
*,
|
||||
required: bool = False,
|
||||
allow_infinite: bool = False,
|
||||
) -> float | None:
|
||||
value = str(value or "").strip()
|
||||
if not value:
|
||||
if required:
|
||||
result.add("error", "missing_number", f"{label} is required", path=path)
|
||||
return None
|
||||
try:
|
||||
number = float(value)
|
||||
except ValueError:
|
||||
result.add("error", "invalid_number", f"{label} must be numeric", path=path)
|
||||
return None
|
||||
if allow_infinite:
|
||||
if math.isnan(number):
|
||||
result.add("error", "invalid_number", f"{label} must not be NaN", path=path)
|
||||
return None
|
||||
elif not math.isfinite(number):
|
||||
result.add("error", "invalid_number", f"{label} must be finite", path=path)
|
||||
return None
|
||||
return number
|
||||
|
||||
|
||||
def _parse_number_text(
|
||||
text: str,
|
||||
expected_len: int,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
) -> list[float] | None:
|
||||
parts = str(text or "").split()
|
||||
if len(parts) != expected_len:
|
||||
result.add("error", "invalid_numeric_vector", f"{label} must contain {expected_len} numeric values", path=path)
|
||||
return None
|
||||
values: list[float] = []
|
||||
for part in parts:
|
||||
try:
|
||||
value = float(part)
|
||||
except ValueError:
|
||||
result.add("error", "invalid_number", f"{label} contains a non-numeric value {part!r}", path=path)
|
||||
return None
|
||||
if not math.isfinite(value):
|
||||
result.add("error", "invalid_number", f"{label} values must be finite", path=path)
|
||||
return None
|
||||
values.append(value)
|
||||
return values
|
||||
|
||||
|
||||
def _validate_boolean_text(element: ET.Element, result: ValidationResult, path: str, label: str) -> None:
|
||||
value = str(element.text or "").strip().lower()
|
||||
if value and value not in BOOLEAN_VALUES:
|
||||
result.add("error", "invalid_boolean", f"{label} must be boolean-like", path=path)
|
||||
|
||||
|
||||
def _required_name(element: ET.Element, result: ValidationResult, path: str, label: str) -> str:
|
||||
name = _name(element)
|
||||
if not name:
|
||||
result.add("error", "missing_name", f"{label} name is required", path=path)
|
||||
return name
|
||||
|
||||
|
||||
def _required_child_text(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
) -> str:
|
||||
value = _child_text(parent, tag)
|
||||
if not value:
|
||||
result.add("error", "missing_child_text", f"{label} is required", path=path)
|
||||
return value
|
||||
|
||||
|
||||
def _joint_child_links(joints: list[ET.Element]) -> set[str]:
|
||||
return {_child_text(joint, "child") for joint in joints if _child_text(joint, "child") and "::" not in _child_text(joint, "child")}
|
||||
|
||||
|
||||
def _joint_parent_links(joints: list[ET.Element]) -> set[str]:
|
||||
return {_child_text(joint, "parent") for joint in joints if _child_text(joint, "parent") and "::" not in _child_text(joint, "parent")}
|
||||
|
||||
|
||||
def _model_is_static(model_element: ET.Element) -> bool:
|
||||
value = _child_text(model_element, "static").lower()
|
||||
return value in TRUE_VALUES
|
||||
|
||||
|
||||
def _inertia_is_psd(values: dict[str, float]) -> bool:
|
||||
ixx = values["ixx"]
|
||||
iyy = values["iyy"]
|
||||
izz = values["izz"]
|
||||
ixy = values["ixy"]
|
||||
ixz = values["ixz"]
|
||||
iyz = values["iyz"]
|
||||
minor_xy = (ixx * iyy) - (ixy * ixy)
|
||||
minor_xz = (ixx * izz) - (ixz * ixz)
|
||||
minor_yz = (iyy * izz) - (iyz * iyz)
|
||||
det = (ixx * iyy * izz) + (2 * ixy * ixz * iyz) - (ixx * iyz * iyz) - (iyy * ixz * ixz) - (izz * ixy * ixy)
|
||||
return (
|
||||
ixx >= -PSD_TOLERANCE
|
||||
and iyy >= -PSD_TOLERANCE
|
||||
and izz >= -PSD_TOLERANCE
|
||||
and minor_xy >= -PSD_TOLERANCE
|
||||
and minor_xz >= -PSD_TOLERANCE
|
||||
and minor_yz >= -PSD_TOLERANCE
|
||||
and det >= -PSD_TOLERANCE
|
||||
)
|
||||
|
||||
|
||||
def _nontrivial_pose(values: list[float]) -> bool:
|
||||
return any(abs(value) > POSE_TOLERANCE for value in values)
|
||||
|
||||
|
||||
def _norm(values: list[float]) -> float:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
|
||||
def _children(parent: ET.Element, tag_name: str) -> list[ET.Element]:
|
||||
return [child for child in list(parent) if _local_name(child.tag) == tag_name]
|
||||
|
||||
|
||||
def _first_child(parent: ET.Element, tag_name: str) -> ET.Element | None:
|
||||
return next(iter(_children(parent, tag_name)), None)
|
||||
|
||||
|
||||
def _child_text(parent: ET.Element, tag_name: str) -> str:
|
||||
child = _first_child(parent, tag_name)
|
||||
return str(child.text if child is not None else "").strip()
|
||||
|
||||
|
||||
def _name(element: ET.Element) -> str:
|
||||
return str(element.attrib.get("name") or "").strip()
|
||||
|
||||
|
||||
def _names(elements: list[ET.Element]) -> list[str]:
|
||||
return [_name(element) for element in elements if _name(element)]
|
||||
|
||||
|
||||
def _local_name(tag: object) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
|
||||
|
||||
def _path(tag: str, element: ET.Element, *, fallback: str | None = None) -> str:
|
||||
name = _name(element)
|
||||
if name:
|
||||
return f"/sdf/{tag}[@name='{name}']"
|
||||
return fallback or f"/sdf/{tag}"
|
||||
|
||||
|
||||
def _display_path(path: Path) -> str:
|
||||
resolved = path.resolve()
|
||||
try:
|
||||
return resolved.relative_to(Path.cwd().resolve()).as_posix()
|
||||
except ValueError:
|
||||
return resolved.as_posix()
|
||||
Reference in New Issue
Block a user