switchkins: one dispatch for kinematics modules, halcompile components and out-of-tree modules

docs/src/hal/components.adoc

@@ -316,6 +316,7 @@ Limit its slew rate to less than maxv per second. Limit its second derivative to
 | link:../man/man9/rosekins.9.html[rosekins] |Kinematics for a rose engine ||
 | link:../man/man9/rotatekins.9.html[rotatekins] |The X and Y axes are rotated 45 degrees compared to the joints 0 and 1. ||
 | link:../man/man9/scarakins.9.html[scarakins] |Kinematics for SCARA-type robots. ||
+| link:../man/man9/switchkinscomp.9.html[switchkinscomp] |Switchable kinematics module template ||
 | link:../man/man9/kins.9.html[three21kins] |Analytical kinematics solver for 6-DOF arm + wrist robots. ||
 | link:../man/man9/tripodkins.9.html[tripodkins] |The joints represent the distance of the controlled point from three predefined locations (the motors), giving three degrees of freedom in position (XYZ). ||
 | link:../man/man9/userkins.9.html[userkins] |Template for user-built kinematics ||

docs/src/motion/switchkins.adoc

@@ -46,6 +46,10 @@ The following kinematics modules support switchable kinematics:
 . *three21kins*     (type0:three21kins    type1:identity)
 . *scarakins*      (type0:scarakins      type1:identity)
 . *5axiskins*      (type0:5axiskins      type1:identity) (bridgemill)
+. *millturn*       (type0:identity       type1:turn)
+. *xyzab_tdr_kins* (type0:identity       type1:tcp)
+. *xyzacb_trsrn*   (type0:identity       type1:tcp        type2:tool)
+. *xyzbca_trsrn*   (type0:identity       type1:tcp        type2:tool)
 
 Every module listed above uses its own kinematics for type0 and
 identity kinematics for type1.  Each accepts the module string
@@ -419,9 +423,19 @@ configs/sim/axis/vismach/ .
 . puma/puma560.ini (genserkins)
 . puma/puma.ini (pumakins)
 . hexapod-sim/hexapod.ini (genhexkins)
+. millturn/millturn.ini (millturn)
+. 5axis/table-dual-rotary/xyzab-tdr.ini (xyzab_tdr_kins)
+. 5axis/table-rotary_spindle-rotary-nutating/xyzacb-trsrn_twp/xyzacb-trsrn.ini (xyzacb_trsrn)
+. 5axis/table-rotary_spindle-rotary-nutating/xyzbca-trsrn_twp/xyzbca-trsrn.ini (xyzbca_trsrn)
 
 == User kinematics provisions
 
+There are two ways to supply custom kinematics.  Adding a kinstype to
+a module that is already in the tree is the smaller job; building a
+module of your own gives you every kinstype it provides.
+
+=== Adding a kinstype to an in-tree module
+
 Custom kinematics can be coded and tested on Run-In-Place ('RIP')
 builds.  A template file src/emc/kinematics/userkfuncs.c is provided
 in the distribution.  This file can be copied/renamed to a user
@@ -439,6 +453,57 @@ Preempt-rt make example:
 $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid
 ----
 
+[[sec:switchkins-own-module]]
+=== Building a switchkins module of your own
+
+A complete kinematics module can be built out-of-tree with halcompile
+using the same switchkins implementation the in-tree modules use, so
+it gets the kinematics switching, the 'kinstype.is-N' pins, the
+'coordinates=' identity mapping and the G-code and HAL controls
+without reimplementing any of them.
+
+The implementation is the switchkins_core module.  It provides no
+kinstypes of its own; it exports switchkinsRegister(), switchkinsInit()
+and the rest of the switchkins.h interface, and the identity kinematics
+and helpers kinematics.h declares.  A module of your own includes
+switchkins.h, registers each of its kinstypes and calls switchkinsInit()
+from EXTRA_SETUP(), which halcompile runs after hal_init() and before
+hal_ready().  See <<sec:switchkins-code-notes,Code Notes>> for both
+calls.
+
+The template is src/hal/components/switchkinscomp.comp.  Copy and
+rename it (both the file and the component name) and replace the
+example kinstype with the real kinematics.  Start from the template,
+not from an in-tree component such as millturn.comp: those link the
+switchkins objects into the module through hal/components/Submakefile,
+which a halcompile build outside the tree does not do.
+
+----
+$ halcompile --install user_switchkins.comp
+----
+
+[source,ini]
+----
+[KINS]
+KINEMATICS = user_switchkins
+JOINTS = 3
+----
+
+The HAL file loads switchkins_core ahead of the module:
+
+----
+loadrt switchkins_core
+loadrt [KINS]KINEMATICS
+----
+
+A module loaded without switchkins_core ahead of it fails to load,
+and the error names the first switchkins or kinematics function it
+could not find:
+
+----
+user_switchkins: dlopen: .../rtlib/user_switchkins.so: undefined symbol: ...
+----
+
 == Warnings
 
 Unexpected behavior can result if a G-code program is inadvertently
