[planner] early exit if rotation doesn't match
This commit is contained in:
parent
f864ebbeab
commit
84664303f1
2 changed files with 65 additions and 0 deletions
|
|
@ -857,6 +857,22 @@ class Solver:
|
|||
return False
|
||||
return self.request.out_ptype is None or ptypes_compatible(end_port.ptype, self.request.out_ptype)
|
||||
|
||||
def rotation_matches_request(self, steps: Sequence[SelectedPrimitive]) -> bool:
|
||||
"""
|
||||
Return true when a sequence can produce the requested output rotation.
|
||||
|
||||
Primitive parameters do not affect the rotation contract, so
|
||||
rotation-impossible candidates can be rejected before parameter solving.
|
||||
"""
|
||||
angle = 0.0
|
||||
for step in steps:
|
||||
step_rotation = step.out_port.rotation
|
||||
if step_rotation is None:
|
||||
raise BuildError('Primitive endpoints must have rotation')
|
||||
angle += float(step_rotation) + pi
|
||||
rotation_delta = ((angle - pi) - self.request.out_rotation) % (2 * pi)
|
||||
return is_close(rotation_delta, 0) or is_close(rotation_delta, 2 * pi)
|
||||
|
||||
def finalize(self, steps: Sequence[SelectedPrimitive]) -> Candidate:
|
||||
"""
|
||||
Try all small solve sets for one raw sequence and return the first match.
|
||||
|
|
@ -874,6 +890,8 @@ class Solver:
|
|||
raise BuildError(f'{self.request.route_name} route requires a jog constraint')
|
||||
constraints.append(('y', float(self.request.jog)))
|
||||
route_constraints = tuple(constraints)
|
||||
if not self.rotation_matches_request(steps):
|
||||
raise BuildError(f'{self.request.route_name} composed primitive route is unsupported')
|
||||
adjustable = self.adjustable_indices(steps)
|
||||
solve_sets: list[tuple[int, ...]] = [()]
|
||||
max_solve = min(len(route_constraints), len(adjustable))
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue