Multiaxis kinematics and limits

docs/src/hal/components.adoc

@@ -338,6 +338,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

@@ -44,6 +44,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)
 
 The xyz[ab]c-trt-kins modules by default use type0==xyz[ab]c-trt-kins
 for backwards compatibility.  The provided sim configs alter the
@@ -128,6 +132,9 @@ program behavior in accordance with the active kinematics type.
 . *kinstype.is-1*           Output (bit)
 . *kinstype.is-2*           Output (bit)
 
+A module providing more than three kinematics types has one
+'kinstype.is-N' pin per type.
+
 == Usage
 
 === HAL Connections
@@ -330,9 +337,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
@@ -350,6 +367,47 @@ Preempt-rt make example:
 $ userkfuncs=/home/myname/kins/mykins.c make && sudo make setuid
 ----
 
+=== 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 template is src/hal/components/switchkinscomp.comp.  Copy and
+rename it (both the file and the component name), point its TOPDIR
+at a LinuxCNC source tree, and replace the example kinstype with the
+real kinematics:
+
+[source,c]
+----
+#define TOPDIR /home/myname/linuxcnc-dev
+// ...
+#include USE_TOPDIR(src/emc/kinematics/switchkins.c)
+#include USE_TOPDIR(src/emc/kinematics/kins_util.c)
+----
+
+The module 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.
+
+----
+$ halcompile --install user_switchkins.comp
+----
+
+[source,ini]
+----
+[KINS]
+KINEMATICS = user_switchkins
+JOINTS = 3
+----
+
+[NOTE]
+The switchkins sources are compiled into the module, so it is built
+against one source tree and has to be rebuilt when that tree changes.
+
 == Warnings
 
 Unexpected behavior can result if a G-code program is inadvertently
@@ -374,27 +432,78 @@ 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.
-
-After calling switchkinsSetup(), rtapi_app_main() checks the
-supplied parameters, creates a HAL component, and then invokes
-the setup routine identified for each kinstype (0,1,2).
-
-Each kinstype (0,1,2) setup routine can (optionally) create HAL
-pins and set them to default values.  When all setup routines
-finish, rtapi_app_main() issues hal_ready() for the component
-to complete creation of the module.
+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:
+
+----
+int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv);
+----
+
+'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in
+switchkins.h).  Registering a kinstype twice is an error, and so is
+leaving a gap below the highest kinstype provided.  Either mistake
+fails the module load and says which kinstype is at fault.
+
+Each kinstype gets its own 'kinstype.is-N' pin, so a module
+providing the usual three keeps the pin names it always had.
+
+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.
+
+=== 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().  The component names
+the objects it needs in hal/components/Submakefile:
+
+----
+millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o
+----
 
 // vim: set syntax=asciidoc:

src/Makefile

@@ -397,6 +397,7 @@ SRCHEADERS := \
     hal/drivers/mesa-hostmot2/hostmot2-serial.h \
     emc/linuxcnc.h \
     emc/kinematics/kinematics.h \
+    emc/kinematics/switchkins.h \
     emc/motion/emcmotcfg.h \
     emc/ini/inifile.hh \
     emc/ini/inifile.h \
@@ -1159,6 +1160,7 @@ genhexkins-objs += libnml/posemath/_posemath.o
 genhexkins-objs += libnml/posemath/sincos.o $(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
@@ -1168,20 +1170,23 @@ genserkins-objs += libnml/posemath/gomath.o
 genserkins-objs += libnml/posemath/sincos.o $(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
@@ -1190,6 +1195,7 @@ scarakins-objs += libnml/posemath/_posemath.o
 scarakins-objs += libnml/posemath/sincos.o $(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
@@ -1198,6 +1204,7 @@ pumakins-objs += libnml/posemath/_posemath.o
 pumakins-objs += libnml/posemath/sincos.o $(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
@@ -1206,6 +1213,7 @@ three21kins-objs += libnml/posemath/_posemath.o
 three21kins-objs += libnml/posemath/sincos.o $(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 += 5axiskins.o
@@ -1214,6 +1222,7 @@ obj-m += 5axiskins.o
 5axiskins-objs += libnml/posemath/sincos.o $(MATHSTUB)
 5axiskins-objs += emc/kinematics/kins_util.o
 5axiskins-objs += emc/kinematics/switchkins.o
+5axiskins-objs += emc/kinematics/switchkins_main.o
 5axiskins-objs += $(USERKFUNCS)
 #----------------------------------------------------------------
 

src/emc/kinematics/Submakefile

@@ -35,7 +35,8 @@ $(RDELTAMODULE): $(call TOOBJS, $(RDELTAMODULESRCS))
 PYTARGETS += $(RDELTAMODULE)
 
 EMCKINEMATICSINCS = \
-	./emc/kinematics/kinematics.h
+	./emc/kinematics/kinematics.h \
+	./emc/kinematics/switchkins.h
 
 $(patsubst ./emc/kinematics/%,../include/%,$(EMCKINEMATICSINCS)): ../include/%.h: ./emc/kinematics/%.h
 	cp $^ $@