RFC: feed the trajectory planner real kinematic velocity and acceleration limits

[grandixximo]

RFC, not a merge request. This is the kinematics half of a larger trajectory planner branch, split out and rebased onto current master so it can be judged on its own. It builds, and it ships a diagnostic you can run against a loaded kinematics module to see what it computes. None of it is wired into motion yet, deliberately.

There is a second PR, #4, which does the same job with a much smaller change to the kinematics modules. The two are alternatives, not a stack. Read this one first for the problem statement, then that one for the comparison.

What I want an opinion on

Not the arithmetic, which is standard, and not where it plugs into motion, which I can move. What I want judged is the shape of the obligation this puts on a kinematics module, because that is the part everyone else has to live with, and I have not convinced myself I found the smallest version of it.

Why a module has to do anything at all

The planner is told how fast a move may go by emccanon, which projects the requested feed onto the per-axis limits from the INI file:

tmax = max over axes of (distance[axis] / vel_limit[axis])
v    = distance / tmax

That is correct for a machine whose axes are its joints, and a guess for any machine where they are not. On 5axiskins with a 250 mm pivot, one degree per second of B is 4.36 mm/s at the X joint. The planner sees a rotary move inside the B axis limit and commands it; the X joint is asked for whatever falls out of the kinematics and either follows it or f-errors. The usual workaround is to detune every axis limit in the INI until the worst pose in the worst program stops complaining, which costs speed everywhere else for the sake of one pose.

To do better, whatever computes the limits has to evaluate the kinematics at poses the machine has not reached yet. Kinematics lives in an RT module behind HAL pins. That is the whole difficulty; the mathematics is a page.

The shape in this series, stated as rules

A module that wants to participate must:

1. include kinematics_params.h and add its geometry to the union there;
2. register <module>.params with hal_struct_newf() during setup and fill in the joint and axis mapping;
3. refresh that struct from its HAL pins inside forward and inverse, with a head/tail split-read so a reader never sees a torn copy;
4. carry an if (!haldata) branch in forward and inverse that reads geometry from the snapshot instead of from pins, for the case where the module was dlopened into a userspace process rather than loaded as an RT component;
5. factor its math into functions taking that geometry as an argument, so that branch has something to call;
6. export nonrt_attach(), which maps the struct and returns the module's own forward and inverse pointers.

Commit 1 adds the HAL named struct namespace that rule 2 needs. Commit 2 applies rules 1 to 6 to trivkins and 5axiskins, the two ends of the range: an identity module, where a flag in the struct is enough and userspace needs no module code at all, and a coupled one, where a rotary joint moves the tool through the pivot length and the geometry has to come along. Commit 3 is the loader. Commit 4 is the calculation and the diagnostic.

Six rules is more than I want. Rule 1 is the one I like least, because it makes a shared header grow once per module and leaves out-of-tree modules with nowhere to put their parameters. Rule 5 is the one that produces the diff: on the full branch it is 749 lines in genhexkins and 565 in pentakins, all of it mechanical, all of it a chance to introduce a typo into working kinematics. Rules 3 and 4 put non-RT plumbing inside functions that run in the servo loop.

What else I tried

Ask RT to do it. Userspace writes a pose into a mailbox, RT computes inverse next cycle and writes joints back. No module changes whatsoever. A Jacobian by central differences is 18 inverse evaluations, times a dozen samples per segment, at one per servo cycle: seconds per move, and it spends RT time on planning. Dead on arrival, but worth stating since it is the first thing anyone proposes.

Have RT publish the Jacobian. motion already calls inverse every cycle, so it could do the extra evaluations and publish J at the current commanded pose, with no module changes. But planning needs J at poses further down the queue, not the one being executed. It answers a different question.

Load a second copy of the module in userspace. The .so is already userspace-loadable. Call rtapi_app_main in-process and you get a second set of HAL pins under a colliding component name, unconnected to the first, with pivot-length sitting at its default forever. Renaming to avoid the collision gives you two independent copies of the geometry, which is worse than useless.

Share the values through a signal. Create a signal, link both the RT pin and a userspace shadow pin to it. This breaks every existing config, because setp on a pin that is linked to a signal is an error, and setp is how these parameters are set today.

Clamp in RT instead of planning ahead. motion already knows the joint deltas it just computed, so it could scale the trajectory down when one exceeds a limit, with no module changes and no Jacobian. This is a real technique and it needs no part of this series. It is also reactive: it cannot begin decelerating before the constraint arrives, so it either violates the limit briefly or modulates feed in a way the planner did not plan. It complements lookahead, it does not replace it.

How the limits are computed

Build the Jacobian at a pose by central differences on the module's inverse kinematics, one column per world axis:

J[j][a] = (inverse(pose + h*e_a)[j] - inverse(pose - h*e_a)[j]) / 2h

with h of 0.1 mm or 0.1 degree. Identity kinematics skips this and uses the axis mapping directly.

Take the path tangent, world axis units per unit of path parameter:

tangent[a] = (end[a] - start[a]) / path_length

Joint j then moves at sum over a of |J[j][a]| * |tangent[a]| per unit of path speed, so the cap that joint puts on the move is its own limit divided by that sum, and the move's cap is the smallest across joints:

v_max = min over j of vel_limit[j]  / sum_a |J[j][a]| * |tangent[a]|
a_max = min over j of acc_limit[j]  / sum_a |J[j][a]| * |tangent[a]|
j_max = min over j of jerk_limit[j] / sum_a |J[j][a]| * |tangent[a]|

