Define composable state sets for starts, goals, and path constraints

[siddhss5]

Motivation

CBiRRT is naturally a planner between sets. Let $\mathcal{Q}$ be the configuration space, $\mathcal{S} \subseteq \mathcal{Q}$ the start set, $\mathcal{G} \subseteq \mathcal{Q}$ the goal set, and $\mathcal{C} \subseteq \mathcal{Q}$ the path-admissible set. The planning problem is to find a continuous path $\tau : [0,1] \to \mathcal{Q}$ such that

$$\tau(0) \in \mathcal{S}, \qquad \tau(1) \in \mathcal{G}, \qquad \tau(t) \in \mathcal{C} \quad \forall t \in [0,1].$$

The current API represents these sets through separate arguments such as fixed configurations, start_tsrs, goal_tsrs, and constraint_tsrs. Their composition semantics are implicit and depend on the argument:

• multiple start or goal TSRs are treated as a union;
• multiple path-constraint TSRs are treated as an intersection;
• projection onto an intersection greedily chooses the most violated TSR.

This makes a Python list carry different mathematical meanings in different places. It also conflates the definition of a set with one particular way of sampling or projecting onto it. In particular, projecting onto the most violated TSR is a heuristic for an intersection, not an operation proposed by CBiRRT or implied by the definition of an intersection.

The original CBiRRT paper separates the bidirectional search from a problem-specific ConstrainConfig operation. The later CBiRRT2 formulation further distinguishes direct sampling, projection, and rejection as different constraint-satisfaction strategies. We should make this latent structure explicit before adding TSR chains or more general constraint representations.

References:

• Manipulation Planning on Constraint Manifolds
• Task Space Regions: A Framework for Pose-Constrained Manipulation Planning
• Related follow-on work: Support TSR chains for multi-link pose constraints #7

Proposal

1. Give sets a minimal semantic interface

A state set should first define only membership:

class StateSet(Protocol):
    def contains(self, q: np.ndarray) -> bool: ...

Fixed configurations, finite collections of configurations, predicates, and TSR-induced configuration sets should all be representable through this abstraction.

A TSR is a set in task space. It induces a configuration-space set through forward kinematics:

$$\mathcal{C}_{\mathrm{TSR}} = \{q \in \mathcal{Q} \mid \mathrm{FK}(q) \in \mathrm{TSR}\}.$$

The planner should consume an adapter for this induced set rather than branch on the concrete TSR type.

2. Make Boolean composition explicit

Introduce explicit set constructors with arbitrary nesting:

AnyOf([set_a, set_b])  # union
AllOf([set_a, set_b])  # intersection

Their membership semantics are:

$$q \in \mathrm{AnyOf}(C_i) \iff \exists i : q \in C_i,$$ $$q \in \mathrm{AllOf}(C_i) \iff \forall i : q \in C_i.$$

Nesting must preserve grouping. For example,

$$(L_1 \land R_1) \lor (L_2 \land R_2)$$

describes two matched bimanual grasps, whereas

$$(L_1 \lor L_2) \land (R_1 \lor R_2)$$

allows all four pairings.

We should not add complement/Not initially. Exclusion constraints such as collision are better handled by state and motion validators, and complements generally do not provide useful sampling or projection operations.

3. Separate set semantics from planner capabilities

Sampling, distance or violation evaluation, projection, and motion validation are operations associated with a set, not part of the definition of a set. They should be represented as separate optional capabilities, for example:

class SetSampler(Protocol):
    def sample(self, rng: np.random.Generator) -> np.ndarray | None: ...

class SetProjector(Protocol):
    def project(
        self,
        q_previous: np.ndarray,
        q_proposed: np.ndarray,
    ) -> np.ndarray | None: ...

class MotionValidator(Protocol):
    def is_valid_motion(self, q1: np.ndarray, q2: np.ndarray) -> bool: ...

The exact Python organization can be decided during implementation. The required semantic distinction is:

• a finite set may be directly sampleable but need no projector;
• a collision-free set supports rejection and edge validation but generally no projection;
• a TSR-induced set can support membership, pose sampling, distance evaluation, and IK-based projection;
• an implicit equality constraint may support projection without direct sampling;
• an arbitrary predicate may support only membership.

Composition of capabilities must also be explicit:

• an AnyOf sampler needs a stated mixture policy;
• an AnyOf projector may try children and choose a successful nearby result;
• a single-child AllOf or AnyOf delegates its capabilities to that child and needs no strategy;
• an AllOf projector with two or more children generally requires a joint projection strategy;
• alternating projection or projection onto the most violated child may be provided as named heuristics, but must not be silently implied by AllOf.

4. Express planner inputs in terms of roles

The internal planning problem should distinguish the role a set plays without changing what a set means:

PlanningProblem(
    state_space=...,
    start=...,
    goal=...,
    path_constraint=...,
    motion_validator=...,
)

CBiRRT can then declare the capabilities it requires from each role:

• roots or a sampler for the start and goal sets;
• projection or rejection for the path-admissible set;
• state and full-edge validation for every local extension;
• distance and interpolation supplied by the state space.

The existing plan(...) API should initially remain available and lower its arguments into this representation. This issue should not require a public breaking change.

Representative compositions

A bimanual goal with two matched grasp strategies:

AnyOf([
    AllOf([left_grasp_1, right_grasp_1]),
    AllOf([left_grasp_2, right_grasp_2]),
])

The heavy-object example from the CBiRRT paper is approximately:

$$\mathrm{Upright} \cap \left( \mathrm{Liftable} \cup \bigcup_i (\mathrm{OnSurface}_i \cap \mathrm{TorqueValid}_i) \right).$$

These examples show why both composition operators and their nesting are part of the core problem representation rather than planner-specific conveniences.

Acceptance criteria

• Add a short design document defining the state space, start set, goal set, path-admissible set, membership, and composition semantics.
• Define a minimal configuration-set abstraction whose core semantic operation is membership.
• Implement explicit AnyOf and AllOf composition with arbitrary nesting.
• Keep sampling, projection, violation/distance evaluation, and motion validation as distinct capabilities or strategies.
• Add adapters for finite configuration sets and TSR-induced configuration sets.
• Preserve the existing plan(...) entry point by lowering legacy inputs into the new representation.
• Do not silently provide a generic AllOf projector for two or more children. A single-child AllOf delegates projection to that child. Multi-child intersections require an explicit joint or named heuristic projection strategy.
• Give membership tolerance, projection progress tolerance, tree-connection tolerance, and edge-checking resolution distinct meanings.
• Add tests for union, intersection, nested grouping, unsupported capabilities, and TSRs with non-identity T0_w and Tw_e frames.
• Add at least one planning test whose goal is an AnyOf of alternatives and one whose path constraint is an AllOf composition.
• Document the relationship between this abstraction and TSR chains in Support TSR chains for multi-link pose constraints #7. A TSR chain defines one pose set through kinematic composition; it is not an AllOf of its constituent TSRs.

Non-goals

• Extracting a general ssplanning package.
• Rewriting CBiRRT in C++.
• Integrating OMPL or RoboPlan.
• Implementing TSR chains as part of this issue.
• Supporting arbitrary Boolean negation.
• Claiming that every composed set is sampleable or projectable.

The purpose of this issue is to establish a precise and testable problem representation inside pycbirrt. Broader extraction should be considered only after this interface has been exercised by the existing planner.

[siddhss5]

Implementation plan

Work is split into six slices, one PR each. Slices 1 and 2 are independent; the rest are sequential.

1. State sets: membership, AnyOf/AllOf composition, capabilities #18 State sets: membership, AnyOf/AllOf composition, capabilities (no planner changes)
2. Extract JointSpace from CBiRRT #19 Extract JointSpace from CBiRRT (pure refactor)
3. TSRConfigurationSet adapter #20 TSRConfigurationSet adapter (depends on 1, 2)
4. PlanningProblem and solve(problem) core with legacy lowering #21 PlanningProblem and solve(problem) core with legacy lowering (depends on 3)
5. Split tsr_tolerance into distinct tolerances #22 Split tsr_tolerance into distinct tolerances
6. Composition planning tests and design document #23 Composition planning tests and design document

The design document is written last, once the interface has been exercised by the planner.

[siddhss5]

Done across six PRs, each one slice of the plan above:

slice	PR	what landed
1	#24	StateSet + capabilities, FiniteSet, PredicateSet, AnyOf, AllOf, MostViolatedProjection, hypothesis laws
2	#25	JointSpace
3	#31	TSRConfigurationSet, tsr_weights (after #29 fixed frame composition in projection, #28)
4	#33	PlanningProblem, solve(problem), legacy lowering; sampler returns candidate lists after a UR5e integration check caught a regression
5	#38	distinct membership / connection / edge-resolution / projection-progress tolerances
6	#39	composition planning tests, RejectionSampling, docs/design.md, README section, chain note on #7

Every acceptance criterion above is checked. The single-child rule (an AllOf or AnyOf of one delegates without a strategy) is in, per the discussion.

Found along the way and tracked separately: #35 / mj_manipulator#171 (UR5e joints marked angular; the real-robot path is fixed in #171, pending merge), #36 (2π aliases of IK solutions on wide bounded joints), #32 (two examples crash on an old result field), personalrobotics/tsr#52 and #53 (seeded sampling and a closest-transform method upstream).

[siddhss5]

Outcome

Shipped in v1.1.0. What landed relative to the proposal above, and what changed along the way.

What the representation now is

• PlanningProblem(space, start, goal, validator, path_constraint) and CBiRRT.solve(problem). Roles are explicit and independent of what a set means.
• StateSet requires only contains. SetSampler, SetDistance, SetProjector are optional capabilities; supports(s, Cap) queries them.
• AnyOf / AllOf with arbitrary nesting and exact Boolean membership. Capabilities of composites are explicit: a single child delegates; a multi-child AnyOf samples only with weights and projects to the nearest successful child projection; a multi-child AllOf projects only with a named strategy (MostViolatedProjection) and samples only with one (RejectionSampling).
• Leaves: FiniteSet, PredicateSet, EmptySet, and TSRConfigurationSet, the set a TSR induces through FK, with membership, distance, sampling, and frame-correct projection.
• JointSpace owns limits, metric, direction, interpolation, sampling.
• Five named tolerances replace tsr_tolerance (kept as a deprecated alias).
• plan(...) is unchanged and lowers into the above via pycbirrt.legacy. Only tsr_set and legacy import the tsr package.
• docs/design.md is the reference.

