From 20ff7ff089147282dab40a362ad4e59043c5168a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 11 Aug 2026 22:59:37 +1000 Subject: [PATCH 1/5] switchkins: separate the dispatch from rtapi_app_main() switchkins.c owned rtapi_app_main(), so a module could only use it by having no main of its own, which ruled out halcompile components: the switchable kinematics in hal/components each carry a private copy of the dispatch. Move rtapi_app_main(), rtapi_app_exit() and the coordinates= and sparm= parameters to switchkins_main.c and give switchkins.c one entry point, switchkinsInit(comp_id, kp, coordinates), which counts and validates the registered types, creates the pins and starts on type 0. The caller owns the component. The types switchkinsSetup() supplies now go through switchkinsRegister() like any others, so one registration path checks every type and a double registration is refused. The eight existing modules add switchkins_main.o to their objects and are otherwise untouched. --- src/Makefile | 8 +++ src/emc/kinematics/switchkins.c | 61 ++++++------------ src/emc/kinematics/switchkins.h | 11 +++- src/emc/kinematics/switchkins_main.c | 94 ++++++++++++++++++++++++++++ 4 files changed, 130 insertions(+), 44 deletions(-) create mode 100644 src/emc/kinematics/switchkins_main.c diff --git a/src/Makefile b/src/Makefile index 927b1227fea..8d53d7ec78e 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1180,6 +1180,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,6 +1190,7 @@ 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 @@ -1196,6 +1198,7 @@ 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 @@ -1203,6 +1206,7 @@ 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 +1215,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 +1224,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,6 +1233,7 @@ 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 += 5axiskins.o @@ -1235,6 +1242,7 @@ obj-m += 5axiskins.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) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index f832246a926..a9fa9027cd5 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,7 +27,6 @@ * Using modules must supply function: switchkinsSetup() */ #include -#include #include #include #include @@ -422,12 +421,6 @@ int kinematicsTypeFlags(int ktype) return ktype_flags[ktype]; } // kinematicsTypeFlags() -//********************************************************************* -static char *coordinates; -RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static char *sparm; -RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -443,33 +436,24 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); -MODULE_LICENSE("GPL"); +EXPORT_SYMBOL(switchkinsInit); -static int comp_id; //********************************************************************* -int rtapi_app_main(void) +// The caller owns the hal component: it does hal_init() before this and +// hal_ready() after it. Every switchkins-type must be registered by +// now. +int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates) { - int i,res,identities; - char* emsg="other"; - - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // may also call switchkinsRegister() - res = switchkinsSetup(&kp, - &ksetups[0], &ksetups[1], &ksetups[2], - &kfwds[0], &kfwds[1], &kfwds[2], - &kinvs[0], &kinvs[1], &kinvs[2]); - if (res) {emsg="switchkinsSetp FAIL"; goto error;} - if (register_error) {emsg="switchkinsRegister FAIL"; goto error;} + int i; + int identities; + int res = 0; + char* emsg = "other"; + + kp = *ksetup_parms; // kinematics parms are needed after this returns + + if (register_error) {emsg = "switchkinsRegister FAIL"; goto error;} // an identity type answers the tool frame the same way whichever module // asked for it, so supply it here rather than in every switchkinsSetup() @@ -485,7 +469,7 @@ int rtapi_app_main(void) } } - // the highest type provided by either route sets the count + // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } } @@ -544,11 +528,8 @@ int rtapi_app_main(void) emsg = "incomplete switchkins-type"; goto error; } - comp_id = hal_init(kp.kinsname); - if(comp_id < 0) goto error; - swdata = hal_malloc(sizeof(struct swdata)); - if (!swdata) goto error; + if (!swdata) {emsg = "hal_malloc fail"; goto error;} for (i=0; i < kins_count; i++) { res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]), @@ -562,8 +543,8 @@ int rtapi_app_main(void) res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_a, 0.0, "skgui.a"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_b, 0.0, "skgui.b"); res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_c, 0.0, "skgui.c"); - if (res) {emsg = "hal pin create fail";goto error;} } + if (res) {emsg = "hal pin create fail"; goto error;} switchkins_type = 0; // startup with default type kinematicsSwitch(switchkins_type); @@ -574,14 +555,10 @@ int rtapi_app_main(void) ksetups[i](comp_id,coordinates,&kp); } - hal_ready(comp_id); return 0; error: rtapi_print_msg(RTAPI_MSG_ERR, "\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg); - hal_exit(comp_id); return -1; -} // rtapi_app_main() - -void rtapi_app_exit(void) { hal_exit(comp_id); } +} // switchkinsInit() diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index 77caca90629..f6b8aa06e4f 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -34,14 +34,14 @@ typedef int (*KS)(const int comp_id, // halpins ); //********************************************************************* -// supplied by the using module, provides types 0,1,2 +// supplied by a module using switchkins_main.c, provides types 0,1,2 extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, KI* kinv0, KI* kinv1, KI* kinv2 ); -// called from switchkinsSetup(), once per type it does not provide itself +// provide one switchkins-type, before switchkinsInit() extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); // called from switchkinsSetup() for each type that reports its frames; a type @@ -84,4 +84,11 @@ typedef int (*KJ)(const double *joint, // that does not gets the exact answer if it is an identity type, and // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); + +// create the hal pins and start on type 0; the caller owns the hal +// component and does hal_init() before and hal_ready() after +extern int switchkinsInit(const int comp_id, + kparms* ksetup_parms, + const char* coordinates + ); #endif // } diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c new file mode 100644 index 00000000000..4a4cc05153c --- /dev/null +++ b/src/emc/kinematics/switchkins_main.c @@ -0,0 +1,94 @@ +/* + Copyright 2019 Dewey Garrett + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/* switchkins_main.c provides rtapi_app_main() for kinematics modules +* built around switchkins.c. A module that gets its rtapi_app_main() +* from somewhere else (a halcompile component, for instance) links +* switchkins.c alone and calls switchkinsInit() itself. +* +* Using modules must supply function: switchkinsSetup() +*/ +#include +#include +#include + +#include "switchkins.h" + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); +static char *sparm; +RTAPI_MP_STRING(sparm, "switchkins module-specific parameter"); + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + kparms kp; + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + // defaults prior to switchkinsSetup() call + kp.kinsname = NULL; + kp.halprefix = NULL; + kp.required_coordinates = ""; + kp.max_joints = 0; // Setup must supply + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; // negative means: not used + + kp.sparm = sparm; // module parm passed to kins + + // switchkinsSetup() provides types 0,1,2 and may also call + // switchkinsRegister() for any others + if (switchkinsSetup(&kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp.kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + comp_id = hal_init(kp.kinsname); + if (comp_id < 0) return comp_id; + + if (switchkinsInit(comp_id, &kp, coordinates)) { + hal_exit(comp_id); + return -1; + } + + hal_ready(comp_id); + return 0; +} // rtapi_app_main() + +void rtapi_app_exit(void) { hal_exit(comp_id); } From 6b0504cdbcb9959c1c0a8ac968cc85ca9e1c5edf Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 00:17:36 +1000 Subject: [PATCH 2/5] switchkins: let halcompile components use the switchkins core millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn each carried a copy of the switchkins dispatch, a private switchkins_type, a hand-written kinematicsSwitch() and a setup that had to hal_set_unready() the component again. Now that the dispatch is separate from the main program a component links it and calls switchkinsInit() from EXTRA_SETUP(). Two build changes: the generated per-comp .mak takes a -extra-objs list, and switchkins.h is copied to ../include and installed so resolves from a generated source. Each of the four registers its types and calls switchkinsInit(); their identity type comes from kins_util.c, which gets them the coordinates= parameter, and a bad motion.switchkins-type is refused instead of stranding the module. Pin names are unchanged except millturn's unused in/out template pins. The four sim configs give the same positions through the same MDI sequence as before, in every kinematics type. --- docs/src/motion/switchkins.adoc | 78 ++- src/Makefile | 1 + src/emc/kinematics/switchkins.h | 8 +- src/hal/components/Submakefile | 13 +- src/hal/components/millturn.comp | 268 +++-------- src/hal/components/xyzab_tdr_kins.comp | 387 ++++++--------- src/hal/components/xyzacb_trsrn.comp | 626 +++++++++++------------- src/hal/components/xyzbca_trsrn.comp | 628 +++++++++++-------------- 8 files changed, 865 insertions(+), 1144 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index fbebceccb58..98fe2f3b136 100644 --- a/docs/src/motion/switchkins.adoc +++ b/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,6 +423,10 @@ 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 @@ -466,19 +474,14 @@ protocols. == 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 +529,53 @@ 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(). The component names +the objects it needs in hal/components/Submakefile: + +---- +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +---- === Outline diff --git a/src/Makefile b/src/Makefile index 8d53d7ec78e..93a3cbeabc2 100644 --- a/src/Makefile +++ b/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 \ diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index f6b8aa06e4f..c7114262403 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -1,10 +1,10 @@ /* ** License GPL Version 2 */ -#ifndef SWITCHKINS_H // { -#define SWITCHKINS_H +#ifndef __LINUXCNC_SWITCHKINS_H +#define __LINUXCNC_SWITCHKINS_H -#include +#include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) //is in kinematics.h: motion and the NML status channel need it too @@ -91,4 +91,4 @@ extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); -#endif // } +#endif diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 6c6ed4850de..759fc5bd483 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -76,11 +76,20 @@ endif obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, %.o, $(COMPS) $(COMP_DRIVERS))) +# A component that links objects besides its own names them here as +# -extra-objs. The list is expanded when the .mak is written, +# so it has to be defined in this file (which the .mak depends on). +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := $(SWITCHKINS_OBJS) +xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) +xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) +xyzbca_trsrn-extra-objs := $(SWITCHKINS_OBJS) + objects/%.mak: %.comp hal/components/Submakefile $(ECHO) "Creating $(notdir $@)" @mkdir -p $(dir $@) - $(Q)echo $(notdir $*)-objs := objects/$*.o > $@.tmp - $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o >> $@.tmp + $(Q)echo $(notdir $*)-objs := objects/$*.o $($(notdir $*)-extra-objs) > $@.tmp + $(Q)echo ../rtlib/$(notdir $*)$(MODULE_EXT): objects/rtobjects/$*.o $(addprefix objects/rt,$($(notdir $*)-extra-objs)) >> $@.tmp $(Q)mv -f $@.tmp $@ objects/%.c: %.comp ../bin/halcompile diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 23f538b33ac..4f7c60bb707 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -10,16 +10,15 @@ rotary axis. type1 is a turn (Z-YX) configuration with A configured to be a spindle. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: 'configs/sim/axis/vismach/millturn/millturn.ini'. Further explanations can be found in the README in 'configs/sim/axis/vismach/millturn'. -millturn.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -27,7 +26,7 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. +// Use the *_setup() function for pins and params used by kinematics. pin out sint fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -36,20 +35,10 @@ license "GPL"; author "David Mueller"; ;; -#include - -static struct haldata { - // Example pin pointers: - hal_uint_t in; - hal_uint_t out; - // Example parameters: - //hal_real_t param_rw; - //hal_real_t param_ro; +#include - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -60,121 +49,30 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "millturn" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->in, 0, "%s.in", HAL_PREFIX); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->out, 0, "%s.out", HAL_PREFIX); - // hal parameter examples: - //res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - //res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> mill configuration - //-> turn configuration - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return 0; // the turn mapping, no flag to declare - default: return -1; - } -} - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() +// the turn kinematics need no hal pins of their own +static int turnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() + (void)comp_id; + (void)coords; + (void)kp; + return 0; +} // turnKinematicsSetup() -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - static bool gave_msg; - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - break; - case 1: - pos->tran.x = j[2]; - pos->tran.y = -j[1]; - pos->tran.z = j[0]; - pos->a = j[3]; - break; - } + + pos->tran.x = j[2]; + pos->tran.y = -j[1]; + pos->tran.z = j[0]; + pos->a = j[3]; + // unused coordinates: pos->b = 0; pos->c = 0; @@ -182,77 +80,67 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_ui32(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s the 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // turnKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turnKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - break; - case 1: - j[2] = pos->tran.x; - j[1] = -pos->tran.y; - j[0] = pos->tran.z; - j[3] = pos->a; - break; - } - - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_ui32(haldata->out, hal_get_ui32(haldata->in)); //dereference - //read from param example: *haldata->out = hal_get_real(haldata->param_rw); + j[0] = pos->tran.z; + j[1] = -pos->tran.y; + j[2] = pos->tran.x; + j[3] = pos->a; return 0; -} // kinematicsInverse() +} // turnKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turnKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - int r, c; (void)j; (void)pos; (void)iflags; - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } - } - // the derivative of kinematicsInverse() for each type: which joint - // follows which pose coordinate, and in which sense - switch (switchkins_type) { - case 0: - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - break; - case 1: - jac[2][0] = 1; - jac[1][1] = -1; - jac[0][2] = 1; - jac[3][3] = 1; - break; - } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + // the derivative of turnKinematicsInverse(): which joint follows which + // pose coordinate, and in which sense + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; return 0; -} // kinematicsJacobian() +} // turnKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "millturn"; + kp.halprefix = "millturn"; + kp.required_coordinates = "xyza"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, turnKinematicsSetup, + turnKinematicsForward, + turnKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 78e4c6c1787..7c6994deabd 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -13,16 +13,15 @@ axes XYZAB respectively. type1 is a XYZAB configuration with tool center point (TCP) compensation. +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + For an example configuration, run the sim config: '/configs/sim/axis/vismach/5axis/table-dual-rotary/xyzab-tdr.ini'. Further explanations can be found in the README in '/configs/sim/axis/vismach/5axis/table-dual-rotary/'. -xyzab_tdr_kins.comp was constructed by modifying the template file: -userkins.comp. - -For more information on how to modify userkins.comp run: $ man -userkins. Also, see additional information inside: 'userkins.comp'. - For information on kinematics in general see the kinematics document chapter (docs/src/motion/kinematics.txt) and for switchable kinematics in particular see the switchkins document @@ -31,6 +30,7 @@ chapter (docs/src/motion/switchkins.txt) """; pin out sint dummy=0"one pin needed to satisfy halcompile requirement"; + option extra_setup; license "GPL"; @@ -38,127 +38,61 @@ author "David Mueller"; ;; #include -#include -static struct haldata { +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); - // Declare hal pin pointers used for xyzab_tdr kinematics: +static struct haldata { hal_real_t tool_offset_z; hal_real_t x_offset; hal_real_t z_offset; hal_real_t x_rot_point; hal_real_t y_rot_point; hal_real_t z_rot_point; +} *tdrdata; - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzab_tdr_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - // hal pins required for xyzab_tdr kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_offset, 0.0, "%s.z-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_point, 0.0, "%s.x-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_point, 0.0, "%s.y-rot-point", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_point, 0.0, "%s.z-rot-point", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> XYZAB TCP - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) -{ - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - default: return -1; - } -} - -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - return -1; // FAIL - } - return 0; // ok -} - -KINEMATICS_TYPE kinematicsType() +static int tdrKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() + int res = 0; + (void)coords; + + tdrdata = hal_malloc(sizeof(*tdrdata)); + if (!tdrdata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, + "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, + "%s.z-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, + "%s.x-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, + "%s.y-rot-point", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, + "%s.z-rot-point", kp->halprefix); + if (res) return -1; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return 0; +} // tdrKinematicsSetup() +static int tdrKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -168,39 +102,22 @@ int kinematicsForward(const double *j, double cb = cos(j[4]*TO_RAD); // used to be consistent with math in the documentation - double px = 0; - double py = 0; - double pz = 0; - - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics FORWARD ==================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - break; - case 1: // ========================= TCP kinematics FORWARD ====================== - px = j[0] - x_rot_point; - py = j[1] - y_rot_point; - pz = j[2] - z_rot_point - dt; - - pos->tran.x = cb*px + sb*pz - + x_rot_point; - - pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz - + y_rot_point; - - pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz - + z_rot_point + dz + dt; - - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - break; - } + double px = j[0] - x_rot_point; + double py = j[1] - y_rot_point; + double pz = j[2] - z_rot_point - dt; + + pos->tran.x = cb*px + sb*pz + + x_rot_point; + + pos->tran.y = sa*sb*px + ca*py - cb*sa*pz + sa*dz + + y_rot_point; + + pos->tran.z = - ca*sb*px + sa*py + ca*cb*pz - ca*dz + + z_rot_point + dz + dt; + + pos->a = j[3]; + pos->b = j[4]; + // unused coordinates: pos->c = 0; pos->u = 0; @@ -208,22 +125,22 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // tdrKinematicsForward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdrKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -233,53 +150,38 @@ int kinematicsInverse(const EmcPose * pos, double cb = cos(pos->b*TO_RAD); // used to be consistent with math in the documentation - double qx = 0; - double qy = 0; - double qz = 0; - - switch (switchkins_type) { - case 0:// ====================== IDENTITY kinematics INVERSE ===================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - break; - case 1: // ========================= TCP kinematics INVERSE ====================== - qx = pos->tran.x - x_rot_point - dx; - qy = pos->tran.y - y_rot_point; - qz = pos->tran.z - z_rot_point - dz - dt; - - j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz - + x_rot_point; - - j[1] = ca*qy + sa*qz - + y_rot_point; - - j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz - + z_rot_point + dt; - - j[3] = pos->a; - j[4] = pos->b; - break; - } + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + + j[0] = cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz + + x_rot_point; + + j[1] = ca*qy + sa*qz + + y_rot_point; + + j[2] = sb*qx - sa*cb*qy + ca*cb*qz + sb*dx + cb*dz + + z_rot_point + dt; + + j[3] = pos->a; + j[4] = pos->b; return 0; -} // kinematicsInverse() +} // tdrKinematicsInverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdrKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - double x_rot_point = hal_get_real(haldata->x_rot_point); - double y_rot_point = hal_get_real(haldata->y_rot_point); - double z_rot_point = hal_get_real(haldata->z_rot_point); - double dx = hal_get_real(haldata->x_offset); - double dz = hal_get_real(haldata->z_offset); - double dt = hal_get_real(haldata->tool_offset_z); + double x_rot_point = hal_get_real(tdrdata->x_rot_point); + double y_rot_point = hal_get_real(tdrdata->y_rot_point); + double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double dx = hal_get_real(tdrdata->x_offset); + double dz = hal_get_real(tdrdata->z_offset); + double dt = hal_get_real(tdrdata->tool_offset_z); double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -287,43 +189,58 @@ int kinematicsJacobian(const double *j, double qx = pos->tran.x - x_rot_point - dx; double qy = pos->tran.y - y_rot_point; double qz = pos->tran.z - z_rot_point - dz - dt; - int r, c; - - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { - for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } - } - - switch (switchkins_type) { - case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== - jac[0][0] = 1; - jac[1][1] = 1; - jac[2][2] = 1; - jac[3][3] = 1; - jac[4][4] = 1; - break; - case 1: // ========================= TCP kinematics JACOBIAN ====================== - // the TCP inverse above differentiated: its coefficients of - // qx, qy and qz for the linear columns, and the same terms - // with a or b advanced a quarter turn for the rotary columns - jac[0][0] = cb; - jac[0][1] = sa*sb; - jac[0][2] = -ca*sb; - jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; - jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; - - jac[1][1] = ca; - jac[1][2] = sa; - jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; - - jac[2][0] = sb; - jac[2][1] = -sa*cb; - jac[2][2] = ca*cb; - jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; - jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - break; - } + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // tdrKinematicsInverse() differentiated: its coefficients of qx, qy + // and qz for the linear columns, and the same terms with a or b + // advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; return 0; -} // kinematicsJacobian() +} // tdrKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzab_tdr_kins"; + kp.halprefix = "xyzab_tdr_kins"; + kp.required_coordinates = "xyzab"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, tdrKinematicsSetup, + tdrKinematicsForward, + tdrKinematicsInverse)) { return -1; } + if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 16fdbb801ab..83c8fcdfc78 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -4,6 +4,11 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out sint dummy=0 "dummy pin to satisfy halcompile"; option period no; @@ -14,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,134 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzacb_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzacb_trsrn_kins" - int res=0; - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzacb_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - case 2: return 0; // the tool kinematics, no flag to declare - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -208,20 +132,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -264,9 +175,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -304,10 +213,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -315,98 +220,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[4]*TO_RAD); - double Cs = cos(j[4]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, - {Cv*Ss, r, t}, - {-Sv*Ss, t, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[3]*TO_RAD); - double Cw = cos(j[3]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the A joint carries the work: its frame in machine coordinates - // is a rotation about x by the joint value - const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -454,23 +291,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); Cs = cos(j[4]*TO_RAD); @@ -507,9 +328,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -551,38 +370,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[4]*TO_RAD); + double Cs = cos(j[4]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - break; + // the nutating secondary joint + const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss}, + {Cv*Ss, r, t}, + {-Sv*Ss, t, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // tcpKinematicsToolFrame() + +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[3]*TO_RAD); + double Cw = cos(j[3]*TO_RAD); + + // the A joint carries the work: its frame in machine coordinates + // is a rotation about x by the joint value + const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsWorkFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Ly = hal_get_real(haldata->y_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -590,105 +483,146 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->a*TO_RAD); double Cw = cos(pos->a*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->b*TO_RAD); + double Cs = cos(pos->b*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qy = pos->tran.y; double Qz = pos->tran.z; - double Ay, Az; // the two lever arms the table turns about - int R, C; - - for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { - for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } - } + // the two lever arms the table turns about + double Ay = Dray + Dy + Ly - Qy; + double Az = Draz + Dt + Lz - Qz; + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - switch (switchkins_type) { +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->b*TO_RAD); - Cs = cos(pos->b*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ay = Dray + Dy + Ly - Qy; - Az = Draz + Dt + Lz - Qz; - - // j[0]: Qx plus terms in the head angles only - jac[0][0] = 1; - jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; - jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx - - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; - - // j[1]: -Cw*Ay - Az*Sw plus head terms - jac[1][1] = Cw; - jac[1][2] = Sw; - jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; - jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; - jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Ly; - - // j[2]: -Cw*Az + Ay*Sw plus head terms - jac[2][1] = -Sw; - jac[2][2] = Cw; - jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; - jac[2][4] = (Dt + Lz)*ds + Ly*dt_; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[0][2] = (Cp*SvSs - Sp*t); + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); - jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[1][2] = (Sp*SvSs + Cp*t); + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); - jac[2][0] = -(Ctc*SvSs - Stc*t); - jac[2][1] = (Stc*SvSs + Ctc*t); - jac[2][2] = s; + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzacb_trsrn"; + kp.halprefix = "xyzacb_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 9abb7c5c190..7e63687c283 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -4,6 +4,11 @@ description """ FIXME +The kinematics-type switching, the *kinstype.is-N* pins and the +joints-to-coordinates mapping are provided by switchkins.c, so the +*coordinates=* module parameter and the kinematics switching described in +the switchkins document chapter apply here too. + """; pin out sint dummy=0 "dummy pin to satisfy halcompile"; option period no; @@ -14,8 +19,11 @@ author "David Mueller"; ;; #include -#include +#include + +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); static struct haldata { // these should be parameters really but we want to be able to @@ -36,136 +44,50 @@ static struct haldata { // Declare hal pin pointers used for xyzbca_trsrn kinematics: hal_real_t tool_offset_z; - - //Declare hal pin pointers used for switchable kinematics - hal_bool_t kinstype_is_0; - hal_bool_t kinstype_is_1; - hal_bool_t kinstype_is_2; } *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "xyzbca_trsrn_kins" - int res=0; - // inbherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - // set unready to allow creation of pins - if (hal_set_unready(comp_id)) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pins required for xyzbca_trsrn kinematics: - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", HAL_PREFIX); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", HAL_PREFIX); - - // hal pins required for switchable kinematics: - //default at startup -> identity kinematics - //-> xyzabc TCP - //-> xyzabc TOOL - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_0, 1, "kinstype.is-0"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1"); - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2"); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsSwitchable); -EXPORT_SYMBOL(kinematicsSwitch); -EXPORT_SYMBOL(kinematicsTypeFlags); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); - -static rtapi_u32 switchkins_type; - -int kinematicsSwitchable() {return 1;} - -int kinematicsTypeFlags(int ktype) +// the pins are shared by the TCP and TOOL kinematics; the TOOL type has +// no setup routine of its own +static int trsrnKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - switch (ktype) { - case 0: return KINSTYPE_IDENTITY; - case 1: return KINSTYPE_PRIMARY; - case 2: return 0; // the tool kinematics, no flag to declare - default: return -1; - } -} - + int res = 0; + (void)coords; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) return -1; + + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); + res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); + if (res) return -1; -int kinematicsSwitch(int new_switchkins_type) -{ - switchkins_type = new_switchkins_type; - rtapi_print("kinematicsSwitch(): type=%d\n",switchkins_type); - // create case structure for switchable kinematics - switch (switchkins_type) { - case 0: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE0\n"); - hal_set_bool(haldata->kinstype_is_0, 1); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 1: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 1); - hal_set_bool(haldata->kinstype_is_2, 0); - break; - case 2: rtapi_print_msg(RTAPI_MSG_INFO, - "kinematicsSwitch:TYPE1\n"); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_2, 1); - break; - default: rtapi_print_msg(RTAPI_MSG_ERR, - "kinematicsSwitch:BAD VALUE <%d>\n", - switchkins_type); - hal_set_bool(haldata->kinstype_is_1, 0); - hal_set_bool(haldata->kinstype_is_0, 0); - hal_set_bool(haldata->kinstype_is_2, 0); - return -1; // FAIL - } - return 0; // ok -} + return 0; +} // trsrnKinematicsSetup() -KINEMATICS_TYPE kinematicsType() +static int toolKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) { - return KINEMATICS_BOTH; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + (void)comp_id; + (void)coords; + (void)kp; + return 0; // pins created by trsrnKinematicsSetup() +} // toolKinematicsSetup() + +// tool_kins==0: TCP kinematics, using the current spindle joint positions +// tool_kins==1: TOOL kinematics, using the angles calculated in remap.py +static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) { - (void)fflags; - (void)iflags; - // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -211,20 +133,7 @@ int kinematicsForward(const double *j, // END of custom variable declaration for Forward kinematics - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - case 0: // ========================= IDENTITY kinematics FORWARD ====================== - pos->tran.x = j[0]; - pos->tran.y = j[1]; - pos->tran.z = j[2]; - pos->a = j[3]; - pos->b = j[4]; - pos->c = j[5]; - - break; - - case 1: // ========================= TCP kinematics FORWARD + if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -271,9 +180,7 @@ int kinematicsForward(const double *j, pos->b = j[4]; pos->c = j[5]; - break; - - case 2: // ========================= TOOL kinematics FORWARD + } else { // ========================= TOOL kinematics FORWARD // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -311,10 +218,6 @@ int kinematicsForward(const double *j, pos->a = j[3]; pos->b = j[4]; pos->c = j[5]; - - break; - - } // unused coordinates: pos->u = 0; @@ -322,98 +225,30 @@ int kinematicsForward(const double *j, pos->w = 0; return 0; -} // kinematicsForward() +} // trsrnForward() -// These modules do not link kins_util.c, so they cannot reach the shared -// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols. -static void frame_square_with_machine(PmRotationMatrix *rot) -{ - rot->x.x = 1; rot->y.x = 0; rot->z.x = 0; - rot->x.y = 0; rot->y.y = 1; rot->z.y = 0; - rot->x.z = 0; rot->y.z = 0; rot->z.z = 1; -} - -int kinematicsToolFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int tcpKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Sv = sin(nu*TO_RAD); - double Cv = cos(nu*TO_RAD); - double Ss = sin(j[3]*TO_RAD); - double Cs = cos(j[3]*TO_RAD); - double Sp = sin(j[5]*TO_RAD); - double Cp = cos(j[5]*TO_RAD); - double r = Cs + Sv*Sv*(1-Cs); - double s = Cs + Cv*Cv*(1-Cs); - double t = Sv*Cv*(1-Cs); - int a, b, k; - - // identity kinematics, and tool kinematics where the world axes are the - // tool axes by construction, both leave the tool square with the machine - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the primary joint turns the head about z - const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - - // the nutating secondary joint - const double Rs[3][3] = {{r, -Cv*Ss, t}, - {Cv*Ss, Cs, -Sv*Ss}, - {t, Sv*Ss, s}}; - - double M[3][3]; - for (a = 0; a < 3; a++) { - for (b = 0; b < 3; b++) { - M[a][b] = 0; - for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } - } - } - - rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; - rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; - rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; - - return 0; -} // kinematicsToolFrame() + (void)iflags; + return trsrnForward(j, pos, 0); +} // tcpKinematicsForward() -int kinematicsWorkFrame(const double *j, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int toolKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; - double Sw = sin(j[4]*TO_RAD); - double Cw = cos(j[4]*TO_RAD); - - // in tool kinematics the world axes are the tool axes, so the work is not - // being reported against the machine and there is nothing to turn - if (switchkins_type != 1) { - frame_square_with_machine(rot); - return 0; - } - - // the B joint carries the work: its frame in machine coordinates - // is a rotation about y by the joint value - const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; - - rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; - rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; - rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; - - return 0; -} // kinematicsWorkFrame() - -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ (void)iflags; - (void)fflags; + return trsrnForward(j, pos, 1); +} // toolKinematicsForward() +static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +{ // START of custom variable declaration for Forward kinematics // geometric offsets of the universal spindle head as defined in the ini file @@ -459,23 +294,7 @@ int kinematicsInverse(const EmcPose * pos, // END of custom variable declaration for Forward kinematics - // Update the kinematic joints specified by the - // [KINS]JOINTS setting (4 required for this template). - // define forward kinematic models using case structure for - // for switchable kinematics - switch (switchkins_type) { - - case 0: // ========================= IDENTITY kinematics INVERSE ====================== - j[0] = pos->tran.x; - j[1] = pos->tran.y; - j[2] = pos->tran.z; - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; - - break; - - case 1: // ========================= TCP kinematics INVERSE + if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); Cs = cos(j[3]*TO_RAD); @@ -512,9 +331,7 @@ int kinematicsInverse(const EmcPose * pos, j[4] = pos->b; j[5] = pos->c; - break; - - case 2: // ========================= TOOL kinematics INVERSE + } else { // ========================= TOOL kinematics INVERSE // in TOOL kinematics we use the articulated joint positions from the TWP Ss = sin(theta_2*TO_RAD); Cs = cos(theta_2*TO_RAD); @@ -556,38 +373,112 @@ int kinematicsInverse(const EmcPose * pos, j[3] = pos->a; j[4] = pos->b; j[5] = pos->c; + } + + return 0; +} // trsrnInverse() + +static int tcpKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 0); +} // tcpKinematicsInverse() + +static int toolKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + return trsrnInverse(pos, j, 1); +} // toolKinematicsInverse() + +// The head answers in the convention already, so the native rotation +// registered with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double nu = hal_get_real(haldata->nut_angle); // degrees + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Ss = sin(j[3]*TO_RAD); + double Cs = cos(j[3]*TO_RAD); + double Sp = sin(j[5]*TO_RAD); + double Cp = cos(j[5]*TO_RAD); + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); + int a, b, k; + + // the primary joint turns the head about z + const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}}; - break; + // the nutating secondary joint + const double Rs[3][3] = {{r, -Cv*Ss, t}, + {Cv*Ss, Cs, -Sv*Ss}, + {t, Sv*Ss, s}}; + + double M[3][3]; + for (a = 0; a < 3; a++) { + for (b = 0; b < 3; b++) { + M[a][b] = 0; + for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; } + } } + rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2]; + rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2]; + rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2]; + + return 0; +} // tcpKinematicsToolFrame() + +static int tcpKinematicsWorkFrame(const double *j, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + double Sw = sin(j[4]*TO_RAD); + double Cw = cos(j[4]*TO_RAD); + + // the B joint carries the work: its frame in machine coordinates + // is a rotation about y by the joint value + const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}}; + + rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2]; + rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2]; + rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2]; + return 0; -} // kinematicsInverse() +} // tcpKinematicsWorkFrame() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tcpKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - // the same geometry as kinematicsInverse(), read the same way + // the same geometry as trsrnInverse(), read the same way double Lx = hal_get_real(haldata->x_pivot); double Lz = hal_get_real(haldata->z_pivot); double Dx = hal_get_real(haldata->x_offset); double Dy = hal_get_real(haldata->y_offset); double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees double Dt = hal_get_real(haldata->tool_offset_z); double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); - double Stc = sin(tc); - double Ctc = cos(tc); // The TCP inverse reads the rotary angles from its joint argument, // where the machine is, and its own pose words for the same angles @@ -595,105 +486,146 @@ int kinematicsJacobian(const double *j, // against the pose, which is what a consumer multiplies by. double Sw = sin(pos->b*TO_RAD); double Cw = cos(pos->b*TO_RAD); - double Ss = 0, Cs = 0, Sp = 0, Cp = 0; - double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + double Ss = sin(pos->a*TO_RAD); + double Cs = cos(pos->a*TO_RAD); + double Sp = sin(pos->c*TO_RAD); + double Cp = cos(pos->c*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double t = Sv*Cv*(1-Cs); // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, // SvSs) and the primary angle (Sp, Cp), per degree - double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; - double dSp = 0, dCp = 0; + double dSs = Cs*TO_RAD; + double dr = -Ss*Cv*Cv*TO_RAD; + double ds = -Ss*Sv*Sv*TO_RAD; + double dt_ = Sv*Cv*Ss*TO_RAD; + double dCvSs = Cv*dSs; + double dSvSs = Sv*dSs; + double dSp = Cp*TO_RAD; + double dCp = -Sp*TO_RAD; double Qx = pos->tran.x; double Qz = pos->tran.z; - double Ax, Az; // the two lever arms the table turns about - int R, C; - - for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { - for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } - } + // the two lever arms the table turns about + double Ax = Drax + Dx + Lx - Qx; + double Az = Draz + Dt + Lz - Qz; + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + return 0; +} // tcpKinematicsJacobian() - switch (switchkins_type) { +static int toolKinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)pos; + (void)iflags; - case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== - for (R = 0; R < 6; R++) { jac[R][R] = 1; } - break; + // the head angles come from pins, so the inverse is linear in the pose + // and the rows are its coefficients + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees - case 1: // ========================= TCP kinematics JACOBIAN - Ss = sin(pos->a*TO_RAD); - Cs = cos(pos->a*TO_RAD); - Sp = sin(pos->c*TO_RAD); - Cp = cos(pos->c*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + double Ss = sin(theta_2*TO_RAD); + double Cs = cos(theta_2*TO_RAD); + double Sp = sin(theta_1*TO_RAD); + double Cp = cos(theta_1*TO_RAD); + double CvSs = Cv*Ss; + double SvSs = Sv*Ss; + double r = Cs + Sv*Sv*(1-Cs); + double s = Cs + Cv*Cv*(1-Cs); + double t = Sv*Cv*(1-Cs); - dSs = Cs*TO_RAD; - dr = -Ss*Cv*Cv*TO_RAD; - ds = -Ss*Sv*Sv*TO_RAD; - dt_ = Sv*Cv*Ss*TO_RAD; - dCvSs = Cv*dSs; - dSvSs = Sv*dSs; - dSp = Cp*TO_RAD; - dCp = -Sp*TO_RAD; - - Ax = Drax + Dx + Lx - Qx; - Az = Draz + Dt + Lz - Qz; - - // j[0]: -Cw*Ax + Az*Sw plus head terms - jac[0][0] = Cw; - jac[0][2] = -Sw; - jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; - jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; - jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) - - (CvSs*dSp - dCp*r)*Lx; - - // j[1]: Qy plus head terms - jac[1][1] = 1; - jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; - jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy - + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; - - // j[2]: -Cw*Az - Ax*Sw plus head terms - jac[2][0] = Sw; - jac[2][2] = Cw; - jac[2][3] = (Dt + Lz)*ds + Lx*dt_; - jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; - - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - - case 2: // ========================= TOOL kinematics JACOBIAN - // the head angles come from pins, so the inverse is linear in - // the pose and the rows are its coefficients - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); - jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); - jac[0][2] = (Sp*SvSs + Cp*t); + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); - jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); - jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); - jac[1][2] = -(Cp*SvSs - Sp*t); + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); - jac[2][0] = (Stc*SvSs + Ctc*t); - jac[2][1] = (Ctc*SvSs - Stc*t); - jac[2][2] = s; + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; - jac[3][3] = 1; - jac[4][4] = 1; - jac[5][5] = 1; - break; - } + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; return 0; -} // kinematicsJacobian() +} // toolKinematicsJacobian() + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what switchkinsInit() expects +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "xyzbca_trsrn"; + kp.halprefix = "xyzbca_trsrn_kins"; + kp.required_coordinates = "xyzabc"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; + kp.gui_kinstype = -1; + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, trsrnKinematicsSetup, + tcpKinematicsForward, + tcpKinematicsInverse)) { return -1; } + if (switchkinsRegister(2, toolKinematicsSetup, + toolKinematicsForward, + toolKinematicsInverse)) { return -1; } + if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, + tcpKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } + // the tool kinematics report in tool axes, so the tool is square with + // the world by construction and nothing turns the work against it + if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, + identityKinematicsToolFrame, + &TOOL_FRAME_SPINDLE)) { return -1; } + if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } + + if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } + if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() From 8e024c180995edad8ed781c1a924cc63d7510b76 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 21:02:46 +1000 Subject: [PATCH 3/5] kins: include switchkins.h as an exported header The kinematics modules are users of switchkins, not part of it, so they take the header the way any other user would. switchkins.c and switchkins_main.c keep the quoted form, being the source itself. --- src/emc/kinematics/5axiskins.c | 3 +-- src/emc/kinematics/genhexkins.c | 3 +-- src/emc/kinematics/genserkins.c | 2 +- src/emc/kinematics/pumakins.c | 3 +-- src/emc/kinematics/scarakins.c | 3 +-- src/emc/kinematics/switchkins.c | 1 - src/emc/kinematics/three21kins.c | 3 +-- src/emc/kinematics/xyzac-trt-kins.c | 2 +- src/emc/kinematics/xyzbc-trt-kins.c | 2 +- 9 files changed, 8 insertions(+), 14 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 6e7923d58f4..b2510e094ef 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,9 +59,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct haldata { hal_real_t pivot_length; diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index c634611ee62..0964fa7d088 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -110,10 +110,9 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include "genhexkins.h" -#include "switchkins.h" +#include static struct haldata { hal_real_t basex[NUM_STRUTS]; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 94a325cbcf6..be71f33ffc5 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -42,7 +42,7 @@ frame-larger-than: #include #include "genserkins.h" -#include "switchkins.h" +#include //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index aace42d7b37..b63cbb64c5a 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -20,10 +20,9 @@ #include #include #include -#include #include "pumakins.h" -#include "switchkins.h" +#include struct haldata { hal_real_t a2, a3, d3, d4, d6; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index db3ad15b737..e2dedb241b6 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -19,9 +19,8 @@ #include #include #include -#include -#include "switchkins.h" +#include static struct scara_data { hal_real_t d1, d2, d3, d4, d5, d6; diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index a9fa9027cd5..472394cefdd 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -29,7 +29,6 @@ #include #include #include -#include #include "switchkins.h" diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 30c7f938e15..5cce5796a30 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -2,9 +2,8 @@ #include #include #include -#include -#include "switchkins.h" +#include /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b8bb47bbc1f..b6b35538f25 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 7b61a69e301..401311e4398 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -15,7 +15,7 @@ #include #include -#include "switchkins.h" +#include int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, From e6405a0828b50188f7de08ccdd34d413fa0e6d33 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 12 Aug 2026 11:05:02 +1000 Subject: [PATCH 4/5] switchkins: a core module and a template for out-of-tree kinematics An out-of-tree module could not reach the switchkins implementation, so it reimplemented kinematicsSwitch() and the kinstype.is-N pins or did without. A realtime module cannot link a library, and including the implementation as source puts C files and an extra include path in every module build. switchkins_core is the implementation as a module of its own: switchkins.c and kins_util.c with no kinematics types. switchkins.c already exports its interface, since motion reaches the kinematics entry points in another module; kins_util.c now exports the identity kinematics and helpers kinematics.h declares. A module loaded after switchkins_core includes , registers its types and calls switchkinsInit() from EXTRA_SETUP(); motion finds the kinematics entry points in switchkins_core. The in-tree modules keep linking the objects, so their configs are unchanged. switchkinscomp.comp is the template, built in tree like any other component. tests/kins-switchkins-core loads it behind switchkins_core, homes, moves in both of its types, switches between them and has a type it does not provide refused. Renamed and built with halcompile out of tree it loads the same way. --- docs/src/hal/components.adoc | 1 + docs/src/motion/switchkins.adoc | 84 ++++++++++++- src/Makefile | 7 ++ src/emc/kinematics/kins_util.c | 19 +++ src/emc/kinematics/switchkins_core.c | 43 +++++++ src/hal/components/switchkinscomp.comp | 159 +++++++++++++++++++++++++ tests/kins-switchkins-core/README | 6 + tests/kins-switchkins-core/checkresult | 3 + tests/kins-switchkins-core/sim.hal | 16 +++ tests/kins-switchkins-core/test-ui.py | 107 +++++++++++++++++ tests/kins-switchkins-core/test.ini | 85 +++++++++++++ tests/kins-switchkins-core/test.sh | 3 + tests/kins-switchkins-core/tool.tbl | 1 + 13 files changed, 532 insertions(+), 2 deletions(-) create mode 100644 src/emc/kinematics/switchkins_core.c create mode 100644 src/hal/components/switchkinscomp.comp create mode 100644 tests/kins-switchkins-core/README create mode 100755 tests/kins-switchkins-core/checkresult create mode 100644 tests/kins-switchkins-core/sim.hal create mode 100755 tests/kins-switchkins-core/test-ui.py create mode 100644 tests/kins-switchkins-core/test.ini create mode 100755 tests/kins-switchkins-core/test.sh create mode 100644 tests/kins-switchkins-core/tool.tbl diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index f70961dab05..ebc72ab51ac 100644 --- a/docs/src/hal/components.adoc +++ b/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 || diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 98fe2f3b136..e0a4586ffbd 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -430,6 +430,12 @@ configs/sim/axis/vismach/ . == 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 @@ -447,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 <> 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 @@ -471,6 +528,7 @@ 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 @@ -570,8 +628,30 @@ 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: +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 +<>). 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 diff --git a/src/Makefile b/src/Makefile index 93a3cbeabc2..b00a222f23e 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1237,6 +1237,12 @@ 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 @@ -1409,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)) diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 27773c9edb1..313a4acca20 100644 --- a/src/emc/kinematics/kins_util.c +++ b/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); diff --git a/src/emc/kinematics/switchkins_core.c b/src/emc/kinematics/switchkins_core.c new file mode 100644 index 00000000000..001fb9cc0ea --- /dev/null +++ b/src/emc/kinematics/switchkins_core.c @@ -0,0 +1,43 @@ +/* + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/* switchkins_core.c makes switchkins.c and kins_util.c a module of their +* own, for kinematics modules built out of tree. It provides no +* kinematics types itself: the module loaded after it registers its types +* with switchkinsRegister() and calls switchkinsInit(), both exported with +* the rest of the switchkins and kins_util interface, and motion reaches the +* kinematics through the entry points exported here. +* +* loadrt switchkins_core +* loadrt [KINS]KINEMATICS +*/ +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + comp_id = hal_init("switchkins_core"); + if (comp_id < 0) return comp_id; + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp new file mode 100644 index 00000000000..a732ba0de35 --- /dev/null +++ b/src/hal/components/switchkinscomp.comp @@ -0,0 +1,159 @@ +component switchkinscomp "switchable kinematics module template"; +// NOTE: component name must agree with filename + +description """ +Example of a switchable kinematics module buildable with halcompile. + +The switchkinscomp.comp file (src/hal/components/switchkinscomp.comp) +illustrates a method to use halcompile to build a kinematics module +on top of the switchkins implementation used by the in-tree kinematics +modules, so an out-of-tree module gets the same kinematics switching, +the same 'kinstype.is-N' pins, the same 'coordinates=' identity +mapping, and the same G-code and HAL controls, without reimplementing +any of it. + +The example builds as it stands, its type 1 being an X offset to +replace with the kinematics wanted. The switchkins implementation is +the switchkins_core module, which the HAL file loads first; this +component only registers its types with it. + +To avoid updates that overwrite switchkinscomp.comp, best practice is +to rename the file and its component name (example: +*user_switchkins.comp* creates module: *user_switchkins*). + +The (renamed) component can be built and installed with halcompile +and then used as the kinematics module by inifile setting: + +[source,ini] +---- +[KINS] +KINEMATICS = user_switchkins +JOINTS = 3 +---- + +with switchkins_core loaded ahead of it in the HAL file: + +---- +loadrt switchkins_core +loadrt [KINS]KINEMATICS +---- + +*Note:* If using a deb install: + +1. halcompile and the switchkins header are provided by the deb + package linuxcnc-dev +2. This source file for BRANCHNAME (master, 2.9, etc) is downloadable from github: + +https://github.com/LinuxCNC/linuxcnc/blob/BRANCHNAME/src/hal/components/switchkinscomp.comp + +For information on switchable kinematics see the switchkins document +chapter (docs/src/motion/switchkins.txt). +"""; + +pin out bool is_module=1; //one pin is required to use halcompile + +license "GPL"; +option extra_setup; +;; + +//===================================================================== +// The switchkins implementation is the switchkins_core module, loaded +// before this one. It provides kinematicsForward(), kinematicsInverse(), +// kinematicsSwitch() and the rest of the kinematics interface, dispatches +// each call to the currently selected switchkins-type, and supplies the +// identity kinematics and the coordinates letters-to-joints mapping they +// use. This module only registers its types. + +#include +//===================================================================== + +// module parameter naming the joint order for the identity type +static char *coordinates; +RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); + +//--------------------------------------------------------------------- +// Example switchkins-type. A setup routine creating whatever hal pins +// the kinematics need, plus a forward and an inverse routine. Replace +// the arithmetic with the real kinematics. + +static struct { + hal_real_t x_offset; +} *mydata; + +static int myKinematicsSetup(const int comp_id, + const char* coords, + kparms* kp) +{ + (void)coords; // this type does not use the coordinates mapping + + mydata = hal_malloc(sizeof(*mydata)); + if (!mydata) return -1; + + return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, + "%s.x-offset", kp->halprefix); +} // myKinematicsSetup() + +static int myKinematicsForward(const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + + pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.y = j[1]; + pos->tran.z = j[2]; + + // unused coordinates: + pos->a = pos->b = pos->c = 0; + pos->u = pos->v = pos->w = 0; + + return 0; +} // myKinematicsForward() + +static int myKinematicsInverse(const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[1] = pos->tran.y; + j[2] = pos->tran.z; + + return 0; +} // myKinematicsInverse() + +//--------------------------------------------------------------------- +// rtapi_app_main() is supplied by halcompile, which calls hal_init() +// before EXTRA_SETUP() and hal_ready() after it. That is what +// switchkinsInit() expects, so the switchkins-types are registered and +// the implementation started from here. + +EXTRA_SETUP() { + kparms kp; + (void)__comp_inst; (void)prefix; (void)extra_arg; + + kp.kinsname = "switchkinscomp"; // must agree with the module name + kp.halprefix = "switchkinscomp"; // hal pin names + kp.required_coordinates = "xyz"; + kp.allow_duplicates = 0; + kp.fwd_iterates_mask = 0; // set bit N if type N iterates + kp.gui_kinstype = -1; // negative means: not used + kp.sparm = NULL; + kp.max_joints = strlen(kp.required_coordinates); + + // switchkins-type 0 is the startup default. Types run from 0 to + // SWITCHKINS_MAX_TYPES-1 with no gaps. + if (switchkinsRegister(0, identityKinematicsSetup, + identityKinematicsForward, + identityKinematicsInverse)) { return -1; } + if (switchkinsRegister(1, myKinematicsSetup, + myKinematicsForward, + myKinematicsInverse)) { return -1; } + + return switchkinsInit(comp_id, &kp, coordinates); +} // EXTRA_SETUP() diff --git a/tests/kins-switchkins-core/README b/tests/kins-switchkins-core/README new file mode 100644 index 00000000000..48fa6fe5d91 --- /dev/null +++ b/tests/kins-switchkins-core/README @@ -0,0 +1,6 @@ +A kinematics module built on the switchkins_core module instead of +linking the switchkins implementation: switchkinscomp, the template for +out-of-tree modules, registers an identity type and an X offset type and +takes everything else from switchkins_core, loaded ahead of it. It +homes, runs in both types, switches between them, and refuses a type it +does not have. diff --git a/tests/kins-switchkins-core/checkresult b/tests/kins-switchkins-core/checkresult new file mode 100755 index 00000000000..9d48d3f180e --- /dev/null +++ b/tests/kins-switchkins-core/checkresult @@ -0,0 +1,3 @@ +#!/bin/sh +# the test script counts its own failures +grep -q "^Exiting with 0 errors" "$1" diff --git a/tests/kins-switchkins-core/sim.hal b/tests/kins-switchkins-core/sim.hal new file mode 100644 index 00000000000..3c964260c45 --- /dev/null +++ b/tests/kins-switchkins-core/sim.hal @@ -0,0 +1,16 @@ +loadrt switchkins_core +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 + +setp switchkinscomp.x-offset 5 + +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 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 diff --git a/tests/kins-switchkins-core/test-ui.py b/tests/kins-switchkins-core/test-ui.py new file mode 100755 index 00000000000..2e6595199f1 --- /dev/null +++ b/tests/kins-switchkins-core/test-ui.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 +# A kinematics module on switchkins_core: switchkinscomp registers identity +# as type 0 and an X offset of 5 as type 1, and switchkins_core does the rest. +import hal +import linuxcnc +import sys +import time + +c = linuxcnc.command() +s = linuxcnc.stat() +e = linuxcnc.error_channel() + +errors = 0 + +def error(what): + global errors + errors += 1 + print("*** ERROR %s" % what) + +def kins_type(): + return int(hal.get_value("motion.kins-type")) + +def position(): + s.poll() + return tuple(round(v, 6) for v in s.position[:3]) + +def joints(): + s.poll() + return tuple(round(v, 6) for v in s.joint_position[:3]) + +def drain(): + said = [] + while True: + m = e.poll() + if not m: + return said + said.append(m[1]) + +def wait_idle(): + deadline = time.time() + 30 + while time.time() < deadline: + s.poll() + if s.interp_state == linuxcnc.INTERP_IDLE and not s.queue: + return + time.sleep(0.05) + error("timed out waiting for the interpreter") + +def mdi(cmd): + c.mdi(cmd) + c.wait_complete(30) + wait_idle() + time.sleep(0.2) # let the error channel and the status catch up + +def check(what, got, want): + if got != want: + error("%s is %s, expected %s" % (what, got, want)) + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.wait_complete(30) +c.home(-1) +c.wait_complete(60) +c.mode(linuxcnc.MODE_MDI) +c.wait_complete(30) +drain() + +# the pins switchkins_core creates on the module's own component +check("kinematics type at start", kins_type(), 0) +check("kinstype.is-0", hal.get_value("kinstype.is-0"), True) +check("kinstype.is-1", hal.get_value("kinstype.is-1"), False) + +# identity: joints and coordinates are the same thing +mdi("G0 X10 Y2 Z3") +check("joints in identity", joints(), (10, 2, 3)) +check("position in identity", position(), (10, 2, 3)) + +# type 1 puts X at joint 0 plus the offset, the joints stay where they are +mdi("G12.1 P1") +check("kinematics type after G12.1 P1", kins_type(), 1) +check("kinstype.is-1", hal.get_value("kinstype.is-1"), True) +check("joints after the switch", joints(), (10, 2, 3)) +check("position after the switch", position(), (15, 2, 3)) + +# and moves through the module's inverse +mdi("G0 X20") +check("joints after G0 X20 in type 1", joints(), (15, 2, 3)) +check("position after G0 X20 in type 1", position(), (20, 2, 3)) + +# back to identity +mdi("G12.1 P0") +check("kinematics type after G12.1 P0", kins_type(), 0) +check("position back in identity", position(), (15, 2, 3)) +said = drain() +if said: + error("unexpected messages: %s" % said) + +# a type the module does not have is refused and changes nothing +mdi("G12.1 P2") +said = drain() +if not said: + error("no report of the refused switch to type 2") +check("kinematics type after G12.1 P2", kins_type(), 0) +check("position after the refused switch", position(), (15, 2, 3)) + +print("Exiting with %d errors" % errors) +c.state(linuxcnc.STATE_ESTOP) +sys.exit(1 if errors else 0) diff --git a/tests/kins-switchkins-core/test.ini b/tests/kins-switchkins-core/test.ini new file mode 100644 index 00000000000..29a4e5067a4 --- /dev/null +++ b/tests/kins-switchkins-core/test.ini @@ -0,0 +1,85 @@ +[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] +# switchkins-type 0 is identity, 1 the template's X offset +KINEMATICS = switchkinscomp +JOINTS = 3 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZ +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 + +[JOINT_0] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +MIN_LIMIT = -500 +MAX_LIMIT = 500 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 800 +HOME_SEARCH_VEL = 0 +HOME_SEQUENCE = 0 diff --git a/tests/kins-switchkins-core/test.sh b/tests/kins-switchkins-core/test.sh new file mode 100755 index 00000000000..079fb121f31 --- /dev/null +++ b/tests/kins-switchkins-core/test.sh @@ -0,0 +1,3 @@ +#!/bin/bash -e +rm -f sim.var sim.var.bak +linuxcnc -r test.ini diff --git a/tests/kins-switchkins-core/tool.tbl b/tests/kins-switchkins-core/tool.tbl new file mode 100644 index 00000000000..d793e2d60ed --- /dev/null +++ b/tests/kins-switchkins-core/tool.tbl @@ -0,0 +1 @@ +T1 P1 D0.0 Z12.5 ; From b2038e52923827d722bba3a83b8774916abda55e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 25 Aug 2026 01:03:33 +1000 Subject: [PATCH 5/5] saicanon: report a kinematics switch through the canon macro saicanon.cc printed SELECT_KINS_TYPE with a raw printf, beside the canon output on stdout. saicanon exists to echo the canonical commands, so it now reports through the same macro as the rest of the file, with a line number and the argument. --- src/emc/sai/saicanon.cc | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/emc/sai/saicanon.cc b/src/emc/sai/saicanon.cc index d60d661d4f7..4b65cf77d6c 100644 --- a/src/emc/sai/saicanon.cc +++ b/src/emc/sai/saicanon.cc @@ -1223,8 +1223,5 @@ void UPDATE_TAG(const StateTag& /*tag*/){ void SELECT_KINS_TYPE(int switchkins_type) { - (void)switchkins_type; - printf("saicanon: SELECT_KINS_TYPE\n"); - - return; + ECHO_WITH_ARGS("%d", switchkins_type); }