@@ -463,22 +528,18 @@ The management of coordinate offsets, tool compensation, and
 INI file limits may require complicated and non-standard operating
 protocols.
 
+[[sec:switchkins-code-notes]]
 == Code Notes
 
 Kinematic modules providing switchkins functionality are linked to
-the switchkins.o object (switchkins.c) that provides the module
-'main' program (rtapi_app_main()) and related functions.  This
-'main' program reads (optional) module command-line parameters
-(coordinates, sparm) and passes them to the module-provided
-function switchkinsSetup().
-
-The switchkinsSetup() function identifies kinstype-specific setup
-routines and the functions for forward an inverse calculation for
-each kinstype (0,1,2) and sets a number of configuration
-settings.
-
-A module can provide further kinstypes by calling
-switchkinsRegister() from within switchkinsSetup(), once per
+the switchkins.o object (switchkins.c).  It provides
+kinematicsForward(), kinematicsInverse(), kinematicsSwitch() and
+the rest of the kinematics interface, dispatching each call to the
+kinstype currently selected, and it creates the HAL pins common to
+all switchkins modules.  It does not provide the module 'main'
+program, so a module can get that from wherever suits it.
+
+A kinstype is supplied by calling switchkinsRegister(), once per
 kinstype:
 
 ----
@@ -526,16 +587,75 @@ switchkins.c, returns the flags of every kinstype it can switch to, 0
 when neither applies, and -1 for any other: 'G12.1 P-' refuses a
 kinstype that returns -1.
 
-After calling switchkinsSetup(), rtapi_app_main() checks the
-supplied parameters, creates a HAL component, and then invokes
-the setup routine identified for each kinstype.
+When every kinstype is registered, the module calls:
+
+----
+int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates);
+----
+
+which checks the supplied parameters, creates the HAL pins, selects
+kinstype 0, and then invokes the setup routine registered for each
+kinstype.  The caller owns the HAL component: it does hal_init()
+before switchkinsInit() and hal_ready() after it.
 
 Each kinstype setup routine can (optionally) create HAL
 pins and set them to default values.  A setup routine is called
 once per kinstype it is registered for, so a routine used for two
-kinstypes must not create the same pin twice.  When all setup
-routines finish, rtapi_app_main() issues hal_ready() for the
-component to complete creation of the module.
+kinstypes must not create the same pin twice.
+
+=== Module main program
+
+A module written as a plain C file links switchkins_main.o
+(switchkins_main.c) for its rtapi_app_main().  That 'main' program
+reads the (optional) module command-line parameters (coordinates,
+sparm) and passes them to the module-provided function
+switchkinsSetup():
+
+----
+int switchkinsSetup(kparms* kp,
+                    KS* kset0, KS* kset1, KS* kset2,
+                    KF* kfwd0, KF* kfwd1, KF* kfwd2,
+                    KI* kinv0, KI* kinv1, KI* kinv2);
+----
+
+which identifies the setup, forward and inverse routines for
+kinstypes 0,1,2 and sets a number of configuration settings.  Those
+three are registered for the module, so it can supply further
+kinstypes by calling switchkinsRegister() itself, and registering
+one that switchkinsSetup() has already filled in is the same error
+as any other duplicate.
+
+A module written as a halcompile component gets rtapi_app_main()
+from halcompile instead.  It registers its kinstypes and calls
+switchkinsInit() from its EXTRA_SETUP() routine, which halcompile
+runs after hal_init() and before hal_ready():
+
+[source,c]
+----
+EXTRA_SETUP() {
+    kparms kp;
+    (void)__comp_inst; (void)prefix; (void)extra_arg;
+
+    kp.kinsname = "mykins"; // must agree with the module name
+    // ... the other kparms settings
+    if (switchkinsRegister(0, identityKinematicsSetup,
+                              identityKinematicsForward,
+                              identityKinematicsInverse)) { return -1; }
+    if (switchkinsRegister(1, mySetup, myForward, myInverse)) { return -1; }
+    return switchkinsInit(comp_id, &kp, coordinates);
+}
+----
+
+Where the switchkins code comes from depends on where the component
+is built.  Out of tree, the switchkins_core module provides it, loaded
+ahead of the component (see
+<<sec:switchkins-own-module,Building a switchkins module of your
+own>>).  In tree, the component links the objects into
+itself instead, named in hal/components/Submakefile:
+
+----
+millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o
+----
 
 === Outline
 

