diff --git a/src/Makefile b/src/Makefile index 007475d27b9..d152a224446 100644 --- a/src/Makefile +++ b/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 diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index f6f92c9f8f4..b13c33e77b7 100644 --- a/src/emc/motion/command.c +++ b/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, ¢er, &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); diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index b6e45d590c7..a9ac934f54a 100644 --- a/src/emc/motion/control.c +++ b/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 -} \ No newline at end of file +} diff --git a/src/libnml/posemath/_posemath.c b/src/libnml/posemath/_posemath.c index 4a9d7ebfffa..8cb63722b35 100644 --- a/src/libnml/posemath/_posemath.c +++ b/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) { diff --git a/src/libnml/posemath/posemath.h b/src/libnml/posemath/posemath.h index 445eb1f5bfc..1a15018fe9d 100644 --- a/src/libnml/posemath/posemath.h +++ b/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 */ diff --git a/tests/arc-soft-limits/arc-soft-limits.hal b/tests/arc-soft-limits/arc-soft-limits.hal new file mode 100644 index 00000000000..495bac8a1d0 --- /dev/null +++ b/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 diff --git a/tests/arc-soft-limits/arc-soft-limits.ini b/tests/arc-soft-limits/arc-soft-limits.ini new file mode 100644 index 00000000000..71710cf1854 --- /dev/null +++ b/tests/arc-soft-limits/arc-soft-limits.ini @@ -0,0 +1,92 @@ +[EMC] +VERSION = 1.1 +MACHINE = arc-soft-limits +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[HAL] +HALFILE = arc-soft-limits.hal + +[TRAJ] +COORDINATES = X Y Z +LINEAR_UNITS = mm +ANGULAR_UNITS = degree +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 50 + +[EMCIO] +EMCIO = io +CYCLE_TIME = 0.100 +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = trivkins +JOINTS = 3 + +[AXIS_X] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 + +[AXIS_Y] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 + +[AXIS_Z] +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 +FERROR = 1 +MIN_FERROR = 1 +HOME = 0 +HOME_OFFSET = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 +FERROR = 1 +MIN_FERROR = 1 +HOME = 0 +HOME_OFFSET = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -10 +MAX_LIMIT = 10 +MAX_VELOCITY = 50 +MAX_ACCELERATION = 200 +FERROR = 1 +MIN_FERROR = 1 +HOME = 0 +HOME_OFFSET = 0 +HOME_SEQUENCE = 0 diff --git a/tests/arc-soft-limits/checkresult b/tests/arc-soft-limits/checkresult new file mode 100755 index 00000000000..ae714affbcc --- /dev/null +++ b/tests/arc-soft-limits/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# Assertions in test-ui.py determine success. +exit 0 diff --git a/tests/arc-soft-limits/queued-invalid.ngc b/tests/arc-soft-limits/queued-invalid.ngc new file mode 100644 index 00000000000..e749666901e --- /dev/null +++ b/tests/arc-soft-limits/queued-invalid.ngc @@ -0,0 +1,4 @@ +G21 G90 G17 G61 G94 F2000 +G1 X9 Y0 Z0 +G2 I-10 +M2 diff --git a/tests/arc-soft-limits/queued-valid.ngc b/tests/arc-soft-limits/queued-valid.ngc new file mode 100644 index 00000000000..4dfb09888df --- /dev/null +++ b/tests/arc-soft-limits/queued-valid.ngc @@ -0,0 +1,4 @@ +G21 G90 G17 G61 G94 F2000 +G1 X-8 Y0 Z0 +G2 I8 +M2 diff --git a/tests/arc-soft-limits/test-ui.py b/tests/arc-soft-limits/test-ui.py new file mode 100755 index 00000000000..97b79498657 --- /dev/null +++ b/tests/arc-soft-limits/test-ui.py @@ -0,0 +1,189 @@ +#!/usr/bin/env python3 +"""Reject circular moves that exceed soft limits before motion, including in MDI (issue #3839).""" + +import math +import time + +import linuxcnc +import linuxcnc_util + + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() +machine = linuxcnc_util.LinuxCNC(command=c, status=s, error=e) + + +def errors(): + messages = [] + while True: + error = e.poll() + if error is None: + return messages + messages.append(error[1]) + + +def drain_after_abort(): + # Motion errors are forwarded one per task cycle. Wait for the trailing + # diagnostics before starting another case, whose error channel must be empty. + quiet_until = time.monotonic() + 0.1 + deadline = time.monotonic() + 2 + while time.monotonic() < deadline: + if errors(): + quiet_until = time.monotonic() + 0.1 + elif time.monotonic() >= quiet_until: + return + time.sleep(0.01) + raise AssertionError("error channel did not settle after abort") + + +def mdi(command): + c.mdi(command) + result = c.wait_complete(10) + messages = errors() + assert result == linuxcnc.RCS_DONE, (command, result, messages) + assert not messages, (command, messages) + + +def rejected(command, axis, direction): + expected = "would exceed {}'s {} limit".format(axis, direction) + s.poll() + start = s.position[:3] + assert not errors() + c.mdi(command) + result = c.wait_complete(5) + # Error-channel delivery and the status channel are asynchronous. + messages = [] + deadline = time.monotonic() + 1 + while time.monotonic() < deadline: + messages.extend(errors()) + if any("Circular move" in message and expected in message + for message in messages): + break + time.sleep(0.01) + s.poll() + position = s.position[:3] + diagnostic = (command, result, start, position, messages) + assert all(abs(a - b) < 1e-8 for a, b in zip(start, position)), diagnostic + assert any("Circular move" in message and expected in message + for message in messages), diagnostic + print("Rejected before motion: " + command, flush=True) + c.abort() + c.wait_complete(5) + drain_after_abort() + + +def origin(): + mdi("G17 G90 G10 L2 P1 X0 Y0 Z0 R0") + mdi("G54 G0 X0 Y0 Z0") + + +def queued_program(filename, reject=False): + # Hold the machine at the origin while both commands reach motion. + # The circle must be checked from the queued line's endpoint. + origin() + c.feedrate(0) + c.mode(linuxcnc.MODE_AUTO) + c.wait_complete(5) + c.program_open(filename) + c.auto(linuxcnc.AUTO_RUN, 0) + deadline = time.monotonic() + 5 + messages = [] + expected = "would exceed X's negative limit" + while time.monotonic() < deadline: + messages.extend(errors()) + s.poll() + if reject and any("Circular move" in message and expected in message + for message in messages): + break + if not reject and (messages or s.queue >= 2): + break + time.sleep(0.01) + assert all(abs(p) < 1e-8 for p in s.position[:3]), s.position + if reject: + assert any("Circular move" in message and expected in message + for message in messages), (filename, s.queue, messages) + c.abort() + c.wait_complete(5) + drain_after_abort() + else: + assert not messages, (filename, messages) + assert s.queue >= 2, (filename, s.queue) + c.feedrate(1) + deadline = time.monotonic() + 10 + while time.monotonic() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and s.inpos: + break + time.sleep(0.01) + assert s.interp_state == linuxcnc.INTERP_IDLE and s.inpos, filename + if not reject: + assert abs(s.position[0] + 8) < 1e-7, s.position + assert not errors() + c.mode(linuxcnc.MODE_MDI) + c.wait_complete(5) + errors() + print("Queued start checked: " + filename, flush=True) + + +machine.wait_for_linuxcnc_startup() +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(5) +c.home(-1) +machine.wait_for_home([1, 1, 1, 0, 0, 0, 0, 0, 0]) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(5) +mdi("G21 G90 G17 G40 G49 G54 G61 G94 F2000") + +# The reported reproduction: the end point is the start point. +rejected("G2 I1000 F2000", "X", "positive") + +# Both directions, all planes, positive and negative limits. All endpoints +# are inside the limits and would pass the old endpoint-only check. +for command, axis, direction in [ + ("G17 G2 I6", "X", "positive"), + ("G17 G3 I-6", "X", "negative"), + ("G17 G2 J6", "Y", "positive"), + ("G17 G3 J-6", "Y", "negative"), + ("G18 G2 K6", "Z", "positive"), + ("G18 G3 K-6", "Z", "negative"), + ("G19 G2 J6", "Y", "positive"), + ("G19 G3 K-6", "Z", "negative"), + ("G17 G2 I6 P3 Z2", "X", "positive")]: + rejected(command, axis, direction) + +origin() +# Tangency is allowed. Also check a multi-turn helix that stays in bounds. +mdi("G2 I5") +mdi("G3 I5") +mdi("G2 I4 P3 Z2") +origin() +# A short part of an enormous circle is legal; checking the entire circle's +# bounding box would incorrectly reject it. +mdi("G2 X{:.12f} Y5 I1000".format(1000 - math.sqrt(1000**2 - 5**2))) + +# Partial arcs with legal endpoints but an interior extremum out of range. +mdi("G0 X9 Y-2") +rejected("G3 X9 Y2 I-1 J2", "X", "positive") +mdi("G0 X-9 Y-2") +rejected("G2 X-9 Y2 I1 J2", "X", "negative") + +# A small radius mismatch is allowed by the interpreter, producing a spiral. +origin() +rejected("G2 X-0.01 Y0 I5", "X", "positive") +mdi("G2 X0.01 Y0 I5") + +# G10 rotation tilts the G18 plane in machine coordinates. The second arc +# is a helix whose maximum Y is between the usual circle quadrant angles. +origin() +mdi("G10 L2 P1 R45") +mdi("G18 G2 I6") +rejected("G18 G2 I6 Y6", "Y", "positive") +mdi("G18 G2 I4 Y2") + +queued_program("queued-valid.ngc") +queued_program("queued-invalid.ngc", reject=True) + +print("Arc soft-limit checks passed", flush=True) diff --git a/tests/arc-soft-limits/test.sh b/tests/arc-soft-limits/test.sh new file mode 100755 index 00000000000..bc44094eb7b --- /dev/null +++ b/tests/arc-soft-limits/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -e +rm -f sim.var sim.var.bak +touch sim.var +linuxcnc -r arc-soft-limits.ini diff --git a/tests/arc-soft-limits/tool.tbl b/tests/arc-soft-limits/tool.tbl new file mode 100644 index 00000000000..eb319a21d01 --- /dev/null +++ b/tests/arc-soft-limits/tool.tbl @@ -0,0 +1 @@ +; No tools are needed for the arc soft-limit test. diff --git a/tests/posemath/arc-bounds/expected b/tests/posemath/arc-bounds/expected new file mode 100644 index 00000000000..46d390590a1 --- /dev/null +++ b/tests/posemath/arc-bounds/expected @@ -0,0 +1 @@ +arc bounds passed diff --git a/tests/posemath/arc-bounds/test.c b/tests/posemath/arc-bounds/test.c new file mode 100644 index 00000000000..06e3eaa40e9 --- /dev/null +++ b/tests/posemath/arc-bounds/test.c @@ -0,0 +1,227 @@ +#include +#include +#include +#include +#include +#include "posemath.h" + +static void require(int condition, const char *message) +{ + if (!condition) { + fprintf(stderr, "%s\n", message); + exit(1); + } +} + +static void near(double value, double expected, const char *message) +{ + if (!isfinite(value) || fabs(value - expected) > 1e-11) { + fprintf(stderr, "%s: %.17g != %.17g\n", message, value, expected); + exit(1); + } +} + +static PmCircle make_circle(PmCartesian start, PmCartesian end, + PmCartesian center, PmCartesian normal, int turn) +{ + PmCircle circle; + require(pmCircleInit(&circle, &start, &end, ¢er, &normal, turn) == PM_OK, + "circle initialization failed"); + return circle; +} + +static double coordinate(PmCartesian p, int axis) +{ + return axis == 0 ? p.x : axis == 1 ? p.y : p.z; +} + +/* An independent containment check against the path evaluator, also checking + that the bounds are tight to the resolution of the reference points. */ +static void check_points(PmCircle circle) +{ + PmCartesian min, max, point; + double seen_min[3] = {DBL_MAX, DBL_MAX, DBL_MAX}; + double seen_max[3] = {-DBL_MAX, -DBL_MAX, -DBL_MAX}; + int i, axis; + const int count = 4000; + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "bounds failed"); + for (i = 0; i <= count; i++) { + require(pmCirclePoint(&circle, circle.angle * i / count, &point) == PM_OK, + "point failed"); + for (axis = 0; axis < 3; axis++) { + double value = coordinate(point, axis); + double tolerance = 1e-10 * (1 + fabs(value)); + if (value < coordinate(min, axis) - tolerance + || value > coordinate(max, axis) + tolerance) { + fprintf(stderr, "point %d axis %d: %.17g outside [%.17g, %.17g], " + "angle %.17g spiral %.17g\n", i, axis, value, + coordinate(min, axis), coordinate(max, axis), + circle.angle, circle.spiral); + exit(1); + } + seen_min[axis] = fmin(seen_min[axis], value); + seen_max[axis] = fmax(seen_max[axis], value); + } + } + for (axis = 0; axis < 3; axis++) { + double speed = circle.radius + fabs(circle.spiral) + + (fabs(circle.spiral) + fabs(coordinate(circle.rHelix, axis))) / circle.angle; + double tolerance = speed * circle.angle / count + 1e-9; + require(seen_min[axis] - coordinate(min, axis) <= tolerance, + "lower bound is unnecessarily wide"); + require(coordinate(max, axis) - seen_max[axis] <= tolerance, + "upper bound is unnecessarily wide"); + } +} + +static unsigned random_state = 3839; +static double random_unit(void) +{ + random_state = random_state * 1664525U + 1013904223U; + return (random_state >> 8) / 16777216.0; +} + +static void check_tiny_spirals(PmCircle circle) +{ + /* Do not depend on the platform's rounding of the endpoint radii. */ + double ulp = nextafter(circle.radius, INFINITY) - circle.radius; + for (int sign = -1; sign <= 1; sign += 2) { + circle.spiral = sign * ulp; + check_points(circle); + } +} + +int main(void) +{ + PmCartesian zero = {0, 0, 0}, z = {0, 0, 1}; + PmCartesian start = {1, 0, 0}, end = {0, 1, 0}, min, max; + PmCircle circle = make_circle(start, end, zero, z, 0); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "quarter circle failed"); + near(min.x, 0, "quarter minimum X"); + near(min.y, 0, "quarter minimum Y"); + near(max.x, 1, "quarter maximum X"); + near(max.y, 1, "quarter maximum Y"); + check_points(circle); + + /* Exact tangency remains legal with the motion layer's 1e-12 epsilon, + even when coordinates are much larger than that absolute tolerance. */ + for (int i = 0; i < 2; i++) { + double radius = i ? 1e9 : 1000; + PmCartesian center = {radius, 0, 0}; + PmCartesian tangent = {2 * radius, 0, 0}; + circle = make_circle(tangent, tangent, center, z, 0); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "tangent circle failed"); + require(min.x >= -1e-12 && max.x <= 2 * radius + 1e-12, + "legal tangent circle was expanded past its limit"); + near(min.y, -radius, "tangent minimum Y"); + near(max.y, radius, "tangent maximum Y"); + } + + /* Clockwise takes the other three quarters and crosses negative limits. */ + circle = make_circle(start, end, zero, z, -1); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "clockwise bounds failed"); + near(min.x, -1, "clockwise minimum X"); + near(min.y, -1, "clockwise minimum Y"); + check_points(circle); + + /* G0 X-36.2949 Y44.4302; G2 X-96.1034 Y10.8100 R-109.857. + Use the interpreter's R-word center calculation. Equal intended radii + can leave a tiny spiral which must not hide whole interior extrema. */ + { + volatile double input[] = {-36.2949, 44.4302, -96.1034, 10.8100, -109.857}; + PmCartesian r_start = {input[0], input[1], 0}; + PmCartesian r_end = {input[2], input[3], 0}; + double radius = fabs(input[4]); + double mid_x = (r_start.x + r_end.x) / 2; + double mid_y = (r_start.y + r_end.y) / 2; + double half_length = hypot(mid_x - r_end.x, mid_y - r_end.y); + double theta = atan2(r_end.y - r_start.y, r_end.x - r_start.x) + + 1.570796326794896619231321691639751442L; + double offset = radius * cos(asin(half_length / radius)); + PmCartesian center = {mid_x + offset * cos(theta), mid_y + offset * sin(theta), 0}; + circle = make_circle(r_start, r_end, center, z, -1); + check_points(circle); + check_tiny_spirals(circle); + } + + /* A narrow arc must not inherit the bounds of its entire circle. */ + start = (PmCartesian){cos(0.2), sin(0.2), 0}; + end = (PmCartesian){cos(0.3), sin(0.3), 0}; + circle = make_circle(start, end, zero, z, 0); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "short arc failed"); + near(min.x, end.x, "short arc minimum X"); + near(max.x, start.x, "short arc maximum X"); + + /* The spiral's X maximum lies between the cardinal directions: at pi/4. */ + start = (PmCartesian){1 - PM_PI / 4, 0, 0}; + end = (PmCartesian){0, 1 + PM_PI / 4, 0}; + circle = make_circle(start, end, zero, z, 0); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "spiral bounds failed"); + near(max.x, sqrt(0.5), "spiral interior maximum"); + check_points(circle); + circle = make_circle(end, start, zero, z, -1); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "inward spiral failed"); + near(max.x, sqrt(0.5), "inward spiral interior maximum"); + check_points(circle); + + /* An inward spiral may end at its center. With extra turns, the reversed + last revolution used for bounds starts at exactly zero radius. */ + circle.spiral = -circle.radius; + check_points(circle); + circle.angle += 2 * PM_2_PI; + check_points(circle); + + /* A tilted helix has extrema shifted from the planar cardinal angles. */ + PmCartesian normal = {0, sqrt(0.5), sqrt(0.5)}; + start = (PmCartesian){1, 0, 0}; + end = (PmCartesian){1, PM_PI * sqrt(0.5) / 5, PM_PI * sqrt(0.5) / 5}; + circle = make_circle(start, end, zero, normal, 0); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "tilted helix failed"); + double theta = acos(-0.1); + near(max.y, (sin(theta) + theta / 10) * sqrt(0.5), "tilted helix maximum Y"); + check_points(circle); + check_tiny_spirals(circle); + + /* Work must not scale with the number of turns. */ + circle = make_circle(start, start, zero, z, INT_MAX); + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "many-turn circle failed"); + near(min.x, -1, "many-turn minimum X"); + near(max.y, 1, "many-turn maximum Y"); + circle.spiral = 1; + circle.rHelix.z = 10; + require(pmCircleBounds(&circle, &min, &max) == PM_OK, "many-turn spiral failed"); + require(min.x < -1.99999999 && max.x > 1.99999999, + "many-turn spiral missed the last turn"); + near(min.z, 0, "many-turn helix minimum Z"); + near(max.z, 10, "many-turn helix maximum Z"); + + /* Reproducible varied planes, directions, helices and inward/outward + spirals exercise the general geometry independently of machine setup. */ + for (int i = 0; i < 200; i++) { + PmCartesian center = {random_unit() * 10, random_unit() * 10, random_unit() * 10}; + normal = (PmCartesian){random_unit() - 0.5, random_unit() - 0.5, random_unit() - 0.5}; + start = (PmCartesian){center.x + 1 + random_unit() * 10, + center.y + random_unit() * 10, center.z + random_unit() * 10}; + end = (PmCartesian){center.x + random_unit() * 20 - 10, + center.y + random_unit() * 20 - 10, center.z + random_unit() * 20 - 10}; + circle = make_circle(start, end, center, normal, i % 9 - 4); + check_points(circle); + } + + require(pmCircleBounds(NULL, &min, &max) != PM_OK, "null circle accepted"); + circle.angle = 0; + require(pmCircleBounds(&circle, &min, &max) != PM_OK, "zero angle accepted"); + circle.angle = NAN; + require(pmCircleBounds(&circle, &min, &max) != PM_OK, "NaN angle accepted"); + circle.angle = 1; + circle.radius = 0; + require(pmCircleBounds(&circle, &min, &max) != PM_OK, "zero radius accepted"); + circle.radius = 1; + circle.spiral = -2; + require(pmCircleBounds(&circle, &min, &max) != PM_OK, "negative end radius accepted"); + circle.spiral = 0; + circle.center.x = INFINITY; + require(pmCircleBounds(&circle, &min, &max) != PM_OK, "infinite center accepted"); + puts("arc bounds passed"); + return 0; +} diff --git a/tests/posemath/arc-bounds/test.sh b/tests/posemath/arc-bounds/test.sh new file mode 100755 index 00000000000..630c79d9606 --- /dev/null +++ b/tests/posemath/arc-bounds/test.sh @@ -0,0 +1,6 @@ +#!/bin/sh +set -eu +trap 'rm -f test' EXIT +gcc -O2 -Wall -Wextra -DULAPI -I"$HEADERS" test.c \ + -L"$LIBDIR" -Wl,-rpath,"$LIBDIR" -lposemath -lm -o test +./test