motion: keep the last pose as the forward's guess after a failed forward

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;
 

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.

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"

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

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)

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

tests/kins-forward-seed/test.sh

@@ -0,0 +1,3 @@
+#!/bin/bash -e
+rm -f sim.var sim.var.bak
+linuxcnc -r test.ini

tests/kins-forward-seed/tool.tbl

@@ -0,0 +1 @@
+T1 P1 D0.0 Z12.5 ;