kinematics: answer which joints point the tool somewhere

The frames say where the tool points for a set of joint values.  The question
a tilted work plane actually asks is the other way round: which joint values
point it at the orientation the program wants.  Today that is answered outside
the kinematics, in per-machine trigonometry in the TWP remap, once per machine
and once per pair of rotary letters, which is how the copy in the tree came to
check the primary angle against the secondary joint's limits.

kinematicsToolFrameInverse() asks the module instead.  It reports every set of
joint values that reaches the orientation, and nothing else: no joint limits
and no preference between the answers, because the caller knows the limits and
knows whether the operator asked for the shortest move or for one direction of
rotation only.  A module that reports its frames needs to supply nothing, the
generic search in kins_util.c drives them; a module with a closed form
registers it and that is used instead.

The request is a tool axis and optionally a tool x as well, since a five axis
machine cannot pin down the spin about the tool and asking it to would be
asking for the impossible.  Where a request leaves the machine free, at a
singular pose or on a machine with a third orientation joint, one point of the
family is reported, the one nearest the seed, with the number of free
directions alongside.  Returning samples of a curve as though they were
alternatives would be worse than saying so.

Three details in the search are not incidental.  The damping is adaptive,
which is what keeps a rank deficient pose from turning finite difference noise
into a step of thousands of degrees.  The Jacobian is central differenced,
because a one sided error is first order in the step and shows up as a
spurious singular value, which is exactly what the rank test must not see.
And the joint unit is discovered by adding a whole turn and asking whether the
frame came back, because every module in the tree takes degrees but the
interface does not say so.

The unit test drives it through a table rotary machine, a nutating head and a
machine with both, and checks the nutating answers against the closed form the
remap uses.

Author: grandixximo

docs/src/motion/kinematics-conventions.adoc

@@ -264,6 +264,11 @@ are tilted work plane handling, tool length compensation along a tilted axis,
 previews and simulation models, and probing routines that have to say which way
 the stylus faces.
 
+The joint values that reach a requested orientation, through
+`kinematicsToolFrameInverse()`, which is the inverse of the pair above and the
+question a tilted work plane asks when it has to orient the machine.
+<<sec:orientation-inverse,The Orientation Inverse>> says what it answers.
+
 The Jacobian, relating commanded velocity to joint velocity at a given pose, so
 that a feed can be checked against the joint velocity, acceleration and limit
 values it will actually demand, and so that proximity to a singularity is a
@@ -276,6 +281,61 @@ None needs state carried between calls, and none needs the module to be running
 in a realtime thread to be useful: the interesting callers, a limit check
 before a move and a preview before a program runs, are not in the servo loop.
 
+[[sec:orientation-inverse]]
+== The Orientation Inverse
+
+Pointing the tool somewhere is the question a tilted work plane asks on every
+orienting move, and the question a program written as tool vectors asks on
+every block. It is the inverse of the tool frame: not where the tool points
+for these joints, but which joints point it there.
+
+It has more than one answer. A five-axis machine reaches a given tool axis two
+ways, one with the secondary rotary positive and one with it negative, and
+which of the two is wanted depends on the joint limits and on what the operator
+asked for, shortest move or one direction of rotation only. So the module
+reports every set of joint values that reaches the orientation and stops there.
+It does not apply the joint limits and it does not rank the answers, because
+neither is geometry: a module that picked for the caller would be picking with
+less to go on than the caller has.
+
+The request is a tool axis, and optionally a tool x as well. Leaving tool x out
+is the normal case on a five-axis machine, whose two rotaries cannot pin down
+the spin about the tool anyway, and it is what <<sec:tool-frame,The Tool
+Frame>> deals with through the virtual rotation. Asking for both is what a
+machine with a third orientation joint needs, and what makes its answer a list
+rather than a family.
+
+Some requests do not pin the machine down. A five-axis machine asked to point
+the tool along the axis its primary rotary turns about can hold any primary
+angle; a machine with three orientation joints asked only for a tool axis has a
+whole curve of solutions. In both cases the answer is a continuum, so one point
+of it is reported, the one nearest where the machine already is, along with the
+number of directions left free. A caller that hands back a list of samples from
+a curve as though they were alternatives is telling the operator something
+false.
+
+=== What a module has to supply
+
+Nothing, if it already reports its frames. The shared code answers the question
+by searching: it finds which joints move `transpose(work) * tool`, and solves
+for them. That is the whole reason the frames are worth reporting. A module
+that supplies them gets the inverse without deriving anything.
+
+A module with a closed form registers it and that is used instead. It is faster
+than a search and it knows its own degenerate poses without having to discover
+them. The nutating heads are the case in point: for those,
+
+  cos(secondary) = (Kzz - Cv^2) / (1 - Cv^2)
+
+with `Cv = cos(v)` for a nutation angle `v` and `Kzz` the z component of the
+requested tool axis, and the primary follows from the other two components.
+Both roots of the arc cosine are solutions, which is where the pair of answers
+comes from.
+
+The search is not a realtime routine. How long it takes depends on the machine
+and on the request, and the callers that want it, orienting a tilted work plane
+and previewing a program, are not in the servo loop.
+
 [[sec:writing-a-module]]
 == Writing a Module
 
