metafarmers/visual-servo-manipulation Private

12345678910111213141516171819202122232425

from __future__ import annotationsfrom math import sqrtdef execute(payload: dict) -> dict:    error = payload.get("positionErrorMm")    if not isinstance(error, list) or len(error) != 3:        raise ValueError("positionErrorMm must contain x, y, z")    max_step = float(payload.get("maxStepMm", 10))    tolerance = float(payload.get("toleranceMm", 2))    if max_step <= 0 or tolerance < 0:        raise ValueError("motion limits must be positive")    vector = [float(value) for value in error]    step = [max(-max_step, min(max_step, value)) for value in vector]    residual_vector = [value - motion for value, motion in zip(vector, step)]    residual = sqrt(sum(value * value for value in residual_vector))    converged = sqrt(sum(value * value for value in vector)) <= tolerance    if converged:        step = [0.0, 0.0, 0.0]        residual = 0.0    return {"component": "metafarmers/visual-servo-manipulation", "accepted": True,            "status": "ALIGNED" if converged else "ALIGNING", "target": "acu",            "targetId": str(payload.get("targetId", "unknown")), "stepMm": step,            "residualMm": round(residual, 3), "converged": converged}