What this enables that the list API could not express

• Grouped alternatives: (L1 ∧ R1) ∨ (L2 ∧ R2) vs (L1 ∨ L2) ∧ (R1 ∨ R2), tested.
• Non-TSR sets as goals or constraints: any predicate, joint-space boxes, custom sets; intersected with TSR regions as needed.
• Constraints enforced by rejection when no projector exists.
• Provenance as a path through the nesting (PlanResult.start_source / goal_source), not a flat index.
• New constraint types (chains, multi-link, implicit equality) as new set classes with no change to the search.

Decisions made during implementation, beyond the proposal

1. Single-child composites delegate without a strategy (discussed above). A one-TSR constraint costs nothing.
2. A sample draw returns a list of candidates, not one configuration. A TSR pose has several IK branches and the set cannot know which is collision-free; returning one and capping per pose discarded the only valid branch on a real UR5e start region and produced no roots. The planner validates candidates and keeps up to max_ik_per_pose per draw, which is exactly the legacy semantics.
3. Collision is never part of a set. Sets use raw IKSolver.solve plus joint limits; the validator is the planner's. This also removed the solve_valid requirement from sets.
4. RejectionSampling was added because the proposal's own bimanual example, a union of intersections, could not be sampled without it.
5. Roots come from S ∩ C (and G ∩ C) by rejection, stated explicitly, with the legacy exceptions and messages preserved, including error ordering (start validated before "no goal"), which is what EmptySet is for.
6. is_finite / members: an intersection with a finite child is finite, so AllOf(FiniteSet, predicate) enumerates roots.
7. Goal/start bias applies to any sampleable set, finite ones included.
8. Seeded planning is now actually reproducible. TSR.sample uses numpy's global state (tsr#52); the adapter samples the bounds with the planner's generator.

Found and fixed along the way

• Projection ignored T0_w / Tw_e (Constraint projection ignores TSR frames (T0_w, Tw_e) #28): failed outright for any framed constraint TSR.
• UR5e examples and the downstream planner factory marked bounded ±2π joints as angular, producing full-turn jumps in paths (UR5e examples mark bounded joints as angular, producing full-turn jumps in paths #35, mj_manipulator#170). Verified against UR's spec: only the UR3e has a continuous wrist.
• CI never honored a lock and drifted formatter versions (CI ignores uv.lock under UV_NO_SOURCES, so tool versions drift #26); two examples had been crashing since March (examples/planar_arm.py and multi_config_demo.py crash on PlanResult.tree_start #32).

Deferred

• 2π aliases of IK solutions on wide bounded joints (TSRConfigurationSet omits 2π aliases of IK solutions on joints with range > 2π #36).
• TSR chains as one induced set, and multi-link constraints as AllOf with a stacked-Jacobian projection (Support TSR chains for multi-link pose constraints #7, see comment there).
• A closest-world-transform method on TSR upstream (tsr#53) would remove the frame composition from pycbirrt.

[siddhss5]

Post-release hardening review

The v1.1.0 architecture is sound and the full suite passes (152 passed, 1 skipped), but focused adversarial checks found five correctness gaps plus one follow-on interface improvement.

Verified correctness gaps

• Preserve explicit roots in mixed finite and sampleable unions #42 preserves explicit configurations as roots when a role is a union of finite and sampled alternatives.
• Enforce JointSpace validity for every accepted configuration #43 makes ambient JointSpace membership part of admissibility for roots, samples, projections, and edge states.
• Validate final connection gaps and smoothing shortcuts #47 requires an exact validated final connection, removes the unchecked smoothing snap, and compares geometric path length rather than waypoint count.
• Define a composable violation contract for intersection projection #44 separates geometric distance from normalized constraint violation so MostViolatedProjection cannot select an already satisfied child.
• Use PlanningProblem.space consistently during search #45 uses PlanningProblem.space consistently throughout direct solve(problem) queries.

Follow-on interface

• Make local-motion validation an explicit planning capability #46 makes local-motion validation explicit and replaceable after Validate final connection gaps and smoothing shortcuts #47 establishes the correct default behavior.

Suggested order:

1. Preserve explicit roots in mixed finite and sampleable unions #42 and Enforce JointSpace validity for every accepted configuration #43 are independent.
2. Validate final connection gaps and smoothing shortcuts #47 should build on the admissibility boundary from Enforce JointSpace validity for every accepted configuration #43.
3. Define a composable violation contract for intersection projection #44 and Use PlanningProblem.space consistently during search #45 are independent of the connection work.
4. Make local-motion validation an explicit planning capability #46 should follow Validate final connection gaps and smoothing shortcuts #47 so the abstraction is extracted from validated behavior.

The first five issues have minimal reproductions and concrete acceptance criteria. #46 is deliberately an enhancement rather than a release-blocking correctness claim.