src/Makefile

@@ -402,6 +402,7 @@ SRCHEADERS := \
     hal/drivers/mesa-hostmot2/hostmot2-serial.h \
     emc/linuxcnc.h \
     emc/kinematics/kinematics.h \
+    emc/kinematics/switchkins.h \
     emc/nml_intf/emcmotcfg.h \
     emc/ini/axis_kinds.hh \
     emc/ini/inifile.hh \
@@ -1180,6 +1181,7 @@ genhexkins-objs += libposemath/_posemath.o
 genhexkins-objs += $(MATHSTUB)
 genhexkins-objs += emc/kinematics/kins_util.o
 genhexkins-objs += emc/kinematics/switchkins.o
+genhexkins-objs += emc/kinematics/switchkins_main.o
 genhexkins-objs += $(USERKFUNCS)
 
 obj-m += genserkins.o
@@ -1189,20 +1191,23 @@ genserkins-objs += libposemath/gomath.o
 genserkins-objs += $(MATHSTUB)
 genserkins-objs += emc/kinematics/kins_util.o
 genserkins-objs += emc/kinematics/switchkins.o
+genserkins-objs += emc/kinematics/switchkins_main.o
 genserkins-objs += $(USERKFUNCS)
 
 obj-m += xyzac-trt-kins.o
 xyzac-trt-kins-objs := emc/kinematics/xyzac-trt-kins.o
 xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o
 xyzac-trt-kins-objs += emc/kinematics/kins_util.o
 xyzac-trt-kins-objs += emc/kinematics/switchkins.o
+xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o
 xyzac-trt-kins-objs += $(USERKFUNCS)
 
 obj-m += xyzbc-trt-kins.o
 xyzbc-trt-kins-objs := emc/kinematics/xyzbc-trt-kins.o
 xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o
 xyzbc-trt-kins-objs += emc/kinematics/kins_util.o
 xyzbc-trt-kins-objs += emc/kinematics/switchkins.o
+xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o
 xyzbc-trt-kins-objs += $(USERKFUNCS)
 
 obj-m += scarakins.o
@@ -1211,6 +1216,7 @@ scarakins-objs += libposemath/_posemath.o
 scarakins-objs += $(MATHSTUB)
 scarakins-objs += emc/kinematics/kins_util.o
 scarakins-objs += emc/kinematics/switchkins.o
+scarakins-objs += emc/kinematics/switchkins_main.o
 scarakins-objs += $(USERKFUNCS)
 
 obj-m += pumakins.o
@@ -1219,6 +1225,7 @@ pumakins-objs += libposemath/_posemath.o
 pumakins-objs += $(MATHSTUB)
 pumakins-objs += emc/kinematics/kins_util.o
 pumakins-objs += emc/kinematics/switchkins.o
+pumakins-objs += emc/kinematics/switchkins_main.o
 pumakins-objs += $(USERKFUNCS)
 
 obj-m += three21kins.o
@@ -1227,14 +1234,22 @@ three21kins-objs += libposemath/_posemath.o
 three21kins-objs += $(MATHSTUB)
 three21kins-objs += emc/kinematics/kins_util.o
 three21kins-objs += emc/kinematics/switchkins.o
+three21kins-objs += emc/kinematics/switchkins_main.o
 three21kins-objs += $(USERKFUNCS)
 
+obj-m += switchkins_core.o
+switchkins_core-objs := emc/kinematics/switchkins_core.o
+switchkins_core-objs += emc/kinematics/switchkins.o
+switchkins_core-objs += emc/kinematics/kins_util.o
+switchkins_core-objs += $(MATHSTUB)
+
 obj-m += 5axiskins.o
 5axiskins-objs := emc/kinematics/5axiskins.o
 5axiskins-objs += libposemath/_posemath.o
 5axiskins-objs += $(MATHSTUB)
 5axiskins-objs += emc/kinematics/kins_util.o
 5axiskins-objs += emc/kinematics/switchkins.o