Sample that along the move and keep the tightest result, since J is pose dependent. The condition number of J falls out of the same evaluation and gives a caller a way to back off near a singularity instead of driving into it.

The absolute values make this a bound rather than an equality. Sign cancellation between axes that would let a joint move slower is discarded, so the answer is conservative and never optimistic. The exact answer is a small LP per sample, which does not belong in a planning path.

Try it

halrun -I
halcmd: loadrt 5axiskins coordinates=XYZBCW
halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \
          --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \
          --end 0,0,0,0,90,0,0,0,0 \
          --vel 100,100,100,30,30,100 --acc 500,500,500,200,200,500 \
          --jerk 5000,5000,5000,2000,2000,5000 --samples 5

module        : 5axiskins (XYZBCW, 6 joints)
path length   : 90.000000 (tangent units per path unit)
tangent       :  B=1.0000

s=0.000  vel<=    22.918 (j0)  acc<=     114.6 (j0)  jerk<=      1145.9 (j0)  cond=4.48
        Jacobian at start (rows = joints, cols = XYZABCUVW):
        j0:   1.0000   0.0000   0.0000   0.0000  -4.3633   0.0000   0.0000   0.0000  -0.0000
        j1:   0.0000   1.0000   0.0000   0.0000   0.0000  -0.0000   0.0000   0.0000   0.0000
        j2:   0.0000   0.0000   1.0000   0.0000   0.0000   0.0000   0.0000   0.0000   1.0000
        j3:   0.0000   0.0000   0.0000   0.0000   1.0000   0.0000   0.0000   0.0000   0.0000
        j4:   0.0000   0.0000   0.0000   0.0000   0.0000   1.0000   0.0000   0.0000   0.0000
        j5:   0.0000   0.0000   0.0000   0.0000   0.0000   0.0000   0.0000   0.0000   1.0000
s=0.250  vel<=    24.807 (j0)  acc<=     124.0 (j0)  jerk<=      1240.3 (j0)  cond=4.17
s=0.500  vel<=    30.000 (j3)  acc<=     162.1 (j0)  jerk<=      1620.6 (j0)  cond=3.32
s=0.750  vel<=    24.807 (j2)  acc<=     124.0 (j2)  jerk<=      1240.3 (j2)  cond=4.17
s=1.000  vel<=    22.918 (j2)  acc<=     114.6 (j2)  jerk<=      1145.9 (j2)  cond=4.48

segment cap   : vel 22.918 (joint 0 at s=0.000), acc 114.6 (joint 0), jerk 1145.9 (joint 0)
worst cond    : 4.476

A pure 90 degree B move at a 250 mm pivot. dX/dB is 4.3633 mm per degree, which is 250 * pi/180, so the 100 mm/s X joint allows 22.9 deg/s of B rather than the 30 deg/s the B axis limit alone suggests. Today the planner commands 30 and the X joint is asked for 131 mm/s. The binding joint migrates from X to B to Z as B sweeps, so one number for the whole move is the minimum over it, which is what the last line reports.

The pivot reads 250 rather than any value you setp, because this harness loads no motion thread and so never calls forward or inverse, and a snapshot only refreshes when it does. On a real machine do_forward_kins() covers that every servo cycle. #4 does not have this dependency at all, which is one of the reasons I prefer it.

The identity case matters just as much, because a change that slowed trivkins machines down would not be acceptable:

--start 0,0,0 --end 30,40,0 --vel 100,50,80 --acc 500,250,400
s=0.000  vel<=62.500 (j1)  acc<=312.5 (j1)  cond=1.00

A 3-4-5 move with Y limited to 50: 50/0.8 = 62.5, exactly what emccanon computes today. The general formula reduces to existing behaviour when the Jacobian is the identity.

Where it would plug in

The output is three scalars per move, which is what makes it cheap to adopt. The caller I have in mind is emcTrajLinearMove and emcTrajCircularMove in taskintf.cc: userspace, single threaded, already holding both endpoints, and upstream of the command that reaches motion. Compute the caps there, lower vel, ini_maxvel, acc and ini_maxjerk in the outgoing command, send it otherwise unchanged. No RT changes, and every planner benefits rather than only the one this came from.

Smaller things worth knowing

Tool offset in the trt kinematics is the one parameter in the union that changes inside a program. It scales the rotary to linear coupling the way pivot length does, it changes at G43, and canon's flush_segments does not drain the motion queue, so anything computing caps ahead of execution reads the offset RT is running now rather than the one that will be in force when the move executes. Recomputing on an offset change is easy; I would like to know whether anything else has the same property.

The numerical Jacobian uses fixed 0.1 mm and 0.1 degree steps. Fine for what I tested, but near a delta's workspace edge or a hexapod's strut limit the perturbed pose can be unreachable and inverse can fail or return nonsense. Several modules have an analytic Jacobian that is easy to write; should they be allowed to supply one?

The cap is the minimum over a whole move, so a long G1 through one bad pose is slowed for its entire length. The planner this came from hides that by chopping moves up; anything feeding TP0 or TP1 would not.

Adding the struct namespace changes hal_data_t, so HAL_VER goes 0x14 to 0x15 and mixed binaries refuse to talk. Unavoidable for any shmem addition, and another thing that disappears under the alternative.

Not included on purpose: the full branch adds halcmd show struct, which wants porting to the hal_query_t interface rather than the hal_priv.h walk it does today, and an identity fast path in the planner's own segment routine that takes the plain minimum over moving joints and so reports 50 where the tangent form correctly gives 62.5. That fast path should be deleted, not ported.