From b6c00868262dc5eff54d2a926445be8db58a06b9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sun, 1 Nov 2026 14:26:42 +1100 Subject: [PATCH] motion: keep the last pose as the forward's guess after a failed forward With KINEMATICS_BOTH, motion seeds the forward of the feedback, and in joint mode of the command, with the pose of the cycle before; after one failed forward it seeded from world_home instead, every cycle until the forward solved again. For an iterative forward far from home that seed can fail again or converge to another pose with the same joints, and since that pose is a solution, the following cycles stay on it: the position reads wrong until the machine comes back near home. Home is now the guess only until the forward has solved once since all joints were homed; a failed forward puts back the pose it was given, which is the next guess. In the hexapod-sim geometry, over 23141 platform poses whose straight path from home the forward follows cycle by cycle, the forward started at home fails at 675 and lands on another pose at 319, all near the edges (X and Y at 15, tilts to 45 degrees, turns to 90); in the middle of the workspace it finds them all. tests/kins-forward-seed moves the platform to one of those, makes the forward fail for a few cycles with genhexkins.limit-iterations 0, and requires the feedback and the joint mode command to come back to the platform; without the change both stay on X-14.69 Y-14.81 Z31.67 A28.6 B18.2 C97.1. --- src/emc/motion/control.c | 34 +++++-- tests/kins-forward-seed/README | 8 ++ tests/kins-forward-seed/checkresult | 3 + tests/kins-forward-seed/sim.hal | 16 ++++ tests/kins-forward-seed/test-ui.py | 89 +++++++++++++++++ tests/kins-forward-seed/test.ini | 144 ++++++++++++++++++++++++++++ tests/kins-forward-seed/test.sh | 3 + tests/kins-forward-seed/tool.tbl | 1 + 8 files changed, 290 insertions(+), 8 deletions(-) create mode 100644 tests/kins-forward-seed/README create mode 100755 tests/kins-forward-seed/checkresult create mode 100644 tests/kins-forward-seed/sim.hal create mode 100755 tests/kins-forward-seed/test-ui.py create mode 100644 tests/kins-forward-seed/test.ini create mode 100755 tests/kins-forward-seed/test.sh create mode 100644 tests/kins-forward-seed/tool.tbl diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 7c9a35c237b..5b423759945 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -688,6 +688,9 @@ static void do_forward_kins(void) double joint_pos[EMCMOT_MAX_JOINTS] = {0,}; int joint_num, result; emcmot_joint_t *joint; + /* the forward has solved since all joints were last homed */ + static bool fb_solved = 0; + EmcPose seed; /* copy joint position feedback to local array */ for (joint_num = 0; joint_num < NO_OF_KINS_JOINTS; joint_num++) { @@ -710,25 +713,32 @@ static void do_forward_kins(void) case KINEMATICS_BOTH: if (get_allhomed()) { - /* is previous value suitable for use as initial guess? */ - if (!emcmotStatus->carte_pos_fb_ok) { - /* no, use home position as initial guess */ + /* home is the initial guess only until the forward has solved + once: after a failure, home can be far from the joints, and + an iterative forward started there fails again or converges + to another pose with the same joints, and then stays on it */ + if (!emcmotStatus->carte_pos_fb_ok && !fb_solved) { emcmotStatus->carte_pos_fb = emcmotStatus->world_home; } + seed = emcmotStatus->carte_pos_fb; /* calculate Cartesean position feedback from joint pos fb */ result = kinematicsForward(joint_pos, &emcmotStatus->carte_pos_fb, &fflags, &iflags); /* check to make sure kinematics converged */ if (result < 0) { - /* error during kinematics calculations */ + /* error during kinematics calculations; keep the last + pose as the next guess */ + emcmotStatus->carte_pos_fb = seed; emcmotStatus->carte_pos_fb_ok = 0; } else { /* it worked! */ emcmotStatus->carte_pos_fb_ok = 1; + fb_solved = 1; } } else { emcmotStatus->carte_pos_fb_ok = 0; + fb_solved = 0; } break; @@ -1258,6 +1268,9 @@ static void get_pos_cmds(long period) emcmot_joint_t *joint; double positions[EMCMOT_MAX_JOINTS]; double vel_lim; + /* the forward has solved since all joints were last homed */ + static bool cmd_solved = 0; + EmcPose seed; /* used in teleop mode to compute the max accell requested */ int onlimit = 0; @@ -1377,24 +1390,29 @@ static void get_pos_cmds(long period) case KINEMATICS_BOTH: if (get_allhomed()) { - /* is previous value suitable for use as initial guess? */ - if (!emcmotStatus->carte_pos_cmd_ok) { - /* no, use home position as initial guess */ + /* home is the initial guess only until the forward has + solved once, as for the feedback */ + if (!emcmotStatus->carte_pos_cmd_ok && !cmd_solved) { emcmotStatus->carte_pos_cmd = emcmotStatus->world_home; } + seed = emcmotStatus->carte_pos_cmd; /* calculate Cartesean position command from joint coarse pos cmd */ result = kinematicsForward(positions, &emcmotStatus->carte_pos_cmd, &fflags, &iflags); /* check to make sure kinematics converged */ if (result < 0) { - /* error during kinematics calculations */ + /* error during kinematics calculations; keep the last + pose as the next guess */ + emcmotStatus->carte_pos_cmd = seed; emcmotStatus->carte_pos_cmd_ok = 0; } else { /* it worked! */ emcmotStatus->carte_pos_cmd_ok = 1; + cmd_solved = 1; } } else { emcmotStatus->carte_pos_cmd_ok = 0; + cmd_solved = 0; } break; diff --git a/tests/kins-forward-seed/README b/tests/kins-forward-seed/README new file mode 100644 index 00000000000..cc5588b3b76 --- /dev/null +++ b/tests/kins-forward-seed/README @@ -0,0 +1,8 @@ +When the forward kinematics of a hexapod fails, motion must start the next +forward from the last pose it had, not from home. An iterative forward +started at home far from the joints can converge to another platform pose +with the same strut lengths, and stays on it from then on. + +The platform is moved far from home, and genhexkins.limit-iterations is +set to 0 for a few cycles so the forward fails; then the feedback position, +and in joint mode the commanded position, must be where the platform is. diff --git a/tests/kins-forward-seed/checkresult b/tests/kins-forward-seed/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-forward-seed/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-forward-seed/sim.hal b/tests/kins-forward-seed/sim.hal new file mode 100644 index 00000000000..e92c60eb526 --- /dev/null +++ b/tests/kins-forward-seed/sim.hal @@ -0,0 +1,16 @@ +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 J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb +net J5 joint.5.motor-pos-cmd => joint.5.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-forward-seed/test-ui.py b/tests/kins-forward-seed/test-ui.py new file mode 100755 index 00000000000..ac1c4105856 --- /dev/null +++ b/tests/kins-forward-seed/test-ui.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# One failed forward far from home. Motion must find the platform where +# it is on the next cycle, not on another pose with the same struts. +# Every step waits for what it needs, not for a time: tests run in +# parallel and under load. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def wait_for(what, done, timeout=30): + deadline = time.time() + timeout + while time.time() < deadline: + s.poll() + if done(): + return True + time.sleep(0.05) + error("timed out waiting for %s" % what) + return False + +def fresh(): + # a status motion wrote after everything seen so far: the heartbeat + # counts servo cycles, and a new value was copied after this call + s.poll() + beat = s.heartbeat + wait_for("a new motion status", lambda: s.heartbeat != beat) + +def off(pose, want): + return max(abs(a - b) for a, b in zip(pose[:6], want)) + +def glitch(): + # no iteration allowed: the forward fails, then solves again from + # whatever guess motion gives it + hal.set_p("genhexkins.limit-iterations", "0") + wait_for("the forward to fail", lambda: hal.get_value("genhexkins.fwd-kins-fail")) + hal.set_p("genhexkins.limit-iterations", "120") + wait_for("the forward to solve again", lambda: not hal.get_value("genhexkins.fwd-kins-fail")) + fresh() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +wait_for("machine on", lambda: s.task_state == linuxcnc.STATE_ON) +c.home(-1) +wait_for("all joints homed", lambda: all(s.homed[:6])) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +wait_for("MDI mode", lambda: s.task_mode == linuxcnc.MODE_MDI) + +# far from home (0 0 20 0 0 0), yet on the way the forward follows the +# platform cycle by cycle; started from home, it converges to a platform +# pose with the same six struts near X-14.69 Y-14.81 Z31.67 A28.6 B18.2 C97.1 +target = (-15, -15, 32, 30, 15, 90) +c.mdi("G0 X-15 Y-15 Z32 A30 B15 C90") +c.wait_complete(30) +wait_for("the move to end", lambda: s.interp_state == linuxcnc.INTERP_IDLE + and not s.queue and s.inpos) +fresh() +if off(s.actual_position, target) > 1e-6: + error("feedback position %s before the failure, expected %s" % (s.actual_position[:6], target)) + +glitch() +if off(s.actual_position, target) > 1e-6: + error("feedback position %s after a failed forward, expected %s" % (s.actual_position[:6], target)) + +# in joint mode motion also runs the forward on the commanded joints +c.mode(linuxcnc.MODE_MANUAL) +c.wait_complete(30) +c.teleop_enable(0) +c.wait_complete(30) +wait_for("joint mode", lambda: s.motion_mode == linuxcnc.TRAJ_MODE_FREE) +glitch() +if off(s.position, target) > 1e-6: + error("commanded position %s in joint mode after a failed forward, expected %s" + % (s.position[:6], target)) + +print("Exiting with %d errors" % errors) +c.state(linuxcnc.STATE_ESTOP) +sys.exit(1 if errors else 0) diff --git a/tests/kins-forward-seed/test.ini b/tests/kins-forward-seed/test.ini new file mode 100644 index 00000000000..843ea379b90 --- /dev/null +++ b/tests/kins-forward-seed/test.ini @@ -0,0 +1,144 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +RS274NGC_STARTUP_CODE = G17 G21 G40 G49 G54 G64 P0.001 G80 G90 G92.1 G94 G97 G98 +PARAMETER_FILE = sim.var + +[KINS] +# the hexapod-sim geometry, the module's defaults; switchkins-type 0 is +# the hexapod +KINEMATICS = genhexkins +JOINTS = 6 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZABC +HOME = 0 0 20 0 0 0 +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 200 +MAX_LINEAR_VELOCITY = 346 +MAX_LINEAR_ACCELERATION = 800 +DEFAULT_LINEAR_ACCELERATION = 800 +MAX_ANGULAR_VELOCITY = 360 + +[EMCMOT] +EMCMOT = motmod +SERVO_PERIOD = 1000000 +COMM_TIMEOUT = 4 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.010 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[AXIS_X] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Y] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_Z] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 + +[AXIS_A] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_B] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[AXIS_C] +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 60 +MAX_ACCELERATION = 200 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_5] +TYPE = LINEAR +MIN_LIMIT = 0 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_OFFSET = 29.7466 +HOME = 29.7466 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-forward-seed/test.sh b/tests/kins-forward-seed/test.sh new file mode 100755 index 00000000000..079fb121f31 --- /dev/null +++ b/tests/kins-forward-seed/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-forward-seed/tool.tbl b/tests/kins-forward-seed/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-forward-seed/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ;