namespace PsHttpGate { public static partial class Comman...

Prompt

namespace PsHttpGate { public static partial class CommandHandlers { private const int VoxelRepairDefaultMaxInsertedVoxels = 6; private const int VoxelRepairDefaultMaxCandidatesPerEdge = 150; private const int VoxelRepairDefaultMaxSearchRadiusVoxels = 6; private static object VoxelRepairPath(Dictionary<string, object> parameters) { var s = RequireVoxelSession(); EnsureNoVoxelJob(); var g = s.Grid; var voxelIdx = new List<int>(); if (parameters.TryGetValue("voxels", out var voxObj) && voxObj is System.Collections.IEnumerable voxEnum && !(voxObj is string)) { foreach (var v in voxEnum) { int idx = Convert.ToInt32(v); if (idx < 0 || idx >= g.Total) throw new PsActionException("invalid_input", $"Voxel index {idx} is out of range (0..{g.Total - 1})."); voxelIdx.Add(idx); } } else if (parameters.TryGetValue("points", out var ptsObj) && ptsObj is System.Collections.IEnumerable ptsEnum && !(ptsObj is string)) { foreach (var p in ptsEnum) { double[] pt = ReadXyz(p, "points[]"); int idx = g.WorldToIndex(pt[0], pt[1], pt[2]); if (idx < 0) throw new PsActionException("invalid_input", "A 'points[]' entry falls outside the voxel grid -- repair needs every waypoint mapped to a real voxel index."); voxelIdx.Add(idx); } } else { throw new ArgumentException("Give the ordered selection as 'voxels' (array of grid indices) or 'points' (array of {x,y,z} in mm, must fall inside the grid)."); } if (voxelIdx.Count < 2) throw new PsActionException("invalid_input", "Give at least 2 waypoints -- there must be >= 1 edge to repair."); string motion = ParamString(parameters, "motion", "linear"); int substeps = Math.Max(1, ParamInt(parameters, "substeps", 10)); int maxInsertedVoxels = Math.Max(0, ParamInt(parameters, "max_inserted_voxels", VoxelRepairDefaultMaxInsertedVoxels)); double maxSeconds = ParamDouble(parameters, "max_seconds", 8.0); int maxSearchRadiusVoxels = Math.Max(1, ParamInt(parameters, "max_search_radius_voxels", VoxelRepairDefaultMaxSearchRadiusVoxels)); int maxCandidatesPerEdge = Math.Max(1, ParamInt(parameters, "max_candidates_per_edge", VoxelRepairDefaultMaxCandidatesPerEdge)); var clock = Stopwatch.StartNew(); var result = new List<int> { voxelIdx[0] }; var edgeReports = new List<object>(); var unresolved = new List<string>(); int insertedTotal = 0; int repairedEdges = 0; using (var work = BeginVoxelWork(s)) { var robot = work.Robot; object fromPose = PoseAt(s, g.CenterArray(voxelIdx[0])); object fromSolved = SolveIkSolution(robot, fromPose, work.OriginalJoints); if (fromSolved == null) { return new { robot = s.RobotName, originalWaypointCount = voxelIdx.Count, resultVoxels = voxelIdx, resultWaypointCount = voxelIdx.Count, insertedVoxels = 0, repairedEdges = 0, edges = edgeReports, unresolved = new List<string> { "waypoint 0 has no IK solution at all -- nothing downstream can be repaired" }, status = "NOT_CERTIFIED", elapsedMs = Math.Round(clock.Elapsed.TotalMilliseconds), message = "The FIRST waypoint is unreachable -- fix it directly (spin_waypoint / a different voxel) before repairing the rest of the path.", }; } double[] fromJoints = ExtractJointValues(fromSolved); ApplyPoseData(robot, fromSolved); for (int i = 0; i < voxelIdx.Count - 1; i++) { int fromIdx = voxelIdx[i], toIdx = voxelIdx[i + 1]; if (clock.Elapsed.TotalSeconds > maxSeconds) { unresolved.Add($"edge {i}->{i + 1} (voxel {fromIdx}->{toIdx}): repair budget (max_seconds={maxSeconds}) exhausted before this edge was reached"); edgeReports.Add(new { from = i, to = i + 1, fromVoxel = fromIdx, toVoxel = toIdx, blocked = (bool?)null, reason = "budget exhausted", repair = (object)null }); result.Add(toIdx); continue; } object toPose = PoseAt(s, g.CenterArray(toIdx)); object toSolved = SolveIkSolution(robot, toPose, fromJoints); bool blocked; string reason; double[] toJoints = null; if (toSolved == null) { blocked = true; reason = "goal waypoint has no IK solution in the incoming branch"; } else { toJoints = ExtractJointValues(toSolved); var sweep = SweepBetween(robot, fromPose, fromJoints, toPose, toJoints, motion, substeps, s.NearMissMm, s.CollisionTarget); blocked = sweep.collidingSteps > 0 || sweep.unreachableSteps > 0; reason = sweep.collidingSteps > 0 ? $"{sweep.collidingSteps} swept sample(s) collide" + NameList(sweep.collidedWith) : sweep.unreachableSteps > 0 ? $"{sweep.unreachableSteps} swept sample(s) are unreachable" : null; } if (!blocked) { edgeReports.Add(new { from = i, to = i + 1, fromVoxel = fromIdx, toVoxel = toIdx, blocked = false, reason, repair = (object)null }); result.Add(toIdx); ApplyPoseData(robot, toSolved); fromPose = toPose; fromJoints = toJoints; continue; } var tierDiagnostics = new List<string>(); byte toState = EvaluateVoxel(s, work, toIdx); if (toState != VoxelCell.Free) tierDiagnostics.Add($"waypoint_check: ); var apex = TryTier2_ClearanceApex(s, work, fromIdx, toIdx, maxSearchRadiusVoxels, maxCandidatesPerEdge); tierDiagnostics.Add($"tier2_clearance_apex: {apex.diagnostic}"); List<int> chainResult = apex.chain; if (chainResult != null && insertedTotal + (chainResult.Count - 2) > maxInsertedVoxels) { tierDiagnostics.Add($"candidate chain needed {chainResult.Count - 2} inserted voxel(s), only {maxInsertedVoxels - insertedTotal} left of max_inserted_voxels -- refused, never silently truncated"); chainResult = null; } if (chainResult == null) { string diag = string.Join("; ", tierDiagnostics); unresolved.Add($"edge {i}->{i + 1} (voxel {fromIdx}->{toIdx}): {reason}; no verified repair found within budget -- {diag}"); edgeReports.Add(new { from = i, to = i + 1, fromVoxel = fromIdx, toVoxel = toIdx, blocked = true, reason, repair = new { attempted = true, resolvedBy = (string)null, verified = false, diagnostics = tierDiagnostics } }); result.Add(toIdx); if (toSolved != null) { ApplyPoseData(robot, toSolved); fromPose = toPose; fromJoints = toJoints; } continue; } var verify = VerifyVoxelChain(s, work, chainResult, fromPose, fromJoints, motion, substeps); if (!verify.ok) { unresolved.Add($"edge {i}->{i + 1} (voxel {fromIdx}->{toIdx}): clearance_apex candidate FAILED independent re-verification ({verify.reason}) -- refused, not spliced in"); edgeReports.Add(new { from = i, to = i + 1, fromVoxel = fromIdx, toVoxel = toIdx, blocked = true, reason, repair = new { attempted = true, resolvedBy = "clearance_apex", verified = false, reason = verify.reason }, }); result.Add(toIdx); if (toSolved != null) { ApplyPoseData(robot, toSolved); fromPose = toPose; fromJoints = toJoints; } continue; } int insertedHere = chainResult.Count - 2; insertedTotal += insertedHere; repairedEdges++; for (int k = 1; k < chainResult.Count; k++) result.Add(chainResult[k]); edgeReports.Add(new { from = i, to = i + 1, fromVoxel = fromIdx, toVoxel = toIdx, blocked = true, reason, repair = new { attempted = true, resolvedBy = "clearance_apex", verified = true, insertedVoxels = chainResult.GetRange(1, insertedHere) }, }); fromPose = verify.finalPose; fromJoints = verify.finalJoints; } } bool resolved = unresolved.Count == 0; return new { robot = s.RobotName, originalWaypointCount = voxelIdx.Count, resultVoxels = result, resultWaypointCount = result.Count, insertedVoxels = insertedTotal, repairedEdges, edges = edgeReports, unresolved, status = resolved ? "verified_local" : "NOT_CERTIFIED", elapsedMs = Math.Round(clock.Elapsed.TotalMilliseconds), message = resolved ? }; } private static (List<int> chain, string diagnostic) TryTier2_ClearanceApex(VoxelSession s, VoxelWork work, int fromIdx, int toIdx, int maxSearchRadiusVoxels, int maxCandidates) { var g = s.Grid; g.Center(fromIdx, out double fx, out double fy, out double fz); g.Center(toIdx, out double tx, out double ty, out double tz); double[] mid = { (fx + tx) / 2.0, (fy + ty) / 2.0, (fz + tz) / 2.0 }; int midIdx = g.WorldToIndex(mid[0], mid[1], mid[2]); if (midIdx < 0) midIdx = fromIdx; // segment midpoint fell outside the grid box -- search around the FROM voxel instead var candidates = new List<(int idx, double distMm)> { (midIdx, 0.0) }; candidates.AddRange(GrowingShellCandidates(g, midIdx, maxSearchRadiusVoxels)); int probed = 0, freeCount = 0; int bestIdx = -1; double bestClearance = -1; foreach (var cand in candidates) { if (cand.idx == fromIdx || cand.idx == toIdx) continue; if (probed >= maxCandidates) break; byte state = EvaluateVoxel(s, work, cand.idx); probed++; if (state != VoxelCell.Free) continue; freeCount++; double clearance = g.ClearanceMm.TryGetValue(cand.idx, out double c) ? c : 0.0; if (clearance > bestClearance) { bestClearance = clearance; bestIdx = cand.idx; } } if (bestIdx < 0) return (null, $"probed {probed}/{candidates.Count} candidate(s) within a {maxSearchRadiusVoxels}-voxel radius of the segment midpoint, {freeCount} were free -- " + "widen max_search_radius_voxels/max_candidates_per_edge, or this edge may need a 2+ point retreat/traverse detour that a single apex voxel can't express"); return (new List<int> { fromIdx, bestIdx, toIdx }, $"apex voxel {bestIdx} ({bestClearance:F0}mm clearance, probed {probed}/{candidates.Count} candidate(s))"); } private static (bool ok, string reason, object finalPose, double[] finalJoints) VerifyVoxelChain( VoxelSession s, VoxelWork work, List<int> chain, object fromPose, double[] fromJoints, string motion, int substeps) { var robot = work.Robot; object curPose = fromPose; double[] curJoints = fromJoints; for (int k = 1; k < chain.Count; k++) { object targetPose = PoseAt(s, s.Grid.CenterArray(chain[k])); object solved = SolveIkSolution(robot, targetPose, curJoints); if (solved == null) return (false, $"voxel {chain[k]} has no IK solution continuing from the previous branch", null, null); double[] targetJoints = ExtractJointValues(solved); var sweep = SweepBetween(robot, curPose, curJoints, targetPose, targetJoints, motion, substeps, s.NearMissMm, s.CollisionTarget); if (sweep.collidingSteps > 0 || sweep.unreachableSteps > 0) return (false, $"sub-edge into voxel {chain[k]} still collides/unreachable ({sweep.collidingSteps} colliding, {sweep.unreachableSteps} unreachable steps)", null, null); ApplyPoseData(robot, solved); // deterministic FROM state for the next sub-edge curPose = targetPose; curJoints = targetJoints; } return (true, null, curPose, curJoints); } } } Redesign these heuristics to handle complex 3D environments where the robot must find a collision-free path while dynamically. However, any existing waypoints in the original path must be preserved and remain part of the final trajectory. The added waypoints should therefore serve only as local detours between the existing waypoints, without removing, replacing, or significantly altering the original path structure.