@@ -300,6 +360,11 @@ Tool X::
   Derive the value from the module's own rotation matrices, and write those
   matrices down in the module.
 
+Orientation inverse::
+  A module that reports its frames gets it from the shared search and needs to
+  do nothing. Register a closed form only where one exists, and where it does,
+  say which poses it treats as degenerate.
+
 Geometry stays in the module::
   Whatever a consumer needs to know about the machine's shape is answered by
   the module. A consumer that restates it has taken a copy that nothing keeps

src/emc/kinematics/kinematics.h

@@ -239,6 +239,68 @@ extern int toolFrameInWork(const PmRotationMatrix *work,
    actually hold.  Used to check a declared rotation once, at load. */
 extern int toolFrameIsProper(const PmRotationMatrix *m);
 
+/* The inverse of kinematicsToolFrame(): which joint values point the tool
+   along a requested direction.  This is the question a tilted work plane asks
+   when it has to orient the machine, and the one vector format G-code asks
+   for every block.
+
+   axis_in_work is the wanted tool axis and x_in_work the wanted tool x, both
+   in workpiece coordinates, both in the sense of transpose(work) * tool.
+   x_in_work may be NULL, which leaves the spin about the tool free and is the
+   normal case on a five axis machine, whose two rotaries cannot pin it down.
+
+   seed is a full set of joint values, normally where the machine is now.  The
+   joints that do not affect the tool orientation are copied from it, and it
+   breaks the tie where a machine has more orientation joints than the request
+   constrains.
+
+   solutions receives max_solutions complete sets of joint values, one after
+   another, each num_joints long.  free_directions, if not NULL, receives one
+   entry per solution: 0 where the joints are pinned down, and n where the
+   solution is one point of an n dimensional family, which happens at a
+   singular pose and on a machine with a spare orientation joint.  In that case
+   one representative is reported, the one nearest the seed, because the answer
+   is a continuum and a list of samples from it would be arbitrary.
+
+   Joint limits are not applied and no solution is preferred over another: the
+   module answers what the geometry permits, and the caller picks by whatever
+   rule it works to, shortest move or positive rotation only or whatever else.
+
+   Returns the number of solutions, 0 if the orientation cannot be reached, or
+   -1 if the module cannot answer.
+
+   This is not a realtime routine.  It searches, and how long it takes depends
+   on the machine and the request. */
+#define TOOL_FRAME_MAX_SOLUTIONS 8
+#define TOOL_FRAME_MAX_FREE      4
+
+extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work,
+                                      const PmCartesian *x_in_work,
+                                      const double *seed,
+                                      double *solutions,
+                                      int max_solutions,
+                                      int *free_directions);
+
+/* The generic implementation of the above, driven by a module's own frame
+   functions, so that a module gets it for free once it supplies them.  A
+   module with a closed form registers that instead: it is faster, and it
+   knows its own degenerate poses without having to find them.
+
+   num_joints is the length of seed and of each row of solutions. */
+typedef int (*kinsFrameFunc)(const double *joint,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int toolFrameSolve(kinsFrameFunc work,
+                          kinsFrameFunc tool,
+                          int num_joints,
+                          const PmCartesian *axis_in_work,
+                          const PmCartesian *x_in_work,
+                          const double *seed,
+                          double *solutions,
+                          int max_solutions,
+                          int *free_directions);
+
 extern int kinematicsSwitchable(void);
 extern int kinematicsSwitch(int switchkins_type);
 //NOTE: switchable kinematics may require Interp::Synch