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,77 @@
|
||||
---
|
||||
name: srdf
|
||||
description: MoveIt2 SRDF generation, validation, and planning-semantics workflow. Use when creating, editing, regenerating, inspecting, or validating `.srdf` files, `gen_srdf()` sources, MoveIt planning groups, virtual joints, passive joints, end effectors, group states, disabled collisions, URDF-linked planning semantics, or SRDF handoff for live review. Use the URDF skill for robot structure, the SDF skill for simulator descriptions, and the cad-viewer skill for rendering, live review links, and optional MoveIt2 controls.
|
||||
---
|
||||
|
||||
# SRDF
|
||||
|
||||
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 for MoveIt semantic robot descriptions on top of an existing valid URDF. SRDF defines planning semantics; it does not define physical robot structure.
|
||||
|
||||
SRDF correctness is a **planning semantics** problem. The common failure is not invalid XML; it is a plausible SRDF that gives MoveIt the wrong planning group, wrong tool link, wrong default state, unsafe disabled-collision matrix, or wrong joint units. Because language models are weak at spatial and kinematic reasoning, derive planning groups, end effectors, group states, and disabled collisions from the URDF topology, MoveIt Setup Assistant output, sampled collision analysis, or explicit user data. Do not infer them from visual appearance alone.
|
||||
|
||||
## Format boundary
|
||||
|
||||
- **URDF** owns physical robot structure: links, joints, geometry, inertials, limits, mimic joints, transmissions, and robot-state publishing.
|
||||
- **SRDF** owns MoveIt semantics: virtual joints, passive joints, planning groups, group states, end effectors, and disabled collision pairs.
|
||||
- **SDF** owns simulator/world semantics: physics, sensors, lights, plugins, worlds, and simulation-specific metadata.
|
||||
|
||||
Do not place geometry, inertials, joint origins, link poses, mesh references, physical joint limits, transmissions, or `ros2_control` interfaces in SRDF.
|
||||
|
||||
## CAD Viewer Handoff
|
||||
|
||||
After completing SRDF work that creates or modifies a `.srdf`, 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); include optional MoveIt2 controls in the handoff only when the user needs interactive IK or path-planning review. If `$cad-viewer` is unavailable or startup fails, report that instead of silently omitting the handoff.
|
||||
|
||||
## Required workflow
|
||||
|
||||
1. **Start from a valid URDF.** Generate or fix the URDF first. The SRDF generator validates against the source-relative `.urdf` path supplied by `gen_srdf()`.
|
||||
2. **Identify the planning task.** Record whether the goal is arm IK, gripper control, mobile base planning, dual-arm planning, tool use, or local smoke testing.
|
||||
3. **Create or update the planning ledger.** Use `references/planning-ledger.md` before writing XML.
|
||||
4. **Define virtual and passive joints deliberately.** Use them when needed by the robot model, even though the current lightweight runtime does not fully inventory them yet.
|
||||
5. **Define planning groups from URDF topology.** Prefer chain groups for serial manipulators when base/tip form a real path. Use joint/link/subgroup definitions only when they are deliberate.
|
||||
6. **Define end effectors after group membership is known.** Avoid overlap between an end-effector group and its parent group. Record the actual target/TCP link.
|
||||
7. **Define group states in URDF-native units.** Revolute and continuous values are radians; prismatic values are meters. Do not store degrees in SRDF.
|
||||
8. **Generate disabled collisions from evidence.** Use adjacency, MoveIt Setup Assistant sampling, or explicit user-provided collision matrices. Do not invent broad disable lists.
|
||||
9. **Regenerate only explicit SRDF targets.** Generation validates the generated SRDF against the linked URDF before writing.
|
||||
10. **Run MoveIt smoke tests when available.** Use MoveIt Setup Assistant or a project MoveIt launch directly.
|
||||
11. **Report assumptions and skipped checks.** Include incomplete validation, missing MoveIt environment, manually reasoned collision disables, and inferred target links.
|
||||
|
||||
## Commands
|
||||
|
||||
Run with the Python environment for the project or workspace. Treat `python` in examples as an interpreter placeholder; if bare `python` is unavailable, substitute `python3`, a project virtualenv interpreter, or the configured interpreter path.
|
||||
|
||||
From this skill directory, the SRDF launcher shape is:
|
||||
|
||||
```bash
|
||||
python scripts/srdf path/to/source.py
|
||||
python scripts/srdf path/to/source.py -o path/to/robot.srdf
|
||||
python scripts/srdf path/to/a.py=out/a.srdf path/to/b.py=out/b.srdf
|
||||
```
|
||||
|
||||
Relative source targets and CLI output overrides are resolved from the current working directory. When running from outside this skill directory, prefix the launcher path so target files still resolve from the intended workspace.
|
||||
|
||||
## Hard rules
|
||||
|
||||
- SRDF must reference an existing valid URDF.
|
||||
- The SRDF robot name must match the URDF robot name.
|
||||
- Group states use URDF-native units: radians for revolute/continuous, meters for prismatic.
|
||||
- Disabled collision pairs require truthful reasons and provenance.
|
||||
- End-effector groups should not share links with their parent planning group.
|
||||
- `$cad-viewer` owns optional local `moveit2_server` guidance for interactive planning review.
|
||||
- Visual rendering review is useful but cannot prove planning correctness.
|
||||
|
||||
## References
|
||||
|
||||
- Generation command: `references/gen-srdf.md`
|
||||
- Generator contract: `references/generator-contract.md`
|
||||
- SRDF workflow: `references/srdf-workflow.md`
|
||||
- Planning ledger: `references/planning-ledger.md`
|
||||
- Validation scope: `references/validation.md`
|
||||
- End effectors: `references/end-effectors.md`
|
||||
- Disabled collisions: `references/disabled-collisions.md`
|
||||
- Runtime notes and current limitations: `references/implementation-notes.md`
|
||||
|
||||
For local MoveIt2 controls, use `$cad-viewer`; in that skill, read `references/moveit2-server.md`.
|
||||
@@ -0,0 +1,4 @@
|
||||
interface:
|
||||
display_name: "SRDF"
|
||||
short_description: "Generate and validate MoveIt2 SRDF semantics."
|
||||
default_prompt: "Use $srdf to generate and validate MoveIt2 SRDF files, then hand generated SRDFs to $cad-viewer for review and optional MoveIt2 controls."
|
||||
@@ -0,0 +1,55 @@
|
||||
# SRDF disabled collisions
|
||||
|
||||
Disabled collisions are planning-safety data. Treat them as derived evidence, not as decorative XML.
|
||||
|
||||
## Valid sources
|
||||
|
||||
Use one of these sources:
|
||||
|
||||
- adjacent-link policy from the URDF kinematic graph;
|
||||
- MoveIt Setup Assistant self-collision matrix generation;
|
||||
- sampled collision analysis from a known MoveIt configuration;
|
||||
- explicit user-provided collision matrix;
|
||||
- a manually reviewed pair with a specific rationale.
|
||||
|
||||
Do not infer disabled collision pairs from visual appearance or vague prose.
|
||||
|
||||
## XML shape
|
||||
|
||||
```xml
|
||||
<disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent"/>
|
||||
```
|
||||
|
||||
The current runtime requires:
|
||||
|
||||
- `link1` and `link2`;
|
||||
- both links to exist in the URDF;
|
||||
- distinct link names;
|
||||
- a non-empty `reason`;
|
||||
- no duplicate or reversed duplicate pairs.
|
||||
|
||||
## Reason and provenance
|
||||
|
||||
Use truthful reasons. Examples:
|
||||
|
||||
| Reason | Typical source |
|
||||
|---|---|
|
||||
| `Adjacent` | URDF graph adjacency |
|
||||
| `Never` | Setup Assistant sampled matrix |
|
||||
| `Always` | Setup Assistant sampled matrix |
|
||||
| `Default` | Setup Assistant sampled matrix |
|
||||
| `Manual: tool fixture is outside workspace envelope` | Explicit human review |
|
||||
|
||||
The current parser classifies reasons into broad provenance buckets such as adjacent, sampled, setup assistant, manual, or assumed. Avoid `assumed` unless the user explicitly requested a provisional SRDF and the risk is reported.
|
||||
|
||||
## Review checklist
|
||||
|
||||
Before committing a disabled collision pair:
|
||||
|
||||
- Is the pair adjacent or sampled-safe?
|
||||
- Does disabling the pair hide a possible real collision during the planned task?
|
||||
- Was the pair generated with sufficient sampling density?
|
||||
- Is the pair still valid after geometry, limits, or group membership changed?
|
||||
- Was manual rationale written down?
|
||||
|
||||
If many manual pairs are present, prefer regenerating the self-collision matrix with MoveIt Setup Assistant.
|
||||
@@ -0,0 +1,73 @@
|
||||
# SRDF end effectors
|
||||
|
||||
Use this reference when creating or editing `<end_effector>` entries or preparing MoveIt2 pose-target handoffs for `$cad-viewer`.
|
||||
|
||||
## Concept
|
||||
|
||||
An end effector is a semantic designation for a tool, gripper, sensor head, or other terminal group. It is typically connected to a parent planning group through a fixed joint or attachment link.
|
||||
|
||||
Typical shape:
|
||||
|
||||
```xml
|
||||
<group name="gripper">
|
||||
<joint name="finger_joint"/>
|
||||
</group>
|
||||
|
||||
<end_effector
|
||||
name="gripper_eef"
|
||||
parent_link="tool0"
|
||||
group="gripper"
|
||||
parent_group="manipulator"/>
|
||||
```
|
||||
|
||||
## Required ledger fields
|
||||
|
||||
Record:
|
||||
|
||||
- end-effector name;
|
||||
- end-effector group;
|
||||
- parent planning group;
|
||||
- parent link where the end effector attaches;
|
||||
- target/TCP link used for IK and planning;
|
||||
- whether the end-effector group overlaps the parent group;
|
||||
- whether the parent link is adjacent to the end-effector group in the URDF graph.
|
||||
|
||||
## Checks
|
||||
|
||||
Before generation:
|
||||
|
||||
- The end-effector group exists.
|
||||
- The parent group exists when specified.
|
||||
- The parent link exists in the URDF.
|
||||
- The end-effector group and parent group do not share links.
|
||||
- The parent link is in the parent group or adjacent to the end-effector group.
|
||||
- The target/TCP link is explicit when it differs from the inferred group tip.
|
||||
|
||||
The current runtime enforces several of these checks, but target/TCP choice remains a semantic decision. Do not rely on inference when planning to a tool center point.
|
||||
|
||||
## CAD Viewer MoveIt2 target link
|
||||
|
||||
When handing an SRDF to `$cad-viewer` for optional MoveIt2 controls, make the intended target link explicit when possible:
|
||||
|
||||
```json
|
||||
{
|
||||
"protocolVersion": 1,
|
||||
"type": "srdf.solvePose",
|
||||
"payload": {
|
||||
"file": "robot.srdf",
|
||||
"target": {
|
||||
"endEffector": "gripper_eef",
|
||||
"targetLink": "tool0",
|
||||
"frame": "base_link",
|
||||
"xyz": [0.4, 0.0, 0.2],
|
||||
"quat_xyzw": [0, 0, 0, 1]
|
||||
},
|
||||
"moveit2": {
|
||||
"planningGroup": "manipulator",
|
||||
"targetLink": "tool0"
|
||||
}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
Use position-only IK only when orientation is intentionally unconstrained. CAD Viewer owns the local MoveIt2 server startup and protocol details.
|
||||
@@ -0,0 +1,34 @@
|
||||
# SRDF generation command
|
||||
|
||||
Regenerate explicit MoveIt SRDF outputs from Python sources with top-level `gen_srdf()` functions.
|
||||
|
||||
```bash
|
||||
python scripts/srdf path/to/semantic.py
|
||||
python scripts/srdf path/to/semantic.py -o path/to/robot.srdf
|
||||
python scripts/srdf path/to/a.py=out/a.srdf path/to/b.py=out/b.srdf
|
||||
```
|
||||
|
||||
Plain Python targets write a sibling `.srdf` beside the source. `-o` / `--output` is valid only with one plain target. Use `SOURCE.py=OUTPUT.srdf` pairs for custom multi-target destinations.
|
||||
|
||||
`gen_srdf()` must return an envelope dictionary containing:
|
||||
|
||||
- `xml`: complete SRDF `<robot>` XML as an `xml.etree.ElementTree.Element` or XML string;
|
||||
- `urdf`: a POSIX relative path from the generator source file to the linked `.urdf`.
|
||||
|
||||
## What the command does
|
||||
|
||||
The tool:
|
||||
|
||||
1. imports the target Python source;
|
||||
2. calls `gen_srdf()`;
|
||||
3. resolves the linked URDF path relative to the generator source file;
|
||||
4. injects or updates local `tcad:urdf` metadata so downstream tools can find the URDF;
|
||||
5. parses the generated SRDF;
|
||||
6. validates the SRDF against the linked URDF;
|
||||
7. writes the requested `.srdf` only after validation passes.
|
||||
|
||||
There is no hidden SRDF artifact. The generated `.srdf` is the MoveIt inventory handed to `$cad-viewer` for a live CAD Viewer link and optional MoveIt2 controls.
|
||||
|
||||
## Execution safety
|
||||
|
||||
The CLI imports generator modules directly. Top-level Python code in the generator file will execute. Use this command only for trusted project sources.
|
||||
@@ -0,0 +1,126 @@
|
||||
# SRDF generator contract
|
||||
|
||||
Define a top-level zero-argument `gen_srdf()` function in a Python source file. It must return an envelope dictionary with the root SRDF XML and the owning URDF path.
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def gen_srdf():
|
||||
robot = ET.Element("robot", {"name": "sample_robot"})
|
||||
group = ET.SubElement(robot, "group", {"name": "manipulator"})
|
||||
ET.SubElement(group, "chain", {"base_link": "base_link", "tip_link": "tool0"})
|
||||
return {
|
||||
"xml": robot,
|
||||
"urdf": "sample_robot.urdf",
|
||||
}
|
||||
```
|
||||
|
||||
## Required envelope
|
||||
|
||||
```python
|
||||
{
|
||||
"xml": robot_root_element_or_xml_string,
|
||||
"urdf": "relative/path/to/robot.urdf",
|
||||
}
|
||||
```
|
||||
|
||||
The current runtime accepts only `xml` and `urdf` fields. Do not add unsupported fields.
|
||||
|
||||
## XML root
|
||||
|
||||
The XML root must be:
|
||||
|
||||
```xml
|
||||
<robot name="...">
|
||||
```
|
||||
|
||||
The SRDF `robot` name must match the linked URDF robot name.
|
||||
|
||||
## URDF path
|
||||
|
||||
The `urdf` field is resolved relative to the generator source file. It must:
|
||||
|
||||
- be a non-empty string;
|
||||
- use POSIX `/` separators;
|
||||
- be relative, not absolute;
|
||||
- end in `.urdf`;
|
||||
- refer to an existing file.
|
||||
|
||||
The CLI injects or updates:
|
||||
|
||||
```xml
|
||||
<tcad:urdf path="..."/>
|
||||
```
|
||||
|
||||
The `tcad` prefix is emitted with the `https://text-to-cad.dev/srdf` namespace. The injected path is relative from the generated `.srdf` location to the linked URDF. This metadata is a local SRDF convention for downstream tools; it is not a core SRDF semantic element. Readers also accept existing legacy `<explorer:urdf/>` metadata.
|
||||
|
||||
## Supported target forms
|
||||
|
||||
```bash
|
||||
python scripts/srdf path/to/source.py
|
||||
python scripts/srdf path/to/source.py -o path/to/robot.srdf
|
||||
python scripts/srdf a.py=out/a.srdf b.py=out/b.srdf
|
||||
```
|
||||
|
||||
The generated output path is selected by the CLI. The generator should not write the `.srdf` file itself.
|
||||
|
||||
## Group-state units
|
||||
|
||||
Group-state joint values are URDF-native values:
|
||||
|
||||
- revolute and continuous joints: radians;
|
||||
- prismatic joints: meters.
|
||||
|
||||
Do not store degrees in SRDF. Legacy UI/protocol degree fields are compatibility aliases and must be converted by joint type.
|
||||
|
||||
## Example with common semantic elements
|
||||
|
||||
```python
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
|
||||
def gen_srdf():
|
||||
robot = ET.Element("robot", {"name": "sample_robot"})
|
||||
|
||||
ET.SubElement(
|
||||
robot,
|
||||
"virtual_joint",
|
||||
{
|
||||
"name": "fixed_base",
|
||||
"type": "fixed",
|
||||
"parent_frame": "world",
|
||||
"child_link": "base_link",
|
||||
},
|
||||
)
|
||||
|
||||
arm = ET.SubElement(robot, "group", {"name": "manipulator"})
|
||||
ET.SubElement(arm, "chain", {"base_link": "base_link", "tip_link": "tool0"})
|
||||
|
||||
gripper = ET.SubElement(robot, "group", {"name": "gripper"})
|
||||
ET.SubElement(gripper, "joint", {"name": "finger_joint"})
|
||||
|
||||
ET.SubElement(
|
||||
robot,
|
||||
"end_effector",
|
||||
{
|
||||
"name": "gripper_eef",
|
||||
"parent_link": "tool0",
|
||||
"group": "gripper",
|
||||
"parent_group": "manipulator",
|
||||
},
|
||||
)
|
||||
|
||||
home = ET.SubElement(robot, "group_state", {"name": "home", "group": "manipulator"})
|
||||
ET.SubElement(home, "joint", {"name": "shoulder_pan_joint", "value": "0.0"})
|
||||
|
||||
ET.SubElement(
|
||||
robot,
|
||||
"disable_collisions",
|
||||
{"link1": "base_link", "link2": "shoulder_link", "reason": "Adjacent"},
|
||||
)
|
||||
|
||||
return {"xml": robot, "urdf": "sample_robot.urdf"}
|
||||
```
|
||||
|
||||
Virtual and passive joints are valid SRDF concepts. The current lightweight runtime preserves them but does not yet fully inventory or validate them; verify them with MoveIt Setup Assistant or a MoveIt smoke test.
|
||||
@@ -0,0 +1,37 @@
|
||||
# SRDF implementation notes
|
||||
|
||||
These notes describe the current runtime shape so the documentation does not overstate what the code enforces.
|
||||
|
||||
## Implemented in the current code
|
||||
|
||||
- `scripts/srdf` generates explicit targets only.
|
||||
- `gen_srdf()` must return an envelope with `xml` and `urdf`.
|
||||
- Generated SRDF is validated against the linked URDF before writing.
|
||||
- The CLI injects or updates local `tcad:urdf` metadata.
|
||||
- Group states use `joint_values_by_name` / `jointValuesByName` in URDF-native units.
|
||||
- Deprecated `joint_values_by_name_rad` / `jointValuesByNameRad` aliases remain for compatibility.
|
||||
- Group-state values are checked for group membership, finite values, fixed/mimic status, and URDF limits when available.
|
||||
- End-effector overlap and adjacency checks are implemented.
|
||||
- Disabled-collision reasons are required and classified into broad provenance buckets.
|
||||
- Optional CAD Viewer MoveIt2 controls use `protocolVersion: 1`.
|
||||
- Pose targets support `quat_xyzw` and `rpy`.
|
||||
- Position-only IK is explicit in request settings and defaults based on whether orientation is provided.
|
||||
- Legacy degree fields are converted by joint type so prismatic values remain linear.
|
||||
- Error responses sanitize absolute paths unless debug errors are enabled.
|
||||
|
||||
## Not yet fully implemented in code
|
||||
|
||||
The following remain process requirements and future code-improvement targets:
|
||||
|
||||
- full URDF structural validation shared with the URDF skill;
|
||||
- virtual joint parsing and validation;
|
||||
- passive joint parsing and exclusion from active planning variables;
|
||||
- hard failure for disconnected chain base/tip definitions in every case;
|
||||
- subgroup cycle detection as a first-class validation error;
|
||||
- typed SRDF generation helpers;
|
||||
- structured assumption/warning envelope fields;
|
||||
- sampled self-collision matrix generation;
|
||||
- full MoveIt configuration package generation;
|
||||
- safer generator execution in a subprocess.
|
||||
|
||||
Until those are implemented, rely on the planning ledger, MoveIt Setup Assistant, `$cad-viewer` handoff for visual/MoveIt smoke tests when available, and explicit reporting of skipped checks.
|
||||
@@ -0,0 +1,102 @@
|
||||
# SRDF planning ledger
|
||||
|
||||
Create or update this ledger before writing SRDF XML. The ledger makes planning assumptions explicit and helps prevent plausible but incorrect MoveIt configurations.
|
||||
|
||||
## URDF dependency
|
||||
|
||||
| Field | Value |
|
||||
|---|---|
|
||||
| URDF path | |
|
||||
| SRDF output path | |
|
||||
| Robot name | |
|
||||
| URDF validated? | yes/no; tool/check |
|
||||
| Root link | |
|
||||
| Active joints | |
|
||||
| Fixed joints | |
|
||||
| Mimic joints | |
|
||||
| Passive joints | |
|
||||
| Links used for collision checking | |
|
||||
| Known URDF limitations | |
|
||||
|
||||
## Planning task
|
||||
|
||||
| Field | Value |
|
||||
|---|---|
|
||||
| Main task | IK / plan-to-pose / gripper / mobile base / dual arm / other |
|
||||
| Primary planning group | |
|
||||
| Expected end-effector or TCP | |
|
||||
| Required solver or planner | |
|
||||
| Position-only IK? | yes/no; reason |
|
||||
| Orientation constraints? | yes/no; representation |
|
||||
|
||||
## Virtual joints
|
||||
|
||||
| Name | Type | Parent frame | Child link | Required? | Rationale |
|
||||
|---|---|---|---|---|---|
|
||||
| | fixed / planar / floating | | | | |
|
||||
|
||||
Virtual joints describe the robot root pose relative to an external frame. Use fixed for fixed-base manipulators when the planning setup needs a world attachment; use planar/floating only when the robot model requires that planning freedom.
|
||||
|
||||
## Passive joints
|
||||
|
||||
| Joint | URDF type | Reason passive | Affected groups | Notes |
|
||||
|---|---|---|---|---|
|
||||
| | | | | |
|
||||
|
||||
Passive joints are unactuated. They should not be treated as controllable planning variables.
|
||||
|
||||
## Planning groups
|
||||
|
||||
| Group | Representation | Members | Base link | Tip link | Active joints | Excluded joints | Purpose | Solver expectation |
|
||||
|---|---|---|---|---|---|---|---|---|
|
||||
| | joints / links / chain / subgroups | | | | | | | |
|
||||
|
||||
For serial arms, prefer a chain only when the URDF graph has a real path from base link to tip link. For subgroup groups, check for cycles and duplicate semantics.
|
||||
|
||||
## End effectors
|
||||
|
||||
| Name | End-effector group | Parent group | Parent link | Target/TCP link | Overlap checked? | Adjacent? | Notes |
|
||||
|---|---|---|---|---|---|---|---|
|
||||
| | | | | | | | |
|
||||
|
||||
The end-effector group should normally not share links with its parent group. The target/TCP link should be explicit when it differs from the inferred group tip.
|
||||
|
||||
## Group states
|
||||
|
||||
| State | Group | Joint values | Unit check | Limit check | Purpose |
|
||||
|---|---|---|---|---|---|
|
||||
| | | revolute/continuous rad; prismatic m | | | |
|
||||
|
||||
Do not store degrees in SRDF group states. Do not set fixed or mimic joints in group states.
|
||||
|
||||
## Disabled collisions
|
||||
|
||||
| Link 1 | Link 2 | Reason | Source | Evidence | Risk note |
|
||||
|---|---|---|---|---|---|
|
||||
| | | Adjacent / Never / Always / Default / Manual | Setup Assistant / sampled / adjacency / user | | |
|
||||
|
||||
Do not infer disabled collisions from visual impression. Each pair needs a reason and provenance.
|
||||
|
||||
## MoveIt smoke tests
|
||||
|
||||
| Test | Group | Target link | Target pose/state | Expected result | Actual result | Notes |
|
||||
|---|---|---|---|---|---|---|
|
||||
| IK solve | | | | | | |
|
||||
| Plan-to-pose | | | | | | |
|
||||
| Named state | | | | | | |
|
||||
| Collision check | | | | | | |
|
||||
|
||||
## Assumptions to report
|
||||
|
||||
List every guessed or inferred value:
|
||||
|
||||
- planning group membership;
|
||||
- chain base/tip;
|
||||
- target/TCP link;
|
||||
- virtual joint attachment;
|
||||
- passive joint classification;
|
||||
- group-state value;
|
||||
- disabled collision pair;
|
||||
- solver or planner setting;
|
||||
- orientation/position-only IK assumption;
|
||||
- skipped MoveIt validation.
|
||||
@@ -0,0 +1,76 @@
|
||||
# SRDF workflow
|
||||
|
||||
SRDF is the MoveIt semantic companion to a URDF. Keep physical robot structure, links, joints, geometry, inertials, mesh references, and limits in URDF. Keep planning semantics in SRDF.
|
||||
|
||||
## Edit loop
|
||||
|
||||
1. Start from a valid URDF. SRDF cannot repair wrong link frames, joint origins, limits, or geometry.
|
||||
2. Find the Python source that defines `gen_srdf()`.
|
||||
3. Treat that source as authoritative. Do not hand-edit generated `.srdf` output.
|
||||
4. Fill or update the planning ledger.
|
||||
5. Define virtual joints when the robot root needs a planning/world attachment.
|
||||
6. Define passive joints for unactuated joints.
|
||||
7. Define planning groups from URDF topology.
|
||||
8. Define end effectors only after group membership is known.
|
||||
9. Define group states in URDF-native units and check them against URDF limits.
|
||||
10. Define disabled collisions only from adjacency, sampling, Setup Assistant output, or explicit user evidence.
|
||||
11. Regenerate only the explicit SRDF target with `scripts/srdf`.
|
||||
12. Hand generated or modified `.srdf` files to `$cad-viewer` for live viewer links when available.
|
||||
13. Run MoveIt smoke tests when available. Use `$cad-viewer` for local Viewer-based IK or path-planning controls.
|
||||
14. Report assumptions and skipped checks.
|
||||
|
||||
## Typical SRDF content
|
||||
|
||||
- `<virtual_joint>` entries for root/world attachment.
|
||||
- `<passive_joint>` entries for unactuated joints.
|
||||
- `<group>` planning groups, usually by joint list or chain.
|
||||
- `<end_effector>` entries that connect a tool group to a parent planning group.
|
||||
- `<group_state>` named joint states such as `home`.
|
||||
- `<disable_collisions>` pairs for adjacent, sampled-safe, or intentionally ignored collisions.
|
||||
|
||||
## Planning groups
|
||||
|
||||
MoveIt acts on a selected planning group. Other joints are left stationary unless they belong to the selected group or are otherwise managed by the planning pipeline.
|
||||
|
||||
A group may be represented as:
|
||||
|
||||
- a collection of joints;
|
||||
- a collection of links;
|
||||
- a serial chain from base link to tip link;
|
||||
- a collection of subgroups.
|
||||
|
||||
For serial chains, the base link is the parent link of the first joint in the chain, and the tip link is the child link of the last joint. Verify that the URDF graph actually contains that path.
|
||||
|
||||
## Group states
|
||||
|
||||
`<group_state>` values are stored in URDF-native units:
|
||||
|
||||
- revolute and continuous joints: radians;
|
||||
- prismatic joints: meters.
|
||||
|
||||
The current runtime validates group-state values against group membership, fixed/mimic status, finite numeric values, and URDF limits when limits are available.
|
||||
|
||||
## End effectors
|
||||
|
||||
An end-effector is usually a separate tool or gripper group attached to a parent planning group. Avoid overlap between the end-effector group and the parent group. Record the target/TCP link in the planning ledger and in MoveIt2 request settings when it differs from the inferred group tip.
|
||||
|
||||
## Disabled collisions
|
||||
|
||||
`<disable_collisions>` pairs affect planning safety. Use truthful reasons:
|
||||
|
||||
- `Adjacent`;
|
||||
- `Never`;
|
||||
- `Always`;
|
||||
- `Default`;
|
||||
- `Setup Assistant sampled`;
|
||||
- explicit manual rationale.
|
||||
|
||||
Do not generate broad disabled-collision lists from prose or visual appearance.
|
||||
|
||||
## CAD Viewer handoff and MoveIt2 controls
|
||||
|
||||
After creating or modifying generated `.srdf` files, hand the explicit output path to `$cad-viewer` for a live viewer link when that skill is available. SRDF does not own Viewer startup.
|
||||
|
||||
When the user needs local IK or path-planning controls, include that in the `$cad-viewer` handoff. CAD Viewer owns the local `moveit2_server`, including setup, environment checks, WebSocket URL wiring, and protocol details. Provide the SRDF path plus any known planning group, target/TCP link, target frame, pose, start state, and skipped assumptions.
|
||||
|
||||
The local server is a smoke-test helper, not a replacement for a full MoveIt configuration package.
|
||||
@@ -0,0 +1,117 @@
|
||||
# SRDF validation
|
||||
|
||||
Generation validates every `gen_srdf()` result against its linked URDF before writing. This catches many planning-semantics errors, but it is not a substitute for MoveIt Setup Assistant, MoveIt runtime tests, or collision-matrix sampling.
|
||||
|
||||
Generated or modified `.srdf` files should be handed to `$cad-viewer` for live viewer links when available.
|
||||
|
||||
## Current generation-time checks
|
||||
|
||||
The current runtime checks that:
|
||||
|
||||
- `gen_srdf()` returns an envelope dictionary;
|
||||
- envelope fields are exactly `xml` and `urdf`;
|
||||
- the URDF path is relative to the generator source, uses POSIX separators, ends in `.urdf`, and exists;
|
||||
- SRDF XML parses and has root `<robot name="...">`;
|
||||
- SRDF robot name matches the URDF robot name;
|
||||
- local `tcad:urdf` metadata is injected or updated and matches the linked URDF path relative to the generated SRDF; legacy `explorer:urdf` metadata remains readable;
|
||||
- at least one planning group exists;
|
||||
- planning groups are named and unique;
|
||||
- each planning group defines joints, links, chains, or subgroups;
|
||||
- group joint names, link names, and subgroup names exist;
|
||||
- chain base/tip links exist;
|
||||
- end effectors reference existing parent links and groups;
|
||||
- end-effector groups do not overlap their parent group when a parent group is provided;
|
||||
- end-effector parent links are in the parent group or adjacent to the end-effector group;
|
||||
- group states reference existing groups;
|
||||
- group-state joints exist and belong to the selected group after group expansion;
|
||||
- group-state values are finite;
|
||||
- group states do not set fixed or mimic joints;
|
||||
- revolute/prismatic group-state values are within URDF limits when limits are available;
|
||||
- disabled collision pairs have distinct links, valid link references, non-empty reasons, and no reversed duplicates;
|
||||
- many manually reasoned disabled collision pairs trigger a warning.
|
||||
|
||||
## Optional CAD Viewer MoveIt2 checks
|
||||
|
||||
When `$cad-viewer` starts its local MoveIt2 server for SRDF review, the server additionally checks:
|
||||
|
||||
- request `protocolVersion`;
|
||||
- request type;
|
||||
- repository-relative SRDF path and traversal safety;
|
||||
- linked URDF path from `tcad:urdf` metadata;
|
||||
- planning group selection;
|
||||
- target frame and target link existence;
|
||||
- target orientation shape and quaternion normalization;
|
||||
- position-only IK consistency;
|
||||
- native joint value parsing;
|
||||
- legacy degree-field conversion by joint type;
|
||||
- planner and IK setting basic numeric validity.
|
||||
|
||||
## Important current limitations
|
||||
|
||||
Do not treat a passing validation as proof of planning correctness. The current lightweight runtime does not fully validate:
|
||||
|
||||
- full URDF graph consistency; duplicate URDF links or joints can be collapsed by shallow inventory parsing;
|
||||
- chain base/tip connectivity at generation time unless group-state expansion exposes the issue;
|
||||
- subgroup cycles as hard errors in every path;
|
||||
- `<virtual_joint>` inventory and validation;
|
||||
- `<passive_joint>` inventory and exclusion from all active groups;
|
||||
- collision matrix correctness through sampled self-collision analysis;
|
||||
- actual IK solver availability for each group;
|
||||
- actual planning success in the target MoveIt environment;
|
||||
- controller configuration;
|
||||
- full orientation-constraint behavior beyond request normalization.
|
||||
|
||||
Use MoveIt Setup Assistant or a MoveIt runtime smoke test for these checks.
|
||||
|
||||
## Manual planning checks
|
||||
|
||||
After generation, verify:
|
||||
|
||||
### URDF dependency
|
||||
|
||||
- URDF has been checked by the URDF workflow.
|
||||
- URDF collision geometry is suitable for MoveIt collision checking.
|
||||
- Active, fixed, mimic, and passive joints are understood.
|
||||
|
||||
### Planning groups
|
||||
|
||||
- Each chain has a real URDF path from base to tip.
|
||||
- Each group contains the intended active joints and no accidental fixed/mimic/passive joints.
|
||||
- Subgroups do not create ambiguous or cyclic group definitions.
|
||||
- The selected planner/IK solver supports the group.
|
||||
|
||||
### End effectors
|
||||
|
||||
- End-effector group and parent group do not share links.
|
||||
- Parent link is the real attachment point.
|
||||
- Target/TCP link is explicit for pose requests.
|
||||
|
||||
### Group states
|
||||
|
||||
- Values use radians/meters, not degrees.
|
||||
- Values are inside URDF limits.
|
||||
- Named states are collision-free when intended.
|
||||
|
||||
### Disabled collisions
|
||||
|
||||
- Pairs came from adjacency, sampling, Setup Assistant output, or explicit user evidence.
|
||||
- No broad disable list was invented by the model.
|
||||
- Manual pairs were reviewed for safety.
|
||||
|
||||
## Validation report format
|
||||
|
||||
Use a compact report:
|
||||
|
||||
```text
|
||||
Checks run:
|
||||
- SRDF generation validation: passed
|
||||
- linked URDF validation: previously passed with URDF skill
|
||||
- CAD Viewer link: returned
|
||||
- MoveIt Setup Assistant review: skipped, unavailable
|
||||
- CAD Viewer MoveIt2 IK smoke test: passed for manipulator/tool0
|
||||
- collision matrix sampling: skipped, no MoveIt environment
|
||||
|
||||
Assumptions:
|
||||
- Assumed tool0 is the desired TCP.
|
||||
- Disabled collisions are adjacency-only.
|
||||
```
|
||||
@@ -0,0 +1 @@
|
||||
--editable ./scripts/packages/cadpy_metadata
|
||||
@@ -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 @@
|
||||
"""SRDF 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 srdf.cli import main
|
||||
else:
|
||||
from .cli import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,671 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import importlib.util
|
||||
import inspect
|
||||
from math import isfinite
|
||||
import os
|
||||
import sys
|
||||
import warnings
|
||||
from collections.abc import Sequence
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path, PurePosixPath
|
||||
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 srdf.source import LEGACY_EXPLORER_NAMESPACE, SRDF_METADATA_NAMESPACE, SrdfSource, SrdfSourceError, parse_srdf_xml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _TargetSpec:
|
||||
source_path: Path
|
||||
output_path: Path
|
||||
|
||||
|
||||
def generate_srdf_targets(targets: Sequence[str], *, output: str | Path | None = None) -> 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)
|
||||
return 0
|
||||
|
||||
|
||||
def main(argv: Sequence[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
prog="srdf",
|
||||
description="Generate explicit MoveIt2 SRDF targets from Python sources.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"targets",
|
||||
nargs="+",
|
||||
help="Explicit Python source file or SOURCE.py=OUTPUT.srdf pair defining gen_srdf() to generate.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"-o",
|
||||
"--output",
|
||||
metavar="PATH",
|
||||
help="Write the generated SRDF file to this path. Valid only with one plain Python target.",
|
||||
)
|
||||
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_srdf_targets(args.targets, output=args.output)
|
||||
|
||||
|
||||
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("srdf --output cannot be combined with SOURCE=OUTPUT targets")
|
||||
if output is not None and len(targets) != 1:
|
||||
raise ValueError("srdf --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(".srdf")
|
||||
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("srdf 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("srdf output must be a non-empty path")
|
||||
if "\\" in value:
|
||||
raise ValueError("srdf 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() != ".srdf":
|
||||
raise ValueError("srdf output must end in .srdf")
|
||||
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"srdf 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) -> 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_srdf",
|
||||
outputs=[GenerationOutput(output_path, "srdf")],
|
||||
):
|
||||
return _generate_target_inner(script_path, output_path=output_path)
|
||||
|
||||
|
||||
def _generate_target_inner(script_path: Path, *, output_path: Path) -> Path:
|
||||
|
||||
module = _load_generator_module(script_path)
|
||||
generator = getattr(module, "gen_srdf", None)
|
||||
if not callable(generator):
|
||||
raise RuntimeError(f"{_display_path(script_path)} does not define callable gen_srdf()")
|
||||
if inspect.signature(generator).parameters:
|
||||
raise ValueError(f"{_display_path(script_path)} gen_srdf() must not accept arguments")
|
||||
|
||||
payload = _normalize_srdf_payload(generator(), script_path=script_path)
|
||||
xml = str(payload["xml"])
|
||||
urdf_path = _resolve_relative_file(payload["urdf"], source_path=script_path, suffix=".urdf", label="urdf")
|
||||
urdf_robot = _read_urdf_robot(urdf_path)
|
||||
xml = _xml_with_linked_urdf_metadata(xml, output_path=output_path, urdf_path=urdf_path)
|
||||
srdf_source = parse_srdf_xml(xml, source_path=output_path)
|
||||
_validate_linked_urdf_ref(
|
||||
srdf_source,
|
||||
expected_urdf_ref=_relative_posix_path(urdf_path, output_path.parent),
|
||||
)
|
||||
_validate_srdf_against_urdf(srdf_source, urdf_robot=urdf_robot)
|
||||
_write_srdf_payload(xml, output_path=output_path, script_path=script_path)
|
||||
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 = (
|
||||
"_srdf_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_srdf_payload(raw_payload: object, *, script_path: Path) -> dict[str, object]:
|
||||
if not isinstance(raw_payload, dict):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() must return an envelope dict")
|
||||
allowed_fields = {"xml", "urdf"}
|
||||
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_srdf() envelope has unsupported field(s): {joined}")
|
||||
if "xml" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope must define 'xml'")
|
||||
if "urdf" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope must define 'urdf'")
|
||||
xml = _normalize_xml_value(raw_payload.get("xml"), script_path=script_path, label="gen_srdf() envelope field 'xml'")
|
||||
if not xml.strip():
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope field 'xml' must be non-empty")
|
||||
if not isinstance(raw_payload.get("urdf"), str) or not str(raw_payload.get("urdf")).strip():
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope field 'urdf' must be a non-empty string")
|
||||
return {
|
||||
"xml": xml,
|
||||
"urdf": str(raw_payload["urdf"]).strip(),
|
||||
}
|
||||
|
||||
|
||||
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 _resolve_relative_file(raw_value: object, *, source_path: Path, suffix: str, label: str) -> Path:
|
||||
if not isinstance(raw_value, str) or not raw_value.strip():
|
||||
raise ValueError(f"{label} must be a non-empty relative path")
|
||||
value = raw_value.strip()
|
||||
if "\\" in value:
|
||||
raise ValueError(f"{label} must use POSIX '/' separators")
|
||||
pure = PurePosixPath(value)
|
||||
if pure.is_absolute() or any(part in {"", "."} for part in pure.parts):
|
||||
raise ValueError(f"{label} must be a relative path")
|
||||
path = (source_path.parent / Path(*pure.parts)).resolve()
|
||||
if path.suffix.lower() != suffix:
|
||||
raise ValueError(f"{label} must end in {suffix}")
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"{label} file does not exist: {_display_path(path)}")
|
||||
return path
|
||||
|
||||
|
||||
def _read_urdf_robot(urdf_path: Path) -> dict[str, object]:
|
||||
try:
|
||||
root = ET.parse(urdf_path).getroot()
|
||||
except ET.ParseError as exc:
|
||||
raise ValueError(f"URDF is invalid XML: {_display_path(urdf_path)}") from exc
|
||||
if root.tag != "robot":
|
||||
raise ValueError("URDF root must be <robot>")
|
||||
robot_name = str(root.get("name") or "").strip()
|
||||
links = {
|
||||
str(link.get("name") or "").strip()
|
||||
for link in root.findall("link")
|
||||
if str(link.get("name") or "").strip()
|
||||
}
|
||||
joints: dict[str, dict[str, object]] = {}
|
||||
for joint in root.findall("joint"):
|
||||
name = str(joint.get("name") or "").strip()
|
||||
if not name:
|
||||
continue
|
||||
parent_element = joint.find("parent")
|
||||
child_element = joint.find("child")
|
||||
joint_type = str(joint.get("type") or "").strip()
|
||||
lower: float | None = None
|
||||
upper: float | None = None
|
||||
limit_element = joint.find("limit")
|
||||
if limit_element is not None and joint_type in {"revolute", "prismatic"}:
|
||||
lower = _optional_finite_float(limit_element.get("lower"))
|
||||
upper = _optional_finite_float(limit_element.get("upper"))
|
||||
joints[name] = {
|
||||
"type": joint_type,
|
||||
"parent": str(parent_element.get("link") if parent_element is not None else "").strip(),
|
||||
"child": str(child_element.get("link") if child_element is not None else "").strip(),
|
||||
"lower": lower,
|
||||
"upper": upper,
|
||||
"mimic": joint.find("mimic") is not None,
|
||||
}
|
||||
if not robot_name:
|
||||
raise ValueError("URDF robot name is required")
|
||||
return {"name": robot_name, "links": links, "joints": joints}
|
||||
|
||||
|
||||
def _validate_srdf_against_urdf(srdf_source: SrdfSource, *, urdf_robot: dict[str, object]) -> None:
|
||||
urdf_name = str(urdf_robot["name"])
|
||||
if srdf_source.robot_name != urdf_name:
|
||||
raise SrdfSourceError(f"SRDF robot name {srdf_source.robot_name!r} must match URDF robot name {urdf_name!r}")
|
||||
links = urdf_robot["links"]
|
||||
joints = urdf_robot["joints"]
|
||||
assert isinstance(links, set)
|
||||
assert isinstance(joints, dict)
|
||||
group_names = {group.name for group in srdf_source.planning_groups}
|
||||
groups_by_name = {group.name: group for group in srdf_source.planning_groups}
|
||||
if not group_names:
|
||||
raise SrdfSourceError("SRDF must define at least one planning group")
|
||||
|
||||
for group in srdf_source.planning_groups:
|
||||
if not group.joint_names and not group.link_names and not group.chains and not group.subgroups:
|
||||
raise SrdfSourceError(f"SRDF planning group {group.name!r} must define joints, links, chains, or subgroups")
|
||||
_validate_names_exist(group.joint_names, set(joints), label=f"planning group {group.name!r} joint")
|
||||
_validate_names_exist(group.link_names, links, label=f"planning group {group.name!r} link")
|
||||
_validate_names_exist(group.subgroups, group_names, label=f"planning group {group.name!r} subgroup")
|
||||
for chain in group.chains:
|
||||
if chain.base_link not in links:
|
||||
raise SrdfSourceError(f"planning group {group.name!r} chain references missing base_link {chain.base_link!r}")
|
||||
if chain.tip_link not in links:
|
||||
raise SrdfSourceError(f"planning group {group.name!r} chain references missing tip_link {chain.tip_link!r}")
|
||||
|
||||
for end_effector in srdf_source.end_effectors:
|
||||
if end_effector.parent_link not in links:
|
||||
raise SrdfSourceError(f"end_effector {end_effector.name!r} references missing parent_link {end_effector.parent_link!r}")
|
||||
if end_effector.group not in group_names:
|
||||
raise SrdfSourceError(f"end_effector {end_effector.name!r} references missing group {end_effector.group!r}")
|
||||
if end_effector.parent_group and end_effector.parent_group not in group_names:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {end_effector.name!r} references missing parent_group {end_effector.parent_group!r}"
|
||||
)
|
||||
_validate_end_effector_topology(
|
||||
end_effector,
|
||||
groups_by_name=groups_by_name,
|
||||
urdf_robot=urdf_robot,
|
||||
)
|
||||
|
||||
for group_state in srdf_source.group_states:
|
||||
if group_state.group not in group_names:
|
||||
raise SrdfSourceError(f"group_state {group_state.name!r} references missing group {group_state.group!r}")
|
||||
group_joint_names = set(_joint_names_for_group(groups_by_name[group_state.group], urdf_robot=urdf_robot, groups_by_name=groups_by_name))
|
||||
for joint_name, value in group_state.joint_values_by_name.items():
|
||||
if joint_name not in joints:
|
||||
raise SrdfSourceError(f"group_state {group_state.name!r} joint references missing name {joint_name!r}")
|
||||
if joint_name not in group_joint_names:
|
||||
raise SrdfSourceError(
|
||||
f"group_state {group_state.name!r} joint {joint_name!r} is not in group {group_state.group!r}"
|
||||
)
|
||||
_validate_group_state_joint_value(group_state.name, joint_name, value, joints[joint_name])
|
||||
|
||||
for pair in srdf_source.disabled_collision_pairs:
|
||||
_validate_names_exist((pair.link1, pair.link2), links, label="disable_collisions link")
|
||||
_warn_on_many_manual_disabled_pairs(srdf_source.disabled_collision_pairs)
|
||||
|
||||
|
||||
def _optional_finite_float(value: object) -> float | None:
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
parsed = float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
return parsed if isfinite(parsed) else None
|
||||
|
||||
|
||||
def _joint_path_for_chain(urdf_robot: dict[str, object], *, base_link: str, tip_link: str) -> list[str]:
|
||||
if not base_link or not tip_link or base_link == tip_link:
|
||||
return []
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return []
|
||||
by_parent: dict[str, list[tuple[str, dict[str, object]]]] = {}
|
||||
for joint_name, joint in joints.items():
|
||||
if not isinstance(joint, dict):
|
||||
continue
|
||||
parent = str(joint.get("parent") or "").strip()
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if parent and child:
|
||||
by_parent.setdefault(parent, []).append((str(joint_name), joint))
|
||||
|
||||
stack: list[tuple[str, list[str]]] = [(base_link, [])]
|
||||
visited: set[str] = set()
|
||||
while stack:
|
||||
link_name, path = stack.pop()
|
||||
if link_name == tip_link:
|
||||
return path
|
||||
if link_name in visited:
|
||||
continue
|
||||
visited.add(link_name)
|
||||
for joint_name, joint in reversed(by_parent.get(link_name, [])):
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if child:
|
||||
stack.append((child, [*path, joint_name]))
|
||||
return []
|
||||
|
||||
|
||||
def _joint_names_for_group(
|
||||
group: object,
|
||||
*,
|
||||
urdf_robot: dict[str, object],
|
||||
groups_by_name: dict[str, object],
|
||||
visited: set[str] | None = None,
|
||||
) -> list[str]:
|
||||
names: list[str] = []
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return names
|
||||
|
||||
for joint_name in getattr(group, "joint_names", ()):
|
||||
joint = joints.get(joint_name)
|
||||
if isinstance(joint, dict) and str(joint.get("type") or "") != "fixed" and not bool(joint.get("mimic")):
|
||||
_append_unique(names, [joint_name])
|
||||
|
||||
for chain in getattr(group, "chains", ()):
|
||||
chain_joint_names = []
|
||||
for joint_name in _joint_path_for_chain(
|
||||
urdf_robot,
|
||||
base_link=str(getattr(chain, "base_link", "") or ""),
|
||||
tip_link=str(getattr(chain, "tip_link", "") or ""),
|
||||
):
|
||||
joint = joints.get(joint_name)
|
||||
if isinstance(joint, dict) and str(joint.get("type") or "") != "fixed" and not bool(joint.get("mimic")):
|
||||
chain_joint_names.append(joint_name)
|
||||
_append_unique(names, chain_joint_names)
|
||||
|
||||
if visited is None:
|
||||
visited = set()
|
||||
group_name = str(getattr(group, "name", "") or "")
|
||||
if group_name:
|
||||
visited.add(group_name)
|
||||
for subgroup_name in getattr(group, "subgroups", ()):
|
||||
subgroup_key = str(subgroup_name or "").strip()
|
||||
if not subgroup_key or subgroup_key in visited:
|
||||
continue
|
||||
subgroup = groups_by_name.get(subgroup_key)
|
||||
if subgroup is not None:
|
||||
_append_unique(
|
||||
names,
|
||||
_joint_names_for_group(
|
||||
subgroup,
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
visited=visited,
|
||||
),
|
||||
)
|
||||
return names
|
||||
|
||||
|
||||
def _link_names_for_group(
|
||||
group: object,
|
||||
*,
|
||||
urdf_robot: dict[str, object],
|
||||
groups_by_name: dict[str, object],
|
||||
visited: set[str] | None = None,
|
||||
) -> set[str]:
|
||||
links = set(str(link_name) for link_name in getattr(group, "link_names", ()))
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return links
|
||||
|
||||
def add_joint_links(joint_name: str) -> None:
|
||||
joint = joints.get(joint_name)
|
||||
if not isinstance(joint, dict):
|
||||
return
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if child:
|
||||
links.add(child)
|
||||
|
||||
for joint_name in getattr(group, "joint_names", ()):
|
||||
add_joint_links(str(joint_name))
|
||||
for chain in getattr(group, "chains", ()):
|
||||
links.add(str(getattr(chain, "tip_link", "") or ""))
|
||||
for joint_name in _joint_path_for_chain(
|
||||
urdf_robot,
|
||||
base_link=str(getattr(chain, "base_link", "") or ""),
|
||||
tip_link=str(getattr(chain, "tip_link", "") or ""),
|
||||
):
|
||||
add_joint_links(joint_name)
|
||||
|
||||
if visited is None:
|
||||
visited = set()
|
||||
group_name = str(getattr(group, "name", "") or "")
|
||||
if group_name:
|
||||
visited.add(group_name)
|
||||
for subgroup_name in getattr(group, "subgroups", ()):
|
||||
subgroup_key = str(subgroup_name or "").strip()
|
||||
if not subgroup_key or subgroup_key in visited:
|
||||
continue
|
||||
subgroup = groups_by_name.get(subgroup_key)
|
||||
if subgroup is not None:
|
||||
links.update(
|
||||
_link_names_for_group(
|
||||
subgroup,
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
visited=visited,
|
||||
)
|
||||
)
|
||||
links.discard("")
|
||||
return links
|
||||
|
||||
|
||||
def _joint_adjacent_to_any_link(urdf_robot: dict[str, object], parent_link: str, child_links: set[str]) -> bool:
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return False
|
||||
for joint in joints.values():
|
||||
if not isinstance(joint, dict):
|
||||
continue
|
||||
parent = str(joint.get("parent") or "").strip()
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if (parent == parent_link and child in child_links) or (child == parent_link and parent in child_links):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def _validate_end_effector_topology(
|
||||
end_effector: object,
|
||||
*,
|
||||
groups_by_name: dict[str, object],
|
||||
urdf_robot: dict[str, object],
|
||||
) -> None:
|
||||
group_name = str(getattr(end_effector, "group", "") or "")
|
||||
parent_group_name = str(getattr(end_effector, "parent_group", "") or "")
|
||||
parent_link = str(getattr(end_effector, "parent_link", "") or "")
|
||||
if not group_name or group_name not in groups_by_name:
|
||||
return
|
||||
|
||||
end_effector_links = _link_names_for_group(
|
||||
groups_by_name[group_name],
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
)
|
||||
if parent_group_name and parent_group_name in groups_by_name:
|
||||
parent_group_links = _link_names_for_group(
|
||||
groups_by_name[parent_group_name],
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
)
|
||||
overlap = sorted(end_effector_links & parent_group_links)
|
||||
if overlap:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} group shares link(s) with parent_group: {overlap!r}"
|
||||
)
|
||||
if parent_link and parent_link not in parent_group_links:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} parent_link {parent_link!r} is not in parent_group {parent_group_name!r}"
|
||||
)
|
||||
if end_effector_links and parent_link not in end_effector_links and not _joint_adjacent_to_any_link(
|
||||
urdf_robot,
|
||||
parent_link,
|
||||
end_effector_links,
|
||||
):
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} parent_link {parent_link!r} is not adjacent to its group"
|
||||
)
|
||||
|
||||
|
||||
def _validate_group_state_joint_value(
|
||||
state_name: str,
|
||||
joint_name: str,
|
||||
value: float,
|
||||
joint: object,
|
||||
) -> None:
|
||||
if not isinstance(joint, dict):
|
||||
return
|
||||
joint_type = str(joint.get("type") or "").strip()
|
||||
if joint_type == "fixed":
|
||||
raise SrdfSourceError(f"group_state {state_name!r} cannot set fixed joint {joint_name!r}")
|
||||
if bool(joint.get("mimic")):
|
||||
raise SrdfSourceError(f"group_state {state_name!r} cannot set mimic joint {joint_name!r}")
|
||||
if joint_type == "continuous":
|
||||
return
|
||||
lower = joint.get("lower")
|
||||
upper = joint.get("upper")
|
||||
if isinstance(lower, float) and value < lower:
|
||||
raise SrdfSourceError(f"group_state {state_name!r} joint {joint_name!r} is below its URDF lower limit")
|
||||
if isinstance(upper, float) and value > upper:
|
||||
raise SrdfSourceError(f"group_state {state_name!r} joint {joint_name!r} is above its URDF upper limit")
|
||||
|
||||
|
||||
def _append_unique(target: list[str], values: list[str]) -> None:
|
||||
seen = set(target)
|
||||
for value in values:
|
||||
if value not in seen:
|
||||
target.append(value)
|
||||
seen.add(value)
|
||||
|
||||
|
||||
def _warn_on_many_manual_disabled_pairs(pairs: object) -> None:
|
||||
manual_count = sum(1 for pair in pairs if getattr(pair, "source", "") == "manual")
|
||||
if manual_count >= 25:
|
||||
warnings.warn(
|
||||
f"SRDF contains {manual_count} manually reasoned disabled collision pairs; prefer sampled/setup-assistant provenance.",
|
||||
stacklevel=3,
|
||||
)
|
||||
|
||||
|
||||
def _validate_linked_urdf_ref(srdf_source: SrdfSource, *, expected_urdf_ref: str) -> None:
|
||||
urdf_ref = str(srdf_source.urdf_ref or "").strip()
|
||||
if not urdf_ref:
|
||||
raise SrdfSourceError("SRDF must include <tcad:urdf path=\"...\"/> metadata")
|
||||
if PurePosixPath(urdf_ref) != PurePosixPath(expected_urdf_ref):
|
||||
raise SrdfSourceError(
|
||||
f"SRDF tcad:urdf path {urdf_ref!r} must match gen_srdf() envelope urdf {expected_urdf_ref!r}"
|
||||
)
|
||||
|
||||
|
||||
def _relative_posix_path(target_path: Path, start_dir: Path) -> str:
|
||||
return Path(os.path.relpath(Path(target_path).resolve(), Path(start_dir).resolve())).as_posix()
|
||||
|
||||
|
||||
def _xml_with_linked_urdf_metadata(xml: str, *, output_path: Path, urdf_path: Path) -> str:
|
||||
try:
|
||||
root = ET.fromstring(xml)
|
||||
except ET.ParseError as exc:
|
||||
raise SrdfSourceError(f"{_display_path(output_path)} could not be parsed as SRDF XML") from exc
|
||||
if root.tag != "robot":
|
||||
raise SrdfSourceError(f"{_display_path(output_path)} root element must be <robot>")
|
||||
ET.register_namespace("tcad", SRDF_METADATA_NAMESPACE)
|
||||
ET.register_namespace("explorer", LEGACY_EXPLORER_NAMESPACE)
|
||||
metadata_tag = f"{{{SRDF_METADATA_NAMESPACE}}}urdf"
|
||||
legacy_metadata_tag = f"{{{LEGACY_EXPLORER_NAMESPACE}}}urdf"
|
||||
metadata_element = None
|
||||
for child in list(root):
|
||||
if child.tag in {metadata_tag, legacy_metadata_tag} or str(child.tag or "") in {"tcad:urdf", "explorer:urdf"}:
|
||||
metadata_element = child
|
||||
break
|
||||
if metadata_element is None:
|
||||
metadata_element = ET.Element(metadata_tag)
|
||||
root.insert(0, metadata_element)
|
||||
else:
|
||||
metadata_element.tag = metadata_tag
|
||||
metadata_element.set("path", _relative_posix_path(urdf_path, output_path.parent))
|
||||
if root.text is None:
|
||||
root.text = "\n "
|
||||
if metadata_element.tail is None:
|
||||
metadata_element.tail = "\n "
|
||||
return _serialize_xml_element(root)
|
||||
|
||||
|
||||
def _validate_names_exist(names: object, allowed: set[str], *, label: str) -> None:
|
||||
for name in names:
|
||||
if name not in allowed:
|
||||
raise SrdfSourceError(f"{label} references missing name {name!r}")
|
||||
|
||||
|
||||
def _write_srdf_payload(xml: str, *, output_path: Path, script_path: Path) -> None:
|
||||
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 SRDF: {output_path}")
|
||||
|
||||
|
||||
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,277 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from math import isfinite
|
||||
from pathlib import Path
|
||||
from typing import Literal
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
SRDF_SUFFIX = ".srdf"
|
||||
SRDF_METADATA_NAMESPACE = "https://text-to-cad.dev/srdf"
|
||||
LEGACY_EXPLORER_NAMESPACE = "https://text-to-cad.dev/explorer"
|
||||
|
||||
|
||||
class SrdfSourceError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfChain:
|
||||
base_link: str
|
||||
tip_link: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfPlanningGroup:
|
||||
name: str
|
||||
joint_names: tuple[str, ...]
|
||||
link_names: tuple[str, ...]
|
||||
chains: tuple[SrdfChain, ...]
|
||||
subgroups: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfEndEffector:
|
||||
name: str
|
||||
parent_link: str
|
||||
group: str
|
||||
parent_group: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfGroupState:
|
||||
name: str
|
||||
group: str
|
||||
joint_values_by_name: dict[str, float]
|
||||
|
||||
@property
|
||||
def joint_values_by_name_rad(self) -> dict[str, float]:
|
||||
"""Deprecated compatibility alias. SRDF values use URDF-native units."""
|
||||
return self.joint_values_by_name
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfDisabledCollisionPair:
|
||||
link1: str
|
||||
link2: str
|
||||
reason: str
|
||||
source: Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"] = "manual"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfSource:
|
||||
file_ref: str
|
||||
source_path: Path
|
||||
robot_name: str
|
||||
urdf_ref: str
|
||||
planning_groups: tuple[SrdfPlanningGroup, ...]
|
||||
end_effectors: tuple[SrdfEndEffector, ...]
|
||||
group_states: tuple[SrdfGroupState, ...]
|
||||
disabled_collision_pairs: tuple[SrdfDisabledCollisionPair, ...]
|
||||
|
||||
|
||||
def file_ref_from_srdf_path(srdf_path: Path) -> str:
|
||||
resolved = srdf_path.resolve()
|
||||
if resolved.suffix.lower() != SRDF_SUFFIX:
|
||||
raise SrdfSourceError(f"{resolved} is not an SRDF source file")
|
||||
return _relative_to_repo(resolved)
|
||||
|
||||
|
||||
def read_srdf_source(srdf_path: Path) -> SrdfSource:
|
||||
resolved_path = srdf_path.resolve()
|
||||
if resolved_path.suffix.lower() != SRDF_SUFFIX:
|
||||
raise SrdfSourceError(f"{resolved_path} is not an SRDF source file")
|
||||
try:
|
||||
root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ET.ParseError) as exc:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as SRDF XML") from exc
|
||||
return parse_srdf_root(root, source_path=resolved_path)
|
||||
|
||||
|
||||
def parse_srdf_xml(xml_text: str, *, source_path: Path) -> SrdfSource:
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} could not be parsed as SRDF XML") from exc
|
||||
return parse_srdf_root(root, source_path=source_path)
|
||||
|
||||
|
||||
def parse_srdf_root(root: ET.Element, *, source_path: Path) -> SrdfSource:
|
||||
if root.tag != "robot":
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} root element must be <robot>")
|
||||
robot_name = str(root.attrib.get("name") or "").strip()
|
||||
if not robot_name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} robot name is required")
|
||||
urdf_ref = _linked_urdf_ref(root)
|
||||
|
||||
planning_groups: list[SrdfPlanningGroup] = []
|
||||
group_names: list[str] = []
|
||||
for group_element in root.findall("group"):
|
||||
name = str(group_element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group name is required")
|
||||
group_names.append(name)
|
||||
planning_groups.append(
|
||||
SrdfPlanningGroup(
|
||||
name=name,
|
||||
joint_names=_child_names(group_element, "joint"),
|
||||
link_names=_child_names(group_element, "link"),
|
||||
chains=tuple(
|
||||
SrdfChain(
|
||||
base_link=str(chain.attrib.get("base_link") or "").strip(),
|
||||
tip_link=str(chain.attrib.get("tip_link") or "").strip(),
|
||||
)
|
||||
for chain in group_element.findall("chain")
|
||||
),
|
||||
subgroups=_child_names(group_element, "group"),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(group_names, source_path=source_path, label="group")
|
||||
|
||||
end_effectors: list[SrdfEndEffector] = []
|
||||
end_effector_names: list[str] = []
|
||||
for element in root.findall("end_effector"):
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} end_effector name is required")
|
||||
end_effector_names.append(name)
|
||||
end_effectors.append(
|
||||
SrdfEndEffector(
|
||||
name=name,
|
||||
parent_link=str(element.attrib.get("parent_link") or "").strip(),
|
||||
group=str(element.attrib.get("group") or "").strip(),
|
||||
parent_group=str(element.attrib.get("parent_group") or "").strip(),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(end_effector_names, source_path=source_path, label="end_effector")
|
||||
|
||||
group_states: list[SrdfGroupState] = []
|
||||
group_state_keys: list[str] = []
|
||||
for element in root.findall("group_state"):
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
group = str(element.attrib.get("group") or "").strip()
|
||||
if not name or not group:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state requires name and group")
|
||||
group_state_keys.append(f"{group}/{name}")
|
||||
joint_values: dict[str, float] = {}
|
||||
joint_names: list[str] = []
|
||||
for joint_element in element.findall("joint"):
|
||||
joint_name = str(joint_element.attrib.get("name") or "").strip()
|
||||
if not joint_name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state joint name is required")
|
||||
joint_names.append(joint_name)
|
||||
try:
|
||||
joint_value = float(joint_element.attrib.get("value"))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be numeric"
|
||||
) from exc
|
||||
if not isfinite(joint_value):
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be finite"
|
||||
)
|
||||
joint_values[joint_name] = joint_value
|
||||
_raise_on_duplicates(joint_names, source_path=source_path, label=f"group_state {name!r} joint")
|
||||
group_states.append(SrdfGroupState(name=name, group=group, joint_values_by_name=joint_values))
|
||||
_raise_on_duplicates(group_state_keys, source_path=source_path, label="group_state")
|
||||
|
||||
disabled_pairs: list[SrdfDisabledCollisionPair] = []
|
||||
pair_keys: list[str] = []
|
||||
for element in root.findall("disable_collisions"):
|
||||
link1 = str(element.attrib.get("link1") or "").strip()
|
||||
link2 = str(element.attrib.get("link2") or "").strip()
|
||||
if not link1 or not link2:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions requires link1 and link2")
|
||||
if link1 == link2:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions cannot repeat the same link")
|
||||
reason = str(element.attrib.get("reason") or "").strip()
|
||||
if not reason:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} disable_collisions for {link1!r}/{link2!r} requires a reason"
|
||||
)
|
||||
pair_key = "/".join(sorted((link1, link2)))
|
||||
pair_keys.append(pair_key)
|
||||
disabled_pairs.append(
|
||||
SrdfDisabledCollisionPair(
|
||||
link1=link1,
|
||||
link2=link2,
|
||||
reason=reason,
|
||||
source=_disabled_collision_source(reason),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(pair_keys, source_path=source_path, label="disable_collisions")
|
||||
|
||||
return SrdfSource(
|
||||
file_ref=file_ref_from_srdf_path(source_path),
|
||||
source_path=source_path.resolve(),
|
||||
robot_name=robot_name,
|
||||
urdf_ref=urdf_ref,
|
||||
planning_groups=tuple(planning_groups),
|
||||
end_effectors=tuple(end_effectors),
|
||||
group_states=tuple(group_states),
|
||||
disabled_collision_pairs=tuple(disabled_pairs),
|
||||
)
|
||||
|
||||
|
||||
def _local_name(tag: str) -> str:
|
||||
return str(tag or "").rsplit("}", 1)[-1].split(":", 1)[-1]
|
||||
|
||||
|
||||
def _linked_urdf_ref(root: ET.Element) -> str:
|
||||
for child in list(root):
|
||||
if _local_name(child.tag) != "urdf":
|
||||
continue
|
||||
tag = str(child.tag or "")
|
||||
if (
|
||||
tag.startswith(f"{{{SRDF_METADATA_NAMESPACE}}}")
|
||||
or tag.startswith(f"{{{LEGACY_EXPLORER_NAMESPACE}}}")
|
||||
or tag.startswith("tcad:")
|
||||
or tag.startswith("explorer:")
|
||||
):
|
||||
return str(child.attrib.get("path") or "").strip()
|
||||
return ""
|
||||
|
||||
|
||||
def _child_names(element: ET.Element, tag: str) -> tuple[str, ...]:
|
||||
names: list[str] = []
|
||||
for child in element.findall(tag):
|
||||
name = str(child.attrib.get("name") or "").strip()
|
||||
if name:
|
||||
names.append(name)
|
||||
return tuple(names)
|
||||
|
||||
|
||||
def _raise_on_duplicates(names: list[str], *, source_path: Path, label: str) -> None:
|
||||
seen = set()
|
||||
duplicates = []
|
||||
for name in names:
|
||||
if name in seen:
|
||||
duplicates.append(name)
|
||||
seen.add(name)
|
||||
if duplicates:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} has duplicate {label} name(s): {sorted(set(duplicates))!r}"
|
||||
)
|
||||
|
||||
|
||||
def _disabled_collision_source(
|
||||
reason: str,
|
||||
) -> Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"]:
|
||||
normalized = str(reason or "").strip().lower()
|
||||
if "adjacent" in normalized:
|
||||
return "adjacent"
|
||||
if any(token in normalized for token in ("never", "always", "sample", "default")):
|
||||
return "sampled"
|
||||
if "setup" in normalized or "assistant" in normalized:
|
||||
return "setup_assistant"
|
||||
if "assum" in normalized:
|
||||
return "assumed"
|
||||
return "manual"
|
||||
|
||||
|
||||
def _relative_to_repo(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