File size: 6,989 Bytes
985c397 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 | # -*- coding: utf-8 -*-
# SPDX-License-Identifier: LGPL-2.1-or-later
# ***************************************************************************
# * *
# * Copyright (c) 2025 sliptonic sliptonic@freecad.org *
# * *
# * This file is part of FreeCAD. *
# * *
# * FreeCAD is free software: you can redistribute it and/or modify it *
# * under the terms of the GNU Lesser General Public License as *
# * published by the Free Software Foundation, either version 2.1 of the *
# * License, or (at your option) any later version. *
# * *
# * FreeCAD is distributed in the hope that it will be useful, but *
# * WITHOUT ANY WARRANTY; without even the implied warranty of *
# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
# * Lesser General Public License for more details. *
# * *
# * You should have received a copy of the GNU Lesser General Public *
# * License along with FreeCAD. If not, see *
# * <https://www.gnu.org/licenses/>. *
# * *
# ***************************************************************************
import Part
import Path
from FreeCAD import Vector
from typing import List, Optional
if False:
Path.Log.setLevel(Path.Log.Level.DEBUG, Path.Log.thisModule())
Path.Log.trackModule(Path.Log.thisModule())
else:
Path.Log.setLevel(Path.Log.Level.INFO, Path.Log.thisModule())
def check_collision(
start_position: Vector,
target_position: Vector,
solids: Optional[List[Part.Shape]] = None,
tolerance: float = 0.001,
) -> bool:
"""
Check if a direct move from start to target would collide with solids.
Returns True if collision detected, False if path is clear.
"""
if start_position == target_position:
return False
# Build collision model
collision_model = None
if solids:
solids = [s for s in solids if s]
if len(solids) == 1:
collision_model = solids[0]
elif len(solids) > 1:
collision_model = Part.makeCompound(solids)
if not collision_model:
return False
# Create direct path wire
wire = Part.Wire([Part.makeLine(start_position, target_position)])
distance = wire.distToShape(collision_model)[0]
return distance < tolerance
def get_linking_moves(
start_position: Vector,
target_position: Vector,
local_clearance: float,
global_clearance: float,
tool_shape: Part.Shape, # required placeholder
solids: Optional[List[Part.Shape]] = None,
retract_height_offset: Optional[float] = None,
skip_if_no_collision: bool = False,
) -> list:
"""
Generate linking moves from start to target position.
If skip_if_no_collision is True and the direct path at the current height
is collision-free, returns empty list (useful for canned drill cycles that
handle their own retraction).
"""
if start_position == target_position:
return []
# For canned cycles: if we're already at a safe height and can move directly, skip linking
if skip_if_no_collision:
if not check_collision(start_position, target_position, solids):
return []
if local_clearance > global_clearance:
raise ValueError("Local clearance must not exceed global clearance")
if retract_height_offset is not None and retract_height_offset < 0:
raise ValueError("Retract offset must be positive")
# Collision model
collision_model = None
if solids:
solids = [s for s in solids if s]
if len(solids) == 1:
collision_model = solids[0]
elif len(solids) > 1:
collision_model = Part.makeCompound(solids)
# Determine candidate heights
if retract_height_offset is not None:
if retract_height_offset > 0:
retract_height = max(start_position.z, target_position.z) + retract_height_offset
candidate_heights = {retract_height, local_clearance, global_clearance}
else: # explicitly 0
retract_height = max(start_position.z, target_position.z)
candidate_heights = {retract_height, local_clearance, global_clearance}
else:
candidate_heights = {local_clearance, global_clearance}
heights = sorted(candidate_heights)
# Try each height
for height in heights:
wire = make_linking_wire(start_position, target_position, height)
if is_wire_collision_free(wire, collision_model):
cmds = Path.fromShape(wire).Commands
# Ensure all commands have complete XYZ coordinates
# Path.fromShape() may omit coordinates that don't change
current_pos = start_position
complete_cmds = []
for i, cmd in enumerate(cmds):
params = dict(cmd.Parameters)
# Fill in missing coordinates from current position
x = params.get("X", current_pos.x)
y = params.get("Y", current_pos.y)
# For the last command (plunge to target), use target.z if Z is missing
if "Z" not in params and i == len(cmds) - 1:
z = target_position.z
else:
z = params.get("Z", current_pos.z)
complete_cmds.append(Path.Command("G0", {"X": x, "Y": y, "Z": z}))
current_pos = Vector(x, y, z)
return complete_cmds
raise RuntimeError("No collision-free path found between start and target positions")
def make_linking_wire(start: Vector, target: Vector, z: float) -> Part.Wire:
p1 = Vector(start.x, start.y, z)
p2 = Vector(target.x, target.y, z)
edges = []
# Only add retract edge if there's actual movement
if not start.isEqual(p1, 1e-6):
edges.append(Part.makeLine(start, p1))
# Only add traverse edge if there's actual movement
if not p1.isEqual(p2, 1e-6):
edges.append(Part.makeLine(p1, p2))
# Only add plunge edge if there's actual movement
if not p2.isEqual(target, 1e-6):
edges.append(Part.makeLine(p2, target))
return Part.Wire(edges) if edges else Part.Wire([Part.makeLine(start, target)])
def is_wire_collision_free(
wire: Part.Wire, solid: Optional[Part.Shape], tolerance: float = 0.001
) -> bool:
if not solid:
return True
distance = wire.distToShape(solid)[0]
return distance >= tolerance
|