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 ;