motion: check circular moves against axis soft limits

src/Makefile

@@ -1224,6 +1224,8 @@ motmod-objs += emc/motion/simple_tp.o
 motmod-objs += emc/motion/emcmotutil.o
 motmod-objs += emc/motion/stashf.o
 motmod-objs += emc/motion/dbuf.o
+motmod-objs += libnml/posemath/_posemath.o
+motmod-objs += libnml/posemath/sincos.o $(MATHSTUB)
 
 obj-m += homemod.o
 homemod-objs := emc/motion/homemod.o

src/emc/motion/command.c

@@ -185,14 +185,10 @@ void apply_spindle_limits(spindle_status_t *s){
 }
 
 
-/* inRange() returns non-zero if the position lies within the joint
-   limits, or 0 if not.  It also reports an error for each joint limit
-   violation.  It's possible to get more than one violation per move. */
-STATIC int inRange(EmcPose pos, int id, char *move_type)
+/* Check Cartesian axis limits without applying inverse kinematics. */
+STATIC int axisInRange(EmcPose pos, int id, char *move_type)
 {
-    double joint_pos[EMCMOT_MAX_JOINTS];
-    int joint_num, axis_num;
-    emcmot_joint_t *joint;
+    int axis_num;
     int in_range = 1;
     int failing_axes[EMCMOT_MAX_AXIS];
     double targets[EMCMOT_MAX_AXIS];
@@ -225,7 +221,16 @@ STATIC int inRange(EmcPose pos, int id, char *move_type)
         }
     }
 
-    /* Now, check that the endpoint puts the joints within their limits too */
+    return in_range;
+}
+
+/* Check the endpoint against both Cartesian axis and joint limits. */
+STATIC int inRange(EmcPose pos, int id, char *move_type)
+{
+    double joint_pos[EMCMOT_MAX_JOINTS];
+    int joint_num;
+    emcmot_joint_t *joint;
+    int in_range = axisInRange(pos, id, move_type);
 
     /* fill in all joints with 0 */
     for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) {
@@ -271,6 +276,33 @@ STATIC int inRange(EmcPose pos, int id, char *move_type)
     return in_range;
 }
 
+/* Unlike a line, a circular move can leave the axis limits even when both
+   endpoints are inside.  Check the bounds of the same circle as tpAddCircle,
+   starting at the end of the queued moves, not at the current position.
+   These are Cartesian bounds: they are not poses on the path and must not
+   be passed to inverse kinematics.  Joint limits are still checked at the
+   endpoint; arbitrary kinematics can have other extrema along the path. */
+STATIC int circleInRange(EmcPose start, EmcPose end, PmCartesian center,
+                         PmCartesian normal, int turn, int id)
+{
+    PmCircle circle;
+    EmcPose lower = end, upper = end;
+
+    if (!inRange(end, id, "Circular")) {
+        return 0;
+    }
+    if (pmCircleInit(&circle, &start.tran, &end.tran, &center, &normal, turn)
+        || pmCircleBounds(&circle, &lower.tran, &upper.tran)) {
+        reportError(_("Cannot determine circular move bounds on line %d"), id);
+        return 0;
+    }
+
+    /* ABCUVW are interpolated linearly and need only endpoint checks. */
+    int lower_in_range = axisInRange(lower, id, "Circular");
+    int upper_in_range = axisInRange(upper, id, "Circular");
+    return lower_in_range && upper_in_range;
+}
+
 /* legacy note:
    clearHomes() will clear the homed flags for joints that have moved
    since homing, outside coordinated control, for machines with no
@@ -1085,7 +1117,10 @@ void emcmotCommandHandler_locked(void *arg, long servo_period)
 		emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_COMMAND;
 		SET_MOTION_ERROR_FLAG(1);
 		break;
-	    } else if (!inRange(emcmotCommand->pos, emcmotCommand->id, "Circular")) {
+	    } else if (!circleInRange(emcmotInternal->coord_tp.goalPos,
+                                      emcmotCommand->pos, emcmotCommand->center,
+                                      emcmotCommand->normal, emcmotCommand->turn,
+                                      emcmotCommand->id)) {
 		emcmotStatus->commandStatus = EMCMOT_COMMAND_INVALID_PARAMS;
 		tpAbort(&emcmotInternal->coord_tp);
 		SET_MOTION_ERROR_FLAG(1);

src/emc/motion/control.c

@@ -1479,12 +1479,10 @@ static void get_pos_cmds(long period)
 	break;
     }
     /* check command against soft limits */
-    /* This is a backup check, it should be impossible to command
-	a move outside the soft limits.  However there is at least
-	two cases that isn't caught upstream:
-	1) if an arc has both endpoints inside the limits, but the curve extends outside,
-	2) if homing params are wrong then after homing joint pos_cmd are outside,
-	the upstream checks will pass it.
+    /* This is a backup check.  Upstream checks cover Cartesian bounds and
+       endpoint joint positions, but general kinematics can put a joint
+       outside its limits between endpoints.  Incorrect homing parameters
+       or limits changed after queueing a move can also escape those checks.
     */
     for (joint_num = 0; joint_num < ALL_JOINTS; joint_num++) {
 	/* point to joint data */
@@ -2182,4 +2180,4 @@ static void update_status(void)
 	old_motion_flag = emcmotStatus->motionFlag;
     }
 #endif
-}
+}

src/libnml/posemath/_posemath.c

@@ -1958,6 +1958,224 @@ int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * con
     return pmErrno = PM_OK;
 }
 
+/* One coordinate, over at most one revolution.  The local angle avoids losing
+   root-search precision when the circle contains many revolutions. */
+typedef struct {
+    double center, radius, radial_rate, helix, helix_rate, amplitude, phase;
+    double min, max;
+    int valid;
+} PmCircleBound;
+
+static double circleBoundValue(const PmCircleBound *b, double angle)
+{
+    return b->center + b->amplitude * (b->radius + b->radial_rate * angle)
+        * cos(angle - b->phase) + b->helix + b->helix_rate * angle;
+}
+
+static double circleBoundDerivative(PmCircleBound *b, double angle)
+{
+    double s = sin(angle - b->phase), c = cos(angle - b->phase);
+    double radius = b->radius + b->radial_rate * angle;
+    double value = b->amplitude * (b->radial_rate * c - radius * s) + b->helix_rate;
+    if (!isfinite(value))
+        b->valid = 0;
+    return value;
+}
+
+/* The callers isolate at most one root before calling this bounded search. */
+static void circleBoundRoot(PmCircleBound *b, double *lo, double *hi)
+{
+    double left = circleBoundDerivative(b, *lo);
+    int i;
+    for (i = 0; i < 60; i++) {
+        double mid = *lo + (*hi - *lo) * 0.5;
+        double value = circleBoundDerivative(b, mid);
+        if (mid == *lo || mid == *hi)
+            break;
+        if ((value < 0) == (left < 0)) {
+            *lo = mid;
+            left = value;
+        } else {
+            *hi = mid;
+        }
+    }
+}
+
+/* f'' = -amplitude * hypot(R, 2*k) * cos(phase), with R = radius + k*angle.
+   This phase is continuous and strictly increasing for nonnegative R:
+   phase' = 1 + 2*k*k / (R*R + 4*k*k).  Unlike signs of f'' near tan's poles,
+   it can isolate inflections even when k is only radius roundoff. */
+static double circleBoundInflectionPhase(const PmCircleBound *b, double angle)
+{
+    double radius = b->radius + b->radial_rate * angle;
+    return angle - b->phase - atan2(b->radial_rate, radius * 0.5);
+}
+
+static void circleBoundInflectionRoot(const PmCircleBound *b, double phase,
+        double *lo, double *hi)
+{
+    int i;
+    for (i = 0; i < 60; i++) {
+        double mid = *lo + (*hi - *lo) * 0.5;
+        if (mid == *lo || mid == *hi)
+            break;
+        if (circleBoundInflectionPhase(b, mid) < phase)
+            *lo = mid;
+        else
+            *hi = mid;
+    }
+}
+
+static void circleBoundExtend(PmCircleBound *b, double value, double error)
+{
+    if (!isfinite(value) || !isfinite(error))
+        b->valid = 0;
+    b->min = fmin(b->min, value - error);
+    b->max = fmax(b->max, value + error);
+}
+
+static void circleBoundInclude(PmCircleBound *b, double lo, double hi, int stationary)
+{
+    double mid = lo + (hi - lo) * 0.5;
+    double value = circleBoundValue(b, mid);
+    double radius = fmax(fabs(b->radius + b->radial_rate * lo),
+                         fabs(b->radius + b->radial_rate * hi));
+    /* Enclose the remaining root interval, rather than sampling its midpoint.
+       At a stationary point f'=0, so the error is bounded using f'' and the
+       square of the interval width.  Ordinary floating-point rounding is
+       treated like pmCirclePoint, without expanding exact tangent arcs. */
+    double error = stationary
+        ? b->amplitude * (2 * fabs(b->radial_rate) + radius) * (hi - lo) * (hi - lo) / 8
+        : (b->amplitude * (fabs(b->radial_rate) + radius)
+           + fabs(b->helix_rate)) * (hi - lo) * 0.5;
+    circleBoundExtend(b, value, error);
+}
+
+/* On this interval the first derivative is monotone. */
+static void circleBoundMonotone(PmCircleBound *b, double lo, double hi)
+{
+    double left = circleBoundDerivative(b, lo);
+    double right = circleBoundDerivative(b, hi);
+    circleBoundInclude(b, lo, lo, 0);
+    circleBoundInclude(b, hi, hi, 0);
+    if ((left < 0 && right > 0) || (left > 0 && right < 0)) {
+        circleBoundRoot(b, &lo, &hi);
+        circleBoundInclude(b, lo, hi, 1);
+    }
+}
+
+static void circleBoundInterval(PmCircleBound *b, double span)
+{
+    double monotone_start = 0;
+    int i;
+    circleBoundInclude(b, 0, 0, 0);
+    circleBoundInclude(b, span, span, 0);
+    if (b->amplitude == 0)
+        return;
+
+    if (b->radial_rate == 0) {
+        /* Circles and helices have analytic stationary points. */
+        double ratio = b->helix_rate / (b->amplitude * b->radius);
+        if (fabs(ratio) <= 1) {
+            double root = asin(ratio);
+            for (i = 0; i < 2; i++) {
+                double angle = fmod(b->phase + (i ? PM_PI - root : root), PM_2_PI);
+                if (angle < 0)
+                    angle += PM_2_PI;
+                if (angle <= span) {
+                    if (b->helix_rate == 0)
+                        circleBoundExtend(b, b->center + b->helix
+                                + (i ? -1 : 1) * b->amplitude * b->radius, 0);
+                    else
+                        circleBoundInclude(b, angle, angle, 0);
+                }
+            }
+        }
+        return;
+    }
+
+    /* Inflections occur at phase = pi/2 + n*pi.  Over one revolution the
+       atan2 term changes by at most pi/2, so there are at most three roots.
+       Splitting at all of them leaves monotone f' on each interval. */
+    double first_phase = circleBoundInflectionPhase(b, 0);
+    double last_phase = circleBoundInflectionPhase(b, span);
+    double phase = PM_PI / 2
+        + PM_PI * (floor((first_phase - PM_PI / 2) / PM_PI) + 1);
+    for (i = 0; i < 3 && phase <= last_phase; i++, phase += PM_PI) {
+        double root_lo = monotone_start, root_hi = span;
+        circleBoundInflectionRoot(b, phase, &root_lo, &root_hi);
+        circleBoundMonotone(b, monotone_start, root_lo);
+        circleBoundInclude(b, root_lo, root_hi, 0);
+        monotone_start = root_hi;
+    }
+    circleBoundMonotone(b, monotone_start, span);
+}
+
+/* Bound the complete Cartesian path, not only its end or sampled points.
+   At a fixed phase, radius and helix displacement are linear in the turn
+   number.  Thus extrema are in the first or last revolution, even for a
+   many-turn spiral/helix.  This keeps the work bounded in the motion thread. */
+int pmCircleBounds(PmCircle const * const circle,
+        PmCartesian * const min, PmCartesian * const max)
+{
+    double centers[3], tangents[3], perpendiculars[3], helices[3];
+    double lower[3], upper[3];
+    int axis, turn;
+    if (!circle || !min || !max || !isfinite(circle->radius)
+            || !isfinite(circle->angle) || !isfinite(circle->spiral)
+            || circle->radius <= 0 || circle->angle <= 0
+            || !isfinite(circle->radius + circle->spiral)
+            || circle->radius + circle->spiral < 0
+            || !isfinite(circle->normal.x) || !isfinite(circle->normal.y)
+            || !isfinite(circle->normal.z)
+            || (circle->normal.x == 0 && circle->normal.y == 0 && circle->normal.z == 0))
+        return pmErrno = PM_ERR;
+
+    centers[0] = circle->center.x; centers[1] = circle->center.y; centers[2] = circle->center.z;
+    tangents[0] = circle->rTan.x; tangents[1] = circle->rTan.y; tangents[2] = circle->rTan.z;
+    perpendiculars[0] = circle->rPerp.x; perpendiculars[1] = circle->rPerp.y; perpendiculars[2] = circle->rPerp.z;
+    helices[0] = circle->rHelix.x; helices[1] = circle->rHelix.y; helices[2] = circle->rHelix.z;
+    for (axis = 0; axis < 3; axis++) {
+        double u = tangents[axis] / circle->radius;
+        double v = perpendiculars[axis] / circle->radius;
+        PmCircleBound b;
+        b.center = centers[axis];
+        b.radial_rate = circle->spiral / circle->angle;
+        b.helix_rate = helices[axis] / circle->angle;
+        b.amplitude = sqrt(u * u + v * v);
+        b.min = DBL_MAX;
+        b.max = -DBL_MAX;
+        b.valid = 1;
+        if (!isfinite(b.center) || !isfinite(b.radial_rate)
+                || !isfinite(b.helix_rate) || !isfinite(b.amplitude)
+                || !isfinite(helices[axis]))
+            return pmErrno = PM_ERR;
+        for (turn = 0; turn < 2; turn++) {
+            /* Traverse the last revolution backwards from the endpoint.
+               Subtracting 2*pi from a huge total angle would lose precision. */
+            double c = turn ? cos(circle->angle) : 1;
+            double s = turn ? sin(circle->angle) : 0;
+            b.radius = turn ? circle->radius + circle->spiral : circle->radius;
+            b.helix = turn ? helices[axis] : 0;
+            b.phase = atan2(turn ? u * s - v * c : v, u * c + v * s);
+            if (turn) {
+                b.radial_rate = -b.radial_rate;
+                b.helix_rate = -b.helix_rate;
+            }
+            circleBoundInterval(&b, fmin(PM_2_PI, circle->angle));
+            if (circle->angle <= PM_2_PI)
+                break;
+        }
+        if (!b.valid || !isfinite(b.min) || !isfinite(b.max) || b.min > b.max)
+            return pmErrno = PM_ERR;
+        lower[axis] = b.min;
+        upper[axis] = b.max;
+    }
+    min->x = lower[0]; min->y = lower[1]; min->z = lower[2];
+    max->x = upper[0]; max->y = upper[1]; max->z = upper[2];
+    return pmErrno = PM_OK;
+}
+
 int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end)
 {
     if (!circ || new_angle <= DOUBLE_FUZZ) {

src/libnml/posemath/posemath.h

@@ -949,6 +949,10 @@ extern "C" {
             PmCartesian const * const center, PmCartesian const * const normal, int turn);
 
     extern int pmCirclePoint(PmCircle const * const circle, double angle, PmCartesian * const point);
+    /* Cartesian bounds of an initialized circle, including spiral and helix.
+       These are not joint-space bounds for nonidentity kinematics. */
+    extern int pmCircleBounds(PmCircle const * const circle,
+            PmCartesian * const min, PmCartesian * const max);
     extern int pmCircleStretch(PmCircle * const circ, double new_angle, int from_end);
 
 /* slicky macros for item-by-item copying between C and C++ structs */

tests/arc-soft-limits/arc-soft-limits.hal

@@ -0,0 +1,9 @@
+loadrt [KINS]KINEMATICS
+loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS
+addf motion-command-handler servo-thread
+addf motion-controller servo-thread
+
+net xpos joint.0.motor-pos-cmd => joint.0.motor-pos-fb
+net ypos joint.1.motor-pos-cmd => joint.1.motor-pos-fb
+net zpos joint.2.motor-pos-cmd => joint.2.motor-pos-fb
+net estop iocontrol.0.user-enable-out => iocontrol.0.emc-enable-in