+5axiskins-objs += emc/kinematics/switchkins_main.o
 5axiskins-objs += $(USERKFUNCS)
 #----------------------------------------------------------------
 
@@ -1400,6 +1415,7 @@ endif
 ../rtlib/homemod$(MODULE_EXT): $(addprefix objects/rt,$(homemod-objs))
 ../rtlib/trivkins$(MODULE_EXT): $(addprefix objects/rt,$(trivkins-objs))
 ../rtlib/5axiskins$(MODULE_EXT): $(addprefix objects/rt,$(5axiskins-objs))
+../rtlib/switchkins_core$(MODULE_EXT): $(addprefix objects/rt,$(switchkins_core-objs))
 ../rtlib/maxkins$(MODULE_EXT): $(addprefix objects/rt,$(maxkins-objs))
 ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs))
 ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs))

src/emc/kinematics/5axiskins.c

@@ -59,9 +59,8 @@
 #include <rtapi_ctype.h>
 #include <hal.h>
 #include <emcmotcfg.h>
-#include <kinematics.h>
 
-#include "switchkins.h"
+#include <switchkins.h>
 
 static struct haldata {
     hal_real_t pivot_length;

src/emc/kinematics/genhexkins.c

@@ -110,10 +110,9 @@
 #include <rtapi_string.h>
 #include <hal.h>
 #include <emcmotcfg.h>
-#include <kinematics.h>             /* these decls, KINEMATICS_FORWARD_FLAGS */
 
 #include "genhexkins.h"
-#include "switchkins.h"
+#include <switchkins.h>
 
 static struct haldata {
     hal_real_t basex[NUM_STRUTS];

src/emc/kinematics/genserkins.c

@@ -42,7 +42,7 @@ frame-larger-than:
 #include <emcmotcfg.h>
 
 #include "genserkins.h"
-#include "switchkins.h"
+#include <switchkins.h>
 
 //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system)
 #undef  GO_REAL_EPSILON

src/emc/kinematics/kins_util.c

@@ -1281,3 +1281,22 @@ int identityKinematicsJacobian(const double *joint,
                                       (const double (*)[EMCMOT_MAX_AXIS])dP,
                                       jac);
 } // identityKinematicsJacobian()
+
+EXPORT_SYMBOL(map_coordinates_to_jnumbers);
+EXPORT_SYMBOL(mapped_joints_to_position);
+EXPORT_SYMBOL(position_to_mapped_joints);
+EXPORT_SYMBOL(identityKinematicsSetup);
+EXPORT_SYMBOL(identityKinematicsForward);
+EXPORT_SYMBOL(identityKinematicsInverse);
+EXPORT_SYMBOL(identityKinematicsWorkFrame);
+EXPORT_SYMBOL(identityKinematicsToolFrame);
+EXPORT_SYMBOL(identityKinematicsJacobian);
+EXPORT_SYMBOL(toolFrameIsProper);
+EXPORT_SYMBOL(toolFrameApplyNative);
+EXPORT_SYMBOL(toolFrameInWork);
+EXPORT_SYMBOL(toolFrameSolve);
+EXPORT_SYMBOL(kinsJacobianFromInverse);
+EXPORT_SYMBOL(kinsJacobianFromMappedAxes);
+EXPORT_SYMBOL(kinsJacobianFromDhArm);
+EXPORT_SYMBOL(TOOL_FRAME_SPINDLE);
+EXPORT_SYMBOL(TOOL_FRAME_FLANGE);

src/emc/kinematics/pumakins.c

@@ -20,10 +20,9 @@
 #include <rtapi_math.h>
 #include <rtapi_string.h>
 #include <hal.h>
-#include <kinematics.h>
 
 #include "pumakins.h"
-#include "switchkins.h"
+#include <switchkins.h>
 
 struct haldata {
     hal_real_t a2, a3, d3, d4, d6;

src/emc/kinematics/scarakins.c

@@ -19,9 +19,8 @@
 #include <rtapi_math.h>
 #include <rtapi_string.h>
 #include <hal.h>
-#include <kinematics.h>
 
-#include "switchkins.h"
+#include <switchkins.h>
 
 static struct scara_data {
     hal_real_t d1, d2, d3, d4, d5, d6;