diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal index c45e7c5765b..3780f9ce3d0 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal @@ -17,7 +17,6 @@ net :gui-pivot-len <= 5axisgui.pivot_len net :gui-pivot-len => 5axiskins.pivot-length net :tool-len <= motion.tooloffset.z -net :tool-len => 5axiskins.tool-length net :tool-len => 5axisgui.tool_length net :tool-diam <= halui.tool.diameter diff --git a/configs/sim/axis/vismach/5axis/max5/max5kins.hal b/configs/sim/axis/vismach/5axis/max5/max5kins.hal index 85b9d37c2be..04b5dc47843 100644 --- a/configs/sim/axis/vismach/5axis/max5/max5kins.hal +++ b/configs/sim/axis/vismach/5axis/max5/max5kins.hal @@ -16,9 +16,7 @@ loadusr -W ./max5gui.py # set a visible tool setp max5gui.tool-radius 3 -# the tool length is applied along the tool, not along Z, so the tip stays -# on the programmed point as B tilts -net tool-len motion.tooloffset.z max5gui.tool-length maxkins.tool-length +net tool-len motion.tooloffset.z max5gui.tool-length # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/debian/linuxcnc.install.in b/debian/linuxcnc.install.in index d66045b43a5..a71302665a0 100644 --- a/debian/linuxcnc.install.in +++ b/debian/linuxcnc.install.in @@ -36,6 +36,7 @@ usr/bin/hy_vfd usr/bin/image-to-gcode usr/bin/inivalue usr/bin/inivar +usr/bin/kinslimits usr/bin/latency-histogram usr/bin/latency-plot usr/bin/latency-test diff --git a/docs/src/code/code-notes.adoc b/docs/src/code/code-notes.adoc index dc81d1257e9..e3b72e492a9 100644 --- a/docs/src/code/code-notes.adoc +++ b/docs/src/code/code-notes.adoc @@ -1312,8 +1312,7 @@ settings.tool_offset:: + * Used to compute position in various places. * Sent to Motion via the +EMCMOT_SET_OFFSET+ message. - All motion does with the offsets is export them to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+. - FIXME: export these from someplace closer to the tool table (io or interp, probably) and remove the EMCMOT_SET_OFFSET message. + Motion exports the offsets to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+ and hands them to the kinematics module through +kinematicsSetTool()+, for a module whose maths needs the tool length. settings.pockets_max:: Used interchangeably with +CANON_POCKETS_MAX+ (a #defined constant, set to 1000 as of April 2020). diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index f2bd1051d4c..79a3a116e21 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -252,9 +252,10 @@ replacing it. Put a given length in one column or the other, not both. *maxkins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B tilts. Net it from *motion.tooloffset.z*. - Left unconnected, the tool length stays where canon put it, along machine - Z, which is only correct at B0. + stays on the programmed point as B tilts. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. === pentakins - Pentapod Kinematics @@ -413,12 +414,13 @@ expected by it (XYZBCW `->` joints 0..5) *5axiskins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B and C move. Net it from - *motion.tooloffset.z*. Left unconnected, the tool length stays where canon - put it, along machine Z, which is only correct at B0. A tool length in the - W column of the tool table reaches the same place, once a block commands W, - and adds to this pin rather than replacing it. Put a given length in one - column or the other, not both. + stays on the programmed point as B and C move. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. A tool length in the W column of the tool + table reaches the same place, once a block commands W, and adds to this + one rather than replacing it. Put a given length in one column or the + other, not both. == SEE ALSO diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 9391f8611a9..1c6e0fe406c 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -317,23 +317,17 @@ See the simulation INI files for details of the HAL connections used for the vis === Tool-Length Compensation -In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, LinuxCNC has a variable called "motion.tooloffset.z". If this variable is passed on to the kinematic component (and vismach python script), then the necessary additional Z-offset for a new tool can be accounted for by adding the component statement, for example: +In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, the kinematics accounts for the tool length in effect, for example: image::5-axis-figures/equation__38.png[align="center"] -The required HAL connection (for xyzac-trt) is: +Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. -[source,hal] ----- -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset ----- - -where: +Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: +[source,hal] ---- -:tool-offset ---------------- signal name -motion.tooloffset.z --------- output HAL pin from LinuxCNC motion module -xyzac-trt-kins.tool-offset -- input HAL pin to xyzac-trt-kins +net :tool-offset motion.tooloffset.z xyzac-trt-gui.tool-offset ---- == Custom Kinematics Components @@ -383,17 +377,18 @@ KINEMATICS = kinsname where "kinsname" is the name of your kins program. Additional HAL pins may be created by the module for variable configuration items -such as the D~x~, D~y~, D~z~, tool-offset used in the xyzac-trt kinematics module. +such as the D~x~, D~y~, D~z~ used in the xyzac-trt kinematics module. These pins can be connected to a signal for dynamic control or set once with HAL connections like: [source,hal] ---- # set offset parameters -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset setp xyzac-trt-kins.y-offset 0 setp xyzac-trt-kins.z-offset 20 ---- +The tool length is not among them: motion hands it to the module from the tool table. + == Figures .Table tilting/rotating configuration diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 5740fbc7012..02446cf425a 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -178,7 +178,7 @@ half turn about one of the two transverse axes, and which one is chosen decides where tool X lands. Because it is a rotation in its own right, a module declares it rather than -applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +applying it by hand, in the `native` field of its ops table. Shared code applies it and checks once, at load, that it is orthonormal with determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a @@ -289,6 +289,8 @@ All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +<> is how a module is written so that they +can call it. [[sec:orientation-inverse]] == The Orientation Inverse @@ -438,28 +440,28 @@ every consumer would otherwise guess it. === What a module has to supply -Nothing. If a module registers no Jacobian, the shared code computes one by +Nothing. If a module supplies no Jacobian, the shared code computes one by central differences: it calls the module's inverse eighteen times, stepping each pose coordinate a small amount to either side on the solution branch the inverse flags select, and differences the results. The answer is as precise as the inverse itself; behind an inverse that iterates, that is the -iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that -registers nothing; an identity type answers exactly. +iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type +whose ops table has no Jacobian; an identity type answers exactly. This cost matters because `kinematicsJacobian()` is meant to run in the servo thread, where checking a feed limit calls it once per cycle: each call then costs eighteen inverses. On the closed-form inverses in the tree that is under two microseconds. `genserkins`, the one module whose inverse iterates, would cost near eighty microseconds per call this way, against -under two for the geometric Jacobian it registers instead. +under two for the geometric Jacobian it supplies instead. -So a module whose inverse iterates should register a closed-form Jacobian, -as below. Nothing enforces this. A module that registers nothing still gets +So a module whose inverse iterates should supply a closed-form Jacobian, +as below. Nothing enforces this. A module that supplies none still gets a correct Jacobian, just one that costs eighteen inverses and carries the iteration's precision, which behind an iterating inverse is too slow for the servo thread. -A module with a closed form registers it with `switchkinsRegisterJacobian()`. +A module with a closed form puts it in the `jacobian` field of its ops table. It is exact, it costs what the inverse costs, and it knows its own singular poses rather than discovering them as an inverse that fails a step away from the pose. Every module in the tree supplies one. The two arms whose inverse is @@ -476,6 +478,100 @@ rather than from the pose, which the nutating heads do, has an inverse whose derivative about the pose is not the coupling the machine has. Such a module supplies the closed form, taken against the pose. +[[sec:parameters]] +== The Parameter Block + +Everything above is a function of the joint values, the tool and the machine's +geometry. A module written the old way reads its geometry from HAL pins it +created, keeps its kinematics type and its iteration scratch in statics, and so +can only answer for the machine as it is now, from inside the realtime thread. +Anything else that needs the same maths, a planner evaluating poses the machine +has not reached, task checking a program at load, a tool asking what if, had to +carry a second copy of it, and the two copies drift. + +A module is written instead as functions of two blocks the caller supplies. +`kins_params` describes the machine: the kinematics type, the joint map from +`coordinates=`, the tool offset, and the geometry as an array of doubles. One +copy may be shared by any number of callers, since nothing writes it during a +call. `kins_scratch` is what one caller carries between its own calls: the pose +an iterating forward last found, which seeds the next, and what the module +reports about the call it just made. It is never shared between callers, so +motion and a planner evaluating the same module cannot disturb each other. + +=== The table + +A module declares its geometry as a table of named entries, one per value it +reads. The name is the pin name the config already uses, less the module +prefix, so nothing in a config changes. + +[source,c] +---- +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; +---- + +An entry is an input, an output, or an input that can be poked (`KINS_IO`, a +`HAL_IO` pin). The maths reads `p->geometry[P_PIVOT_LENGTH]` where it read a +pin, and writes an output into `s->out[]` at the same index. An entry flagged +as the tool is the tool length along the tool axis; the shared code puts its +value in `p->tool.tran.z` as well, which is what the maths reads, so that a +caller outside realtime can supply the tool from the tool table without there +being a pin. + +=== The ops table + +The maths of one kinematics type is a `kins_ops` table: the forward and inverse, +the optional work and tool frames with the native rotation that relates the +tool frame to the convention, and the optional Jacobian. A type whose forward +iterates from the pose it is handed says so, and the shared code seeds it with +the last answer after a switch. A type also says what it IS: `identity` marks +the no-transform type `G13.1` cancels to, `primary` the working transform +`G43.4` switches to (see the Switchable Kinematics chapter). A module with +several types has one geometry table and one ops table per type, registered +with `switchkinsRegisterOps()`; a module with one type describes itself in a +`kins_module` and links `kins_single.c`. + +=== What the shared code does + +In realtime it makes one HAL pin per table entry, copies the pins into the +block before every call and the outputs back after it, and supplies the classic +entry points, `kinematicsForward()` and the rest, so that motion sees no +difference. It also exports `kinematicsSetTool()`, through which motion hands +the module the tool offset in effect whenever that changes, so the tool comes +from the tool table and not from a net the config had to remember. A table +entry flagged as the tool is read from its pin only until motion has sent +anything; after that the pin is overwritten with motion's value, and the shared +code says once if the pin is left disagreeing with it, which is a config +setting a tool length where the tool table should. Motion references the call +weakly, so a module written before it still loads and keeps its pin. + +Outside realtime a module exports `kinsDescribe()`, which hands a caller its +table and the ops of each type; the caller fills a block from wherever it likes +and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, +`kinsOpsJacobian()` and the frame calls, with the same defaults applied, so +both sides get the same answers. The non-realtime loader in +`kinematics_userspace/` binds the pins of the running module by the table's +names. Its tool is the caller's where the caller gives one through +`kinematicsUserSetTool()`, since a planner knows what a segment runs under +better than the machine does; otherwise it is motion's, from motion's own +offset pins, and the loader says once when the module's tool pin disagrees with +them. `kinslimits` is built on it. + +A module that does not provide the form keeps working as it did. It just cannot +be evaluated outside realtime, which the loader reports. + +=== What stays outside the block + +The kinematics type is in the block, so a caller evaluating a program that +switches type puts the type each block will run under in its own block, and +nothing is switched globally. The tool is in the block, from motion in realtime +and from the caller outside it. The joint map is in the block, from +`coordinates=`. Nothing else the maths needs exists, +and a module that finds it needs something else has found a parameter it should +declare. + [[sec:writing-a-module]] == Writing a Module @@ -516,6 +612,13 @@ Geometry stays in the module:: the module. A consumer that restates it has taken a copy that nothing keeps in step, which is the situation this chapter exists to end. +No state:: + Write the maths as functions of the parameter block and the scratch, as + <> describes: geometry in the table, + the kinematics type and the tool from the block, and anything carried + between calls in the scratch. A static in a module is a second machine + that only the realtime thread can see. + Mount orientation is not this:: A tool or holder mount orientation is a different quantity: a right-angle head, a tool held at an angle, an end effector clocked on its flange. Those diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..32b87d51a4a 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -320,9 +320,12 @@ See <>. [NOTE] The majority of provided kinematics modules support a single -kinematics type and use the directive "*KINS_NOT_SWITCHABLE*" to -supply defaults for the required kinematicsSwitchable() and -kinematicsSwitch() functions. +kinematics type; they are written on the parameter block and link +'kins_single.c', which supplies these entry points and answers "not +switchable" for them. A single-type module written without the +parameter block can use the directive "*KINS_NOT_SWITCHABLE*" to +supply defaults for kinematicsSwitchable(), kinematicsSwitch() and +kinematicsTypeFlags(). ---- int kinematicsHome(EmcPose *world, double *joint, diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index e0a4586ffbd..edb93924ae2 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -181,8 +181,9 @@ meant to be removed in the future. === G-code commands 'G12.1 P-' selects a kinstype and 'G13.1' cancels back to identity -kinematics. Which kinstype is identity is declared by the module (see -Code Notes), not fixed to a number: +kinematics. Which kinstype that is, the module declares (see Code +Notes), not fixed to a number; on a module whose slides do not line up +with its frame it is the machine frame type the module declares instead: [source,ngc] ---- @@ -218,9 +219,9 @@ Selection is not cancelled by the end of a program or by an abort, so that the kinstype continues to match the position readout. A program that should leave the machine in identity kinematics ends with 'G13.1'. -A module that declares no identity kinstype refuses 'G13.1' with an -error and can still be driven by number with 'G12.1'; see Code Notes -for how a module declares its types. +A module that declares no identity or machine frame kinstype refuses +'G13.1' with an error and can still be driven by number with 'G12.1'; +see Code Notes for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length @@ -440,7 +441,8 @@ 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 directory and edited to supply custom kinematics with -kinstype==2. +kinstype==2: the in-tree modules register its USERK_OPS as that +kinstype, so the forward and inverse in the copy are what runs. The user custom kinematics file can be compiled from out-of-tree source locations for rt-preempt implementations or by replacing @@ -463,13 +465,14 @@ it gets the kinematics switching, the 'kinstype.is-N' pins, the 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. +kinstypes of its own; it exports switchkinsInit() and the rest of the +switchkins.h interface, and the identity kinematics and helpers +kins_module.h declares. A module of your own includes switchkins.h, +supplies switchkinsSetup() and a kinsDescribe() that replays it through +switchkinsDescribeWith(), and calls switchkinsRunSetupWith() and +switchkinsInit() from EXTRA_SETUP(), which halcompile runs after +hal_init() and before hal_ready(). See +<> for the calls. The template is src/hal/components/switchkinscomp.comp. Copy and rename it (both the file and the component name) and replace the @@ -539,17 +542,26 @@ 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: +A kinstype is supplied by calling switchkinsRegisterOps(), once per +kinstype, with the maths of that type written as functions of the +parameter block (see the Kinematics Conventions chapter): + +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- + +The geometry of the whole module is one table, named in the kparms +fields 'params' and 'nparams'; every kinstype reads it from the block. +The older form, switchkinsRegister() with a setup, forward and inverse +routine per kinstype that read pins of their own, is still accepted: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.h). A kinstype has to come from one route or the -other, so registering one that switchkinsSetup() has already -filled in is an error, and so is leaving a gap below the highest +kinematics.h). Registering a kinstype twice, by +either route, is an error, and so is leaving a gap below the highest kinstype provided. Either mistake fails the module load and says which kinstype is at fault. @@ -561,26 +573,37 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform +. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, + the rotaries as joints A kinstype registered with switchkinsRegisterOps() carries its flag in -the ops table itself, as the 'identity' or 'primary' field; a kinstype -registered the older way gets it from a call, again from within +the ops table itself, as the 'identity', 'primary' or 'machine' field; a +kinstype registered the older way gets it from a call, again from within switchkinsSetup(): ---- int switchkinsDeclare(int ktype, int flags); ---- -G-code reads these declarations: 'G13.1' cancels to the kinstype -declared KINSTYPE_IDENTITY, and 'G43.4' switches to the kinstype +G-code reads these declarations: 'G13.1' and 'G49' cancel to the +kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics are not in the conventional order still gets working -spellings. At most one kinstype may be declared identity and at most -one primary, and declaring a kinstype the module does not provide -fails the module load. A module that declares nothing keeps working -exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' -are an error, since the numbers of the identity and primary kinematics -are then a guess. +spellings. The machine frame type is the machine with the orientation +left out, XYZ the pivot in machine coordinates and the rotary letters +the rotary joints; on a machine whose slides line up with its frame that +is the identity type, so a module that declares no machine frame +kinstype has its identity kinstype stand in, and every shipped module is +of that kind. A module whose carriage does not line up, a slanted slide +or a composite Y on a mill-turn, declares its machine frame kinstype +separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints +really are the axes, since motion and the planner skip the maths on that +flag alone. At most one kinstype may be declared identity, at most one +primary and at most one machine frame, and declaring a kinstype the +module does not provide fails the module load. A module that declares +nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but +'G13.1' and 'G43.4' are an error, since the numbers of the identity and +primary kinematics are then a guess. A module that implements kinematicsTypeFlags() itself, without switchkins.c, returns the flags of every kinstype it can switch to, 0 @@ -593,15 +616,21 @@ 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. +which checks the supplied parameters, creates the HAL pins, the +table's among them, selects kinstype 0, and then invokes the setup +routine of each kinstype registered the older way. 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. +A module built this way also exports kinsDescribe(), through which a +copy of it loaded outside realtime learns its table and the maths of +each kinstype; the non-realtime loader and kinslimits use it. It +exports kinematicsSetTool() as well, through which motion hands it +the tool offset in effect whenever that changes. A table entry +flagged as the tool is overwritten with it, and the entry's pin only +matters until motion has sent anything, so a config need not net +motion.tooloffset.z to the module. A kinstype registered the older +way reads its own pins and is not affected. === Module main program @@ -618,53 +647,90 @@ int switchkinsSetup(kparms* kp, 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. +which sets the configuration settings, names the geometry table and +registers the kinstypes with switchkinsRegisterOps(). The pointer +arguments are the older route for kinstypes 0,1,2; a module using +them leaves the rest alone. switchkinsRunSetup() in +switchkins_setup.c is what runs switchkinsSetup() and registers what +it returned, for the 'main' program and for kinsDescribe() alike. 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(): +from halcompile instead. It supplies the same switchkinsSetup() and +a kinsDescribe() that replays it, and from its EXTRA_SETUP() routine, +which halcompile runs after hal_init() and before hal_ready(), runs +switchkinsSetup() and then switchkinsInit(): [source,c] ---- +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} + +EXPORT_SYMBOL(kinsDescribe); + 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; } + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } ---- +switchkinsRunSetupWith() and switchkinsDescribeWith() take the +module's switchkinsSetup() as an argument, so they work from +switchkins_core. switchkinsRunSetup() and the kinsDescribe() in +switchkins_setup.c call the switchkinsSetup() they were linked with, +and only a module built in tree links them. + 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: +own>>). In tree, the component links the objects into itself instead, +named in hal/components/Submakefile: ---- -millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o ---- === Outline -The two routes in one switchkinsSetup(), with the kinematics itself -left out. Types 0 to 2 are filled in through the pointer arguments as -they always were, and a fourth is registered: +A switchkinsSetup() with the kinematics itself left out: the table, +the ops table of the machine's own kinstype, and the shared identity +and userk ops for the other two: [source,c] ---- +static const kins_param_desc my_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 100.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, // the tool length +}; +enum { P_PIVOT_LENGTH, P_TOOL_OFFSET }; + +static int my_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double pivot = p->geometry[P_PIVOT_LENGTH]; // where a pin was read + double tool = p->tool.tran.z; // the tool, from wherever the caller has it + // ... +} + +static int my_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +static const kins_ops my_ops = { + .forward = my_forward, + .inverse = my_inverse, + // .work, .tool, .native and .jacobian are optional, see kins_module.h +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -675,37 +741,24 @@ int switchkinsSetup(kparms* kp, kp->halprefix = "mykins"; // hal pin names kp->required_coordinates = "xyzab"; kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); // remaining kparms fields - *kset0 = identityKinematicsSetup; // kinstype 0 is the startup default - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = myKinematicsSetup; - *kfwd1 = myKinematicsForward; - *kinv1 = myKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; - - // any further kinstype comes from switchkinsRegister(), and the - // numbering carries on from the three above with no gaps - if (switchkinsRegister(3, myOtherKinematicsSetup, - myOtherKinematicsForward, - myOtherKinematicsInverse)) { return -1; } + switchkinsRegisterOps(0, &my_ops); // kinstype 0 is the startup default + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); + // any further kinstype is registered the same way, and the + // numbering carries on with no gaps return 0; } // switchkinsSetup() ---- -A module wanting fewer than three kinstypes leaves the unused pointer -arguments alone and starts registering at the first free number. - For the surrounding shape, the in-tree switchkinsSetup() routines are in src/emc/kinematics: 5axiskins.c, xyzac-trt-kins.c, genserkins.c, scarakins.c and the others listed at the top of this document. None of -them registers a fourth kinstype yet, so the call above has no in-tree +them registers a fourth kinstype yet, so a call for one has no in-tree example to copy. // vim: set syntax=asciidoc: diff --git a/include/kinematics.h b/include/kinematics.h index 4820b806833..f19a8dba1e0 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -107,6 +107,39 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* Switchable kinematics: a module provides several kinematics, numbered +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. The +** count is here because motion and the NML status channel need it. +*/ +#define SWITCHKINS_MAX_TYPES 9 + +/* What a kinematics type IS, declared by the module with +** switchkinsDeclare() and read back with kinematicsTypeFlags(). +** G13.1 resolves the machine frame type from these flags instead of +** assuming a number; a module that declares nothing leaves its types +** numeric-only and G13.1 refuses to guess. +** +** The machine frame type is the one whose world is the machine frame +** with the orientation left out: XYZ is the pivot, the point the rotary +** joints do not move, in machine coordinates, and the rotary letters are +** the rotary joints as they are. On a machine whose slides line up with +** its frame that is the identity type, and where a module declares no +** machine frame type its identity type stands in. A module whose +** carriage does not line up, a slanted slide or an offset pivot, declares +** its machine frame type separately and keeps identity for a type whose +** joints really are the axes, since a consumer skips the maths on that +** flag alone. +*/ +#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ +#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine + coordinates, the rotaries as joints */ + +/* flags of a kinematics type, 0 when neither applies, or -1 for a type +** the module does not provide (and for every type on a machine with plain +** kinematics); G12.1 refuses a type that returns -1 */ +extern int kinematicsTypeFlags(int ktype); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that @@ -161,119 +194,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* Switchable kinematics: a module provides several kinematics, numbered -** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. -** The count is here, not in switchkins.h, because motion and the NML -** status channel need it. -*/ -#define SWITCHKINS_MAX_TYPES 9 - -/* What a kinematics type IS, declared by the module with -** switchkinsDeclare() and read back with kinematicsTypeFlags(). -** G13.1 resolves "identity" from these flags instead of assuming a -** number; a module that declares nothing leaves its types numeric-only -** and G13.1 refuses to guess. -*/ -#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ -#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ - -/* flags of a kinematics type, 0 when neither applies, or -1 for a type -** the module does not provide (and for every type on a machine with plain -** kinematics); G12.1 refuses a type that returns -1 */ -extern int kinematicsTypeFlags(int ktype); - -/* parameters for use with switchkins.c */ -typedef struct kinematics_parms { - char* sparm; // module string parameter passed to kins - char* kinsname; // must agree with module(file) name - char* halprefix; // for hal pin hames - char* required_coordinates; - int max_joints; - int allow_duplicates; - int fwd_iterates_mask; // identify kins types that use iterative - // forward kinematics (typ: genhex) - // bitmask: 0x0 none - // bitmask: 0x1 bit0: switchkins_type==0 - // bitmask: 0x2 bit1: switchkins_type==1 - // bitmask: 0x4 bit2: switchkins_type==2 - int gui_kinstype; // may be reqd for parallel kins with vismach - // to select switchkins_type for gui pins -} kparms; - -/* map letters in a coordinates string to joint numbers -** sequentially. Axis indices are 0:x,1:y,...,etc -** Example: coordinates=XYZYAC -** Result: axis_idx_for_jno[0] = 0 ==> X -** axis_idx_for_jno[1] = 1 ==> Y -** axis_idx_for_jno[2] = 2 ==> Z -** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) -** axis_idx_for_jno[4] = 1 ==> A -** axis_idx_for_jno[5] = 1 ==> C -*/ -extern int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[]); - -extern int mapped_joints_to_position(const int max_joints, - const double* joints, - EmcPose* pose); - -extern int position_to_mapped_joints(const int max_joints, - const EmcPose* pos, - double* joints); - -/* bitmap of joints assigned to an axis index (0:x,1:y,...,8:w) by the -** coordinates mapping; 0 when unused or not initialized */ -extern int identityKinematicsAxisJointsBitmap(int axis_num); - -extern int identityKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int identityKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int identityKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -/* joints are axes, so neither frame ever turns */ -extern int identityKinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int identityKinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -/* Rotations relating a module's own frame to the tool frame convention. - TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. - TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 - flange frame, whose z points out of the mechanical interface towards the - work, into the convention. */ -extern const PmRotationMatrix TOOL_FRAME_SPINDLE; -extern const PmRotationMatrix TOOL_FRAME_FLANGE; - -/* Post-multiply a module's native frame by the rotation it declared, in - place. Modules built on switchkins.c never call this, the dispatch does it - for them; a standalone module calls it before returning. - Returns 0, or -1 if native is not a proper rotation. */ -extern int toolFrameApplyNative(PmRotationMatrix *rot, - const PmRotationMatrix *native); - -/* out = transpose(work) * tool, the tool frame in workpiece coordinates. - out may alias neither input. */ -extern int toolFrameInWork(const PmRotationMatrix *work, - const PmRotationMatrix *tool, - PmRotationMatrix *out); - -/* True if m is orthonormal with determinant +1, so a frame a machine can - actually hold. Used to check a declared rotation once, at load. */ -extern int toolFrameIsProper(const PmRotationMatrix *m); /* The inverse of kinematicsToolFrame(): which joint values point the tool along a requested direction. This is the question a tilted work plane asks @@ -348,28 +268,6 @@ extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin); -/* The generic implementation of the above, driven by a module's own frame - functions, so that a module gets it for free once it supplies them. A - module with a closed form registers that instead: it is faster, and it - knows its own degenerate poses without having to find them. - - num_joints is the length of seed and of each row of solutions. */ -typedef int (*kinsFrameFunc)(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int toolFrameSolve(kinsFrameFunc work, - kinsFrameFunc tool, - int num_joints, - const PmCartesian *axis_in_work, - const PmCartesian *x_in_work, - const double *seed, - unsigned int held, - double *solutions, - int max_solutions, - int *free_directions, - double *tool_spin); - /* How each joint responds to a unit rate of each pose coordinate: jac[j][a] = d joint[j] / d pose[a] @@ -415,73 +313,17 @@ extern int kinematicsJacobian(const double *joint, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); -typedef int (*kinsInverseFunc)(const EmcPose *world, - double *joint, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - -/* The generic Jacobian, by central differences of an inverse about world: - two inverse calls per pose coordinate, eighteen in all, on the solution - branch iflags selects. The joint array handed to every call starts from - joint, so a module that reads its joint argument sees the machine where - it is. - - The answer is as good as the inverse: a closed form gives it to rounding, - an inverse that iterates to a tolerance gives it to that tolerance over - the step, and should supply its own. num_joints is the module's joint - count. Returns 0, or -1 if any inverse fails. */ -#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ - -extern int kinsJacobianFromInverse(kinsInverseFunc inverse, - int num_joints, - const double *joint, - const EmcPose *world, - const KINEMATICS_INVERSE_FLAGS *iflags, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* For a module whose inverse computes a position P and then hands it to - position_to_mapped_joints(): given dP[axis][pose], how each coordinate of - P responds to each pose coordinate, fill in jac so that every joint gets - the row of the letter it is mapped to. Duplicate letters get duplicate - rows, which is the gantry case. */ -extern int kinsJacobianFromMappedAxes(int max_joints, - const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* The Jacobian of a serial arm of six revolute joints from its - Denavit-Hartenberg chain. Two conventions carry that name and put the - four parameters on different links; this is the modified one of John J. - Craig, Introduction to Robotics: Mechanics and Control, where link i is - Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the - z of the frame Rx and Tx leave it in. (The original 1955 convention is - Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit - here.) The tool point `tool` lies along the z of the last frame, and - the pose of that point is reported as X Y Z and the RPY of the last - frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. - Each joint's axis crossed with the vector from it to the tool point - gives the point's rate per radian of the joint, the axis itself the - angular rate; that 6x6 inverted is the joint rate per unit of twist, and - the RPY rates reach the twist through the matrix of the axes each one - turns about, which is where world's B and C come in. alpha and joint in - degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac - are filled, the rest zero. Returns 0, or -1 at a singular pose, where no - finite joint rate follows the pose. */ -extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], - const double d[6], const double *joint, - double tool, const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* joints are axes: a 1 per joint in the column of its letter */ -extern int identityKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); -//NOTE: switchable kinematics may require Interp::Synch -// before/after invoking kinematicsSwitch() -// A convenient command to synch is: M66 E0 L0 + +/* The tool offset motion applies, handed to the module. Motion calls this + whenever the offset changes (G43, G49) and references it weakly, so a + module that does not export it still loads and keeps reading whatever + tool pin it has. kins_single.c and switchkins.c export it for every + module written on the parameter block: the tool then comes from the tool + table through motion, and the module's tool pin, where it has one, is + read only until motion has spoken. */ +extern int kinematicsSetTool(const EmcPose *tool); #define KINS_NOT_SWITCHABLE \ extern int kinematicsSwitchable() {return 0;} \ @@ -491,75 +333,6 @@ EXPORT_SYMBOL(kinematicsSwitchable); \ EXPORT_SYMBOL(kinematicsSwitch); \ EXPORT_SYMBOL(kinematicsTypeFlags); - -// support for template for user-defined switchkins_type==2 -extern int userkKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int userkKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int userkKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); -//********************************************************************* -// xyzac,xyzbc; -extern int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - - -extern int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -//********************************************************************* #ifdef __cplusplus } #endif diff --git a/include/kinematics_user.h b/include/kinematics_user.h new file mode 100644 index 00000000000..8ee1891c065 --- /dev/null +++ b/include/kinematics_user.h @@ -0,0 +1,324 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The kinematics module is loaded into this process and evaluated through + * its parameter block form (see kins_module.h). The block is filled from + * input pins belonging to the caller's HAL component, connected to the + * same signals the running RT instance reads, so the maths runs on live + * values; the tool is the caller's where it gives one, and motion's + * otherwise, from motion's tool offset pins where motion is loaded. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include "emcpos.h" /* EmcPose */ +#include "kins_module.h" /* KINEMATICS_TYPE, flags */ +#include "hal.h" /* hal_type_t, HAL_NAME_LEN */ + +#ifdef __cplusplus +#include "axis_kinds.hh" /* AXIS_X to AXIS_W */ +extern "C" { +#else +/* Axis coordinate indices for EmcPose, as axis_kinds.hh has them for C++ */ +enum AxisIndex { + AXIS_X, AXIS_Y, AXIS_Z, + AXIS_A, AXIS_B, AXIS_C, + AXIS_U, AXIS_V, AXIS_W, +}; +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +#define AXIS_COUNT 9 + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * The module's pins are read through HAL, by name, so call this after + * hal_init(). Nothing is made in HAL: no pin, no signal. + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * As kinematicsUserInit(), with the module's sparm= parameter as well, for + * a module whose kinematics types depend on it (5axiskins identityfirst). + */ +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix); + +/** + * Select which kinematics type of a switchable module to evaluate. + * Type 0 is selected after init. + * + * @return 0, or -1 if the module has no such type in the block form + */ +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); + +/** + * How many kinematics types the module has (1 for one that does not switch). + */ +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); + +/** + * The tool offset to evaluate with: what the caller knows the segment + * runs under, from canon or the tool table, rather than the offset the + * machine happens to have now. It stands until replaced, or until NULL + * puts the context back to taking the tool from motion. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * The joint array goes in as well as out: motion hands a module the + * joints the machine is at, and a module may read them (a nutating + * head takes its rotary angles from there, an iterating inverse starts + * there), so pass the current joints, not zeros. + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Joint positions in and out [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * A module whose forward iterates (the hexapod, the pentapod) starts + * from the pose in *world, so hand it one near the answer. + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates, and the seed on input + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the + * module's closed form where it has one and by differencing its inverse + * where it does not. The inverse is run at the pose first, seeded with + * what the last kinematicsUserInverse() found, so the derivative is + * taken on the solution branch the caller is on. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + +/** + * The parameter block as it stands, refreshed from HAL first. For + * reporting; the block belongs to the context. + */ +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Copy the bound pins into the block now. Every evaluation does this + * itself; call it only to observe the values. + * + * @param ctx Kinematics context + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no kinsDescribe() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * The frames, as the module reports them: the work frame and the tool + * frame at a joint set, each against the machine (see kinematics.h). + * + * @return 0, or -1 if the module supplies no frame for the selected type + */ +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); + +/** + * The tool frame inverse of kinematics.h, on the loaded module and the + * selected type: the joint sets that point the tool axis, and where given + * the tool x, along the directions asked for, in work coordinates. Same + * arguments and answers as kinematicsToolFrameInverse(). + */ +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin); + +/** + * Which joints turn the work at the seed, a bit per joint; what a caller + * passes as held to keep the table still. See toolFrameWorkJoints(). + */ +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask); + +/** + * The two rotaries that orient the tool, primary and secondary, told apart + * by which one carries the other's axis. The sign of the secondary names + * the pose a five axis machine reaches a tool direction in. Returns 0, or + * -1 where the machine has any number of orienting rotaries but two. + * See toolFrameOrientJoints(). + */ +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary); + +/** + * The rotary whose sign tells apart the two poses that reach one tool + * direction, Heidenhain's master axis for SEQ+ and SEQ-: the first rotary + * met going from the tool to the work. Two heads, the secondary, with or + * without a table behind them; two tables, the primary, the one carrying + * the other; one head and a table, the head. Returns 0 with the joint, or + * -1 where the machine has any other number or mix of orienting rotaries. + */ +int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, + int* joint); + +/** + * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the + * HAL file hands it to loadrt: the module name first, then any of + * coordinates=, sparm= and kinstype=, in any order. + */ +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/include/kins_module.h b/include/kins_module.h new file mode 100644 index 00000000000..13dc8f62411 --- /dev/null +++ b/include/kins_module.h @@ -0,0 +1,447 @@ +/******************************************************************** +* Description: kins_module.h +* The module side of a kinematics module: what a module is written with, +* as opposed to kinematics.h, which is what motion and the interpreter +* call. The joint map and identity helpers, the tool frame library, the +* generic Jacobian, and the parameter block form of a module with the +* shared code that runs an ops table. Nothing here needs HAL; the pins +* made from a module's table are in kins_rt.h. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_MODULE_H +#define __LINUXCNC_KINS_MODULE_H + +#include "kinematics.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* parameters for use with switchkins.c */ +typedef struct kinematics_parms { + char* sparm; // module string parameter passed to kins + char* kinsname; // must agree with module(file) name + char* halprefix; // for hal pin hames + char* required_coordinates; + int max_joints; + int allow_duplicates; + int fwd_iterates_mask; // identify kins types that use iterative + // forward kinematics (typ: genhex) + // bitmask: 0x0 none + // bitmask: 0x1 bit0: switchkins_type==0 + // bitmask: 0x2 bit1: switchkins_type==1 + // bitmask: 0x4 bit2: switchkins_type==2 + int gui_kinstype; // may be reqd for parallel kins with vismach + // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; +} kparms; + +/* map letters in a coordinates string to joint numbers +** sequentially. Axis indices are 0:x,1:y,...,etc +** Example: coordinates=XYZYAC +** Result: axis_idx_for_jno[0] = 0 ==> X +** axis_idx_for_jno[1] = 1 ==> Y +** axis_idx_for_jno[2] = 2 ==> Z +** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) +** axis_idx_for_jno[4] = 1 ==> A +** axis_idx_for_jno[5] = 1 ==> C +*/ +extern int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[]); + +extern int mapped_joints_to_position(const int max_joints, + const double* joints, + EmcPose* pose); + +extern int position_to_mapped_joints(const int max_joints, + const EmcPose* pos, + double* joints); + +/* bitmap of joints assigned to an axis index (0:x,1:y,...,8:w) by the +** coordinates mapping; 0 when unused or not initialized */ +extern int identityKinematicsAxisJointsBitmap(int axis_num); + +extern int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int identityKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int identityKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +/* joints are axes, so neither frame ever turns */ +extern int identityKinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int identityKinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Rotations relating a module's own frame to the tool frame convention. + TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. + TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 + flange frame, whose z points out of the mechanical interface towards the + work, into the convention. */ +extern const PmRotationMatrix TOOL_FRAME_SPINDLE; +extern const PmRotationMatrix TOOL_FRAME_FLANGE; + +/* Post-multiply a module's native frame by the rotation it declared, in + place. Modules built on switchkins.c never call this, the dispatch does it + for them; a standalone module calls it before returning. + Returns 0, or -1 if native is not a proper rotation. */ +extern int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native); + +/* out = transpose(work) * tool, the tool frame in workpiece coordinates. + out may alias neither input. */ +extern int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out); + +/* True if m is orthonormal with determinant +1, so a frame a machine can + actually hold. Used to check a declared rotation once, at load. */ +extern int toolFrameIsProper(const PmRotationMatrix *m); + + +/* The generic implementation of the above, driven by a module's own frame + functions, so that a module gets it for free once it supplies them. A + module with a closed form registers that instead: it is faster, and it + knows its own degenerate poses without having to find them. + + num_joints is the length of seed and of each row of solutions. */ +typedef int (*kinsFrameFunc)(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Which joints turn the work: a bit per joint whose motion changes the + work frame at the seed. This is what a caller needs to hold the table + still while the head orients the tool (Heidenhain COORD ROT), or to let + it take part (TABLE ROT), without a config entry naming it. Returns 0 + or -1 if the frame cannot be evaluated. */ +extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask); + +/* The two rotaries that orient the tool, told apart. One has its axis + fixed in the machine frame, the primary, and the other has its axis + carried by the first, the secondary. The two poses that reach one tool + direction differ in the sign of the secondary, which is what a caller + needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. + + Both are found from the module's own tool frame, by turning each joint a + little and reading the axis of the rotation that results, so a module + declares nothing and a switchkins type that turns nothing answers -1. + Returns 0 with both joints set, or -1 where the machine has any number + of orienting rotaries but two, a robot wrist among them, or where the + frame cannot be evaluated. */ +extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, + int *primary, int *secondary); + +extern int toolFrameSolve(kinsFrameFunc work, + kinsFrameFunc tool, + int num_joints, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + unsigned int held, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* The Jacobian of a serial arm of six revolute joints from its + Denavit-Hartenberg chain. Two conventions carry that name and put the + four parameters on different links; this is the modified one of John J. + Craig, Introduction to Robotics: Mechanics and Control, where link i is + Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the + z of the frame Rx and Tx leave it in. (The original 1955 convention is + Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit + here.) The tool point `tool` lies along the z of the last frame, and + the pose of that point is reported as X Y Z and the RPY of the last + frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. + Each joint's axis crossed with the vector from it to the tool point + gives the point's rate per radian of the joint, the axis itself the + angular rate; that 6x6 inverted is the joint rate per unit of twist, and + the RPY rates reach the twist through the matrix of the axes each one + turns about, which is where world's B and C come in. alpha and joint in + degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac + are filled, the rest zero. Returns 0, or -1 at a singular pose, where no + finite joint rate follows the pose. */ +extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES SWITCHKINS_MAX_TYPES + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + int pose_seed_ok; /* pose_seed came from a solve that succeeded */ + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double aux[8]; /* whatever else a module carries between calls */ + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. machine says + this is the machine frame type, the pivot in machine coordinates with the + rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module + that leaves it unset on every type has its identity type stand in. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ + int primary; /* the working transform */ + int machine; /* the machine frame */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINS_MODULE_H diff --git a/include/kins_rt.h b/include/kins_rt.h new file mode 100644 index 00000000000..a0c3d3f33ff --- /dev/null +++ b/include/kins_rt.h @@ -0,0 +1,84 @@ +/******************************************************************** +* Description: kins_rt.h +* The RT side of a kinematics module written as pure functions: the HAL +* pins made from its geometry table, and the wrapper that supplies the +* classic entry points for a module with one kinematics type. A module +* with several types gets the same from switchkins.c. +* +* Kept apart from kinematics.h because everything here needs HAL, and +* kinematics.h is read by callers outside RT that do not. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_RT_H +#define __LINUXCNC_KINS_RT_H + +#include "hal.h" +#include "kins_module.h" + +/* one HAL pin handle per table entry, of whichever type the entry has */ +typedef union { + hal_real_t r; + hal_bool_t b; + hal_sint_t s; + hal_uint_t u; +} kins_pin_ref; + +/* Make one pin per table entry, named ., inputs at their + defaults. *out receives the handles, from hal_malloc(), or NULL for an + empty table. Returns 0 or -1. */ +extern int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out); + +/* Copy every input pin into p->geometry[], and the tool entry into + p->tool.tran.z as well. */ +extern void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* Copy s->out[] to every output pin. */ +extern void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s); + +/* Where the RT block's tool comes from. kinematicsSetTool() records what + motion sends in one of these; kinsToolSourceApply() writes it into a + block after the pins have been read, over the tool entry, once motion + has sent anything. Until then the tool entry's pin is all there is, as + under halrun with the module alone. A config that still nets the tool + to the module's pin loses nothing; one that sets that pin to something + else is told, once, after the two have disagreed for a thousand calls, + since the pin lags the send by a cycle. */ +typedef struct { + EmcPose tool; + int have; /* motion has sent a tool */ + int disagreeing; /* consecutive calls with the pin elsewhere */ + int warned; +} kins_tool_source; + +extern void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool); +extern void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* A module with one kinematics type describes itself in a + kins_module_info and hands it to kinsSingleInit(); kins_single.c, linked + in or loaded as kins_single_core, supplies kinematicsForward() and the + rest from it. ops[0] is the maths; the other entries are ignored. An + in-tree module names its description kins_module and links + kins_single_module.c for its kinsDescribe(). */ +extern const kins_module_info kins_module; + +/* Called once from the module's rtapi_app_main() or EXTRA_SETUP(), after + hal_init() and before hal_ready(): makes the pins, builds the block for + coordinates and records the KINEMATICS_TYPE that kinematicsType() will + report. m must outlive the module. Returns 0 or -1. */ +extern int kinsSingleInit(int comp_id, const kins_module_info *m, + const char *coordinates, KINEMATICS_TYPE reported); + +/* What the module's kinsDescribe() answers with: m, one type. */ +extern int kinsSingleDescribe(const kins_module_info *m, + kins_module_info *info); + +#endif diff --git a/include/switchkins.h b/include/switchkins.h index c7114262403..27d50757cbf 100644 --- a/include/switchkins.h +++ b/include/switchkins.h @@ -4,10 +4,10 @@ #ifndef __LINUXCNC_SWITCHKINS_H #define __LINUXCNC_SWITCHKINS_H -#include "kinematics.h" +#include "kins_module.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 +// SWITCHKINS_MAX_TYPES, the most types a module may provide, is in +// kinematics.h: motion and the NML status channel need it too // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -35,6 +35,10 @@ typedef int (*KS)(const int comp_id, // halpins //********************************************************************* // supplied by a module using switchkins_main.c, provides types 0,1,2 +typedef int (*switchkinsSetupFunc)(kparms* ksetup_parms, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -69,9 +73,9 @@ typedef int (*KTI)(const PmCartesian *axis_in_work, extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); // optionally called from switchkinsSetup() to declare what a type IS -// (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, kinematics.h). A module that -// never calls it leaves its types numeric-only: G12.1 P still works, -// G13.1 refuses to guess which type is identity. +// (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE, kinematics.h). +// A module that never calls it leaves its types numeric-only: G12.1 P +// still works, G13.1 refuses to guess which type is the machine frame. extern int switchkinsDeclare(int ktype, int flags); // KinematicsJACOBIAN function (optional, see kinematics.h) @@ -85,10 +89,42 @@ typedef int (*KJ)(const double *joint, // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); +// provide one switchkins-type written as pure functions (see kins_module.h), +// before switchkinsInit(). Its pins come from the table in kparms, shared +// by every type of the module, so it has no setup function. A type may be +// provided this way or through switchkinsRegister(), not both. +extern int switchkinsRegisterOps(int ktype, const kins_ops *ops); + // 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 ); + +// Fill kp with the defaults, run the module's setup and register the three +// types it may return, so that every type goes in by one route. Returns +// 0 or -1. +extern int switchkinsRunSetupWith(switchkinsSetupFunc setup, + kparms* kp, const char* sparm); + +// What the module's kinsDescribe() answers with: the RT instance describes +// itself, a copy outside RT replays setup once. +extern int switchkinsDescribeWith(switchkinsSetupFunc setup, + const char *coordinates, const char *sparm, + kins_module_info *info); + +// The two above on the module's switchkinsSetup(), in switchkins_setup.c, +// which an in-tree module links; a module built out of tree calls the +// With forms itself. +extern int switchkinsRunSetup(kparms* kp, const char* sparm); + +// The module as the core knows it after switchkinsInit(): its table and +// the ops of every type, NULL for one provided the old way. Behind +// kinsDescribe() for the RT instance; a copy outside RT that has not been +// initialised is described by switchkins_setup.c after a replay of setup. +// Returns 0, or -1 before switchkinsInit(). +extern int switchkinsDescribe(kins_module_info *info); +extern int switchkinsDescribeSetup(const kparms *kp, kins_module_info *info); + #endif diff --git a/src/Makefile b/src/Makefile index df19b0c3dc7..e24509ada47 100644 --- a/src/Makefile +++ b/src/Makefile @@ -193,6 +193,7 @@ SUBDIRS := \ \ $(GUI_SUBDIRS) \ emc/usr_intf/axis emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/canterp \ + emc/motion_planning emc/kinematics_userspace \ emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ @@ -1056,37 +1057,66 @@ hal_lib-objs := hal/hal_lib.o $(MATHSTUB) obj-m += trivkins.o trivkins-objs := emc/kinematics/trivkins.o trivkins-objs += emc/kinematics/kins_util.o +trivkins-objs += emc/kinematics/kins_single.o +trivkins-objs += emc/kinematics/kins_single_module.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o +maxkins-objs += emc/kinematics/kins_util.o +maxkins-objs += emc/kinematics/kins_single.o +maxkins-objs += emc/kinematics/kins_single_module.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o +rotatekins-objs += emc/kinematics/kins_util.o +rotatekins-objs += emc/kinematics/kins_single.o +rotatekins-objs += emc/kinematics/kins_single_module.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +tripodkins-objs += emc/kinematics/kins_util.o +tripodkins-objs += emc/kinematics/kins_single.o +tripodkins-objs += emc/kinematics/kins_single_module.o obj-m += corexykins.o corexykins-objs := emc/kinematics/corexykins.o +corexykins-objs += emc/kinematics/kins_util.o +corexykins-objs += emc/kinematics/kins_single.o +corexykins-objs += emc/kinematics/kins_single_module.o obj-m += lineardeltakins.o lineardeltakins-objs := emc/kinematics/lineardeltakins.o +lineardeltakins-objs += emc/kinematics/kins_util.o +lineardeltakins-objs += emc/kinematics/kins_single.o +lineardeltakins-objs += emc/kinematics/kins_single_module.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o +pentakins-objs += emc/kinematics/kins_util.o +pentakins-objs += emc/kinematics/kins_single.o +pentakins-objs += emc/kinematics/kins_single_module.o pentakins-objs += libposemath/_posemath.o pentakins-objs += $(MATHSTUB) obj-m += rotarydeltakins.o rotarydeltakins-objs := emc/kinematics/rotarydeltakins.o +rotarydeltakins-objs += emc/kinematics/kins_util.o +rotarydeltakins-objs += emc/kinematics/kins_single.o +rotarydeltakins-objs += emc/kinematics/kins_single_module.o rotarydeltakins-objs += libposemath/_posemath.o rotarydeltakins-objs += $(MATHSTUB) obj-m += rosekins.o rosekins-objs := emc/kinematics/rosekins.o +rosekins-objs += emc/kinematics/kins_util.o +rosekins-objs += emc/kinematics/kins_single.o +rosekins-objs += emc/kinematics/kins_single_module.o obj-m += scorbot-kins.o scorbot-kins-objs := emc/kinematics/scorbot-kins.o +scorbot-kins-objs += emc/kinematics/kins_util.o +scorbot-kins-objs += emc/kinematics/kins_single.o +scorbot-kins-objs += emc/kinematics/kins_single_module.o ifeq ($(origin userkfuncs), undefined) # use template: @@ -1111,6 +1141,7 @@ 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 += emc/kinematics/switchkins_setup.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1121,6 +1152,7 @@ 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 += emc/kinematics/switchkins_setup.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1129,6 +1161,7 @@ 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 += emc/kinematics/switchkins_setup.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1137,6 +1170,7 @@ 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 += emc/kinematics/switchkins_setup.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1146,6 +1180,7 @@ 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 += emc/kinematics/switchkins_setup.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1155,6 +1190,7 @@ 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 += emc/kinematics/switchkins_setup.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1164,8 +1200,15 @@ 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 += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) +obj-m += kins_single_core.o +kins_single_core-objs := emc/kinematics/kins_single_core.o +kins_single_core-objs += emc/kinematics/kins_single.o +kins_single_core-objs += emc/kinematics/kins_util.o +kins_single_core-objs += $(MATHSTUB) + obj-m += switchkins_core.o switchkins_core-objs := emc/kinematics/switchkins_core.o switchkins_core-objs += emc/kinematics/switchkins.o @@ -1179,6 +1222,7 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o 5axiskins-objs += emc/kinematics/switchkins_main.o +5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- @@ -1345,6 +1389,7 @@ endif ../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/kins_single_core$(MODULE_EXT): $(addprefix objects/rt,$(kins_single_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/5axiskins.c b/src/emc/kinematics/5axiskins.c index b2510e094ef..d1df89d9f36 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -11,10 +11,9 @@ * Copyright (c) 2007 Chris Radek * * Notes: -* 1) pivot_length hal pin must agree with mechanical -* design (including vismach simulation) and augmented -* with current tool z offset -* (typ: mechanical_pivot_length + motion.tooloffset.z) +* 1) pivot-length must agree with the mechanical design +* (including vismach simulation); the tool length is +* the offset motion applies, handed over by motion * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -42,8 +41,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -56,17 +55,30 @@ #include #include #include -#include #include #include #include - -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; -} *haldata; -static int fiveaxis_max_joints; +#include "userkfuncs.h" + +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; + +// assignments of principal joints to axis letters, from the block +// (-1 means not defined) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) static PmCartesian s2r(double r, double t, double p) { // s2r: spherical coordinates to cartesian coordinates @@ -84,28 +96,18 @@ static PmCartesian s2r(double r, double t, double p) { return c; } //s2r() -// assignments of principal joints to axis letters: -// (-1 means not defined (yet)) -static int JX = -1; -static int JY = -1; -static int JZ = -1; -static int JA = -1; -static int JB = -1; -static int JC = -1; -static int JU = -1; -static int JV = -1; -static int JW = -1; - -static int fiveaxis_KinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int fiveaxis_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + joints[JW] + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + joints[JW], joints[JC], 180.0 - joints[JB]); @@ -123,18 +125,20 @@ static int fiveaxis_KinematicsForward(const double *joints, pos->v = (JV != -1)? joints[JV] : 0; return 0; -} //fiveaxis_KinematicsForward() +} // fiveaxis_forward() -static int fiveaxis_KinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int fiveaxis_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + pos->w + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + pos->w, pos->c, 180.0 - pos->b); @@ -155,21 +159,18 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(fiveaxis_max_joints, - &P, - joints); - return 0; -} // fiveaxis_kinematicsInverse() - -static int fiveaxis_KinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) + return kinsPoseToMappedJoints(p, &P, joints); +} // fiveaxis_inverse() + +static int fiveaxis_jacobian(const kins_params *p, + const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)joints; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - const double R = pivot_length + pos->w; + const double R = p->geometry[P_PIVOT_LENGTH] + p->geometry[P_TOOL_LENGTH] + pos->w; const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; @@ -196,89 +197,37 @@ static int fiveaxis_KinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(fiveaxis_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // fiveaxis_KinematicsJacobian() + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_jacobian() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// The head carries the tool and nothing turns the work: the work frame is the +// machine frame and the tool frame Rz(C) Ry(-B), the tilt left handed about y +// (note 10), its third column the tool axis from the tip towards the holder. +static int fiveaxis_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { - int result=0; - int i,jno; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int minjoints = strlen(kp->required_coordinates); - fiveaxis_max_joints = strlen(coordinates); // allow for dup coords - - if (fiveaxis_max_joints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: coordinates=%s requires %d joints, max joints=%d\n", - kp->kinsname, - coordinates, - fiveaxis_max_joints, - kp->max_joints); - goto error; - } - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coordinates (order doesn't matter) - for (i=0; i < minjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jnopivot_length), - DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix); - if(result < 0) goto error; - - result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), - 0.0, "%s.tool-length", kp->halprefix); - if(result < 0) goto error; + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + const double sc = sin(joints[JC]*TO_RAD), cc = cos(joints[JC]*TO_RAD); - rtapi_print("Kinematics Module %s\n",__FILE__); - rtapi_print(" module name = %s\n" - " coordinates = %s Requires: [KINS]JOINTS>=%d\n" - " sparm = %s\n", - kp->kinsname, - coordinates,fiveaxis_max_joints, - kp->sparm?kp->sparm:"NOTSPECIFIED"); - rtapi_print(" default pivot-length = %.3f\n", hal_get_real(haldata->pivot_length)); + rot->x.x = cb * cc; rot->y.x = -sc; rot->z.x = -sb * cc; + rot->x.y = cb * sc; rot->y.y = cc; rot->z.y = -sb * sc; + rot->x.z = sb; rot->y.z = 0; rot->z.z = cb; return 0; - -error: - return -1; -} // fiveaxis_KinematicsSetup() +} // fiveaxis_tool_frame() + +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .work = kinsIdentityFrame, + .tool = fiveaxis_tool_frame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = fiveaxis_jacobian, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -286,40 +235,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "5axiskins"; // !!! must agree with filename kp->halprefix = "5axiskins"; // hal pin names kp->required_coordinates = REQUIRED_COORDINATES; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = fiveaxis_params; + kp->nparams = sizeof(fiveaxis_params)/sizeof(fiveaxis_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = fiveaxis_KinematicsSetup; - *kfwd1 = fiveaxis_KinematicsForward; - *kinv1 = fiveaxis_KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &fiveaxis_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = fiveaxis_KinematicsSetup; - *kfwd0 = fiveaxis_KinematicsForward; - *kinv0 = fiveaxis_KinematicsInverse; - switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &fiveaxis_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..6cc22df32d3 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -2,6 +2,7 @@ GENSERKINSSRCS := emc/kinematics/ugenserkins.c GENSERKINSSRCS += emc/kinematics/genserfuncs.c +GENSERKINSSRCS += emc/kinematics/kins_util.c USERSRCS += $(GENSERKINSSRCS) DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 6d3ac4e75ab..eaaaa060df9 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,13 +8,16 @@ #include #include #include -#include +#include +#include -int kinematicsForward(const double *joints - ,EmcPose *pos - ,const KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { +static int corexy_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)p; + (void)s; (void)fflags; (void)iflags; pos->tran.x = 0.5 * (joints[0] + joints[1]); @@ -30,11 +33,13 @@ int kinematicsForward(const double *joints return 0; } -int kinematicsInverse(const EmcPose *pos - ,double *joints - ,const KINEMATICS_INVERSE_FLAGS *iflags - ,KINEMATICS_FORWARD_FLAGS *fflags - ) { +static int corexy_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)s; (void)iflags; (void)fflags; joints[0] = pos->tran.x + pos->tran.y; @@ -50,12 +55,13 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int corexy_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { int j; + (void)p; (void)joints; (void)pos; (void)iflags; @@ -67,23 +73,26 @@ int kinematicsJacobian(const double *joints, return 0; } -int kinematicsHome(EmcPose *world - ,double *joint - ,KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { - *fflags = 0; - *iflags = 0; - return kinematicsForward(joint, world, fflags, iflags); -} +static const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +// no geometry: the belts are what they are. Joints 0..8 are the nine +// letters in order; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "corexykins", + .halprefix = "corexykins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &corexy_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -91,6 +100,11 @@ int rtapi_app_main(void) { comp_id = hal_init("corexykins"); if(comp_id < 0) return comp_id; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); return 0; } diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 0964fa7d088..34b124ddef4 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -16,17 +16,17 @@ machines referred to as "Stewart Platforms". The functions are general enough to be configured for any platform - configuration. In the functions "genhexKinematicsForward" and - "genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored - in these arrays correspond to the positions of the ends of the i'th - strut. The value stored in a[i] is the position of the end of the i'th - strut attached to the platform, in platform coordinates. The value - stored in b[i] is the position of the end of the i'th strut attached - to the base, in base (world) coordinates. + configuration. In the functions "genhex_forward" and "genhex_inverse" + are arrays "a[i]" and "b[i]". The values stored in these arrays + correspond to the positions of the ends of the i'th strut. The value + stored in a[i] is the position of the end of the i'th strut attached + to the platform, in platform coordinates. The value stored in b[i] is + the position of the end of the i'th strut attached to the base, in + base (world) coordinates. The default values for base and platform joints positions are defined in the header file genhexkins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: genhexkins.base.N.x genhexkins.base.N.y @@ -67,18 +67,18 @@ genhexkins.correction.N - pins showing current values of strut length correction. - The genhexKinematicsInverse function solves the inverse kinematics using + The genhex_inverse function solves the inverse kinematics using a closed form algorithm. The inverse kinematics problem is given the pose of the platform and returns the strut lengths. For this problem there is only one solution that is always returned correctly. - The genhexKinematicsForward function solves the forward kinematics using + The genhex_forward function solves the forward kinematics using an iterative algorithm. Due to the iterative nature of this algorithm - the genhexKinematicsForward function requires an initial value to begin the + the genhex_forward function requires an initial value to begin the iterative routine and then converges to the "nearest" solution. The forward kinematics problem is given the strut lengths and returns the pose of the platform. For this problem there arein multiple - solutions. The genhexKinematicsForward function will return only one of + solutions. The genhex_forward function will return only one of these solutions which will be the solution nearest to the initial value given. It is possible that there are no solutions "near" the given initial value and the iteration will not converge and no @@ -103,6 +103,10 @@ genhexkins.max-iterations - maximum number of iterations spent for a converged solution during current session. + The maths is written as pure functions of the parameter block (see + kins_module.h): the pins above are the table below, read into the block + before every call and written from the scratch after it. + ----------------------------------------------------------------------------*/ #include @@ -113,50 +117,100 @@ #include "genhexkins.h" #include +#include "userkfuncs.h" + +// the table: thirteen entries per strut, then the iteration controls, +// the offsets and the reports. The macros index it. +#define STRUT_ENTRIES 13 +#define P_BASE_X(i) (STRUT_ENTRIES*(i) + 0) +#define P_BASE_Y(i) (STRUT_ENTRIES*(i) + 1) +#define P_BASE_Z(i) (STRUT_ENTRIES*(i) + 2) +#define P_PLAT_X(i) (STRUT_ENTRIES*(i) + 3) +#define P_PLAT_Y(i) (STRUT_ENTRIES*(i) + 4) +#define P_PLAT_Z(i) (STRUT_ENTRIES*(i) + 5) +#define P_BASE_NX(i) (STRUT_ENTRIES*(i) + 6) +#define P_BASE_NY(i) (STRUT_ENTRIES*(i) + 7) +#define P_BASE_NZ(i) (STRUT_ENTRIES*(i) + 8) +#define P_PLAT_NX(i) (STRUT_ENTRIES*(i) + 9) +#define P_PLAT_NY(i) (STRUT_ENTRIES*(i) + 10) +#define P_PLAT_NZ(i) (STRUT_ENTRIES*(i) + 11) +#define P_CORR(i) (STRUT_ENTRIES*(i) + 12) +enum { + P_LAST_ITER = STRUT_ENTRIES*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_SPINDLE_OFFSET, + P_SCREW_LEAD, + P_GUI_X, P_GUI_Y, P_GUI_Z, P_GUI_A, P_GUI_B, P_GUI_C, + P_FWD_FAIL, + P_COUNT +}; -static struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t platformx[NUM_STRUTS]; - hal_real_t platformy[NUM_STRUTS]; - hal_real_t platformz[NUM_STRUTS]; - hal_real_t basenx[NUM_STRUTS]; - hal_real_t baseny[NUM_STRUTS]; - hal_real_t basenz[NUM_STRUTS]; - hal_real_t platformnx[NUM_STRUTS]; - hal_real_t platformny[NUM_STRUTS]; - hal_real_t platformnz[NUM_STRUTS]; - hal_real_t correction[NUM_STRUTS]; - hal_real_t screw_lead; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; - hal_real_t spindle_offset; - hal_bool_t fwd_kins_fail; - - hal_real_t gui_x; - hal_real_t gui_y; - hal_real_t gui_z; - hal_real_t gui_a; - hal_real_t gui_b; - hal_real_t gui_c; - -} *haldata; - -static int genhex_gui_forward_kins(EmcPose *pos) -{ - hal_set_real(haldata->gui_x, pos->tran.x); - hal_set_real(haldata->gui_y, pos->tran.y); - hal_set_real(haldata->gui_z, pos->tran.z); - hal_set_real(haldata->gui_a, pos->a); - hal_set_real(haldata->gui_b, pos->b); - hal_set_real(haldata->gui_c, pos->c); - return 0; -} // genhex_gui_forward_kins +#define STRUT_ROWS(i, bx, by, bz, px, py, pz, bnx, bny, bnz, pnx, pny, pnz) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "platform." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, px }, \ + { "platform." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, py }, \ + { "platform." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pz }, \ + { "base-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bnx }, \ + { "base-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, bny }, \ + { "base-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bnz }, \ + { "platform-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, pnx }, \ + { "platform-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, pny }, \ + { "platform-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pnz }, \ + { "correction." #i, KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 } + +static const kins_param_desc genhex_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, + DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_0_Z, + DEFAULT_BASE_0_NX, DEFAULT_BASE_0_NY, DEFAULT_BASE_0_NZ, + DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_0_NZ), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, + DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_1_Z, + DEFAULT_BASE_1_NX, DEFAULT_BASE_1_NY, DEFAULT_BASE_1_NZ, + DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_1_NZ), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, + DEFAULT_PLATFORM_2_X, DEFAULT_PLATFORM_2_Y, DEFAULT_PLATFORM_2_Z, + DEFAULT_BASE_2_NX, DEFAULT_BASE_2_NY, DEFAULT_BASE_2_NZ, + DEFAULT_PLATFORM_2_NX, DEFAULT_PLATFORM_2_NY, DEFAULT_PLATFORM_2_NZ), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, + DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_3_Z, + DEFAULT_BASE_3_NX, DEFAULT_BASE_3_NY, DEFAULT_BASE_3_NZ, + DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_3_NZ), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, + DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_4_Z, + DEFAULT_BASE_4_NX, DEFAULT_BASE_4_NY, DEFAULT_BASE_4_NZ, + DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_4_NZ), + STRUT_ROWS(5, DEFAULT_BASE_5_X, DEFAULT_BASE_5_Y, DEFAULT_BASE_5_Z, + DEFAULT_PLATFORM_5_X, DEFAULT_PLATFORM_5_Y, DEFAULT_PLATFORM_5_Z, + DEFAULT_BASE_5_NX, DEFAULT_BASE_5_NY, DEFAULT_BASE_5_NZ, + DEFAULT_PLATFORM_5_NX, DEFAULT_PLATFORM_5_NY, DEFAULT_PLATFORM_5_NZ), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IN, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IN, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + [P_SPINDLE_OFFSET] = { "spindle-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + [P_SCREW_LEAD] = { "screw-lead", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_SCREW_LEAD }, + // the pose the forward found, for a vismach gui; switchkins provides + // the skgui.* pins for the same purpose + [P_GUI_X] = { "x", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Y] = { "y", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Z] = { "z", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_A] = { "a", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_B] = { "b", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_C] = { "c", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_FWD_FAIL] = { "fwd-kins-fail", KINS_PARAM_BIT, KINS_OUT, 0, 0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert() ***************************/ @@ -259,45 +313,45 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } // MatMult() -/* declare arrays for base and platform coordinates */ -static PmCartesian b[NUM_STRUTS]; -static PmCartesian a[NUM_STRUTS]; - -/* declare base and platform joint axes vectors */ - -static PmCartesian nb1[NUM_STRUTS]; -static PmCartesian na0[NUM_STRUTS]; - -/************************genhex_read_hal_pins**************************/ - -static int genhex_read_hal_pins(void) { +/* the geometry of one call, taken from the block: base and platform + coordinates, the joint axes vectors and the screw lead */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + PmCartesian a[NUM_STRUTS]; + PmCartesian nb1[NUM_STRUTS]; + PmCartesian na0[NUM_STRUTS]; + double screw_lead; +} genhex_geometry; + +static void geometry_of(const kins_params *p, genhex_geometry *g) { int t; - /* set the base and platform coordinates from hal pin values */ - rtapi_real spindle_offset = hal_get_real(haldata->spindle_offset); - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + /* set the base and platform coordinates from the block */ + const double spindle_offset = p->geometry[P_SPINDLE_OFFSET]; + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + spindle_offset + tool_offset; - a[t].x = hal_get_real(haldata->platformx[t]); - a[t].y = hal_get_real(haldata->platformy[t]); - a[t].z = hal_get_real(haldata->platformz[t]) + spindle_offset + tool_offset; - - nb1[t].x = hal_get_real(haldata->basenx[t]); - nb1[t].y = hal_get_real(haldata->baseny[t]); - nb1[t].z = hal_get_real(haldata->basenz[t]); - na0[t].x = hal_get_real(haldata->platformnx[t]); - na0[t].y = hal_get_real(haldata->platformny[t]); - na0[t].z = hal_get_real(haldata->platformnz[t]); + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + spindle_offset + tool_offset; + g->a[t].x = p->geometry[P_PLAT_X(t)]; + g->a[t].y = p->geometry[P_PLAT_Y(t)]; + g->a[t].z = p->geometry[P_PLAT_Z(t)] + spindle_offset + tool_offset; + + g->nb1[t].x = p->geometry[P_BASE_NX(t)]; + g->nb1[t].y = p->geometry[P_BASE_NY(t)]; + g->nb1[t].z = p->geometry[P_BASE_NZ(t)]; + g->na0[t].x = p->geometry[P_PLAT_NX(t)]; + g->na0[t].y = p->geometry[P_PLAT_NY(t)]; + g->na0[t].z = p->geometry[P_PLAT_NZ(t)]; } - return 0; -} // genhex_read_hal_pins() + g->screw_lead = p->geometry[P_SCREW_LEAD]; +} // geometry_of() /***************************StrutLengthCorrection***************************/ -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, +static int StrutLengthCorrection(const genhex_geometry *g, + const PmCartesian * StrutVectUnit, const PmRotationMatrix * RMatrix, const int strut_number, double * correction) @@ -306,32 +360,34 @@ static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, double dotprod; /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(&g->nb1[strut_number], StrutVectUnit, &nb2); pmCartCartCross(StrutVectUnit, &nb2, &nb3); pmCartUnitEq(&nb3); /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmMatCartMult(RMatrix, &g->na0[strut_number], &na1); pmCartCartCross(&na1, StrutVectUnit, &na2); pmCartUnitEq(&na2); /* define dot product */ pmCartCartDot(&nb3, &na2, &dotprod); - *correction = hal_get_real(haldata->screw_lead) * asin(dotprod) / PM_2_PI; + *correction = g->screw_lead * asin(dotprod) / PM_2_PI; return 0; } // StrutLengthCorrection() -/**************** genhexKinematicsForward() *****************/ -static int genhexKinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +/**************** genhex_forward() *****************/ +static int genhex_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; + genhex_geometry g; PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -350,7 +406,7 @@ static int genhexKinematicsForward(const double * joints, int i; unsigned iteration = 0; - genhex_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -375,13 +431,16 @@ static int genhexKinematicsForward(const double * joints, q_trans.z = pos->tran.z; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +max_error) || (conv_err < -max_error)) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -2; }; @@ -389,9 +448,10 @@ static int genhexKinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -5; } @@ -402,18 +462,19 @@ static int genhexKinematicsForward(const double * joints, estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ for (i = 0; i < NUM_STRUTS; i++) { - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); /* define corrected joint lengths */ InvKinStrutLength += corr; } @@ -454,7 +515,6 @@ static int genhexKinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -472,33 +532,42 @@ static int genhexKinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } - hal_set_bool(haldata->fwd_kins_fail, 0); + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); + s->out[P_FWD_FAIL] = 0; - genhex_gui_forward_kins(pos); + s->out[P_GUI_X] = pos->tran.x; + s->out[P_GUI_Y] = pos->tran.y; + s->out[P_GUI_Z] = pos->tran.z; + s->out[P_GUI_A] = pos->a; + s->out[P_GUI_B] = pos->b; + s->out[P_GUI_C] = pos->c; return 0; -} // genhexKinematicsForward() +} // genhex_forward() -/************************ genhexKinematicsInverse() ************************/ +/************************ genhex_inverse() ************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -static int genhexKinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genhex_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; + genhex_geometry g; PmCartesian aw, temp; PmCartesian InvKinStrutVect, InvKinStrutVectUnit; PmRotationMatrix RMatrix; @@ -506,7 +575,7 @@ static int genhexKinematicsInverse(const EmcPose * pos, int i; double InvKinStrutLength, corr; - genhex_read_hal_pins(); + geometry_of(p, &g); /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; @@ -518,22 +587,22 @@ static int genhexKinematicsInverse(const EmcPose * pos, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of platform strut end from platform to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); + pmMatCartMult(&RMatrix, &g.a[i], &temp); pmCartCartAdd(&pos->tran, &temp, &aw); /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ /* define unit strut vector */ if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { return -1; } /* define correction value and corrected joint lengths */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); - hal_set_real(haldata->correction[i], corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); + s->out[P_CORR(i)] = corr; InvKinStrutLength += corr; } @@ -541,9 +610,9 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() +} //genhex_inverse() -/************************ genhexKinematicsJacobian() ***********************/ +/************************ genhex_jacobian() ***********************/ /* A strut length changes by the component of its platform end's motion along the strut. That end moves with the platform, dP + w x (R a), so the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut @@ -551,11 +620,18 @@ static int genhexKinematicsInverse(const EmcPose * pos, words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward kinematics builds the same rows for its Newton step, in radians. */ -static int genhexKinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse alone, for differencing where the closed form does not apply +static const kins_ops genhex_diff_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, +}; + +static int genhex_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + genhex_geometry g; PmCartesian aw, RMatrix_a, strut, u, moment; PmRotationMatrix RMatrix; PmRpy rpy; @@ -563,13 +639,14 @@ static int genhexKinematicsJacobian(const double * joints, double sb, cb, sc, cc; int i, m; - genhex_read_hal_pins(); + geometry_of(p, &g); /* the screw lead correction is a function of the pose too, and this does not differentiate it; difference the inverse instead */ - if (hal_get_real(haldata->screw_lead) != 0.0) { - return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, - joints, pos, iflags, jac); + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); } memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -590,9 +667,9 @@ static int genhexKinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double len; - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &strut); + pmCartCartSub(&aw, &g.b[i], &strut); pmCartMag(&strut, &len); if (len <= 0) { return -1; } pmCartScalMult(&strut, 1.0/len, &u); @@ -608,145 +685,17 @@ static int genhexKinematicsJacobian(const double * joints, } } return 0; -} // genhexKinematicsJacobian() - -// HAL pin initializaion values. In small arrays so we can easily -// address them in the pin creation loop. -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, - DEFAULT_BASE_3_X, DEFAULT_BASE_4_X, DEFAULT_BASE_5_X, -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, - DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y, DEFAULT_BASE_5_Y, -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, - DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z, DEFAULT_BASE_5_Z, -}; -static const rtapi_real init_platformx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_2_X, - DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_5_X, -}; -static const rtapi_real init_platformy[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_2_Y, - DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_5_Y, -}; -static const rtapi_real init_platformz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Z, DEFAULT_PLATFORM_1_Z, DEFAULT_PLATFORM_2_Z, - DEFAULT_PLATFORM_3_Z, DEFAULT_PLATFORM_4_Z, DEFAULT_PLATFORM_5_Z, -}; -static const rtapi_real init_basenx[NUM_STRUTS] = { - DEFAULT_BASE_0_NX, DEFAULT_BASE_1_NX, DEFAULT_BASE_2_NX, - DEFAULT_BASE_3_NX, DEFAULT_BASE_4_NX, DEFAULT_BASE_5_NX, -}; -static const rtapi_real init_baseny[NUM_STRUTS] = { - DEFAULT_BASE_0_NY, DEFAULT_BASE_1_NY, DEFAULT_BASE_2_NY, - DEFAULT_BASE_3_NY, DEFAULT_BASE_4_NY, DEFAULT_BASE_5_NY, +} // genhex_jacobian() + +// the forward iterates from the pose it is handed, so it is seeded with +// the last answer after a switch +static const kins_ops genhex_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, + .jacobian = genhex_jacobian, + .fwd_iterates = 1, + .primary = 1, }; -static const rtapi_real init_basenz[NUM_STRUTS] = { - DEFAULT_BASE_0_NZ, DEFAULT_BASE_1_NZ, DEFAULT_BASE_2_NZ, - DEFAULT_BASE_3_NZ, DEFAULT_BASE_4_NZ, DEFAULT_BASE_5_NZ, -}; -static const rtapi_real init_platformnx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_2_NX, - DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_5_NX, -}; -static const rtapi_real init_platformny[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_2_NY, - DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_5_NY, -}; -static const rtapi_real init_platformnz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NZ, DEFAULT_PLATFORM_1_NZ, DEFAULT_PLATFORM_2_NZ, - DEFAULT_PLATFORM_3_NZ, DEFAULT_PLATFORM_4_NZ, DEFAULT_PLATFORM_5_NZ, -}; - -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - - if (kp->max_joints < 0 || kp->max_joints > NUM_STRUTS) { - rtapi_print_msg(RTAPI_MSG_ERR, "genhexKinematicsSetup: max_joints %d less than 0 or larger NUM_STRUTS %d\n", - kp->max_joints, NUM_STRUTS); - return -1; - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) { - rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); - return -1; - } - - for (i = 0; i < kp->max_joints; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->basex[i]), - init_basex[i], "%s.base.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basey[i], - init_basey[i], "%s.base.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basez[i], - init_basez[i], "%s.base.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformx[i], - init_platformx[i], "%s.platform.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformy[i], - init_platformy[i], "%s.platform.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformz[i], - init_platformz[i], "%s.platform.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenx[i], - init_basenx[i], "%s.base-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->baseny[i], - init_baseny[i], "%s.base-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenz[i], - init_basenz[i], "%s.base-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnx[i], - init_platformnx[i], "%s.platform-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformny[i], - init_platformny[i], "%s.platform-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnz[i], - init_platformnz[i], "%s.platform-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_OUT, &haldata->correction[i], - 0.0, "%s.correction.%d", kp->halprefix, i); - if (res) {goto error;} - } - - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "genhexkins.last-iterations"); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "genhexkins.max-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->max_error, - 500.0, "genhexkins.max-error"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->conv_criterion, - 1e-9, "genhexkins.convergence-criterion"); - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->iter_limit, - 120, "genhexkins.limit-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "genhexkins.tool-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->spindle_offset, - 0.0, "genhexkins.spindle-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->screw_lead, - DEFAULT_SCREW_LEAD, "genhexkins.screw-lead"); - - if (res) {goto error;} - - //note: switchkins does not uses these as it provides gui.x, gui.y, etc. - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_x, 0.0, "genhexkins.x"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_y, 0.0, "genhexkins.y"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_z, 0.0, "genhexkins.z"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_a, 0.0, "genhexkins.a"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_b, 0.0, "genhexkins.b"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_c, 0.0, "genhexkins.c"); - - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->fwd_kins_fail, - 0, "genhexkins.fwd-kins-fail"); - - if (res) goto error; - return 0; - -error: - return res; -} // genhexKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -754,46 +703,32 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genhexkins"; // !!! must agree with filename kp->halprefix = "genhexkins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = genhex_params; + kp->nparams = P_COUNT; + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); kp->fwd_iterates_mask = 0x2; //genhexkins switchkins_type==1 kp->gui_kinstype = 1; //vismach gui for switchkins_type==1 - - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genhexKinematicsSetup; - *kfwd1 = genhexKinematicsForward; - *kinv1 = genhexKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genhexKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &genhex_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 - - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - switchkinsRegisterJacobian(0, genhexKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &genhex_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } //switchkinsSetup() diff --git a/src/emc/kinematics/genhexkins.h b/src/emc/kinematics/genhexkins.h index 671419efcc9..60b48f7ec54 100644 --- a/src/emc/kinematics/genhexkins.h +++ b/src/emc/kinematics/genhexkins.h @@ -20,7 +20,7 @@ #ifndef GENHEXKINS_H #define GENHEXKINS_H -#include +#include #include #define GENHEX_MAX_JOINTS 6 diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 02703735b2a..c43156490c9 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -28,6 +28,11 @@ Currently the type of the joints is hardcoded to ANGULAR, although the kins support both ANGULAR and LINEAR axes. + The maths is written as pure functions of the parameter block (see + kins_module.h): the pins are the table below, read into the block + before every call, and the link description is built from the block + on each call. + TODO: * make number of joints a loadtime parameter * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR @@ -41,7 +46,7 @@ #include #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include #include "genserkins.h" /* these decls */ @@ -49,44 +54,53 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2640 - #pragma GCC diagnostic warning "-Wframe-larger-than=2648" +// genser_inverse() is 2640 plus the link description it builds + #pragma GCC diagnostic warning "-Wframe-larger-than=3400" #endif -static struct haldata { - hal_uint_t max_iterations; - hal_uint_t last_iterations; - hal_real_t a[GENSER_MAX_JOINTS]; - hal_real_t alpha[GENSER_MAX_JOINTS]; - hal_real_t d[GENSER_MAX_JOINTS]; - hal_sint_t unrotate[GENSER_MAX_JOINTS]; - genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it - // only once in genserKinematicsSetup() -} *haldata = NULL; - -static int total_joints; -double j[GENSER_MAX_JOINTS]; +// the table: four entries per joint, then the iteration count in and out +#define P_A(i) (4*(i) + 0) +#define P_ALPHA(i) (4*(i) + 1) +#define P_D(i) (4*(i) + 2) +#define P_UNROT(i) (4*(i) + 3) +enum { + P_LAST_ITER = 4*GENSER_MAX_JOINTS, + P_MAX_ITER, + P_COUNT +}; -#define KINS_PTR (haldata->kins) +#define JOINT_ROWS(i, a, alpha, d) \ + { "A-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, a }, \ + { "ALPHA-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, alpha }, \ + { "D-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, d }, \ + { "unrotate-" #i, KINS_PARAM_S32, KINS_IN, 0, 0 } + +const kins_param_desc GENSER_PARAMS[P_COUNT] = { + JOINT_ROWS(0, DEFAULT_A1, DEFAULT_ALPHA1, DEFAULT_D1), + JOINT_ROWS(1, DEFAULT_A2, DEFAULT_ALPHA2, DEFAULT_D2), + JOINT_ROWS(2, DEFAULT_A3, DEFAULT_ALPHA3, DEFAULT_D3), + JOINT_ROWS(3, DEFAULT_A4, DEFAULT_ALPHA4, DEFAULT_D4), + JOINT_ROWS(4, DEFAULT_A5, DEFAULT_ALPHA5, DEFAULT_D5), + JOINT_ROWS(5, DEFAULT_A6, DEFAULT_ALPHA6, DEFAULT_D6), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_IN, 0, GENSER_DEFAULT_MAX_ITERATIONS }, +}; +const int GENSER_NPARAMS = P_COUNT; #if GENSER_MAX_JOINTS < 6 #error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h #endif -static int genser_hal_inited = 0; - -int genser_kin_init(void) { - genser_struct *genser = KINS_PTR; +void genser_links_of(const kins_params *p, genser_struct *genser) { int t; static volatile double tst=0;tst=sqrt(tst); // ensure -lm used /* init them all and make them revolute joints */ /* FIXME: should allow LINEAR joints based on HAL param too */ for (t = 0; t < GENSER_MAX_JOINTS; t++) { - genser->links[t].u.dh.a = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); + genser->links[t].u.dh.a = p->geometry[P_A(t)]; + genser->links[t].u.dh.alpha = p->geometry[P_ALPHA(t)]; + genser->links[t].u.dh.d = p->geometry[P_D(t)]; genser->links[t].u.dh.theta = 0; genser->links[t].type = GO_LINK_DH; genser->links[t].quantity = GO_QUANTITY_ANGLE; @@ -95,8 +109,13 @@ int genser_kin_init(void) { /* set a select few to make it PUMA-like */ // FIXME-AJ: make a hal pin, also set number of joints based on it genser->link_num = 6; + genser->iterations = 0; +} // genser_links_of() - return GO_RESULT_OK; +/* the unrotate coupling of one joint, from the block */ +static rtapi_s32 unrotate_of(const kins_params *p, int link) +{ + return (rtapi_s32)p->geometry[P_UNROT(link)]; } /* compute the forward jacobian function: @@ -315,7 +334,7 @@ int genser_kin_jac_fwd(void *kins, } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose - word in EmcPose units, the derivative of genserKinematicsInverse(). + word in EmcPose units, the derivative of genser_inverse(). compute_jinv() gives the geometric inverse Jacobian, radians of joint per unit of base-frame twist. A pose word rate is not a twist: the roll, @@ -327,13 +346,14 @@ int genser_kin_jac_fwd(void *kins, with the unit conversions and the unrotate coupling applied in the order the inverse applies them. */ -int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int genser_jacobian(const kins_params *p, const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)iflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; @@ -343,21 +363,14 @@ int genserKinematicsJacobian(const double *joint, double sb, cb, sc, cc; int link, i, a, m, retval; -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsJacobian: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // the kinematic joint angles, in radians and with the unrotate // coupling removed, exactly as the forward prepares them for (link = 0; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); jest[link] = joint[link] * (PM_PI / 180); if (link && unrotate) jest[link] -= unrotate * jest[link-1]; @@ -403,7 +416,7 @@ int genserKinematicsJacobian(const double *joint, // the unrotate coupling, in link order as the inverse applies it for (link = 1; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if (unrotate) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[link][a] += unrotate * jac[link-1][a]; @@ -412,86 +425,74 @@ int genserKinematicsJacobian(const double *joint, } // uvw pass through as joints 6, 7, 8 - if (total_joints > 6) jac[6][6] = 1; - if (total_joints > 7) jac[7][7] = 1; - if (total_joints > 8) jac[8][8] = 1; + if (p->max_joints > 6) jac[6][6] = 1; + if (p->max_joints > 7) jac[7][7] = 1; + if (p->max_joints > 8) jac[8][8] = 1; return 0; -} // genserKinematicsJacobian() +} // genser_jacobian() /* main function called by emc2 for forward Kins */ -int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) { +static int genser_forward(const kins_params *p, kins_scratch *s, + const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - go_pose *pos; + genser_struct genser; + go_pose pos; go_rpy rpy; go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints int ret = 0; - int i, changed=0; - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsForward: not initialized\n"); - return -1; - } + int i; + + genser_links_of(p, &genser); for (i=0; i< 6; i++) { - // FIXME - debug hack - if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; // convert to radians to pass to genser_kin_fwd jcopy[i] = joint[i] * PM_PI / 180; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); + rtapi_s32 unrotate = unrotate_of(p, i); if ((i) && unrotate) jcopy[i] -= unrotate * jcopy[i-1]; } - if (changed) { - for (i=0; i< 6; i++) - j[i] = joint[i]; - // rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n", - //joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); - } // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler // angles) // to go angles (quaternions) - pos = haldata->pos; rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &pos->rot); - pos->tran.x = world->tran.x; - pos->tran.y = world->tran.y; - pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; //pass through unused 678 as uvw - if (total_joints > 6) world->u = joint[6]; - if (total_joints > 7) world->v = joint[7]; - if (total_joints > 8) world->w = joint[8]; + if (p->max_joints > 6) world->u = joint[6]; + if (p->max_joints > 7) world->v = joint[7]; + if (p->max_joints > 8) world->w = joint[8]; // pos will be the world location // jcopy: joitn position in radians - ret = genser_kin_fwd(KINS_PTR, jcopy, pos); + ret = genser_kin_fwd(&genser, jcopy, &pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); + ret = go_quat_rpy_convert(&pos.rot, &rpy); if (ret < 0) return ret; - world->tran.x = pos->tran.x; - world->tran.y = pos->tran.y; - world->tran.z = pos->tran.z; + world->tran.x = pos.tran.x; + world->tran.y = pos.tran.y; + world->tran.z = pos.tran.z; world->a = rpy.r * 180 / PM_PI; world->b = rpy.p * 180 / PM_PI; world->c = rpy.y * 180 / PM_PI; - if (changed) { -// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - } return 0; } @@ -503,8 +504,6 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) int link; int retval; - genser_kin_init(); - for (link = 0; link < genser->link_num; link++) { retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); if (GO_RESULT_OK != retval) @@ -518,22 +517,25 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) return GO_RESULT_OK; } -int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genser_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); go_pose T_L_0; go_real dvw[6]; go_real jest[GENSER_MAX_JOINTS]; go_real dj[GENSER_MAX_JOINTS]; - go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + go_pose pos; // converted pose from EmcPose + go_pose pest, pestinv, Tdelta; go_rpy rpy; go_rvec rvec; go_cart cart; @@ -541,30 +543,19 @@ int genserKinematicsInverse(const EmcPose * world, int link; int smalls; int retval; + const unsigned max_iterations = (unsigned)p->geometry[P_MAX_ITER]; - // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); - -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsInverse: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); // FIXME-AJ: rpy or zyx ? rpy.y = world->c * PM_PI / 180; rpy.p = world->b * PM_PI / 180; rpy.r = world->a * PM_PI / 180; - go_rpy_quat_convert(&rpy, &haldata->pos->rot); - haldata->pos->tran.x = world->tran.x; - haldata->pos->tran.y = world->tran.y; - haldata->pos->tran.z = world->tran.z; + go_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + pos.tran.z = world->tran.z; go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); @@ -576,9 +567,10 @@ int genserKinematicsInverse(const EmcPose * world, } for (genser->iterations = 0; - genser->iterations < hal_get_ui32(haldata->max_iterations); + genser->iterations < max_iterations; genser->iterations++) { - hal_set_ui32(haldata->last_iterations, genser->iterations); + s->iterations = genser->iterations; + s->out[P_LAST_ITER] = genser->iterations; /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); @@ -597,8 +589,7 @@ int genserKinematicsInverse(const EmcPose * world, } /* pest is the resulting pose estimate given joint estimate */ - genser_kin_fwd(KINS_PTR, jest, &pest); - //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); + genser_kin_fwd(genser, jest, &pest); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* @@ -612,7 +603,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + go_pose_pose_mult(&pestinv, &pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -641,9 +632,9 @@ int genserKinematicsInverse(const EmcPose * world, go_matrix_vector_mult(&Jinv, dvw, dj); //pass through 678 as uvw - if (total_joints > 6) joints[6] = world->u; - if (total_joints > 7) joints[7] = world->v; - if (total_joints > 8) joints[8] = world->w; + if (p->max_joints > 6) joints[6] = world->u; + if (p->max_joints > 7) joints[7] = world->v; + if (p->max_joints > 8) joints[8] = world->w; /* check for small joint increments, if so we're done */ for (link = 0, smalls = 0; link < genser->link_num; link++) { @@ -660,14 +651,10 @@ int genserKinematicsInverse(const EmcPose * world, for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if ((link) && unrotate) joints[link] += unrotate * joints[link-1]; } - //rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", - // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); - //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", - // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); return GO_RESULT_OK; } /* else keep iterating */ @@ -681,6 +668,13 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, + .primary = 1, +}; + /* Extras, not callable using go_kin_ wrapper but if you know you have linked in these kinematics, go ahead and call these for your ad hoc @@ -691,68 +685,3 @@ int genser_kin_inv_iterations(genser_struct * genser) { return genser->iterations; } - -int genser_kin_inv_set_max_iterations(int i) -{ - if (i <= 0) return GO_RESULT_ERROR; - hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; -} - -int genser_kin_inv_get_max_iterations() -{ - return hal_get_ui32(haldata->max_iterations); -} - -static const rtapi_real init_a[GENSER_MAX_JOINTS] = { - DEFAULT_A1, DEFAULT_A2, DEFAULT_A3, DEFAULT_A4, DEFAULT_A5, DEFAULT_A6 -}; -static const rtapi_real init_alpha[GENSER_MAX_JOINTS] = { - DEFAULT_ALPHA1, DEFAULT_ALPHA2, DEFAULT_ALPHA3, DEFAULT_ALPHA4, DEFAULT_ALPHA5, DEFAULT_ALPHA6 -}; -static const rtapi_real init_d[GENSER_MAX_JOINTS] = { - DEFAULT_D1, DEFAULT_D2, DEFAULT_D3, DEFAULT_D4, DEFAULT_D5, DEFAULT_D6 -}; - - -int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - // allow for pass through joints 6,7,8 u,v,w - total_joints = kp->max_joints; - - // only the first 6 joints have A,ALPHA,D,unrotate pins - for (i = 0; i < GENSER_MAX_JOINTS; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a[i]), - init_a[i], "%s.A-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->alpha[i]), - init_alpha[i], "%s.ALPHA-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d[i]), - init_d[i], "%s.D-%d", kp->halprefix, i); - res += hal_pin_new_si32(comp_id, HAL_IN, &(haldata->unrotate[i]), - 0, "%s.unrotate-%d", kp->halprefix, i); - } - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->last_iterations), - 0, "%s.last-iterations",kp->halprefix); - - KINS_PTR = hal_malloc(sizeof(genser_struct)); - haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); - if (KINS_PTR == NULL) {goto error;} - if (haldata->pos == NULL) {goto error;} - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, - GENSER_DEFAULT_MAX_ITERATIONS, "%s.max-iterations",kp->halprefix); - - if (res) {goto error;} - - genser_hal_inited = 1; - return 0; - -error: - return -1; -} // genserKinematicsSetup() diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index be71f33ffc5..f2783b31951 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -4,7 +4,8 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +* 2) the maths and the geometry table are in genserfuncs.c, written as +* pure functions of the parameter block (see kins_module.h) */ /******************************************************************** @@ -43,6 +44,7 @@ frame-larger-than: #include "genserkins.h" #include +#include "userkfuncs.h" //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON @@ -57,41 +59,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "genserkins"; // !!! must agree with filename kp->halprefix = "genserkins"; // hal pin names kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8 kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; + kp->params = GENSER_PARAMS; + kp->nparams = GENSER_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genserKinematicsSetup; - *kfwd1 = genserKinematicsForward; - *kinv1 = genserKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genserKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &GENSER_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - switchkinsRegisterJacobian(0, genserKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &GENSER_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index b74b826d2ec..c50cddde3e3 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -36,7 +36,7 @@ #include /* HAL data types */ #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include /*! The maximum number of joints supported by the general serial @@ -81,8 +81,6 @@ typedef struct { extern int genser_kin_size(void); -extern int genser_kin_init(void); - extern const char * genser_kin_get_name(void); extern int genser_kin_num_joints(void * kins); @@ -125,15 +123,6 @@ extern int genser_kin_fwd_interations(genser_struct * genser); inverse kinematics functions */ extern int genser_kin_inv_iterations(genser_struct * genser); -/*! Sets the maximum number of iterations to use in future calls to - the inverse kinematics functions, after which an error will be - reported */ -extern int genser_kin_inv_set_max_iterations(int i); - -/*! Returns the maximum number of iterations that will be used to - compute inverse kinematics functions */ -extern int genser_kin_inv_get_max_iterations(void); - extern int compute_jfwd(go_link * link_params, int link_number, go_matrix * Jfwd, @@ -142,23 +131,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -extern int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); +/* The kinematics as functions of the parameter block (see kins_module.h): + the DH parameters and the unrotate couplings are the table, the maths + is the ops. genser_links_of() fills a link description from a block, + for a caller that wants the go_ routines directly. */ +extern const kins_param_desc GENSER_PARAMS[]; +extern const int GENSER_NPARAMS; +extern const kins_ops GENSER_OPS; -extern int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); +extern void genser_links_of(const kins_params *p, genser_struct *genser); #endif diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c new file mode 100644 index 00000000000..839313ef965 --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,175 @@ +/******************************************************************** +* Description: kins_single.c +* The classic kinematics entry points for a module with one kinematics +* type written as pure functions. The module hands its description to +* kinsSingleInit(); this file keeps the one RT parameter block, +* fills it from the pins before every call, and hands the call to the +* module's ops. It is the counterpart of switchkins.c for a module that +* does not switch. +* +* License: GPL Version 2 +********************************************************************/ + +#include +#include +#include + +#include +#include + +static kins_params rt_params; +static kins_scratch rt_scratch; +static kins_pin_ref *pins; +static kins_tool_source tool_source; +static int inited; +static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; +static const kins_module_info *km; + +static const kins_ops *ops(void) +{ + return inited ? km->ops[0] : NULL; +} + +// the block sees the pins as they are now, and the tool motion sent +static void read_pins(void) +{ + kinsParamsPinsRead(pins, km->params, km->nparams, + &rt_params); + kinsToolSourceApply(&tool_source, km->halprefix, + km->params, km->nparams, &rt_params); +} + +static void write_pins(void) +{ + kinsParamsPinsWrite(pins, km->params, km->nparams, + &rt_scratch); +} + +int kinsSingleInit(int comp_id, const kins_module_info *m, + const char *coordinates, KINEMATICS_TYPE reported) +{ + if (!m) { return -1; } + km = m; + if (!km->ops[0] || !km->ops[0]->forward + || !km->ops[0]->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsSingleInit: %s supplies no forward or inverse\n", + km->name ? km->name : "?"); + return -1; + } + if (kinsParamsInit(&rt_params, km, coordinates)) { return -1; } + kinsScratchInit(&rt_scratch); + if (kinsParamsPinsCreate(comp_id, km->halprefix, + km->params, km->nparams, + &pins)) { + return -1; + } + reported_type = reported; + inited = 1; + return 0; +} // kinsSingleInit() + +int kinematicsForward(const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsForward(ops(), &rt_params, &rt_scratch, joint, pos, fflags, iflags); + write_pins(); + return r; +} + +int kinematicsInverse(const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsInverse(ops(), &rt_params, &rt_scratch, pos, joint, iflags, fflags); + write_pins(); + return r; +} + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsJacobian(const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); +} + +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// one type and nothing declared about it: -1, no information +int kinematicsTypeFlags(int ktype) +{ + (void)ktype; + return -1; +} + +// The module's description, for a copy of it loaded outside RT. A module +// with one type does not depend on its parameters for its shape, so this +// is the table as declared. +int kinsSingleDescribe(const kins_module_info *m, kins_module_info *info) +{ + if (!m || !info) { return -1; } + *info = *m; + info->ntypes = 1; + return 0; +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(kinsSingleInit); +EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/kins_single_core.c b/src/emc/kinematics/kins_single_core.c new file mode 100644 index 00000000000..f4b58f1204a --- /dev/null +++ b/src/emc/kinematics/kins_single_core.c @@ -0,0 +1,41 @@ +/* + 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. +*/ + +/* kins_single_core.c makes kins_single.c and kins_util.c a module of their +* own, for a kinematics module with one type built out of tree. The +* module loaded after it hands its description to kinsSingleInit(), and +* motion reaches the kinematics through the entry points exported here. +* +* loadrt kins_single_core +* loadrt [KINS]KINEMATICS +*/ +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + comp_id = hal_init("kins_single_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/emc/kinematics/kins_single_module.c b/src/emc/kinematics/kins_single_module.c new file mode 100644 index 00000000000..56476267666 --- /dev/null +++ b/src/emc/kinematics/kins_single_module.c @@ -0,0 +1,23 @@ +/******************************************************************** +* Description: kins_single_module.c +* kinsDescribe() for an in-tree module with one kinematics type: the +* description the module names kins_module, for a copy of the module +* loaded outside RT. Kept apart from kins_single.c so that file can be +* loaded as kins_single_core and serve a module built out of tree. +* +* License: GPL Version 2 +********************************************************************/ + +#include + +#include + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + return kinsSingleDescribe(&kins_module, info); +} + +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 113abd50867..b66b3d9b9e7 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -48,7 +48,9 @@ #include #include #include -#include +#include +#include +#include // principal joint numbers based on module 'coordinates' parameter static int JX = -1; @@ -76,38 +78,23 @@ static int map_initialized = 0; #define MAX_COORDINATES_CHARS 32 static char used_coordinates[MAX_COORDINATES_CHARS+1]; -int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[] ) //result +// Letters to joint numbers, in order, with the checks every caller wants: +// a valid letter set, at most max_joints of them, duplicates only where +// allowed. Fills axis_idx_for_jno (-1 past the last letter) and touches +// nothing else, so the block form and the static form share it. +static int kins_scan_coordinates(const char *coordinates, + int max_joints, + int allow_duplicates, + int axis_idx_for_jno[], + const char *errtag) { - char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; - int jno=0; - bool found=0; + int jno = 0; + bool found = 0; int dups[EMCMOT_MAX_AXIS]; const char *coords = coordinates; char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; int i; - if (strlen(coordinates) > MAX_COORDINATES_CHARS) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers too many chars:%s\n" - ,__FILE__,coordinates); - return -1; - - } - // Note: may be called multiple times for different switchkins - // types but coordinates must agree - if (used_coordinates[0] == 0) { - strcpy(used_coordinates,coordinates); - } else { - if (strcasecmp(coordinates,used_coordinates)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers altered:%s %s\n" - ,__FILE__,used_coordinates,coordinates); - return -1; - } - } for (i=0; i EMCMOT_MAX_JOINTS) ) { @@ -168,6 +155,40 @@ int map_coordinates_to_jnumbers(const char *coordinates, } } } + return 0; +} // kins_scan_coordinates() + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, errtag)) { + return -1; + } for (jno=0; jno < max_joints; jno++) { int bitnumber = 1< Axis %c\n", jno,*(p+axis_idx_for_jno[jno])); } +#ifndef ULAPI + // the module's own report of its type; this file is also built + // outside RT, where there is no module around it if (kinematicsType() != KINEMATICS_BOTH) { rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); } +#endif rtapi_print("\n"); } @@ -559,6 +583,9 @@ int identityKinematicsToolFrame(const double *joints, #define TFS_ITERS 60 #define TFS_FD_STEP 1e-6 // internal radians #define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_PROBE_STEP 0.05 // joint units, to read a joint's own axis +#define TFS_CARRY_STEP 40.0 // joint units, far enough to swing a carried axis +#define TFS_CARRY_TOL 1e-6 // axes closer than this counted as the same #define TFS_RANK_TOL 1e-4 // a direction worth less than this is free #define TFS_SOLVED 1e-18 // sum of squared residuals #define TFS_STEP_LIMIT 0.4 // internal radians per iteration @@ -967,6 +994,127 @@ static int tfs_spin(tfs_ctx *c, const double *joint, return 0; } +// A rotation's nine entries as one array, columns of axes: element +// (row, col) is c[3*col + row]. +static void tfs_entries(const PmRotationMatrix *m, double c[9]) +{ + c[0] = m->x.x; c[1] = m->x.y; c[2] = m->x.z; + c[3] = m->y.x; c[4] = m->y.y; c[5] = m->y.z; + c[6] = m->z.x; c[7] = m->z.y; c[8] = m->z.z; +} + +int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix base, moved; + double joint[EMCMOT_MAX_JOINTS]; + int i, j; + + if (!work || !seed || !mask || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *mask = 0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + if (work(joint, &base, &fflags)) { return -1; } + + // a step of one joint unit: a degree on every module in the tree, and + // a linear joint never turns a frame whatever its unit + for (j = 0; j < num_joints; j++) { + double diff = 0, a[9], b[9]; + + joint[j] = seed[j] + 1.0; + if (work(joint, &moved, &fflags)) { return -1; } + joint[j] = seed[j]; + tfs_entries(&base, a); + tfs_entries(&moved, b); + for (i = 0; i < 9; i++) { diff += fabs(a[i] - b[i]); } + if (diff > TFS_MOVED_TOL) { *mask |= 1u << j; } + } + return 0; +} + +// The axis a joint turns the tool frame about, in machine coordinates: move +// the joint a little and read the rotation that took the frame there. +// Returns 0 and a unit axis where the joint turns the tool, 1 where it does +// not, which is every linear joint and every joint the module ignores. +static int tfs_joint_axis(kinsFrameFunc tool, const double *joint, + int j, double step, double axis[3]) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix r1, r2; + double moved[EMCMOT_MAX_JOINTS]; + double a[9], b[9]; + double len; + int i; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { moved[i] = joint[i]; } + moved[j] += step; + if (tool(joint, &r1, &fflags)) { return -1; } + if (tool(moved, &r2, &fflags)) { return -1; } + + // The rotation from one frame to the other is r2 * transpose(r1), and + // the axis of a small rotation is the skew part of it. Both matrices + // are columns of axes, so element (row, col) is entry 3*col + row. + tfs_entries(&r1, a); + tfs_entries(&r2, b); + axis[0] = axis[1] = axis[2] = 0; + for (i = 0; i < 3; i++) { + // m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] + axis[0] += b[3*i + 2] * a[3*i + 1] - b[3*i + 1] * a[3*i + 2]; + axis[1] += b[3*i + 0] * a[3*i + 2] - b[3*i + 2] * a[3*i + 0]; + axis[2] += b[3*i + 1] * a[3*i + 0] - b[3*i + 0] * a[3*i + 1]; + } + len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); + if (len < 1e-9) { return 1; } + for (i = 0; i < 3; i++) { axis[i] /= len; } + return 0; +} + +int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, int *primary, int *secondary) +{ + double joint[EMCMOT_MAX_JOINTS], elsewhere[EMCMOT_MAX_JOINTS]; + double axis[3], turned[3]; + int turns[2], count = 0, carried = -1; + int i, j, k, r; + + if (!tool || !seed || !primary || !secondary + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *primary = *secondary = -1; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + + for (j = 0; j < num_joints; j++) { + r = tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis); + if (r < 0) { return -1; } + if (r > 0) { continue; } + if (count >= 2) { return -1; } // a wrist, not a head + turns[count++] = j; + } + if (count != 2) { return -1; } + + // whichever axis swings when the other joint moves is the carried one + for (i = 0; i < 2; i++) { + j = turns[i]; + k = turns[1 - i]; + if (tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis) != 0) { return -1; } + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { elsewhere[r] = joint[r]; } + elsewhere[k] += TFS_CARRY_STEP; + if (tfs_joint_axis(tool, elsewhere, j, TFS_PROBE_STEP, turned) != 0) { return -1; } + if (fabs(axis[0]*turned[0] + axis[1]*turned[1] + axis[2]*turned[2] - 1.0) + > TFS_CARRY_TOL) { + if (carried >= 0) { return -1; } // both carried: not a head + carried = j; + } + } + if (carried < 0) { return -1; } + *secondary = carried; + *primary = (turns[0] == carried) ? turns[1] : turns[0]; + return 0; +} + int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, @@ -1304,9 +1452,450 @@ int identityKinematicsJacobian(const double *joint, jac); } // identityKinematicsJacobian() +//---------------------------------------------------------------------- +// The parameter block. See kinematics.h for what it is for. +//---------------------------------------------------------------------- + +int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates) +{ + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno, a; + + if (!p) { return -1; } + if (!coordinates) { coordinates = "XYZABCUVW"; } + + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, + "kinsParamsMapCoordinates: ERROR:\n ")) { + return -1; + } + + // every letter the module cannot do without has to be there + for (a = 0; required_coordinates && required_coordinates[a]; a++) { + char want = required_coordinates[a]; + const char *c; + int seen = 0; + for (c = coordinates; *c; c++) { + if (*c == want || *c == want + ('a' - 'A') || *c == want - ('a' - 'A')) { + seen = 1; break; + } + } + if (!seen) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsMapCoordinates: ERROR:\n required coordinates:%s\n" + " specified coordinates:%s\n", + required_coordinates, coordinates); + return -1; + } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p->joint_of_axis[a] = -1; + p->joints_of_axis[a] = 0; + } + p->max_joints = 0; + for (jno = 0; jno < EMCMOT_MAX_JOINTS; jno++) { + a = axis_idx_for_jno[jno]; + if (a < 0) { break; } + if (p->joint_of_axis[a] < 0) { p->joint_of_axis[a] = jno; } + p->joints_of_axis[a] |= 1 << jno; + p->max_joints = jno + 1; + } + return 0; +} // kinsParamsMapCoordinates() + +int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates) +{ + int i; + + if (!p || !info) { return -1; } + if (info->nparams < 0 || info->nparams > KINS_MAX_PARAMS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsInit: %s declares %d parameters, at most %d allowed\n", + info->name ? info->name : "?", info->nparams, KINS_MAX_PARAMS); + return -1; + } + + memset(p, 0, sizeof(*p)); + p->size = sizeof(*p); + p->ktype = 0; + if (!coordinates) { coordinates = info->required_coordinates; } + if (kinsParamsMapCoordinates(p, coordinates, info->max_joints, + info->allow_duplicates, + info->required_coordinates)) { + return -1; + } + for (i = 0; i < info->nparams; i++) { + p->geometry[i] = info->params[i].dflt; + if (info->params[i].tool) { p->tool.tran.z = info->params[i].dflt; } + } + return 0; +} // kinsParamsInit() + +void kinsScratchInit(kins_scratch *s) +{ + if (s) { memset(s, 0, sizeof(*s)); } +} + +int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos) +{ + int a; + if (!p || !joints || !pos) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j = p->joint_of_axis[a]; + if (j < 0) { continue; } + switch (a) { + case 0: pos->tran.x = joints[j]; break; + case 1: pos->tran.y = joints[j]; break; + case 2: pos->tran.z = joints[j]; break; + case 3: pos->a = joints[j]; break; + case 4: pos->b = joints[j]; break; + case 5: pos->c = joints[j]; break; + case 6: pos->u = joints[j]; break; + case 7: pos->v = joints[j]; break; + default: pos->w = joints[j]; break; + } + } + return 0; +} // kinsMappedJointsToPose() + +static double kins_pose_coord(const EmcPose *pos, int a) +{ + switch (a) { + case 0: return pos->tran.x; + case 1: return pos->tran.y; + case 2: return pos->tran.z; + case 3: return pos->a; + case 4: return pos->b; + case 5: return pos->c; + case 6: return pos->u; + case 7: return pos->v; + default: return pos->w; + } +} + +int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints) +{ + int a, jno; + if (!p || !pos || !joints) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (bits & (1 << jno)) { joints[jno] = kins_pose_coord(pos, a); } + } + } + return 0; +} // kinsPoseToMappedJoints() + +int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int a, jno, col; + if (!p || !dP || !jac) { return -1; } + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (!(bits & (1 << jno))) { continue; } + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[jno][col] = dP[a][col]; } + } + } + return 0; +} // kinsJacobianFromMappedAxesP() + +//---------------------------------------------------------------------- +// identity through the block +//---------------------------------------------------------------------- + +int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return kinsMappedJointsToPose(p, joint, pos); +} + +int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return kinsPoseToMappedJoints(p, pos, joint); +} + +int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; (void)joint; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + (void)joint; (void)pos; (void)iflags; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = (a == b) ? 1.0 : 0.0; } + } + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); +} + +const kins_ops KINS_IDENTITY_OPS = { + .forward = kinsIdentityForward, + .inverse = kinsIdentityInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = kinsIdentityJacobian, + .fwd_iterates = 0, + .identity = 1, +}; + +//---------------------------------------------------------------------- +// asking an ops table, defaults applied +//---------------------------------------------------------------------- + +int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!ops || !ops->forward || !p || !s) { return -1; } + if (ops->fwd_iterates && s->have_pose_seed) { + /* no pose of our own yet: start from the caller's estimate, + which stays in *pos, rather than from a never-solved seed */ + if (s->pose_seed_ok) { *pos = s->pose_seed; } + s->have_pose_seed = 0; + } + r = ops->forward(p, s, joint, pos, fflags, iflags); + if (ops->fwd_iterates && r == 0) { + /* keep the result only when the solve succeeds */ + s->pose_seed = *pos; + s->pose_seed_ok = 1; + } + return r; +} + +int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !ops->inverse || !p || !s) { return -1; } + return ops->inverse(p, s, pos, joint, iflags, fflags); +} + +int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (!ops->work) { return -1; } // not supplied; not an error + return ops->work(p, joint, rot, fflags); +} + +int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (!ops->tool) { return -1; } // not supplied; not an error + r = ops->tool(p, joint, rot, fflags); + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + +int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose q; + int j, a; + + if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } + if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + if (!ops->inverse) { return -1; } + + // the same differences as kinsJacobianFromInverse(), on the block form + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + q = *pos; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&q, a) += KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&q, a) -= 2 * KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < p->max_joints && j < EMCMOT_MAX_JOINTS; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsOpsJacobian() + +//---------------------------------------------------------------------- +// the RT side of the table: one HAL pin per entry, copied into the block +// before a call and out of the scratch after it +//---------------------------------------------------------------------- + +int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out) +{ + kins_pin_ref *pins; + int i, res = 0; + + if (!out) { return -1; } + *out = NULL; + if (nparams < 0 || nparams > KINS_MAX_PARAMS) { return -1; } + if (nparams == 0) { return 0; } + if (!params || !prefix) { return -1; } + + pins = hal_malloc(nparams * sizeof(*pins)); + if (!pins) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: hal_malloc failed\n"); + return -1; + } + for (i = 0; i < nparams; i++) { + const kins_param_desc *d = ¶ms[i]; + hal_pdir_t dir = d->dir == KINS_OUT ? HAL_OUT : d->dir == KINS_IO ? HAL_IO : HAL_IN; + switch (d->type) { + case KINS_PARAM_FLOAT: + res += hal_pin_new_real(comp_id, dir, &pins[i].r, d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_BIT: + res += hal_pin_new_bool(comp_id, dir, &pins[i].b, d->dflt != 0, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_S32: + res += hal_pin_new_si32(comp_id, dir, &pins[i].s, (rtapi_s32)d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_U32: + res += hal_pin_new_ui32(comp_id, dir, &pins[i].u, (rtapi_u32)d->dflt, "%s.%s", prefix, d->name); + break; + default: + res = -1; + } + } + if (res) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: pin create failed for %s\n", prefix); + return -1; + } + *out = pins; + return 0; +} // kinsParamsPinsCreate() + +void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!pins || !params || !p) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double v; + if (d->dir == KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: v = hal_get_real(pins[i].r); break; + case KINS_PARAM_BIT: v = hal_get_bool(pins[i].b) ? 1.0 : 0.0; break; + case KINS_PARAM_S32: v = hal_get_si32(pins[i].s); break; + case KINS_PARAM_U32: v = hal_get_ui32(pins[i].u); break; + default: v = 0; + } + p->geometry[i] = v; + if (d->tool) { p->tool.tran.z = v; } + } +} // kinsParamsPinsRead() + +void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s) +{ + int i; + if (!pins || !params || !s) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + if (d->dir != KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: hal_set_real(pins[i].r, s->out[i]); break; + case KINS_PARAM_BIT: hal_set_bool(pins[i].b, s->out[i] != 0); break; + case KINS_PARAM_S32: hal_set_si32(pins[i].s, (rtapi_s32)s->out[i]); break; + case KINS_PARAM_U32: hal_set_ui32(pins[i].u, (rtapi_u32)s->out[i]); break; + default: break; + } + } +} // kinsParamsPinsWrite() + +void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool) +{ + if (!src || !tool) { return; } + src->tool = *tool; + src->have = 1; +} // kinsToolSourceSet() + +void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!src || !p || !src->have) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double diff; + if (!d->tool || d->dir == KINS_OUT) { continue; } + /* a pin nobody set reads zero, which is not a disagreement */ + diff = p->geometry[i] - src->tool.tran.z; + if (p->geometry[i] != 0.0 && (diff > 1e-9 || diff < -1e-9)) { + if (src->disagreeing < 1000) { + src->disagreeing++; + } else if (!src->warned) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s.%s disagrees with the tool offset motion applies;" + " motion's is used, the pin is not needed\n", + prefix ? prefix : "kins", d->name); + src->warned = 1; + } + } else { + src->disagreeing = 0; + } + p->geometry[i] = src->tool.tran.z; + } + p->tool = src->tool; +} // kinsToolSourceApply() + +#ifdef RTAPI EXPORT_SYMBOL(map_coordinates_to_jnumbers); EXPORT_SYMBOL(mapped_joints_to_position); EXPORT_SYMBOL(position_to_mapped_joints); +EXPORT_SYMBOL(identityKinematicsAxisJointsBitmap); EXPORT_SYMBOL(identityKinematicsSetup); EXPORT_SYMBOL(identityKinematicsForward); EXPORT_SYMBOL(identityKinematicsInverse); @@ -1322,3 +1911,27 @@ EXPORT_SYMBOL(kinsJacobianFromMappedAxes); EXPORT_SYMBOL(kinsJacobianFromDhArm); EXPORT_SYMBOL(TOOL_FRAME_SPINDLE); EXPORT_SYMBOL(TOOL_FRAME_FLANGE); +EXPORT_SYMBOL(toolFrameWorkJoints); +EXPORT_SYMBOL(toolFrameOrientJoints); +EXPORT_SYMBOL(KINS_IDENTITY_OPS); +EXPORT_SYMBOL(kinsIdentityForward); +EXPORT_SYMBOL(kinsIdentityInverse); +EXPORT_SYMBOL(kinsIdentityFrame); +EXPORT_SYMBOL(kinsIdentityJacobian); +EXPORT_SYMBOL(kinsJacobianFromMappedAxesP); +EXPORT_SYMBOL(kinsMappedJointsToPose); +EXPORT_SYMBOL(kinsPoseToMappedJoints); +EXPORT_SYMBOL(kinsParamsInit); +EXPORT_SYMBOL(kinsParamsMapCoordinates); +EXPORT_SYMBOL(kinsScratchInit); +EXPORT_SYMBOL(kinsOpsForward); +EXPORT_SYMBOL(kinsOpsInverse); +EXPORT_SYMBOL(kinsOpsWorkFrame); +EXPORT_SYMBOL(kinsOpsToolFrame); +EXPORT_SYMBOL(kinsOpsJacobian); +EXPORT_SYMBOL(kinsParamsPinsCreate); +EXPORT_SYMBOL(kinsParamsPinsRead); +EXPORT_SYMBOL(kinsParamsPinsWrite); +EXPORT_SYMBOL(kinsToolSourceSet); +EXPORT_SYMBOL(kinsToolSourceApply); +#endif diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h index 6e0b0037375..203343a20e1 100644 --- a/src/emc/kinematics/lineardeltakins-common.h +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -32,10 +32,16 @@ // common routines used by the userspace kinematics and the realtime kinematics // user must include a math.h-type header first // Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +// +// The geometry is a value the caller holds and passes in, so the same +// routines serve the realtime module through its parameter block and the +// python module through its own copy. #include -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; +typedef struct { + double L, R; + double Ax, Ay, Bx, By, Cx, Cy, L2; +} lineardelta_geometry; #define SQ3 (sqrt(3)) @@ -44,31 +50,30 @@ static double Ax, Ay, Bx, By, Cx, Cy, L2; static double sq(double x) { return x*x; } -static void set_geometry(double r_, double l_) +static void lineardelta_set_geometry(lineardelta_geometry *g, double r_, double l_) { - if(L == l_ && R == r_) return; - - L = l_; - R = r_; + g->L = l_; + g->R = r_; - L2 = sq(L); + g->L2 = sq(g->L); - Ax = 0.0; - Ay = R; + g->Ax = 0.0; + g->Ay = g->R; - Bx = -SIN_60 * R; - By = -COS_60 * R; + g->Bx = -SIN_60 * g->R; + g->By = -COS_60 * g->R; - Cx = SIN_60 * R; - Cy = -COS_60 * R; + g->Cx = SIN_60 * g->R; + g->Cy = -COS_60 * g->R; } -static int kinematics_inverse(const EmcPose *pos, double *joints) +static int lineardelta_inverse(const lineardelta_geometry *g, + const EmcPose *pos, double *joints) { double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[0] = z + sqrt(g->L2 - sq(g->Ax-x) - sq(g->Ay-y)); + joints[1] = z + sqrt(g->L2 - sq(g->Bx-x) - sq(g->By-y)); + joints[2] = z + sqrt(g->L2 - sq(g->Cx-x) - sq(g->Cy-y)); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; @@ -80,11 +85,14 @@ static int kinematics_inverse(const EmcPose *pos, double *joints) ? -1 : 0; } -static int kinematics_forward(const double *joints, EmcPose *pos) +static int lineardelta_forward(const lineardelta_geometry *g, + const double *joints, EmcPose *pos) { double q1 = joints[0]; double q2 = joints[1]; double q3 = joints[2]; + const double Ay = g->Ay, Bx = g->Bx, By = g->By, Cx = g->Cx, Cy = g->Cy; + const double L = g->L; double den = (By-Ay)*Cx-(Cy-Ay)*Bx; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index d894bb46adb..d61f3c58ff9 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,51 +18,66 @@ #include #include #include -#include +#include +#include #include "lineardeltakins-common.h" -static struct haldata -{ - hal_real_t r; - hal_real_t l; -} *haldata; +// the two lengths, one pin each +static const kins_param_desc ld_params[] = { + { "R", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_RADIUS }, + { "L", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_DIAGONAL_ROD }, +}; +enum { P_R, P_L }; static int comp_id; -int kinematicsForward(const double * joints, +// the tower positions follow from the block's two lengths +static void geometry_of(const kins_params *p, lineardelta_geometry *g) +{ + lineardelta_set_geometry(g, p->geometry[P_R], p->geometry[P_L]); +} + +static int ld_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + lineardelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return lineardelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int ld_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + lineardelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return lineardelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int ld_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + lineardelta_geometry g; double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; int i, j; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // each carriage is the platform height plus the rise of its rod, and // the rise changes with the horizontal offset from the tower for (i = 0; i < 3; i++) { - double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; - double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double tx = (i == 0) ? g.Ax : (i == 1) ? g.Bx : g.Cx; + double ty = (i == 0) ? g.Ay : (i == 1) ? g.By : g.Cy; double rise = joints[i] - z; if (rise <= 0) { return -1; } jac[i][0] = (tx - x)/rise; @@ -73,32 +88,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops ld_ops = { + .forward = ld_forward, + .inverse = ld_inverse, + .jacobian = ld_jacobian, +}; + +// three towers for the three linear coordinates, the rest passed +// through; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "lineardeltakins", + .halprefix = "lineardeltakins", + .params = ld_params, + .nparams = sizeof(ld_params)/sizeof(ld_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &ld_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("lineardeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->r, DELTA_RADIUS, "lineardeltakins.R")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->l, DELTA_DIAGONAL_ROD, "lineardeltakins.L")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -106,9 +127,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 351746081c7..780542ba380 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -21,11 +21,19 @@ using namespace boost::python; #define isnan(x) std::isnan(x) #include "lineardeltakins-common.h" +// the python module keeps one geometry, set from python +static lineardelta_geometry geometry; + +static void set_geometry(double r, double l) +{ + lineardelta_set_geometry(&geometry, r, l); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +43,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,7 +51,7 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(geometry.R, geometry.L); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index a5725480094..a32981d12cf 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -21,7 +21,8 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -30,27 +31,33 @@ #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; - hal_bool_t conventional_directions; //default is false -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc max_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0 }, +}; +enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; + +#define CON(p) ((p)->geometry[P_CON] != 0 ? 1.0 : -1.0) + +static int max_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4])); - const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4])); + const double zb = (pivot_length + tool_length + joints[8]) * cos(d2r(joints[4])); + const double xb = (pivot_length + tool_length + joints[8]) * sin(d2r(joints[4])); // U correction const double zv = joints[6] * sin(d2r(joints[4])); @@ -82,22 +89,24 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int max_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + pos->w + tool_length) * cos(d2r(pos->b)); - const double xb = (pivot_length + pos->w + tool_length) * sin(d2r(pos->b)); - + const double zb = (pivot_length + tool_length + pos->w) * cos(d2r(pos->b)); + const double xb = (pivot_length + tool_length + pos->w) * sin(d2r(pos->b)); + // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); @@ -122,27 +131,27 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int max_jacobian(const kins_params *p, const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; const double k = M_PI/180; const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); const double x = pos->tran.x, y = pos->tran.y; - const double R = pivot_length + pos->w; + const double R = pivot_length + p->geometry[P_TOOL_LENGTH] + pos->w; int j; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - // kinematicsInverse() with the polar form expanded: rotating (x, y) - // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the - // B and U corrections are what they are written as + // max_inverse() with the polar form expanded: rotating (x, y) by -c + // is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the B and U + // corrections are what they are written as jac[0][0] = cc; jac[0][1] = sc; jac[0][4] = (con * R * cb - pos->u * sb) * k; @@ -164,40 +173,40 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops max_ops = { + .forward = max_forward, + .inverse = max_inverse, + .jacobian = max_jacobian, +}; + +// joints 0..8 are X..W in order, always; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "maxkins", + .halprefix = "maxkins", + .params = max_params, + .nparams = sizeof(max_params)/sizeof(max_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &max_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; int rtapi_app_main(void) { - int result; comp_id = hal_init("maxkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { result = -ENOMEM; goto error; } - - result = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length"); - result += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), 0.0, "maxkins.tool-length"); - // default is unconventional - result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions"); - - if(result < 0) goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return result; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 551b79a4260..4be3a0918cf 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -17,7 +17,7 @@ The default values for base and effector joints positions are defined in the header file pentakins.h. The actual values for a particular - machine can be adjusted by hal parameters: + machine can be adjusted by hal pins: pentakins.base.N.x pentakins.base.N.y @@ -45,6 +45,10 @@ pentakins.tool-offset - tool length from the origin along z axis, changes the effector pivot point. + The maths is written as pure functions of the parameter block (see + kins_module.h): the pins above are the table below, read into the block + before every call, and the entry points come from kins_single.c. + ----------------------------------------------------------------------------*/ #include @@ -52,24 +56,52 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include #include "pentakins.h" -struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t effectorr[NUM_STRUTS]; - hal_real_t effectorz[NUM_STRUTS]; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; -} *haldata; +// the table: five struts' worth of geometry, then the iteration controls +// and reports. P_BASE_X(i) and the rest index it. +#define P_BASE_X(i) (5*(i) + 0) +#define P_BASE_Y(i) (5*(i) + 1) +#define P_BASE_Z(i) (5*(i) + 2) +#define P_EFF_R(i) (5*(i) + 3) +#define P_EFF_Z(i) (5*(i) + 4) +enum { + P_LAST_ITER = 5*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_COUNT +}; +#define STRUT_ROWS(i, bx, by, bz, er, ez) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "effector." #i ".r", KINS_PARAM_FLOAT, KINS_IN, 0, er }, \ + { "effector." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, ez } + +static const kins_param_desc penta_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_0_Z), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_1_Z), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_2_Z), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_3_Z), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, DEFAULT_EFFECTOR_4_R, DEFAULT_EFFECTOR_4_Z), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IO, 0, 100.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IO, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IO, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert5() ***************************/ @@ -180,31 +212,29 @@ static double sqr(double x) return (x)*(x); } -/* declare arrays for base and effector coordinates */ -static PmCartesian b[NUM_STRUTS]; -static double za[NUM_STRUTS], ra[NUM_STRUTS]; - -/************************pentakins_read_hal_pins**************************/ +/* the base and effector geometry of one call, taken from the block */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + double za[NUM_STRUTS], ra[NUM_STRUTS]; +} penta_geometry; -int pentakins_read_hal_pins(void) { +static void geometry_of(const kins_params *p, penta_geometry *g) { int t; - - /* set the base and effector coordinates from hal pin values */ - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + tool_offset; - ra[t] = hal_get_real(haldata->effectorr[t]); - za[t] = hal_get_real(haldata->effectorz[t]) + tool_offset; + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + tool_offset; + g->ra[t] = p->geometry[P_EFF_R(t)]; + g->za[t] = p->geometry[P_EFF_Z(t)] + tool_offset; } - return 0; } /************************ InvKins() ********************************/ -int InvKins(const double * coord, - double * struts) +static int InvKins(const penta_geometry *g, + const double * coord, + double * struts) { PmCartesian xyz, pmcoord, temp; @@ -212,8 +242,6 @@ int InvKins(const double * coord, PmRpy rpy; int i; -// pentakins_read_hal_pins(); - /* define Rotation Matrix */ pmcoord.x = coord[0]; pmcoord.y = coord[1]; @@ -227,32 +255,30 @@ int InvKins(const double * coord, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of effector strut end from effector to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); + pmCartCartSub(&g->b[i], &pmcoord, &temp); pmMatInv(&RMatrix, &InvRMatrix); pmMatCartMult(&InvRMatrix, &temp, &xyz); /* define strut lengths */ - struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); + struts[i] = sqrt( sqr(xyz.z - g->za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - g->ra[i]) ); } return 0; } -/**************************** kinematicsForward() ***************************/ +/**************************** penta_forward() ***************************/ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; -// PmCartesian aw; -// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; -// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; - + penta_geometry g; double Jacobian[NUM_STRUTS][NUM_STRUTS]; double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; @@ -261,14 +287,11 @@ int kinematicsForward(const double * joints, double coord[NUM_STRUTS]; double conv_err = 1.0; -// PmRotationMatrix RMatrix; -// PmRpy q_RPY; - int iterate = 1; int i, j; unsigned iteration = 0; - pentakins_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ if (joints[0] <= 0.0 || @@ -287,12 +310,15 @@ int kinematicsForward(const double * joints, coord[4] = pos->b * PM_PI / 180.0; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +(max_error)) || (conv_err < -(max_error))) { /* we can't converge */ + s->failed = 1; return -2; }; @@ -300,22 +326,23 @@ int kinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ + s->failed = 1; return -5; } /* compute StrutLengthDiff[] by running inverse kins on Cartesian estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ - InvKins(coord, InvKinStrutLength); + InvKins(&g, coord, InvKinStrutLength); for (i = 0; i < NUM_STRUTS; i++) { StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; /* Build Inverse Jacobian Matrix */ coord[i] += 1e-4; - InvKins(coord, jointdelta); + InvKins(&g, coord, jointdelta); coord[i] -= 1e-4; for (j = 0; j < NUM_STRUTS; j++) { InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; @@ -343,7 +370,6 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -358,34 +384,37 @@ int kinematicsForward(const double * joints, pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); return 0; } -/************************ kinematicsInverse() ********************************/ +/************************ penta_inverse() ********************************/ /* the inverse kinematics take world coordinates and determine joint values, given the inverse kinematics flags to resolve any ambiguities. The forward flags are set to indicate their value appropriate to the world coordinates passed in. */ -/************************ kinematicsInverse() ********************************/ - -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int penta_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; + penta_geometry g; double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); + geometry_of(p, &g); coord[0] = pos->tran.x; coord[1] = pos->tran.y; @@ -393,18 +422,19 @@ int kinematicsInverse(const EmcPose * pos, coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; - if (0 != InvKins(coord,joints)) { + if (0 != InvKins(&g, coord, joints)) { return -1; } return 0; } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + penta_geometry g; PmRotationMatrix R; PmRpy rpy; PmCartesian P, d, xyz, wa, wb, dxyz[5]; @@ -412,7 +442,7 @@ int kinematicsJacobian(const double * joints, (void)joints; (void)iflags; - pentakins_read_hal_pins(); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); /* InvKins() differentiated. The effector end of each strut is found in @@ -433,7 +463,7 @@ int kinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double rho, A, B, len; - pmCartCartSub(&b[i], &P, &d); + pmCartCartSub(&g.b[i], &P, &d); /* R^T d, written out since pmMatCartMult applies R */ xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; @@ -460,8 +490,8 @@ int kinematicsJacobian(const double * joints, } rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); - A = xyz.z - za[i]; - B = rho - ra[i]; + A = xyz.z - g.za[i]; + B = rho - g.ra[i]; len = sqrt(sqr(A) + sqr(B)); if (len <= 0 || rho <= 0) { return -1; } for (col = 0; col < 5; col++) { @@ -472,103 +502,43 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +// the forward iterates from the pose it is handed +static const kins_ops penta_ops = { + .forward = penta_forward, + .inverse = penta_inverse, + .jacobian = penta_jacobian, + .fwd_iterates = 1, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +const kins_module_info kins_module = { + .name = "pentakins", + .halprefix = "pentakins", + .params = penta_params, + .nparams = P_COUNT, + .required_coordinates = "XYZAB", + .max_joints = NUM_STRUTS, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &penta_ops }, +}; MODULE_LICENSE("GPL"); int comp_id; -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, DEFAULT_BASE_3_X, DEFAULT_BASE_4_X -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z -}; -static const rtapi_real init_effectorr[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_4_R -}; -static const rtapi_real init_effectorz[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_Z, DEFAULT_EFFECTOR_1_Z, DEFAULT_EFFECTOR_2_Z, DEFAULT_EFFECTOR_3_Z, DEFAULT_EFFECTOR_4_Z -}; - int rtapi_app_main(void) { - int res = 0, i; - comp_id = hal_init("pentakins"); if (comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) - goto error; - - - for (i = 0; i < NUM_STRUTS; i++) { - - if ((res = hal_param_new_real(comp_id, HAL_RW, &(haldata->basex[i]), - init_basex[i], "pentakins.base.%d.x", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basey[i], - init_basey[i], "pentakins.base.%d.y", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basez[i], - init_basez[i], "pentakins.base.%d.z", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorr[i], - init_effectorr[i], "pentakins.effector.%d.r", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorz[i], - init_effectorz[i], "pentakins.effector.%d.z", i)) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "pentakins.last-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "pentakins.max-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->max_error, - 100.0, "pentakins.max-error")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->conv_criterion, - 1e-9, "pentakins.convergence-criterion")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_IO, &haldata->iter_limit, - 120, "pentakins.limit-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "pentakins.tool-offset")) < 0) - goto error; - hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 8b17fae351e..7f02f1777d6 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -23,10 +23,17 @@ #include "pumakins.h" #include - -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +#include "userkfuncs.h" + +// the five dimensions, one pin each; the maths reads them from the block +static const kins_param_desc puma_params[] = { + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A3 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D6 }, +}; +enum { P_A2, P_A3, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -107,11 +114,13 @@ static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) *rot = hom.rot; } // pumaFlangeRotation() -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double s1, s2, s3; double c1, c2, c3; @@ -135,10 +144,10 @@ static int pumaKinematicsForward(const double * joint, s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; /* Calculate term to be used in definition of... */ /* position vector. */ @@ -191,7 +200,7 @@ static int pumaKinematicsForward(const double * joint, *iflags |= PUMA_WRIST_FLIP; } } - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_D6 = p->geometry[P_D6]; /* add effect of d6 parameter */ hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; @@ -216,45 +225,38 @@ static int pumaKinematicsForward(const double * joint, the base z, the upper arm and forearm about axes at right angles to it, the wrist about three axes meeting at its centre, and D6 carries the tool point out along the flange z. */ -static int pumaKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, 0, hal_get_real(haldata->a2), hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { 0, 0, hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + const double a[6] = { 0, 0, p->geometry[P_A2], p->geometry[P_A3], 0, 0 }; + const double d[6] = { 0, 0, p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // pumaKinematicsJacobian() + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // puma_jacobian() -static int pumaKinematicsToolFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_tool_frame(const kins_params *p, const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)fflags; - // answers in the flange frame; switchkins applies the declared half turn + // answers in the flange frame; the declared half turn is applied by + // the shared code pumaFlangeRotation(joint, rot); return 0; -} // pumaKinematicsToolFrame() - -static int pumaKinematicsWorkFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) -{ - (void)joint; - (void)fflags; - // the arm carries the tool and nothing carries the work - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // pumaKinematicsWorkFrame() +} // puma_tool_frame() -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; @@ -290,11 +292,11 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; + const double PUMA_D6 = p->geometry[P_D6]; /* remove effect of d6 parameter */ px = hom.tran.x - PUMA_D6*hom.rot.z.x; @@ -413,29 +415,20 @@ static int pumaKinematicsInverse(const EmcPose * world, return 0; } -int pumaKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_PUMA560_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_PUMA560_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_PUMA560_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_PUMA560_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_PUMA560_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} // pumaKinematicsSetup() +// the arm carries the tool and nothing carries the work, so the work frame +// is the shared identity one. The maths is the ISO 9787 flange frame, so +// the tool axis it produces runs holder towards tip, the opposite of the +// convention; the declared half turn puts it right. No closed form +// Jacobian: the shared code differences the inverse. +static const kins_ops puma_ops = { + .forward = puma_forward, + .inverse = puma_inverse, + .jacobian = puma_jacobian, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -443,51 +436,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "pumakins"; // !!! must agree with filename kp->halprefix = "pumakins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = puma_params; + kp->nparams = sizeof(puma_params)/sizeof(puma_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = pumaKinematicsSetup; - *kfwd1 = pumaKinematicsForward; - *kinv1 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsRegisterJacobian(1, pumaKinematicsJacobian); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &puma_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsRegisterJacobian(0, pumaKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &puma_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index ac7a11159f5..1c807d5c2ce 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,30 +21,37 @@ #include #include #include -#include +#include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t revolutions; - hal_real_t theta_degrees; - hal_real_t bigtheta_degrees; -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse reports the turn count it keeps and the angles it saw +static const kins_param_desc rose_params[] = { + { "revolutions", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "theta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "bigtheta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, +}; +enum { O_REVOLUTIONS, O_THETA, O_BIGTHETA }; + +// what the inverse carries from one call to the next: the quadrant it +// last saw and the turns it has counted. In the scratch, so that each +// caller counts its own. +#define OLDQUAD(s) ((s)->aux[0]) +#define REVOLUTIONS(s) ((s)->aux[1]) + +static int rose_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double radius,z,theta; @@ -66,18 +73,20 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rose_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)iflags; (void)fflags; // There is a potential problem when accumulating bigtheta -- loss of // precision based on size of mantissa -- but in practice, it is probably ok - static int oldquad; - static int revolutions; + int oldquad = (int)OLDQUAD(s); + int revolutions = (int)REVOLUTIONS(s); double theta,bigtheta; int nowquad = 0; @@ -96,9 +105,9 @@ int kinematicsInverse(const EmcPose * pos, theta = atan2(y,x); bigtheta = theta + PM_2_PI * revolutions; - hal_set_real(haldata->revolutions, revolutions); - hal_set_real(haldata->theta_degrees, theta * TO_DEG); - hal_set_real(haldata->bigtheta_degrees, bigtheta * TO_DEG); + s->out[O_REVOLUTIONS] = revolutions; + s->out[O_THETA] = theta * TO_DEG; + s->out[O_BIGTHETA] = bigtheta * TO_DEG; joints[0] = hypot(x,y); joints[1] = z; @@ -110,18 +119,20 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rose_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double x = pos->tran.x, y = pos->tran.y; double r2 = x*x + y*y; double r = sqrt(r2); + (void)p; (void)joints; (void)iflags; // on the axis the angle is undefined and its rate unbounded @@ -134,34 +145,39 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rose_ops = { + .forward = rose_forward, + .inverse = rose_inverse, + .jacobian = rose_jacobian, +}; + +// joints 0..2 are radius, z and the unwrapped angle; the entry points +// come from kins_single.c +const kins_module_info kins_module = { + .name = "rosekins", + .halprefix = "rosekins", + .params = rose_params, + .nparams = sizeof(rose_params)/sizeof(rose_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rose_ops }, +}; static int comp_id; void rtapi_app_exit(void) { hal_exit(comp_id); } int rtapi_app_main(void) { - int ans; comp_id = hal_init("rosekins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { ans = -ENOMEM; goto error; } - - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->revolutions), 0.0, "rosekins.revolutions")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->theta_degrees), 0.0, "rosekins.theta_degrees")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->bigtheta_degrees), 0.0, "rosekins.bigtheta_degrees")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return ans; } diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h index 59cc872200c..95c6ce95c9e 100644 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ b/src/emc/kinematics/rotarydeltakins-common.h @@ -40,6 +40,10 @@ positive, the Z coordinate will get more negative. Joint zero is the one whose thigh swings in the YZ plane. + + The geometry is a value the caller holds and passes in, so the same + routines serve the realtime module through its parameter block and the + python module through its own copy. */ #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H @@ -47,17 +51,19 @@ #include -// distance from origin to a hip joint -static double platformradius; +typedef struct { + // distance from origin to a hip joint + double platformradius; -// thigh connects the hip to the knee -static double thighlength; + // thigh connects the hip to the knee + double thighlength; -// shin (the parallelogram) connects the knee to the foot -static double shinlength; + // shin (the parallelogram) connects the knee to the foot + double shinlength; -// distance from center of foot (controlled point) to an ankle joint -static double footradius; + // distance from center of foot (controlled point) to an ankle joint + double footradius; +} rotarydelta_geometry; #ifndef sq #define sq(a) ((a)*(a)) @@ -66,15 +72,21 @@ static double footradius; #define D2R(d) ((d)*M_PI/180.) #endif -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; +static void rotarydelta_set_geometry(rotarydelta_geometry *g, + double pfr, double tl, double sl, double fr) { + g->platformradius = pfr; + g->thighlength = tl; + g->shinlength = sl; + g->footradius = fr; } // Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { +static int rotarydelta_forward(const rotarydelta_geometry *g, + const double *joints, EmcPose *pos) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double j0 = joints[0], j1 = joints[1], @@ -139,7 +151,12 @@ static int kinematics_forward(const double *joints, EmcPose *pos) { // Given controlled point, find joint zero's angle // (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { +static int rotarydelta_inverse_j0(const rotarydelta_geometry *g, + double x, double y, double z, double *theta) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double a, b, d, knee_y, knee_z; a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - @@ -157,25 +174,26 @@ static int inverse_j0(double x, double y, double z, double *theta) { return 0; } -static void rotate(double *x, double *y, double theta) { +static void rotarydelta_rotate(double *x, double *y, double theta) { double xx, yy; xx = *x, yy = *y; *x = xx * cos(theta) - yy * sin(theta); *y = xx * sin(theta) + yy * cos(theta); } -static int kinematics_inverse(const EmcPose *pos, double *joints) { +static int rotarydelta_inverse(const rotarydelta_geometry *g, + const EmcPose *pos, double *joints) { double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + if(rotarydelta_inverse_j0(g, pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; // now use symmetry property to get the other two just as easily... xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[1])) return -1; xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[2])) return -1; joints[3] = pos->a; joints[4] = pos->b; diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 92e8a763a99..d406406e76d 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,74 +19,89 @@ #include #include #include -#include +#include +#include #include "rotarydeltakins-common.h" -static struct haldata -{ - hal_real_t pfr; - hal_real_t tl; - hal_real_t sl; - hal_real_t fr; -} *haldata; +// the four lengths, one pin each +static const kins_param_desc rd_params[] = { + { "platformradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_PFR }, + { "thighlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_TL }, + { "shinlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_SL }, + { "footradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_FR }, +}; +enum { P_PFR, P_TL, P_SL, P_FR }; static int comp_id; -int kinematicsForward(const double * joints, +static void geometry_of(const kins_params *p, rotarydelta_geometry *g) +{ + rotarydelta_set_geometry(g, p->geometry[P_PFR], p->geometry[P_TL], + p->geometry[P_SL], p->geometry[P_FR]); +} + +static int rd_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + rotarydelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return rotarydelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int rd_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + rotarydelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return rotarydelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int rd_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + rotarydelta_geometry g; int i, j; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // The foot stays a shin length from each knee, so along a leg the // motion of the foot and the motion of the knee agree: // (P - K) . dP = (P - K) . dK/dq dq - // K is the knee less the foot offset, written as kinematics_forward() + // K is the knee less the foot offset, written as rotarydelta_forward() // writes it, and q the hip angle that swings it. for (i = 0; i < 3; i++) { double q = D2R(joints[i]); - double reach = platformradius - footradius + thighlength * cos(q); + double reach = g.platformradius - g.footradius + g.thighlength * cos(q); double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; switch (i) { case 0: kx = 0; ky = -reach; - dkx = 0; dky = thighlength * sin(q); + dkx = 0; dky = g.thighlength * sin(q); break; case 1: kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = -thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = -g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; default: kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; } - kz = -thighlength * sin(q); - dkz = -thighlength * cos(q); + kz = -g.thighlength * sin(q); + dkz = -g.thighlength * cos(q); px = pos->tran.x - kx; py = pos->tran.y - ky; pz = pos->tran.z - kz; @@ -102,36 +117,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rd_ops = { + .forward = rd_forward, + .inverse = rd_inverse, + .jacobian = rd_jacobian, +}; + +// three hips for the three linear coordinates, the rest passed through; +// the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotarydeltakins", + .halprefix = "rotarydeltakins", + .params = rd_params, + .nparams = sizeof(rd_params)/sizeof(rd_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rd_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->pfr, RDELTA_PFR, "rotarydeltakins.platformradius")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->tl, RDELTA_TL, "rotarydeltakins.thighlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->sl, RDELTA_SL, "rotarydeltakins.shinlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->fr, RDELTA_FR, "rotarydeltakins.footradius")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -139,9 +156,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 49a26b3153e..a8227573ab9 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -20,11 +20,19 @@ #include using namespace boost::python; +// the python module keeps one geometry, set from python +static rotarydelta_geometry geometry; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + rotarydelta_set_geometry(&geometry, pfr, tl, sl, fr); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +42,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,7 +50,8 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(geometry.platformradius, geometry.thighlength, + geometry.shinlength, geometry.footradius); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 47f95bee2cd..abcf0676d36 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -7,7 +7,7 @@ * Author: Chris Radek * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2006 All rights reserved. * ********************************************************************/ @@ -17,13 +17,17 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ +#include -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double c_rad = -joints[5]*M_PI/180; @@ -40,11 +44,14 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rotate_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; double c_rad = pos->c*M_PI/180; @@ -61,14 +68,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rotate_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double c_rad = pos->c*M_PI/180; double cc = cos(c_rad), sc = sin(c_rad); int j; + (void)p; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -82,38 +90,40 @@ int kinematicsJacobian(const double *joints, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; - return kinematicsForward(joint, world, fflags, iflags); -} +// no geometry; joints 0..8 are the nine letters in order, and the entry +// points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotatekins", + .halprefix = "rotatekins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rotate_ops }, +}; -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) { comp_id = hal_init("rotatekins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; + if(comp_id < 0) return comp_id; + + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - return comp_id; + + hal_ready(comp_id); + return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index e2dedb241b6..a416724a5c2 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -21,10 +21,7 @@ #include #include - -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; +#include "userkfuncs.h" /* key dimensions @@ -62,13 +59,32 @@ static struct scara_data { on the value of joint[3]. */ +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +// the six dimensions, one pin each; the maths reads them from the block +static const kins_param_desc scara_params[] = { + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D4 }, + { "D5", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D5 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D6 }, +}; +enum { P_D1, P_D2, P_D3, P_D4, P_D5, P_D6 }; + /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ -static -int scaraKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double a0, a1, a3; double x, y, z, c; @@ -83,12 +99,12 @@ int scaraKinematicsForward(const double * joint, a1 = a1 + a0; a3 = a3 + a1; - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); @@ -109,13 +125,15 @@ int scaraKinematicsForward(const double * joint, world->b = joint[5]; return (0); -} //scaraKinematicsForward() +} // scara_forward() -static int scaraKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int scara_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; double a3; double q0, q1; double xt, yt, rsq, cc; @@ -129,12 +147,12 @@ static int scaraKinematicsInverse(const EmcPose * world, /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); @@ -176,17 +194,17 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -static int scaraKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D2 = p->geometry[P_D2]; + const double D4 = p->geometry[P_D4]; + const double D6 = p->geometry[P_D6]; const double a3 = world->c * (PM_PI / 180); const double q1 = joint[1] * (PM_PI / 180); const double xt = world->tran.x - D6*cos(a3); @@ -226,38 +244,14 @@ static int scaraKinematicsJacobian(const double * joint, jac[4][3] = 1; jac[5][4] = 1; return 0; -} // scaraKinematicsJacobian() - -#define DEFAULT_D1 490 -#define DEFAULT_D2 340 -#define DEFAULT_D3 50 -#define DEFAULT_D4 250 -#define DEFAULT_D5 50 -#define DEFAULT_D6 50 - -static int scaraKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d5), DEFAULT_D5, "%s.D5", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -265,41 +259,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = scara_params; + kp->nparams = sizeof(scara_params)/sizeof(scara_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = scaraKinematicsSetup; - *kfwd1 = scaraKinematicsForward; - *kinv1 = scaraKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, scaraKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &scara_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - switchkinsRegisterJacobian(0, scaraKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &scara_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index 828f7b4ec43..b3be3c53db4 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,7 +43,8 @@ #include #include #include -#include +#include +#include // @@ -76,12 +77,15 @@ static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. -int kinematicsForward( +static int scorbot_forward( + const kins_params *p, kins_scratch *s, const double *joints, EmcPose *pose, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags ) { + (void)p; + (void)s; (void)fflags; (void)iflags; EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) @@ -90,16 +94,13 @@ int kinematicsForward( double r; - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); // Link 1 connects j1 (shoulder) to j2 (elbow). r = L1_LENGTH * cos(TO_RAD * joints[1]); j2_vector.tran.x = r * cos(TO_RAD * joints[0]); j2_vector.tran.y = r * sin(TO_RAD * joints[0]); j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); // Link 2 connects j2 (elbow) to j3 (wrist). // J3 is the controlled point. @@ -107,13 +108,11 @@ int kinematicsForward( j3_vector.tran.x = r * cos(TO_RAD * joints[0]); j3_vector.tran.y = r * sin(TO_RAD * joints[0]); j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); // The end-effector location is the sum of the linkage vectors. pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); // A and B are wrist roll and pitch, handled in hal by external kinematics pose->a = joints[3]; @@ -135,15 +134,17 @@ int kinematicsForward( // is the horizontal distance (ie, in the XY plane) of the controlled // point from J0. // -int kinematicsInverse( +static int scorbot_inverse( + const kins_params *p, kins_scratch *s, const EmcPose *pose, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags ) { + (void)p; + (void)s; (void)iflags; (void)fflags; - // EmcPose j1_cart; double distance_to_cp, distance_to_center; double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane @@ -153,16 +154,10 @@ int kinematicsInverse( // the location of J2, this is what we're trying to find double z_j2; - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. // J0 points at the projected (X, Y) point. tan(J0) = Y/X // J0 then defines the plane that the rest of the arm operates in. joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); // FIXME: Until i figure the wrist differential out, the controlled // point will be the location of the wrist joint, J3/J4. @@ -176,19 +171,16 @@ int kinematicsInverse( // of J0. This is just a known, static vector. r_j1 = L0_HORIZONTAL_DISTANCE; z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); // (r_cp, z_cp) is the location of J3 (the controlled point), again in // the plane defined by the angle of J0, with the origin of the // machine. r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); // translate so (r_j1, z_j1) is the origin of the coordinate system r_cp -= r_j1; z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); // // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. @@ -207,86 +199,23 @@ int kinematicsInverse( distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); // find the angle of the vector from the origin to the CP angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); if (z_cp < 0) { angle_to_cp *= -1; } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); // find the angle (Center, J1, J2) j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); // now we can compute the location of J2 z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); - -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); - - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; - } - - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); - - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); - } - - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); - } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics joints[3] = pose->a; joints[4] = pose->b; @@ -295,13 +224,14 @@ int kinematicsInverse( } -int kinematicsJacobian( +static int scorbot_jacobian( + const kins_params *p, const double *joints, const EmcPose *pose, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags ) { - // kinematicsInverse() above, differentiated step by step in the same + // scorbot_inverse() above, differentiated step by step in the same // order, each quantity carried as its gradient over (x, y, z) const double x = pose->tran.x, y = pose->tran.y; const double rho2 = x*x + y*y; @@ -311,6 +241,7 @@ int kinematicsJacobian( double q; int i; + (void)p; (void)joints; (void)iflags; if (rho2 <= 0) { return -1; } @@ -363,15 +294,26 @@ int kinematicsJacobian( return 0; } -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} +static const kins_ops scorbot_ops = { + .forward = scorbot_forward, + .inverse = scorbot_inverse, + .jacobian = scorbot_jacobian, +}; + +// the arm's dimensions are the constants above; no geometry pins. The +// entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "scorbot-kins", + .halprefix = "scorbot-kins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZAB", + .max_joints = 5, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &scorbot_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -381,6 +323,10 @@ int rtapi_app_main(void) { if (comp_id < 0) { return comp_id; } + if (kinsSingleInit(comp_id, &kins_module, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; } @@ -388,4 +334,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 2509e4a9d47..e90b35d14c8 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include +#include #include #include #include +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -46,12 +48,22 @@ static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; +// types written as pure functions (see kins_module.h): the maths of each, +// the one RT parameter block they all read, a scratch per type, and the +// pins made from the module's table +static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; +static kins_params rt_params; +static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; +static kins_pin_ref *pins; +static kins_tool_source tool_source; +static int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; -// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY), declared by -// the module with switchkinsDeclare(); 0==the module said nothing +// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE), +// declared by the module with switchkinsDeclare(); 0==the module said nothing static int ktype_flags[SWITCHKINS_MAX_TYPES] = {0}; static int switchkins_type; @@ -102,6 +114,48 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, the tool motion sent, and +// the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kp.halprefix, kp.params, kp.nparams, + &rt_params); +} + +// the tool from motion, for the types written on the block; a type +// provided the older way reads its own pins and does not see it +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} // kinematicsSetTool() + +static void write_block(int ktype) +{ + kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); +} + +// the forward of one type, whichever way it was provided, from the pose +// it is handed: no seeding, which is the caller's business +static int call_forward(int ktype, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); + write_block(ktype); + return r; + } + if (!kfwds[ktype]) { return -1; } + return kfwds[ktype](joint, pos, fflags, iflags); +} + static int gui_forward_kins(const double *joints, const EmcPose* estimate) { // the hexapod vismach gui uses these hal pins to @@ -113,7 +167,7 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) KINEMATICS_INVERSE_FLAGS iflags; if ( kp.gui_kinstype < 0 || kp.gui_kinstype >= kins_count - || !kfwds[kp.gui_kinstype]) { + || (!kfwds[kp.gui_kinstype] && !kops[kp.gui_kinstype])) { rtapi_print_msg(RTAPI_MSG_ERR, "gui_forward_kins BAD gui_kinstype <%d>\n", kp.gui_kinstype); @@ -123,8 +177,8 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) // no pose of our own yet, start from the caller's lastpose[kp.gui_kinstype] = *estimate; } - res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], - &fflags, &iflags); + res = call_forward(kp.gui_kinstype, joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); lastpose_ok[kp.gui_kinstype] = (res == 0); hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); @@ -163,6 +217,10 @@ int kinematicsSwitch(int new_switchkins_type) if (fwd_iterates[switchkins_type] && lastpose_ok[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } + // a pure type keeps the same restart pose in its own scratch + if (kops[switchkins_type] && kops[switchkins_type]->fwd_iterates) { + rt_scratch[switchkins_type].have_pose_seed = 1; + } return 0; // 0==> no error } // kinematicsSwitch() @@ -174,26 +232,37 @@ int kinematicsForward(const double *joint, int r; EmcPose estimate = *pos; // the caller's guess, the only one we get - if ( fwd_iterates[switchkins_type] - && use_lastpose[switchkins_type] - && lastpose_ok[switchkins_type]) { - // initialize iterative forward kins (ok for identity too) - get_lastpose(switchkins_type,pos); - use_lastpose[switchkins_type] = 0; - } - if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kfwds[switchkins_type]) { + || (!kfwds[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Forward BAD switchkins_type \n", switchkins_type); return -1; } - r = kfwds[switchkins_type](joint, pos, fflags, iflags); - if (fwd_iterates[switchkins_type]) { - save_lastpose(switchkins_type,pos); - lastpose_ok[switchkins_type] = (r == 0); + + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsForward(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, pos, fflags, iflags); + write_block(switchkins_type); + // the gui forward below starts from here, as it did for the + // older form + if (kops[switchkins_type]->fwd_iterates) {save_lastpose(switchkins_type,pos);} + } else { + if ( fwd_iterates[switchkins_type] + && use_lastpose[switchkins_type] + && lastpose_ok[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) { + save_lastpose(switchkins_type,pos); + lastpose_ok[switchkins_type] = (r == 0); + } } if (r) return r; @@ -223,12 +292,20 @@ int kinematicsInverse(const EmcPose * pos, if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kinvs[switchkins_type]) { + || (!kinvs[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsInverse(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + pos, joint, iflags, fflags); + write_block(switchkins_type); + return r; + } r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -239,9 +316,13 @@ int kinematicsToolFrame(const double *joint, { int r; - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsToolFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!ktools[switchkins_type]) { return -1; // this type does not supply one; not an error } r = ktools[switchkins_type](joint, rot, fflags); @@ -256,9 +337,13 @@ int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!kworks[switchkins_type]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the @@ -275,10 +360,12 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + if (!kops[switchkins_type]->tool || !kops[switchkins_type]->work) { + return -1; // this type does not report its frames, so it cannot answer + } + } else if (!ktools[switchkins_type] || !kworks[switchkins_type]) { return -1; // this type does not report its frames, so it cannot answer } @@ -307,6 +394,12 @@ int kinematicsJacobian(const double *joint, if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsJacobian(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, world, jac, iflags); + } // a closed form is exact and knows its own singular poses if (kjacs[switchkins_type]) { return kjacs[switchkins_type](joint, world, jac, iflags); @@ -333,7 +426,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) register_error = 1; return -1; } - if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: switchkins-type %d" " already provided\n", ktype); @@ -346,6 +439,67 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsDeclare(int ktype, int flags) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDeclare: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktype_flags[ktype] = flags; + return 0; +} // switchkinsDeclare() + +int kinematicsTypeFlags(int ktype) +{ + if ( ktype < 0 + || ktype >= kins_count + || (!kfwds[ktype] && !kops[ktype])) { return -1; } + return ktype_flags[ktype]; +} // kinematicsTypeFlags() + +int switchkinsRegisterOps(int ktype, const kins_ops *ops) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + if (!ops || !ops->forward || !ops->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " has no forward or inverse\n", ktype); + register_error = 1; + return -1; + } + if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kops[ktype] = ops; + if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + if (ops->primary) { ktype_flags[ktype] |= KINSTYPE_PRIMARY; } + if (ops->machine) { ktype_flags[ktype] |= KINSTYPE_MACHINE; } + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -400,26 +554,6 @@ int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) return 0; } // switchkinsRegisterToolFrameInverse() -int switchkinsDeclare(int ktype, int flags) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsDeclare: BAD switchkins_type <%d>" - " (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - ktype_flags[ktype] = flags; - return 0; -} // switchkinsDeclare() - -int kinematicsTypeFlags(int ktype) -{ - if (ktype < 0 || ktype >= kins_count || !kfwds[ktype]) { return -1; } - return ktype_flags[ktype]; -} // kinematicsTypeFlags() - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -429,13 +563,134 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); +EXPORT_SYMBOL(switchkinsRunSetupWith); +EXPORT_SYMBOL(switchkinsDescribeWith); + +//********************************************************************* +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) +{ + int i, n = 0; + + if (!k || !info) { return -1; } + if (k->nparams < 0 || k->nparams > KINS_MAX_PARAMS + || (k->nparams > 0 && !k->params)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: %s declares a bad parameter table\n", + k->kinsname ? k->kinsname : "?"); + return -1; + } + memset(info, 0, sizeof(*info)); + info->name = k->kinsname; + info->halprefix = k->halprefix ? k->halprefix : k->kinsname; + info->params = k->params; + info->nparams = k->nparams; + info->required_coordinates = k->required_coordinates; + info->max_joints = k->max_joints; + info->allow_duplicates = k->allow_duplicates; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + info->ops[i] = kops[i]; + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { n = i + 1; } + } + info->ntypes = n; + return 0; +} // switchkinsDescribeSetup() + +int switchkinsDescribe(kins_module_info *info) +{ + if (!inited) { return -1; } + return switchkinsDescribeSetup(&kp, info); +} // switchkinsDescribe() + +int switchkinsRunSetupWith(switchkinsSetupFunc setup, + kparms* kp, const char* sparm) +{ + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + if (!setup || !kp) { return -1; } + memset(kp, 0, sizeof(*kp)); + + // 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 = (char*)sparm; // module parm passed to kins + + // setup provides types 0,1,2 and may also call + // switchkinsRegister() or switchkinsRegisterOps() for any others + if (setup(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; + } + return 0; +} // switchkinsRunSetupWith() + +int switchkinsDescribeWith(switchkinsSetupFunc setup, + const char *coordinates, const char *sparm, + kins_module_info *info) +{ + // One copy of the module per process, however many callers load it: + // the replay registers the types in the module's statics, so it runs + // once and later calls describe what it left. + static kparms kp; + static int replayed; // 0 not yet, 1 done, -1 failed + static char sparm_seen[256]; + const char *s = sparm ? sparm : ""; + (void)coordinates; // the map is the caller's business, see kinsParamsInit() + + if (!info) { return -1; } + + // the RT instance knows itself already + if (switchkinsDescribe(info) == 0) { return 0; } + + if (!replayed) { + rtapi_strlcpy(sparm_seen, s, sizeof(sparm_seen)); + // a copy outside RT: register the types the way the module would + replayed = switchkinsRunSetupWith(setup, &kp, sparm ? sparm_seen : NULL) ? -1 : 1; + } else if (strncmp(sparm_seen, s, sizeof(sparm_seen) - 1)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDescribeWith: already set up with sparm '%s'," + " not '%s'\n", sparm_seen, s); + return -1; + } + if (replayed < 0) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // switchkinsDescribeWith() //********************************************************************* // The caller owns the hal component: it does hal_init() before this and @@ -446,7 +701,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities; + int identities, primaries, machines; int res = 0; char* emsg = "other"; @@ -470,13 +725,21 @@ int switchkinsInit(const int comp_id, // 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; } + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { kins_count = i + 1; } } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } - // declarations must name provided types, and identity is unique: - // G13.1 resolves it from the flags, so two answers is a load error + // declarations must name provided types, and each flag is unique: + // G13.1 and G49 resolve the machine frame type from the flags and + // G43.4 the primary, so two answers is a load error. A module that + // names no machine frame type has its identity type stand in, which + // is the truth on every machine whose slides line up with its frame. identities = 0; + primaries = 0; + machines = 0; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (!ktype_flags[i]) { continue; } if (i >= kins_count) { @@ -485,14 +748,25 @@ int switchkinsInit(const int comp_id, " not provided\n", i); emsg = "declared switchkins-type not provided"; goto error; } - if (ktype_flags[i] & KINSTYPE_IDENTITY) { identities++; } - rtapi_print("switchkins-type %d declared:%s%s\n", i, + if (ktype_flags[i] & KINSTYPE_IDENTITY) { + identities++; + if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } + } + if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } + rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", - (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : ""); + (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", + (ktype_flags[i] & KINSTYPE_MACHINE) ? " machine" : ""); } if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } + if (primaries > 1) { + emsg = "more than one primary switchkins-type declared"; goto error; + } + if (machines > 1) { + emsg = "more than one machine frame switchkins-type declared"; goto error; + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1< #include @@ -41,43 +42,8 @@ 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; - } + if (switchkinsRunSetup(&kp, sparm)) { return -1; } comp_id = hal_init(kp.kinsname); if (comp_id < 0) return comp_id; diff --git a/src/emc/kinematics/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..d4001dfbaea --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,29 @@ +/* + License GPL Version 2 +*/ + +/* switchkins_setup.c: the part of an in-tree switchkins module that names +* its switchkinsSetup(). Kept apart from switchkins.c so that the core +* names nothing a module defines and can be loaded as switchkins_core. +* +* switchkinsRunSetup() is what rtapi_app_main() calls before +* switchkinsInit(). kinsDescribe() is the description a copy of the +* module loaded outside RT answers with. +*/ +#include + +#include + +int switchkinsRunSetup(kparms* kp, const char* sparm) +{ + return switchkinsRunSetupWith(switchkinsSetup, kp, sparm); +} // switchkinsRunSetup() + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coordinates, sparm, info); +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 5cce5796a30..438d36ecf0c 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -4,6 +4,7 @@ #include #include +#include "userkfuncs.h" /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 @@ -27,9 +28,18 @@ /* flags for forward kinematics */ #define THREE21_REACH 0x01 -struct haldata { - hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; -} *haldata = NULL; +// the eight dimensions, one pin each; the maths reads them from the block +static const kins_param_desc three21_params[] = { + { "A1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A1 }, + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A3 }, + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D6 }, +}; +enum { P_A1, P_A2, P_A3, P_D1, P_D2, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -41,20 +51,22 @@ static double angleDiff(double a, double b) return d; } -static int three21KinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; @@ -193,38 +205,40 @@ static int three21KinematicsForward(const double * joint, with the shoulder set A1 out along the first link and D1 up the base, D2 and D3 both along the axis the upper arm turns about, and D6 carrying the tool point out along the flange z. */ -static int three21KinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, hal_get_real(haldata->a1), hal_get_real(haldata->a2), - hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { hal_get_real(haldata->d1), hal_get_real(haldata->d2), - hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; - - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // three21KinematicsJacobian() - -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) + const double a[6] = { 0, p->geometry[P_A1], p->geometry[P_A2], + p->geometry[P_A3], 0, 0 }; + const double d[6] = { p->geometry[P_D1], p->geometry[P_D2], + p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // three21_jacobian() + +static int three21_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double t1, t2, t3; double k; @@ -367,31 +381,14 @@ static int three21KinematicsInverse(const EmcPose * world, return 0; } -int three21KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a1), DEFAULT_THREE21_A1, "%s.A1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_THREE21_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_THREE21_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_THREE21_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_THREE21_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_THREE21_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_THREE21_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_THREE21_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} +// no frames reported and no closed form Jacobian: the shared code +// differences the inverse +static const kins_ops three21_ops = { + .forward = three21_forward, + .inverse = three21_inverse, + .jacobian = three21_jacobian, + .primary = 1, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -399,41 +396,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "three21kins"; kp->halprefix = "three21kins"; kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = three21_params; + kp->nparams = sizeof(three21_params)/sizeof(three21_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = three21KinematicsSetup; - *kfwd1 = three21KinematicsForward; - *kinv1 = three21KinematicsInverse; - switchkinsRegisterJacobian(1, three21KinematicsJacobian); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &three21_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - switchkinsRegisterJacobian(0, three21KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &three21_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index ab5c7081b19..7ce648fc581 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -4,10 +4,10 @@ * * Derived from a work by Fred Proctor * -* Author: +* Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -67,20 +67,16 @@ #include #include #include -#include /* these decls */ - -/* ident tag */ -#ifndef __GNUC__ -#ifndef __attribute__ -#define __attribute__(x) -#endif -#endif +#include /* these decls */ +#include -static struct haldata { - hal_real_t bx; - hal_real_t cx; - hal_real_t cy; -} *haldata = NULL; +// the base geometry, one pin each, poked from HAL as before +static const kins_param_desc tripod_params[] = { + { "Bx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cy", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, +}; +enum { P_BX, P_CX, P_CY }; #define sq(x) ((x)*(x)) @@ -124,11 +120,13 @@ static struct haldata { solutions. Positive means the tripod is above the xy plane, negative means below. */ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_forward(const kins_params *p, kins_scratch *s_, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s_; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -138,9 +136,9 @@ int kinematicsForward(const double * joints, #define Dz (pos->tran.z) double P, Q, R; double s, t, u; - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; P = sq(AD); Q = sq(BD) - sq(Bx); @@ -184,11 +182,13 @@ int kinematicsForward(const double * joints, #undef Dz } -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tripod_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -196,9 +196,9 @@ int kinematicsInverse(const EmcPose * pos, #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z) - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); @@ -219,14 +219,14 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; /* the three strut base points, in the order of the joints */ const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; int i; @@ -248,170 +248,40 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -#ifdef MAIN - -#include -#include - -/* - Interactive testing of kins. - - Syntax: a.out -*/ -int main(int argc, char *argv[]) -{ -#ifndef BUFFERLEN -#define BUFFERLEN 256 -#endif - char buffer[BUFFERLEN]; - char cmd[BUFFERLEN]; - EmcPose pos, vel; - double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; - char inverse; - char flags; - KINEMATICS_FORWARD_FLAGS fflags; - - inverse = 0; /* forwards, by default */ - flags = 0; /* didn't provide flags */ - fflags = 0; /* above xy plane, by default */ - if (argc != 4 || - 1 != sscanf(argv[1], "%lf", &Bx) || - 1 != sscanf(argv[2], "%lf", &Cx) || - 1 != sscanf(argv[3], "%lf", &Cy)) { - fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); - return 1; - } - - while (! feof(stdin)) { - if (inverse) { - printf("inv> "); - } - else { - printf("fwd> "); - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - if (1 != sscanf(buffer, "%255s", cmd)) { - continue; - } - - if (! strcmp(cmd, "quit")) { - break; - } - if (! strcmp(cmd, "i")) { - inverse = 1; - continue; - } - if (! strcmp(cmd, "f")) { - inverse = 0; - continue; - } - if (! strcmp(cmd, "ff")) { - if (1 != sscanf(buffer, "%*s %lu", &fflags)) { - printf("need forward flag\n"); - } - continue; - } - - if (inverse) { /* inverse kins */ - if (3 != sscanf(buffer, "%lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z)) { - printf("need X Y Z\n"); - continue; - } - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - } - } - } - else { /* forward kins */ - if (flags) { - if (4 != sscanf(buffer, "%lf %lf %lf %lu", - &joints[0], - &joints[1], - &joints[2], - &fflags)) { - printf("need 3 strut values and flag\n"); - continue; - } - } - else { - if (3 != sscanf(buffer, "%lf %lf %lf", - &joints[0], - &joints[1], - &joints[2])) { - printf("need 3 strut values\n"); - continue; - } - } - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - } - } - } - } /* end while (! feof(stdin)) */ - - return 0; -} - -#endif /* MAIN */ - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +static const kins_ops tripod_ops = { + .forward = tripod_forward, + .inverse = tripod_inverse, + .jacobian = tripod_jacobian, +}; + +// three struts for three coordinates; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "tripodkins", + .halprefix = "tripodkins", + .params = tripod_params, + .nparams = sizeof(tripod_params)/sizeof(tripod_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &tripod_ops }, +}; MODULE_LICENSE("GPL"); - - static int comp_id; int rtapi_app_main(void) { - int res = 0; - comp_id = hal_init("tripodkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if(!haldata) goto error; - - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->bx), 1.0, "tripodkins.Bx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cx), 1.0, "tripodkins.Cx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cy), 1.0, "tripodkins.Cy")) < 0) goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index f04d9642622..0dac5708344 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,62 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include -#include - - -#define SET(f) pos->f = joints[i] - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - return identityKinematicsForward(joints, pos, fflags, iflags); -} - -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ - return identityKinematicsInverse(pos, joints, iflags, fflags); -} - -int kinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsToolFrame(joints, rot, fflags); -} - -int kinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsWorkFrame(joints, rot, fflags); -} - -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - return identityKinematicsJacobian(joints, pos, jac, iflags); -} - -static KINEMATICS_TYPE ktype = -1; - -KINEMATICS_TYPE kinematicsType() -{ - return ktype; -} +#include +#include + +// joints are axes, through whatever map coordinates= gives; the maths is +// the shared identity and the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "trivkins", + .halprefix = "trivkins", + .params = NULL, + .nparams = 0, + .required_coordinates = "", + .max_joints = EMCMOT_MAX_JOINTS, + .allow_duplicates = 1, + .ntypes = 1, + .ops = { &KINS_IDENTITY_OPS }, +}; #define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; @@ -74,19 +39,40 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; +// say so when the joints are not in axis order, and which type suits that +static void show_map(KINEMATICS_TYPE ktype) +{ + kins_params p; + int a, unconventional = 0; + + if (kinsParamsInit(&p, &kins_module, coordinates)) { return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p.joint_of_axis[a] >= 0 && p.joint_of_axis[a] != a) { unconventional = 1; } + if (p.joints_of_axis[a] & (p.joints_of_axis[a] - 1)) { unconventional = 1; } + } + if (!unconventional || !strcasecmp(coordinates, "xz")) { return; } + + rtapi_print("\ntrivkins: coordinates:%s\n", coordinates); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j; + for (j = 0; j < p.max_joints; j++) { + if (p.joints_of_axis[a] & (1 << j)) { + rtapi_print(" Joint %d ==> Axis %c\n", j, "XYZABCUVW"[a]); + } + } + } + if (ktype != KINEMATICS_BOTH) { + rtapi_print("trivkins: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); +} + int rtapi_app_main(void) { - kparms ksetup; + KINEMATICS_TYPE ktype; switch (*kinstype) { case 'b': case 'B': ktype = KINEMATICS_BOTH; break; @@ -98,12 +84,11 @@ int rtapi_app_main(void) { comp_id = hal_init("trivkins"); if(comp_id < 0) return comp_id; - // see typedef for KS KinematicsSETUP: - ksetup.max_joints = EMCMOT_MAX_JOINTS; - ksetup.allow_duplicates = 1; - if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { - return -1; //setup failed + if (kinsSingleInit(comp_id, &kins_module, coordinates, ktype)) { + hal_exit(comp_id); + return -1; } + show_map(ktype); hal_ready(comp_id); return 0; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index b445524b6e9..4aa6c99a925 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,69 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html - +* +* Written as pure functions of the parameter block (see kins_module.h): +* the geometry is the table below, the joint map comes from the block, +* and the tool length is p->tool.tran.z. ********************************************************************/ #include #include -#include -#include #include -#include - -static int trtfuncs_max_joints; - -// joint number assignments (-1 ==> not assigned) -static int JX = -1; -static int JY = -1; -static int JZ = -1; - -static int JA = -1; -static int JB = -1; -static int JC = -1; - -static int JU = -1; -static int JV = -1; -static int JW = -1; - -struct haldata { - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t z_offset; - hal_real_t tool_offset; - hal_bool_t conventional_directions; // default: false -} *haldata; - - -int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - int i,jno,res=0; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int rqdjoints = strlen(kp->required_coordinates); - - if (rqdjoints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: supports %d joints, <%s> requires %d\n", - kp->kinsname, - kp->max_joints, - coordinates, - rqdjoints); - goto error; - } - trtfuncs_max_joints = kp->max_joints; - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coords (order doesn't matter) - for (i=0; i < rqdjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jno < EMCMOT_MAX_JOINTS; jno++) { - if (axis_idx_for_jno[jno] == 0 && JX==-1) {JX = jno;} - if (axis_idx_for_jno[jno] == 1 && JY==-1) {JY = jno;} - if (axis_idx_for_jno[jno] == 2 && JZ==-1) {JZ = jno;} - if (axis_idx_for_jno[jno] == 3 && JA==-1) {JA = jno;} - if (axis_idx_for_jno[jno] == 4 && JB==-1) {JB = jno;} - if (axis_idx_for_jno[jno] == 5 && JC==-1) {JC = jno;} - if (axis_idx_for_jno[jno] == 6 && JU==-1) {JU = jno;} - if (axis_idx_for_jno[jno] == 7 && JV==-1) {JV = jno;} - if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} - } - - rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); - for (jno=0; jno Axis %c\n", - jno,"XYZABCUVW"[axis_idx_for_jno[jno]]); - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_rot_point), - 0.0, "%s.x-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_rot_point), - 0.0, "%s.y-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_rot_point), - 0.0, "%s.z-rot-point",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->z_offset), - 0.0, "%s.z-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_offset), - 0.0, "%s.tool-offset",kp->halprefix); - res += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), - 0, "%s.conventional-directions", kp->halprefix); - if (res) {goto error;} - return 0; - -error: - rtapi_print_msg(RTAPI_MSG_ERR,"trtKinematicsSetup() FAIL\n"); - return -1; -} // trtKinematicsSetup() - -int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include +#include "trtfuncs.h" + +// the geometry both machines share, one pin each +const kins_param_desc TRT_PARAMS[] = { + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0.0 }, // default: false +}; +const int TRT_NPARAMS = sizeof(TRT_PARAMS)/sizeof(TRT_PARAMS[0]); + +enum { TRT_XR, TRT_YR, TRT_ZR, TRT_XO, TRT_YO, TRT_ZO, TRT_TOOL, TRT_CON }; + +// joint number assignments from the block (-1 ==> not assigned) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the direction sign the conventional-directions pin selects +#define CON(p) ((p)->geometry[TRT_CON] != 0 ? 1.0 : -1.0) + +static int xyzac_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dt = hal_get_real(haldata->tool_offset); - const double dy = hal_get_real(haldata->y_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dt = p->tool.tran.z; + const double dy = p->geometry[TRT_YO]; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) @@ -198,25 +117,27 @@ int xyzacKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzacKinematicsForward() +} // xyzac_forward() -int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzac_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = pos->a*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); EmcPose P; // computed position @@ -253,16 +174,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzac_inverse() - return 0; -} // xyzacKinematicsInverse() - -int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; // the forward transform's coefficients for a displacement of the X, Y and @@ -271,7 +188,7 @@ int xyzacKinematicsWorkFrame(const double *joints, const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad); rot->y.x = con * sin(c_rad); @@ -286,32 +203,21 @@ int xyzacKinematicsWorkFrame(const double *joints, rot->z.z = cos(a_rad); return 0; -} // xyzacKinematicsWorkFrame() +} // xyzac_work_frame() -int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() - -int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzac_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -320,12 +226,12 @@ int xyzacKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // the computed position P of xyzacKinematicsInverse(), differentiated: - // its coefficients for x, y and z, and the same expressions with the + // the computed position P of xyzac_inverse(), differentiated: its + // coefficients for x, y and z, and the same expressions with the // rotation taken a quarter turn on for a and for c dP[0][0] = cc; dP[0][1] = con * sc; @@ -345,29 +251,42 @@ int xyzacKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzacKinematicsJacobian() - -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzac_jacobian() + +// both rotaries carry the work, so the tool never turns in the machine: +// the tool frame is the shared identity one +const kins_ops XYZAC_OPS = { + .forward = xyzac_forward, + .inverse = xyzac_inverse, + .work = xyzac_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzac_jacobian, + .primary = 1, +}; + +static int xyzbc_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // Note: 'principal' joints are used - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - con * sin(c_rad) * (joints[JY] - y_rot_point) @@ -396,25 +315,27 @@ int xyzbcKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzbcKinematicsForward() +} // xyzbc_forward() -int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzbc_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); // the offsets seen from the tilted table: the same rotation the // forward applies to them, in the same sense @@ -451,23 +372,19 @@ int xyzbcKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzbcKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() -int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - // see the comment in xyzacKinematicsWorkFrame() + // see the comment in xyzac_work_frame() const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad) * cos(b_rad); rot->y.x = con * sin(c_rad) * cos(b_rad); @@ -482,32 +399,21 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() - -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzbc_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -516,12 +422,12 @@ int xyzbcKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the - // inverse depend on b as well + // see the comment in xyzac_jacobian(); dpx and dpz of the inverse + // depend on b as well dP[0][0] = cc * cb; dP[0][1] = con * sc * cb; dP[0][2] = - con * sb; @@ -540,7 +446,16 @@ int xyzbcKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzbcKinematicsJacobian() + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbc_jacobian() + +const kins_ops XYZBC_OPS = { + .forward = xyzbc_forward, + .inverse = xyzbc_inverse, + .work = xyzbc_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzbc_jacobian, + .primary = 1, +}; diff --git a/src/emc/kinematics/trtfuncs.h b/src/emc/kinematics/trtfuncs.h new file mode 100644 index 00000000000..f913322d798 --- /dev/null +++ b/src/emc/kinematics/trtfuncs.h @@ -0,0 +1,20 @@ +/******************************************************************** +* Description: trtfuncs.h +* The two table-rotary tilting geometries, xyzac and xyzbc, written +* once in trtfuncs.c for the two modules that load them. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_TRTFUNCS_H +#define __LINUXCNC_TRTFUNCS_H + +#include + + +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; + +#endif diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..0d2c05b5a41 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -13,6 +13,7 @@ #include /* ulapi */ +#include #include /* struct timeval */ #include "genserkins.h" @@ -43,14 +44,25 @@ int main(int argc, char *argv[]) int retval = 0; double start, end; int comp_id; - kparms kp; - kp.max_joints = GENSER_MAX_JOINTS; - kp.allow_duplicates = 0; + kins_module_info info; + kins_params params; + kins_scratch scratch; - comp_id = hal_init("usergenserkins"); - if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + /* the module described the way kinsDescribe() would, then a block at + the table defaults; setp has no say here */ + memset(&info, 0, sizeof(info)); + info.name = "genserkins"; + info.halprefix = "genserkins"; + info.params = GENSER_PARAMS; + info.nparams = GENSER_NPARAMS; + info.required_coordinates = "XYZABC"; + info.max_joints = GENSER_MAX_JOINTS; + info.ntypes = 1; + info.ops[0] = &GENSER_OPS; - genser_kin_init(); + comp_id = hal_init("usergenserkins"); + if (kinsParamsInit(¶ms, &info, "XYZABC")) printf("unexpected\n"); + kinsScratchInit(&scratch); /* syntax is a.out {i|f # # # # # #} */ if (argc == 8) { @@ -123,14 +135,14 @@ fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); if (0 != retval) { printf("fwd kins error %d\n", retval); } @@ -220,14 +232,14 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); } else { fprintf(stderr,"gki1:\n"); retval = - genserKinematicsInverse(&pos, joints, &iflags, &fflags); + GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("%f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); } else { retval = - genserKinematicsForward(joints, &pos, &fflags, &iflags); + GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { @@ -271,13 +283,13 @@ fprintf(stderr,"gki1:\n"); &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { printf("?\n"); } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); printf("xyzabc: %f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); if (0 != retval) { printf("fwd kins error %d\n", retval); } else { - retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + retval = GENSER_OPS.inverse(¶ms, &scratch, &pos, joints, &iflags, &fflags); printf("j0--j5: %f %f %f %f %f %f\n", joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); if (0 != retval) { diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..9dc5ee17ec2 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -2,6 +2,12 @@ ** switchable kinematics functions. ** License GPL Version 2 ** +** Two forms are here. USERK_OPS is the current one: identity through +** the parameter block, with no state of its own, registered by a module +** with switchkinsRegisterOps(2, &USERK_OPS). The functions below it are +** the older form, kept for the modules that still register their types +** through switchkinsSetup()'s out parameters. +** ** Example Usage (for customizing the genser-switchkins module): ** (works with rtpreempt only rtai --> Makefile needs work) ** @@ -18,12 +24,41 @@ // typical includes: //#include // if reqd #include -#include +#include +#include "userkfuncs.h" // Add for kins based on genserkins: // #include "genserkins.h" //includes gomath,hal //********************************************************************** +// the current form: pure functions of the block + +static int userk_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *world, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + // replace with the machine's own forward; the block carries the + // geometry (p->geometry[]), the joint map and the tool + return kinsIdentityForward(p, s, joint, world, fflags, iflags); +} + +static int userk_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *world, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinsIdentityInverse(p, s, world, joint, iflags, fflags); +} + +const kins_ops USERK_OPS = { + .forward = userk_forward, + .inverse = userk_inverse, + // .work, .tool, .native and .jacobian are optional, see kins_module.h +}; + +//********************************************************************** +// the older form // static local variables and functions go here static int userk_inited = 0; diff --git a/src/emc/kinematics/userkfuncs.h b/src/emc/kinematics/userkfuncs.h new file mode 100644 index 00000000000..d0f06b78ccf --- /dev/null +++ b/src/emc/kinematics/userkfuncs.h @@ -0,0 +1,30 @@ +/******************************************************************** +* Description: userkfuncs.h +* The user-defined kinematics template, userkfuncs.c, shared by the +* switchkins modules as their third type. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_USERKFUNCS_H +#define __LINUXCNC_USERKFUNCS_H + +#include + + +extern const kins_ops USERK_OPS; + +extern int userkKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int userkKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int userkKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +#endif diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b6b35538f25..2d99eb5f6aa 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kins_module.h) */ #include @@ -16,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -23,47 +26,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzac-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzac-trt-kins"; // hal pin names kp->required_coordinates = "xyzac"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzacKinematicsForward; - *kinv1 = xyzacKinematicsInverse; - switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZAC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzacKinematicsForward; - *kinv0 = xyzacKinematicsInverse; - switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZAC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 401311e4398..acb5fcc3b05 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kins_module.h) */ #include @@ -16,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -23,47 +26,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzbc-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzbc-trt-kins"; // hal pin names kp->required_coordinates = "xyzbc"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzbcKinematicsForward; - *kinv1 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZBC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzbcKinematicsForward; - *kinv0 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZBC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..92de369697a --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,787 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen, asks it to describe itself through + * kinsDescribe(), and evaluates its kinematics through the parameter + * block (see kins_module.h). The block is filled from HAL: the RT + * instance's own pins, read by name whenever the block is refreshed, so + * the values are the live ones and nothing is made in HAL to get at them. + * The tool is + * the caller's where it has given one, since a planner knows what a + * segment runs under better than the machine does; otherwise it comes + * from motion's own tooloffset pins where motion is loaded, so that the + * tool the module sees is the one motion has, whether or not the config + * netted it to the module's pin. + * + * A module exporting no kinsDescribe() is not an error; the context comes + * back flagged rt_only and answers nothing. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_RTLIB_DIR, MODULE_EXT */ + +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); + +#define MAX_PINS (KINS_MAX_PARAMS + AXIS_COUNT) + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no kinsDescribe() */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + void *core_handle; /* the core module it takes shared code from */ + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ + int num_joints; + char module_name[64]; + int comp_id; /* the caller's component, which has HAL mapped */ + char pin_name[MAX_PINS][HAL_NAME_LEN + 1]; /* the RT instance's pins read here */ + int num_pins; + int pin_of_param[KINS_MAX_PARAMS]; /* -1 if not read */ + int pin_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; + EmcPose caller_tool; /* from kinematicsUserSetTool() */ + int have_caller_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ +}; + +/* ======================================================================== + * Pin reading + * ======================================================================== */ + +/* The block is read from the RT instance's own pins by name at every + refresh, so a pin netted later reads its signal; nothing is made in HAL + to get at them. */ +static int pin_value(const char *pin_name, hal_type_t type, double *out) +{ + hal_query_t q; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + q.pp.type = type; + if (hal_get_p(&q, NULL, NULL) != 0) return -1; + switch (type) { + case HAL_BOOL: *out = q.pp.value.b ? 1.0 : 0.0; break; + case HAL_SINT: *out = q.pp.value.s; break; + case HAL_UINT: *out = q.pp.value.u; break; + default: *out = q.pp.value.r; break; + } + return 0; +} + +/* Does a pin of this name exist? Silent: absence is an answer, not an error. */ +static int pin_exists(const char *pin_name) +{ + hal_query_t q; + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + return hal_getref_p(&q) == 0; +} + +/* Note pin_name for reading, once it has answered with the type; returns + its index, or -1. */ +static int note_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) +{ + double value; + + if (pin_value(pin_name, type, &value) != 0) { + fprintf(stderr, "kinematicsUserInit: no pin '%s' of the type expected\n", + pin_name); + return -1; + } + if (ctx->num_pins >= MAX_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to read\n"); + return -1; + } + snprintf(ctx->pin_name[ctx->num_pins], sizeof(ctx->pin_name[0]), "%s", pin_name); + return ctx->num_pins++; +} + +static hal_type_t hal_type_of(kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return HAL_BOOL; + case KINS_PARAM_S32: return HAL_SINT; + case KINS_PARAM_U32: return HAL_UINT; + default: return HAL_REAL; + } +} + +/* Note every input of the table, and motion's tool where motion is there. */ +static int note_all(KinematicsUserContext *ctx) +{ + static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; + char name[HAL_NAME_LEN + 1]; + int i; + + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->pin_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->pin_of_tool[i] = -1; + ctx->tool_param = -1; + + for (i = 0; i < ctx->info.nparams; i++) { + const kins_param_desc *d = &ctx->info.params[i]; + if (d->dir == KINS_OUT) continue; + if (d->tool) ctx->tool_param = i; + snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); + ctx->pin_of_param[i] = note_pin(ctx, name, hal_type_of(d->type)); + if (ctx->pin_of_param[i] < 0) return -1; + } + + /* motion publishes the tool it applies; take it from there when it is + loaded, so the module sees the tool whether or not the config netted + it through. Under halrun with the module alone there is no motion, + and the module's own tool entry is all there is. */ + for (i = 0; i < AXIS_COUNT; i++) { + snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); + if (!pin_exists(name)) continue; + ctx->pin_of_tool[i] = note_pin(ctx, name, HAL_REAL); + if (ctx->pin_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now, and the tool of whoever + knows it best: the caller, then motion, then the module's own pin. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + /* a pin that stops answering, its module unloaded, keeps its last value */ + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->pin_of_param[i]; + double value; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], hal_type_of(ctx->info.params[i].type), &value) == 0) { + ctx->params.geometry[i] = value; + } + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + if (ctx->have_caller_tool) { + ctx->params.tool = ctx->caller_tool; + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; + } + return; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->pin_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + if (pin_value(ctx->pin_name[c], HAL_REAL, &tool[i]) != 0) continue; + have_motion_tool = 1; + } + if (!have_motion_tool) return; + + /* the module's pin and motion disagree: the config lost the tool + somewhere between them. Say so once; motion's value is the one + being cut with. A pin nobody set reads zero, which is not a + disagreement. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && ctx->params.geometry[ctx->tool_param] != 0.0 + && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + fprintf(stderr, + "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" + " using motion's value\n", + ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, + ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->warned_tool = 1; + } + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +/* The core module a module built out of tree takes the shared kinematics + code from, named by what its dynamic symbols leave undefined; NULL for a + module that needs none. Read from the file, since a lazily loaded + module cannot be asked what it has not yet bound. */ +static const char *module_core(const char *path) +{ + const char *core = NULL; + FILE *f; + long size; + unsigned char *buf; + const ElfW(Ehdr) *eh; + const ElfW(Shdr) *sh; + int i; + + f = fopen(path, "rb"); + if (!f) return NULL; + if (fseek(f, 0, SEEK_END) || (size = ftell(f)) <= 0 || fseek(f, 0, SEEK_SET)) { + fclose(f); + return NULL; + } + buf = malloc(size); + if (!buf || fread(buf, 1, size, f) != (size_t)size) { + free(buf); + fclose(f); + return NULL; + } + fclose(f); + + eh = (const ElfW(Ehdr) *)buf; + if ((size_t)size < sizeof(*eh) || memcmp(eh->e_ident, ELFMAG, SELFMAG) + || eh->e_shoff + (size_t)eh->e_shnum * sizeof(ElfW(Shdr)) > (size_t)size) { + free(buf); + return NULL; + } + sh = (const ElfW(Shdr) *)(buf + eh->e_shoff); + for (i = 0; i < eh->e_shnum && !core; i++) { + const ElfW(Sym) *sym; + const char *names; + size_t n, k; + if (sh[i].sh_type != SHT_DYNSYM || sh[i].sh_link >= eh->e_shnum) continue; + if (sh[i].sh_offset + sh[i].sh_size > (size_t)size + || sh[sh[i].sh_link].sh_offset + sh[sh[i].sh_link].sh_size > (size_t)size) break; + sym = (const ElfW(Sym) *)(buf + sh[i].sh_offset); + names = (const char *)(buf + sh[sh[i].sh_link].sh_offset); + n = sh[i].sh_size / sizeof(ElfW(Sym)); + for (k = 0; k < n && !core; k++) { + const char *name; + if (sym[k].st_shndx != SHN_UNDEF + || sym[k].st_name >= sh[sh[i].sh_link].sh_size) continue; + name = names + sym[k].st_name; + if (!strcmp(name, "switchkinsInit")) core = "switchkins_core"; + else if (!strcmp(name, "kinsSingleInit")) core = "kins_single_core"; + } + } + free(buf); + return core; +} + +static void unload_module(KinematicsUserContext *ctx) +{ + if (ctx->rt_handle) dlclose(ctx->rt_handle); + if (ctx->core_handle) dlclose(ctx->core_handle); + ctx->rt_handle = NULL; + ctx->core_handle = NULL; +} + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates, + const char *sparm) +{ + char module_path[512]; + void *handle, *hal_lib; + const char *core; + kins_describe_fn describe; + + snprintf(module_path, sizeof(module_path), + "%s/%s%s", EMC2_RTLIB_DIR, module_name, MODULE_EXT); + + /* A module calls rtapi_print() and the rest of the HAL library, and a + program holding that library under a shared object of its own (a GUI + holds it under the interpreter it loaded) keeps those symbols out of + the scope a module resolves against: the module loads and then dies + at the first call it cannot bind. Failing here is not itself an + error, since a program that links the library has them in reach. */ + hal_lib = dlopen("liblinuxcnchal.so.0", RTLD_LAZY | RTLD_GLOBAL); + if (!hal_lib) { + fprintf(stderr, "kinematicsUserInit: dlopen 'liblinuxcnchal.so.0':" + " %s\n", dlerror()); + } + + /* A module built out of tree takes the shared kinematics code from + switchkins_core or kins_single_core, as it does in RT; that core goes + in scope for it and goes with it. Only such a module brings a core + in, so the core never stands in for code a module carries itself. + The core goes first: the module takes data from it as well, the + identity ops, and data binds as the module loads, lazily or not, + where a program holding this library under a shared object of its + own (a GUI again) has no other copy in scope. */ + core = module_core(module_path); + if (core) { + char core_path[512]; + snprintf(core_path, sizeof(core_path), + "%s/%s%s", EMC2_RTLIB_DIR, core, MODULE_EXT); + ctx->core_handle = dlopen(core_path, RTLD_LAZY | RTLD_GLOBAL); + if (!ctx->core_handle) { + fprintf(stderr, "kinematicsUserInit: '%s' needs %s: %s\n", + module_name, core, dlerror()); + ctx->rt_only = 1; + return -1; + } + } + + /* lazily: a halcompile component references hal_export_funct() and + the rest of what its rtapi_app_main() needs, which only the realtime + HAL library provides, and nothing here calls that main. What is + called, kinsDescribe() and the ops, resolves when it is called. A + module that does not load here at all is one only RT can run. */ + handle = dlopen(module_path, RTLD_LAZY | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + unload_module(ctx); + ctx->rt_only = 1; + return -1; + } + ctx->rt_handle = handle; + + describe = (kins_describe_fn)dlsym(handle, "kinsDescribe"); + if (!describe) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" + " it cannot be evaluated outside RT\n", module_name); + unload_module(ctx); + ctx->rt_only = 1; + return -1; + } + + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", + module_name); + unload_module(ctx); + ctx->rt_only = 1; + return -1; + } + + if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { + fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" + " parameter block form\n", module_name); + unload_module(ctx); + ctx->rt_only = 1; + return -1; + } + + if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { + fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", + module_name, coordinates ? coordinates : "(default)"); + unload_module(ctx); + ctx->rt_only = 1; + return -1; + } + kinsScratchInit(&ctx->scratch); + + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + (void)prefix; + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (note_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot read the pins of '%s'\n", + kins_type); + ctx->rt_only = 1; + } + } + + ctx->initialized = 1; + return ctx; +} + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + return kinematicsUserInitSparm(kins_type, num_joints, coordinates, NULL, + comp_id, prefix); +} + +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (ktype < 0 || ktype >= ctx->info.ntypes || !ctx->info.ops[ktype]) { + return -1; + } + ctx->ktype = ktype; + ctx->params.ktype = ktype; + kinsScratchInit(&ctx->scratch); + ctx->kins_type = ctx->info.ops[ktype]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ntypes; +} + +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (tool) { + ctx->caller_tool = *tool; + ctx->have_caller_tool = 1; + } else { + ctx->have_caller_tool = 0; + } + return 0; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + + if (!ctx || !ctx->initialized || !world || !joints) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + /* the joints go in as well as out: motion hands a module where the + machine is, and some read that (a nutating head takes its rotary + angles from it), so the caller's array is the seed */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + memcpy(ctx->last_joints, j, sizeof(ctx->last_joints)); + return 0; +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + + if (!ctx || !ctx->initialized || !joints || !world) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } + /* a forward that iterates starts from the pose it is handed, so the + caller's world is the seed; any other gets a clean one */ + if (!ctx->info.ops[ctx->ktype]->fwd_iterates) { + memset(world, 0, sizeof(*world)); + } + return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, &fflags, &iflags); +} + +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS]; + int r, a; + + if (!ctx || !ctx->initialized || !world || !J) return -1; + if (ctx->rt_only) return -1; + + refresh(ctx); + /* the joints at this pose, on the branch the last inverse was on */ + memcpy(j, ctx->last_joints, sizeof(j)); + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, jac, &iflags) != 0) { + return -1; + } + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) { + for (a = 0; a < AXIS_COUNT; a++) J[r][a] = jac[r][a]; + } + return 0; +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + refresh(ctx); + return 0; +} + +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return NULL; + refresh(ctx); + return &ctx->params; +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +/* ======================================================================== + * Frames and the tool frame inverse + * + * toolFrameSolve() drives a pair of frame functions that take joints alone, + * the shape the RT modules export; the block form takes the parameters as + * well. The context the solver is running for is parked in a file static + * for the duration of the call, which is fine for the single threaded + * callers this has (the interpreter, a planner), and would not be for two + * threads solving at once. + * ======================================================================== */ + +static KinematicsUserContext *frame_ctx; + +static int frame_work(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static int frame_tool(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static void pad_joints(KinematicsUserContext *ctx, const double *in, double *out) +{ + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + out[i] = (i < ctx->num_joints) ? in[i] : 0.0; + } +} + +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin) +{ + double j[EMCMOT_MAX_JOINTS]; + int found; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + if (!ctx->info.ops[ctx->ktype]->work || !ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + found = toolFrameSolve(frame_work, frame_tool, ctx->num_joints, + axis_in_work, x_in_work, j, held, + solutions, max_solutions, free_directions, tool_spin); + frame_ctx = NULL; + return found; +} + +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !mask) return -1; + if (!ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameWorkJoints(frame_work, ctx->num_joints, j, mask); + frame_ctx = NULL; + return r; +} + +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !primary || !secondary) return -1; + if (!ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, primary, secondary); + frame_ctx = NULL; + return r; +} + +int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, + int* joint) +{ + double j[EMCMOT_MAX_JOINTS]; + unsigned int table_mask = 0, head_mask = 0; + int primary = -1, secondary = -1, r = -1; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !joint) return -1; + *joint = -1; + if (!ctx->info.ops[ctx->ktype]->tool || !ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + if (toolFrameWorkJoints(frame_work, ctx->num_joints, j, &table_mask) == 0 + && toolFrameWorkJoints(frame_tool, ctx->num_joints, j, &head_mask) == 0 + && !(table_mask & head_mask)) { + int tables = __builtin_popcount(table_mask), heads = __builtin_popcount(head_mask); + // the first rotary met going from the tool to the work; a table + // behind two heads is the one Q0 holds still + if (heads == 2) { + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, &primary, &secondary); + *joint = secondary; + } else if (heads == 1 && tables > 0) { + *joint = __builtin_ctz(head_mask); + r = 0; + } else if (heads == 0 && tables == 2) { + r = toolFrameOrientJoints(frame_work, ctx->num_joints, j, &primary, &secondary); + *joint = primary; + } + } + frame_ctx = NULL; + if (r != 0) { *joint = -1; } + return r; +} + +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix) +{ + char buf[256], *tok, *save = NULL; + char module[64] = "", coords[64] = "", sparm[64] = ""; + + if (!kinematics) return NULL; + snprintf(buf, sizeof(buf), "%s", kinematics); + for (tok = strtok_r(buf, " \t", &save); tok; tok = strtok_r(NULL, " \t", &save)) { + if (!module[0]) { + snprintf(module, sizeof(module), "%s", tok); + } else if (!strncmp(tok, "coordinates=", 12)) { + snprintf(coords, sizeof(coords), "%s", tok + 12); + } else if (!strncmp(tok, "sparm=", 6)) { + snprintf(sparm, sizeof(sparm), "%s", tok + 6); + } + /* kinstype= and anything else is the RT loader's business */ + } + if (!module[0]) return NULL; + return kinematicsUserInitSparm(module, num_joints, + coords[0] ? coords : NULL, + sparm[0] ? sparm : NULL, + comp_id, prefix); +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + if (!ctx) return; + + unload_module(ctx); + free(ctx); +} diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b8094e03e9b..a9b16b6185c 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -70,6 +70,11 @@ #include "homing.h" #include "axis.h" +// the kinematics module takes the tool offset from here when it can; a +// module written before the call exports no such symbol, and the weak +// reference leaves it NULL rather than refusing to load motion +#pragma weak kinematicsSetTool + #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -2038,6 +2043,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); emcmotStatus->tool_offset = emcmotCommand->tool_offset; + if (kinematicsSetTool) { + kinematicsSetTool(&emcmotStatus->tool_offset); + } break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..54a9b384935 --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,51 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +# kins_util.c is the shared kinematics code the modules link; the loader +# needs the same block helpers and ops dispatch on this side of dlopen. +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) emc/kinematics/kins_util.c + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +# -Bsymbolic: the library carries its own copy of the kinematics helpers, +# and the loader brings switchkins_core and kins_single_core into scope +# with the same names, which must not take over the library's own calls +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -Wl,-Bsymbolic -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +$(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh + cp $^ $@ +HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..ba8c69fdd42 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,80 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..2db3cf00d3e --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,83 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities, from the module's own closed form through the non-RT + * kinematics loader. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +#include +#include + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..d8bcbd82b57 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,234 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +#include +#include + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc new file mode 100644 index 00000000000..1b750be6776 --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,269 @@ +/******************************************************************** + * Description: kinslimits.cc + * Diagnostic tool: print the Jacobian and the world-space velocity, + * acceleration and jerk caps that a given kinematics module imposes + * on a straight move between two poses. + * + * The tool attaches to a running HAL instance, loads the kinematics + * module through the non-RT interface, samples the move, and reports + * the most restrictive cap found along it. The sampling loop here is + * the same one the trajectory planner uses to cap a segment. + * + * Example (in a terminal with a running config, or under halrun): + * + * halrun -I + * halcmd: loadrt 5axiskins coordinates=XYZBCW + * halcmd: setp 5axiskins.pivot-length 100 + * halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \ + * --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \ + * --end 100,0,0,0,90,0,0,0,0 \ + * --vel 100,100,100,30,30,30 --acc 500,500,500,200,200,200 + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include "jacobian.hh" +#include "joint_limits.hh" + +using namespace motion_planning; + +static const char *AXIS_NAME[9] = {"X","Y","Z","A","B","C","U","V","W"}; + +static std::vector parse_list(const char *s) +{ + std::vector out; + const char *p = s; + while (*p) { + char *endp = nullptr; + double v = strtod(p, &endp); + if (endp == p) break; + out.push_back(v); + p = endp; + while (*p == ',' || *p == ' ') p++; + } + return out; +} + +static void list_to_pose(const std::vector& v, EmcPose *p) +{ + double a[9] = {0,0,0,0,0,0,0,0,0}; + for (size_t i = 0; i < v.size() && i < 9; i++) a[i] = v[i]; + p->tran.x = a[0]; p->tran.y = a[1]; p->tran.z = a[2]; + p->a = a[3]; p->b = a[4]; p->c = a[5]; + p->u = a[6]; p->v = a[7]; p->w = a[8]; +} + +static double pose_axis(const EmcPose& p, int ax) +{ + switch (ax) { + case 0: return p.tran.x; case 1: return p.tran.y; case 2: return p.tran.z; + case 3: return p.a; case 4: return p.b; case 5: return p.c; + case 6: return p.u; case 7: return p.v; default: return p.w; + } +} + +static void set_pose_axis(EmcPose *p, int ax, double val) +{ + switch (ax) { + case 0: p->tran.x = val; break; case 1: p->tran.y = val; break; + case 2: p->tran.z = val; break; case 3: p->a = val; break; + case 4: p->b = val; break; case 5: p->c = val; break; + case 6: p->u = val; break; case 7: p->v = val; break; + default: p->w = val; break; + } +} + +static void usage(const char *argv0) +{ + fprintf(stderr, + "usage: %s --module NAME --joints N --coords LETTERS\n" + " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" + " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--samples N] [--singularity COND]\n" + "\n" + "Prints the Jacobian and the world-space caps the joint limits imply\n" + "for a straight move from --start to --end. Requires a running HAL\n" + "instance with the kinematics module loaded.\n", argv0); +} + +int main(int argc, char **argv) +{ + const char *module = nullptr; + const char *coords = nullptr; + int num_joints = 0; + int samples = 11; + double singularity = 100.0; + std::vector start_v, end_v, vel_v, acc_v, jerk_v; + + for (int i = 1; i < argc; i++) { + const char *a = argv[i]; + const char *next = (i + 1 < argc) ? argv[i + 1] : nullptr; + if (!strcmp(a, "--module") && next) { module = next; i++; } + else if (!strcmp(a, "--coords") && next) { coords = next; i++; } + else if (!strcmp(a, "--joints") && next) { num_joints = atoi(next); i++; } + else if (!strcmp(a, "--samples") && next) { samples = atoi(next); i++; } + else if (!strcmp(a, "--singularity") && next){ singularity = atof(next); i++; } + else if (!strcmp(a, "--start") && next) { start_v = parse_list(next); i++; } + else if (!strcmp(a, "--end") && next) { end_v = parse_list(next); i++; } + else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } + else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } + else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else { usage(argv[0]); return 1; } + } + + if (!module || !coords || num_joints < 1 || + start_v.empty() || end_v.empty() || vel_v.empty() || acc_v.empty()) { + usage(argv[0]); + return 1; + } + if ((int)vel_v.size() < num_joints || (int)acc_v.size() < num_joints) { + fprintf(stderr, "kinslimits: --vel and --acc need %d entries\n", num_joints); + return 1; + } + if (samples < 2) samples = 2; + + int comp_id = hal_init("kinslimits"); + if (comp_id < 0) { + fprintf(stderr, "kinslimits: hal_init failed (is HAL running?)\n"); + return 1; + } + + KinematicsUserContext *ctx = kinematicsUserInit(module, num_joints, coords, + comp_id, "kinslimits"); + if (!ctx) { + fprintf(stderr, "kinslimits: kinematicsUserInit failed for '%s'\n", module); + hal_exit(comp_id); + return 1; + } + if (kinematicsUserIsRtOnly(ctx)) { + fprintf(stderr, "kinslimits: '%s' is RT-only, no non-RT interface\n", module); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + JacobianCalculator jac; + JointLimitCalculator lim; + if (!jac.init(ctx) || !lim.init(num_joints)) { + fprintf(stderr, "kinslimits: calculator init failed\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + std::vector minpos(num_joints, -1e9), maxpos(num_joints, 1e9); + if ((int)jerk_v.size() < num_joints) jerk_v.assign(num_joints, 1e9); + lim.updateAllLimits(vel_v.data(), acc_v.data(), + minpos.data(), maxpos.data(), jerk_v.data()); + + EmcPose start, end; + list_to_pose(start_v, &start); + list_to_pose(end_v, &end); + + /* Path parameter: XYZ arc length, falling back to the largest rotary + delta for a pure rotary move, matching what the planner uses. */ + double dx = end.tran.x - start.tran.x; + double dy = end.tran.y - start.tran.y; + double dz = end.tran.z - start.tran.z; + double target = sqrt(dx*dx + dy*dy + dz*dz); + if (target < 1e-12) { + for (int ax = 3; ax < 9; ax++) { + double d = fabs(pose_axis(end, ax) - pose_axis(start, ax)); + if (d > target) target = d; + } + } + if (target < 1e-12) { + fprintf(stderr, "kinslimits: start and end are the same pose\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + /* tangent[a] = d(world axis a) / d(path parameter) */ + double tangent[9]; + for (int ax = 0; ax < 9; ax++) { + tangent[ax] = (pose_axis(end, ax) - pose_axis(start, ax)) / target; + } + + printf("module : %s (%s, %d joints)%s\n", module, coords, num_joints, + kinematicsUserIsIdentity(ctx) ? " [identity]" : ""); + printf("path length : %.6f (tangent units per path unit)\n", target); + printf("tangent :"); + for (int ax = 0; ax < 9; ax++) { + if (fabs(tangent[ax]) > 1e-12) printf(" %s=%.4f", AXIS_NAME[ax], tangent[ax]); + } + printf("\n\n"); + + double min_vel = 1e9, min_acc = 1e9, min_jerk = 1e9, max_cond = 1.0; + int at_vel = -1, at_acc = -1, at_jerk = -1; + double min_vel_s = 0.0; + + for (int i = 0; i < samples; i++) { + double frac = (double)i / (double)(samples - 1); + EmcPose p; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(&p, ax, + pose_axis(start, ax) + frac * (pose_axis(end, ax) - pose_axis(start, ax))); + } + + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + if (kinematicsUserInverse(ctx, &p, joints) != 0) { + printf("sample %2d: inverse kinematics failed\n", i); + continue; + } + + double J[9][9]; + if (!jac.compute(p, J)) { + printf("sample %2d: Jacobian failed\n", i); + continue; + } + + double jpad[9] = {0}; + for (int j = 0; j < num_joints && j < 9; j++) jpad[j] = joints[j]; + + JointLimitResult r; + if (!lim.computeForTangent(J, jpad, tangent, r, singularity)) { + printf("sample %2d: limit calculation failed\n", i); + continue; + } + + printf("s=%.3f vel<=%10.3f (j%d) acc<=%10.1f (j%d) jerk<=%12.1f (j%d) cond=%.2f\n", + frac, r.max_world_vel, r.limiting_joint_vel, + r.max_world_acc, r.limiting_joint_acc, + r.max_world_jerk, r.limiting_joint_jerk, r.condition_number); + + if (r.max_world_vel < min_vel) { min_vel = r.max_world_vel; at_vel = r.limiting_joint_vel; min_vel_s = frac; } + if (r.max_world_acc < min_acc) { min_acc = r.max_world_acc; at_acc = r.limiting_joint_acc; } + if (r.max_world_jerk < min_jerk) { min_jerk = r.max_world_jerk; at_jerk = r.limiting_joint_jerk; } + if (r.condition_number > max_cond) max_cond = r.condition_number; + + if (i == 0) { + printf(" Jacobian at start (rows = joints, cols = XYZABCUVW):\n"); + for (int j = 0; j < num_joints && j < 9; j++) { + printf(" j%d:", j); + for (int ax = 0; ax < 9; ax++) printf(" %8.4f", J[j][ax]); + printf("\n"); + } + } + } + + printf("\nsegment cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), jerk %.1f (joint %d)\n", + min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); + printf("worst cond : %.3f\n", max_cond); + + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 0; +} diff --git a/src/emc/rs274ngc/interp_convert.cc b/src/emc/rs274ngc/interp_convert.cc index 6a4e77b8c71..9b89659db75 100644 --- a/src/emc/rs274ngc/interp_convert.cc +++ b/src/emc/rs274ngc/interp_convert.cc @@ -6829,10 +6829,11 @@ int Interp::convert_tool_length_offset(int g_code, //!< g_code being execu if (g_code == G_49 && settings->kins_by_g43_4) { // G49 undoes what G43.4 did: after the cancel it drops the machine - // to identity kinematics, as if G13.1 had run on the next line. A - // kinematics the program selected itself is left alone, and a - // module that declares no identity type keeps the plain cancel. - int identity = flagged_kins_type(KINSTYPE_IDENTITY); + // to the machine frame type, identity kinematics on a machine whose + // slides line up, as if G13.1 had run on the next line. A kinematics + // the program selected itself is left alone, and a module that + // declares no such type keeps the plain cancel. + int identity = flagged_kins_type(KINSTYPE_MACHINE); if (identity >= 0) { switch_kins_type(identity, settings); } settings->kins_by_g43_4 = false; } @@ -6901,9 +6902,10 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 int kins_type; if (code == G_13_1) { - // G13.1 cancels to identity kinematics; which type that is, the - // module declares, the number is not the answer - kins_type = flagged_kins_type(KINSTYPE_IDENTITY); + // G13.1 cancels to the machine frame type, identity kinematics on a + // machine whose slides line up; which type that is, the module + // declares, the number is not the answer + kins_type = flagged_kins_type(KINSTYPE_MACHINE); if (kins_type < 0) { CHKS(kins_type_info_available(), NCE_NO_IDENTITY_KINEMATICS_TYPE); kins_type = 0; // no kinematics attached: standalone interpreter diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index f7f8dfcacfc..620f6ecfee6 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -207,7 +207,7 @@ #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_INPUT _("Queue is not empty after external input") #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_KINS_TYPE_NOT_PROVIDED _("G12.1 P word does not name a kinematics type this module provides") -#define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity type (see the switchkins documentation)") +#define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity or machine frame type (see the switchkins documentation)") #define NCE_NO_PRIMARY_KINEMATICS_TYPE _("G43.4 needs the kinematics module to declare its primary type (see the switchkins documentation)") #define NCE_ANALOG_INPUT_WITH_WAIT_NOT_IMMEDIATE _("Can't select analog input with wait type != immediate return") #define NCE_ZERO_TIMEOUT_WITH_WAIT_NOT_IMMEDIATE _("Zero timeout with wait type != immediate return") diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 759fc5bd483..2404cbb6cf7 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -79,7 +79,9 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # 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 +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o +matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o emc/kinematics/kins_single_module.o +userkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 8bf76899c8e..ff542cde5d7 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -40,7 +40,7 @@ mechanical issues, including: 3. Parallelism between spindle rotational axis and Z movement. 4. Perpendicularity between spindle rotational axis and X/Y movement. -The matrix coefficients are set by parameters C_xx .. C_zz. +The matrix coefficients are set by the pins C_xx .. C_zz. For 3 axis machine, the equations become: .... @@ -152,7 +152,7 @@ Specify matrixkins in LinuxCNC INI file as: KINEMATICS=matrixkins ---- -In your HAL configuration file, set the parameters C_xx .. C_zz: +In your HAL configuration file, set the pins C_xx .. C_zz: [source,hal] ---- @@ -167,7 +167,7 @@ setp matrixkins.C_zy 0 # Skew Y axis towards Z axis setp matrixkins.C_zz 1 # Z axis scale ---- -The parameters can be modified during runtime using halcmd. +The pins can be modified during runtime using halcmd. To avoid sudden movements, it is better to turn off machine power before changes. If recalibration is performed with already existing calibration being in effect, @@ -180,68 +180,30 @@ option extra_setup; license "GPL"; ;; -static struct haldata { - hal_real_t C_xx; - hal_real_t C_xy; - hal_real_t C_xz; - hal_real_t C_yx; - hal_real_t C_yy; - hal_real_t C_yz; - hal_real_t C_zx; - hal_real_t C_zy; - hal_real_t C_zz; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; - 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; - - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); - - 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; -} - -#include -#include - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include +#include + +// the calibration matrix, one pin each; the maths reads it from the block +static const kins_param_desc matrix_params[] = { + { "C_xx", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_xy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_xz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yy", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_yz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zz", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, +}; +enum { C_XX, C_XY, C_XZ, C_YX, C_YY, C_YZ, C_ZX, C_ZY, C_ZZ }; + +static int matrix_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // For forward kinematics (joint to axis position) we @@ -251,20 +213,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); - - rtapi_real det = a * (e * i - f * h) - - b * (d * i - f * g) - + c * (d * h - e * g); - rtapi_real invdet = 1.0 / det; + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; + + const double det = a * (e * i - f * h) + - b * (d * i - f * g) + + c * (d * h - e * g); + const double invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -290,22 +252,24 @@ int kinematicsForward(const double *j, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int matrix_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; @@ -323,10 +287,10 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int matrix_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; (void)j; @@ -337,15 +301,41 @@ int kinematicsJacobian(const double *j, } // the inverse is the calibration matrix itself, so its derivative is // that matrix, and the pass-through axes are ones - jac[0][0] = hal_get_real(haldata->C_xx); - jac[0][1] = hal_get_real(haldata->C_xy); - jac[0][2] = hal_get_real(haldata->C_xz); - jac[1][0] = hal_get_real(haldata->C_yx); - jac[1][1] = hal_get_real(haldata->C_yy); - jac[1][2] = hal_get_real(haldata->C_yz); - jac[2][0] = hal_get_real(haldata->C_zx); - jac[2][1] = hal_get_real(haldata->C_zy); - jac[2][2] = hal_get_real(haldata->C_zz); + jac[0][0] = p->geometry[C_XX]; + jac[0][1] = p->geometry[C_XY]; + jac[0][2] = p->geometry[C_XZ]; + jac[1][0] = p->geometry[C_YX]; + jac[1][1] = p->geometry[C_YY]; + jac[1][2] = p->geometry[C_YZ]; + jac[2][0] = p->geometry[C_ZX]; + jac[2][1] = p->geometry[C_ZY]; + jac[2][2] = p->geometry[C_ZZ]; for (r = 3; r < 9; r++) { jac[r][r] = 1; } return 0; } + +static const kins_ops matrix_ops = { + .forward = matrix_forward, + .inverse = matrix_inverse, + .jacobian = matrix_jacobian, +}; + +// the entry points come from kins_single.c, linked in +const kins_module_info kins_module = { + .name = "matrixkins", + .halprefix = "matrixkins", + .params = matrix_params, + .nparams = sizeof(matrix_params)/sizeof(matrix_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &matrix_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH); +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 4f7c60bb707..aeee6b3b76f 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -26,7 +26,6 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// 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; @@ -49,22 +48,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -// the turn kinematics need no hal pins of their own -static int turnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; -} // turnKinematicsSetup() - -static int turnKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the turn kinematics: no geometry, written as pure functions of the +// parameter block (see kins_module.h) +static int turn_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; @@ -81,13 +74,16 @@ static int turnKinematicsForward(const double *j, pos->w = 0; return 0; -} // turnKinematicsForward() +} // turn_forward() -static int turnKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turn_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; @@ -97,25 +93,53 @@ static int turnKinematicsInverse(const EmcPose * pos, j[3] = pos->a; return 0; -} // turnKinematicsInverse() +} // turn_inverse() -static int turnKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turn_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; (void)j; (void)pos; (void)iflags; 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 + // the derivative of turn_inverse(): 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; -} // turnKinematicsJacobian() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "millturn"; + kp->halprefix = "millturn"; + kp->required_coordinates = "xyza"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &turn_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects @@ -123,24 +147,6 @@ 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; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index a732ba0de35..4550d7d1fb4 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -17,6 +17,12 @@ 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. +The kinematics are written as functions of a parameter block, see +kins_module.h and the Kinematics Conventions chapter: the geometry is +declared once in a table, one HAL pin is made per entry, and the maths +reads the block where it would have read a pin. The same maths can +then be evaluated outside realtime. + 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*). @@ -61,8 +67,8 @@ option extra_setup; // 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. +// identity kinematics, the parameter block helpers and the coordinates +// letters-to-joints mapping. This module only supplies its types. #include //===================================================================== @@ -72,36 +78,31 @@ 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; +// The geometry: one HAL pin per entry, named ., read +// into the block before every call. Add whatever the real kinematics +// need; an entry flagged as the tool arrives in p->tool.tran.z as well. - return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); -} // myKinematicsSetup() +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; -static int myKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +//--------------------------------------------------------------------- +// Example switchkins-type: a forward and an inverse over the block. +// Replace the arithmetic with the real kinematics. The frames and the +// Jacobian are optional, see kins_module.h. + +static int myForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.x = j[0] + p->geometry[P_X_OFFSET]; pos->tran.y = j[1]; pos->tran.z = j[2]; @@ -110,50 +111,83 @@ static int myKinematicsForward(const double *j, pos->u = pos->v = pos->w = 0; return 0; -} // myKinematicsForward() +} // myForward() -static int myKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int myInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; j[1] = pos->tran.y; j[2] = pos->tran.z; return 0; -} // myKinematicsInverse() +} // myInverse() + +static const kins_ops my_ops = { + .forward = myForward, + .inverse = myInverse, +}; + +//--------------------------------------------------------------------- +// The module's configuration and its switchkins-types. Type 0 is the +// startup default. Types run from 0 to SWITCHKINS_MAX_TYPES-1 with no +// gaps. + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + // the pointer arguments are the older way of providing types 0 to 2 + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + + 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->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); + + if (switchkinsRegisterOps(0, &KINS_IDENTITY_OPS)) { return -1; } + if (switchkinsRegisterOps(1, &my_ops)) { return -1; } + return 0; +} // switchkinsSetup() + +//--------------------------------------------------------------------- +// A copy of the module loaded outside realtime describes itself by +// replaying switchkinsSetup(). + +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} // kinsDescribe() + +EXPORT_SYMBOL(kinsDescribe); //--------------------------------------------------------------------- // 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. +// switchkinsInit() expects, so setup is run 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; } - + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a9cdd374bac..263aee5a86a 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -16,9 +16,8 @@ where '2.8' is the branch name (use 'master' for the master branch). For a RIP (run-in-place) build, the file is located in the git tree as: `src/hal/components/userkins.comp`. -Edit the functions kinematicsForward() and kinematicsInverse() as required. - -If required, add HAL pins following examples in the template code. +Edit the functions userkins_forward() and userkins_inverse() as required, +and list the geometry the maths needs in the *userkins_params* table. Build and install the component using halcompile: @@ -43,6 +42,17 @@ JOINTS=3 # number of joints used in your modified userkins.comp ---- +A module built with halcompile takes the shared code from the +kins_single_core module, which the HAL file loads ahead of it: + +---- +loadrt kins_single_core +loadrt [KINS]KINEMATICS +---- + +The userkins built in tree has that code linked in and needs no +kins_single_core. + Note: the manpage for userkins is not updated by `halcompile --install` To use a different component name, rename the file (example mykins.comp) and @@ -50,16 +60,18 @@ change all instances of `userkins` to `mykins`. === NOTES +* The kinematics are written as functions of a parameter block, see + kins_module.h: the geometry is declared once in the *userkins_params* + table, one HAL pin is made per entry, and the maths reads + *p->geometry[]* where it would have read a pin. The classic entry + points (kinematicsForward() and the rest) are supplied by kins_single.c, + in kins_single_core, so nothing here touches HAL and the same maths can + be evaluated outside realtime. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. -* HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which - halcompile runs once when the module is loaded, before the component is - made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() 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; @@ -68,21 +80,18 @@ license "GPL"; author "Dewey Garrett"; ;; -#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; -} *haldata; -// hal pin/param types: -// hal_bool_t boolean bit -// hal_uint_t unsigned integer -// hal_sint_t signed integer -// hal_real_t floating point (double precision) +#include +#include + +// The geometry, one HAL pin per entry, named userkins.. An entry +// is an input (read into p->geometry[] before every call), an output +// (written from s->out[] after it), or an input that can be poked +// (KINS_IO). The example pair below echoes 'in' to 'out'. +static const kins_param_desc userkins_params[] = { + { "in", KINS_PARAM_U32, KINS_IN, 0, 0 }, + { "out", KINS_PARAM_U32, KINS_OUT, 0, 0 }, +}; +enum { P_IN, P_OUT }; FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -93,61 +102,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "userkins" - 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 pin examples: - res += hal_pin_new_uint(comp_id, HAL_IN , &(haldata->in) , 0, "%s.in" , HAL_PREFIX); - res += hal_pin_new_uint(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); - - 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 -} - -KINS_NOT_SWITCHABLE -// see millturn.comp for example of switchable kinematics - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_IDENTITY; // 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() - -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; - static bool gave_msg; // [KINS]JOINTS=3 pos->tran.x = j[0]; // X coordinate pos->tran.y = j[1]; // Y coordinate @@ -160,23 +124,17 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_uint(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() +} // userkins_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int userkins_inverse(const kins_params *p, kins_scratch *s, + 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 (3 required for this template). @@ -189,25 +147,26 @@ int kinematicsInverse(const EmcPose * pos, j[1] = pos->tran.y; // joint 1 j[2] = pos->tran.z; // joint 2 - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_uint(haldata->out, hal_get_uint(haldata->in)); //dereference - //read from param example: hal_set_uint(haldata->out, hal_get_real(haldata->param_rw)); + // example output: echo the 'in' pin to the 'out' pin + s->out[P_OUT] = p->geometry[P_IN]; return 0; -} // kinematicsInverse() +} // userkins_inverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; + (void)p; (void)j; (void)pos; (void)iflags; // How each joint responds to each pose coordinate, the derivative of - // kinematicsInverse(): for this template joint 0 follows x, joint 1 - // follows y and joint 2 follows z, each one for one. See kinematics.h. + // userkins_inverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kins_module.h. + // Leave .jacobian out of the ops below to have it differenced instead. for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } } @@ -215,4 +174,44 @@ int kinematicsJacobian(const double *j, jac[1][1] = 1; jac[2][2] = 1; return 0; -} // kinematicsJacobian() +} // userkins_jacobian() + +static const kins_ops userkins_ops = { + .forward = userkins_forward, + .inverse = userkins_inverse, + .jacobian = userkins_jacobian, + // .work, .tool and .native report the frames, see kins_module.h +}; + +const kins_module_info kins_module = { + .name = "userkins", + .halprefix = "userkins", + .params = userkins_params, + .nparams = sizeof(userkins_params)/sizeof(userkins_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &userkins_ops }, +}; + +// A copy of the module loaded outside realtime asks for its description. +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + return kinsSingleDescribe(&kins_module, info); +} + +EXPORT_SYMBOL(kinsDescribe); + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects. KINEMATICS_IDENTITY is what +// kinematicsType() reports; use KINEMATICS_BOTH for a machine whose +// joints are not the axes, or to let a gui display joint values in the +// preview before homing. +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_IDENTITY); +} diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 7c6994deabd..0444c68e1d1 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -44,55 +44,33 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -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; - -static int tdrKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - 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; - - return 0; -} // tdrKinematicsSetup() - -static int tdrKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block (see +// kins_module.h), and the tool length from p->tool.tran.z +static const kins_param_desc tdr_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_XO, P_ZO, P_XR, P_YR, P_ZR }; + +static int tdr_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - 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 x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -125,22 +103,24 @@ static int tdrKinematicsForward(const double *j, pos->w = 0; return 0; -} // tdrKinematicsForward() +} // tdr_forward() -static int tdrKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdr_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - 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 x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - 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 dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -167,21 +147,21 @@ static int tdrKinematicsInverse(const EmcPose * pos, j[4] = pos->b; return 0; -} // tdrKinematicsInverse() +} // tdr_inverse() -static int tdrKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdr_jacobian(const kins_params *p, 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(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 x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -192,9 +172,9 @@ static int tdrKinematicsJacobian(const double *j, 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 + // tdr_inverse() 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; @@ -214,7 +194,36 @@ static int tdrKinematicsJacobian(const double *j, jac[3][3] = 1; jac[4][4] = 1; return 0; -} // tdrKinematicsJacobian() +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, + .primary = 1, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzab_tdr_kins"; + kp->halprefix = "xyzab_tdr_kins"; + kp->required_coordinates = "xyzab"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = tdr_params; + kp->nparams = sizeof(tdr_params)/sizeof(tdr_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tdr_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects @@ -222,25 +231,6 @@ 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; } - + if (switchkinsRunSetup(&kp, NULL)) { 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 83c8fcdfc78..2ee44e38849 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; 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 - // change them for demonstration purposes - hal_real_t y_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t y_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzacb_trsrn kinematics: - - // Declare hal pin pointers used for xyzacb_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// 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) -{ - 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; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kins_module.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "y-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Ly = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Dray = (p)->geometry[P_ROT_AXIS] - (Dy + Ly); \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // 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) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - 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 - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -130,8 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); @@ -144,32 +103,32 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dx - + (Cp*CvSs + Sp*r)*Ly - + Dy*Sp - + Dx + pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dx + + (Cp*CvSs + Sp*r)*Ly + + Dy*Sp + + Dx + Px; - pos->tran.y = - Cp*Cw*Dy - - Cw*Dx*Sp - - Cw*(Dray - Py) - - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly - + (Draz - Pz)*Sw - + Dray - + Dy + pos->tran.y = - Cp*Cw*Dy + - Cw*Dx*Sp + - Cw*(Dray - Py) + - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly + + (Draz - Pz)*Sw + + Dray + + Dy + Ly; - pos->tran.z = - Cp*Dy*Sw - - Dx*Sp*Sw - - Cw*(Draz - Pz) - - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) - + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly - - (Dray - Py)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = - Cp*Dy*Sw + - Dx*Sp*Sw + - Cw*(Draz - Pz) + - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) + + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly + - (Dray - Py)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -187,27 +146,27 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) - - (Cs*Ctc - CvSs*Stc)*Dx - + ((Ctc*CvSs + Stc*r)*Cp - + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) - - (Ctc*CvSs + Stc*r)*Dy - - (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) + - (Cs*Ctc - CvSs*Stc)*Dx + + ((Ctc*CvSs + Stc*r)*Cp + + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) + - (Ctc*CvSs + Stc*r)*Dy + - (Ctc*SvSs - Stc*t)*(Lz + Pz) - Ly*Stc; - pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) - + (Ctc*CvSs + Cs*Stc)*Dx - - ((CvSs*Stc - Ctc*r)*Cp - + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) - + (CvSs*Stc - Ctc*r)*Dy - - Ctc*Ly + pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) + + (Ctc*CvSs + Cs*Stc)*Dx + - ((CvSs*Stc - Ctc*r)*Cp + + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) + + (CvSs*Stc - Ctc*r)*Dy + - Ctc*Ly + (Stc*SvSs + Ctc*t)*(Lz + Pz); - pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) - + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) - - Dx*SvSs - + (Lz + Pz)*s - - Dy*t + pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) + + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) + - Dx*SvSs + + (Lz + Pz)*s + - Dy*t - Lz; pos->a = j[3]; @@ -222,47 +181,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, 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 - 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 - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); - - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -271,7 +218,7 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) double Cv = cos(nu*TO_RAD); double Stc = sin(tc); double Ctc = cos(tc); - + // in TCP we use the current positions of the spindle joints // in TOOL we will use the angle values calculated in remap.py double Ss = 0; @@ -286,10 +233,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -303,25 +248,25 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - - Dy*Sp - - Dx + j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + - Dy*Sp + - Dx + Qx; - j[1] = Cp*Dy - + Dx*Sp - - Cw*(Dray + Dy + Ly - Qy) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Ly - - (Draz + Dt + Lz - Qz)*Sw + j[1] = Cp*Dy + + Dx*Sp + - Cw*(Dray + Dy + Ly - Qy) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Ly + - (Draz + Dt + Lz - Qz)*Sw + Dray; - j[2] = (Dt + Lz)*s - + Ly*t - - Cw*(Draz + Dt + Lz - Qz) - + (Dray + Dy + Ly - Qy)*Sw + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + Draz; j[3] = pos->a; @@ -329,80 +274,84 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) j[5] = pos->c; } 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); - 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); - - j[0] = Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - + (Cp*SvSs - Sp*t)*Lz - + ((Cp*Cs - CvSs*Sp)*Ctc - - (Cp*CvSs + Sp*r)*Stc)*Qx - - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy - + (Cp*SvSs - Sp*t)*Qz - - Dy*Sp - - Dx; - - j[1] = Cp*Dy - - (CvSs*Sp - Cp*r)*Ly - + (Sp*SvSs + Cp*t)*Lz - + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - + Dx*Sp - - Dy - - Ly; - - j[2] = - (Ctc*SvSs - Stc*t)*Qx - + (Stc*SvSs + Ctc*t)*Qy - + Lz*s - + Qz*s - + Ly*t - - Lz; - - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; + // in TOOL kinematics we use the articulated joint positions from the TWP + 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); + + j[0] = Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + + (Cp*SvSs - Sp*t)*Lz + + ((Cp*Cs - CvSs*Sp)*Ctc + - (Cp*CvSs + Sp*r)*Stc)*Qx + - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy + + (Cp*SvSs - Sp*t)*Qz + - Dy*Sp + - Dx; + + j[1] = Cp*Dy + - (CvSs*Sp - Cp*r)*Ly + + (Sp*SvSs + Cp*t)*Lz + + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + + Dx*Sp + - Dy + - Ly; + + j[2] = - (Ctc*SvSs - Stc*t)*Qx + + (Stc*SvSs + Ctc*t)*Qy + + Lz*s + + Qz*s + + Ly*t + - Lz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; } return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, 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, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[4]*TO_RAD); @@ -437,10 +386,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[3]*TO_RAD); double Cw = cos(j[3]*TO_RAD); @@ -456,23 +406,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, 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 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 nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -536,7 +478,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -547,10 +489,10 @@ static int toolKinematicsJacobian(const double *j, // 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 + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -586,43 +528,56 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, + .primary = 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 +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzacb_trsrn"; + kp->halprefix = "xyzacb_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // 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; } - + if (switchkinsRunSetup(&kp, NULL)) { 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 7e63687c283..f914794a1f4 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; 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 - // change them for demonstration purposes - hal_real_t x_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t x_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzbca_trsrn kinematics: - - // Declare hal pin pointers used for xyzbca_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// 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) -{ - 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; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kins_module.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Lx = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Drax = (p)->geometry[P_ROT_AXIS] - Lx - Dx; \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // 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) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - 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 - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -130,9 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); @@ -144,37 +102,33 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - // onLy used to be consistent with math in documentation - Px = j[0]; - Py = j[1]; - Pz = j[2]; - - pos->tran.x = - Cp*Cw*Dx - + Cw*Dy*Sp - - Cw*(Drax - Px) - - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx - - (Draz - Pz)*Sw - + Drax - + Dx + + pos->tran.x = - Cp*Cw*Dx + + Cw*Dy*Sp + - Cw*(Drax - Px) + - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx + - (Draz - Pz)*Sw + + Drax + + Dx + Lx; - pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dy - - (Cp*CvSs + Sp*r)*Lx - - Dx*Sp - + Dy + pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dy + - (Cp*CvSs + Sp*r)*Lx + - Dx*Sp + + Dy + Py; - pos->tran.z = Cp*Dx*Sw - - Dy*Sp*Sw - - Cw*(Draz - Pz) - + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) - - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx - + (Drax - Px)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = Cp*Dx*Sw + - Dy*Sp*Sw + - Cw*(Draz - Pz) + + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) + - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx + + (Drax - Px)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -192,27 +146,25 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) - + (CvSs*Stc - Ctc*r)*Dx - + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) - - (Ctc*CvSs + Cs*Stc)*Dy - - Ctc*Lx + pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) + + (CvSs*Stc - Ctc*r)*Dx + + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) + - (Ctc*CvSs + Cs*Stc)*Dy + - Ctc*Lx + (Stc*SvSs + Ctc*t)*(Lz + Pz); - - pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) - + (Ctc*CvSs + Stc*r)*Dx - + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) - - (Cs*Ctc - CvSs*Stc)*Dy - + (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) + + (Ctc*CvSs + Stc*r)*Dx + + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) + - (Cs*Ctc - CvSs*Stc)*Dy + + (Ctc*SvSs - Stc*t)*(Lz + Pz) + Lx*Stc; - - pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) - - (Cp*SvSs - Sp*t)*(Dy + Py) - + Dy*SvSs - + (Lz + Pz)*s - - Dx*t + pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) + - (Cp*SvSs - Sp*t)*(Dy + Py) + + Dy*SvSs + + (Lz + Pz)*s + - Dx*t - Lz; pos->a = j[3]; @@ -227,44 +179,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, 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 - 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 - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -288,11 +231,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics - + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -304,27 +244,27 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) SvSs = Sv*Ss; r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - Dy*Sp - - Cw*(Drax + Dx + Lx - Qx) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Lx - + (Draz + Dt + Lz - Qz)*Sw + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - Dy*Sp + - Cw*(Drax + Dx + Lx - Qx) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Lx + + (Draz + Dt + Lz - Qz)*Sw + Drax; - j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - + Dx*Sp - - Dy + j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + + Dx*Sp + - Dy + Qy; - j[2] = (Dt + Lz)*s - + Lx*t - - Cw*(Draz + Dt + Lz - Qz) - - (Drax + Dx + Lx - Qx)*Sw + j[2] = (Dt + Lz)*s + + Lx*t + - Cw*(Draz + Dt + Lz - Qz) + - (Drax + Dx + Lx - Qx)*Sw + Draz; j[3] = pos->a; @@ -342,32 +282,31 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (CvSs*Sp - Cp*r)*Lx - + (Sp*SvSs + Cp*t)*Lz - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx - - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - - Dy*Sp - - Dx + + j[0] = Cp*Dx + - (CvSs*Sp - Cp*r)*Lx + + (Sp*SvSs + Cp*t)*Lz + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx + - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + - Dy*Sp + - Dx - Lx; - j[1] = Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - - (Cp*SvSs - Sp*t)*Lz - + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx - + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy - - (Cp*SvSs - Sp*t)*Qz - + Dx*Sp + j[1] = Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + - (Cp*SvSs - Sp*t)*Lz + + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx + + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy + - (Cp*SvSs - Sp*t)*Qz + + Dx*Sp - Dy; - - j[2] = (Stc*SvSs + Ctc*t)*Qx - + (Ctc*SvSs - Stc*t)*Qy - + Lz*s - + Qz*s - + Lx*t + j[2] = (Stc*SvSs + Ctc*t)*Qx + + (Ctc*SvSs - Stc*t)*Qy + + Lz*s + + Qz*s + + Lx*t - Lz; j[3] = pos->a; @@ -378,34 +317,38 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, 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, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[3]*TO_RAD); @@ -440,10 +383,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[4]*TO_RAD); double Cw = cos(j[4]*TO_RAD); @@ -459,23 +403,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, 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 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 nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -539,7 +475,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -550,10 +486,10 @@ static int toolKinematicsJacobian(const double *j, // 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 + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -589,43 +525,56 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, + .primary = 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 +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzbca_trsrn"; + kp->halprefix = "xyzbca_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // 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; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 5fefd687bac..48c09ee0d98 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 5 ] \ +[ "$(grep -c 'frames agree' "$1")" = 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index 3388d938055..ed6acf27a08 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -52,6 +52,9 @@ RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine"); static int spin = -1; RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none"); +static int quill = -1; +RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -206,6 +209,16 @@ static void check(const double *j, int own_kinematics) && close3(&tool.z, 0, 0, 1), "the spindle stays square", j); } + if (quill >= 0) { + /* the joint runs the tool out along its own axis, away from the + holder, so the tip moves along the tool axis reversed: the one + tie between the reported frame and the forward transform on a + machine that turns nothing but the tool */ + response(j, quill, &d); + expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + "the quill runs out along the tool axis", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; @@ -228,7 +241,7 @@ int rtapi_app_main(void) const int angles = sizeof(angle) / sizeof(angle[0]); double j[EMCMOT_MAX_JOINTS]; int a, b, c, t; - int checked = 0; + int checked = 0, own = 0; if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints); @@ -246,6 +259,7 @@ int rtapi_app_main(void) memset(j, 0, sizeof(j)); if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } + own = 0; /* every kinematics the module offers, not just the one it starts in: the frames a switchable module reports are per type, and the @@ -254,6 +268,7 @@ int rtapi_app_main(void) if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } if (!supplies_frames(j)) { continue; } checked++; + if (t == ktype) { own = 1; } for (a = 0; a < angles; a++) { if (r1 >= 0) { j[r1] = angle[a]; } @@ -272,6 +287,16 @@ int rtapi_app_main(void) if (!kinematicsSwitchable()) { break; } } + /* the identity type a switchable module carries supplies frames of + its own, so a module that reports none for the machine it models + would otherwise pass on its neighbour's answers */ + if (checked && !own) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: FAIL the module reports no frames for" + " kinematics type %d, the machine it models\n", ktype); + failures++; + } + if (!checked) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports frames for no type\n"); diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index b1471ea45b7..109e69821dc 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,5 +49,8 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ + "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" + run "pumakins" "setp pumakins.A2 300" \ "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..ca2b15df9a9 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python3 +# The non-realtime half of the parameter block parity test. +# +# Evaluates the module paritycheck was loaded after through the +# non-realtime loader (libkinslimits, kinematicsUserInit and friends), +# which dlopens the module, asks it to describe itself through +# kinsDescribe(), fills a parameter block from the module's own pins and +# calls the same ops the realtime wrapper calls. The answers have to +# match what paritycheck published, to rounding, or the module is not +# the pure function of its parameters it claims to be. +# +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT +# POSE and JNT are comma separated numbers, as given to paritycheck; +# COORDS and SPARM are a dash when the module was loaded without them. + +import ctypes +import os +import sys + +import hal + +EMC2_HOME = os.environ.get("EMC2_HOME") +# local, the way a GUI holds them under the interpreter it loads: what a +# module needs in scope, the HAL library and an out of tree module's core, +# the loader puts there itself +def lib(name): + if EMC2_HOME: + return ctypes.CDLL(os.path.join(EMC2_HOME, "lib", name)) + return ctypes.CDLL(name) + +class EmcPose(ctypes.Structure): + _fields_ = [(n, ctypes.c_double) for n in "xyzabcuvw"] + +MAX_JOINTS = 9 +AXES = 9 +Joints = ctypes.c_double * MAX_JOINTS +Jac = (ctypes.c_double * AXES) * MAX_JOINTS + +module, joints, coords, ktype, frompose = sys.argv[1], int(sys.argv[2]), sys.argv[3], int(sys.argv[4]), int(sys.argv[5]) +pose_in = [float(v) for v in sys.argv[6].split(",")] +jnt_in = [float(v) for v in sys.argv[7].split(",")] +# a dash stands for an absent value, since halcmd hands quotes through +if coords == "-": + coords = "" +sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +# the orientation joints this machine is known to have, "primary,secondary"; +# "-" where it has no such pair, "no-frame" where it reports no tool frame +orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" +posejoint_in = sys.argv[10] if len(sys.argv) > 10 else "no-frame" +pose_in += [0.0] * (AXES - len(pose_in)) +jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) + +halc = lib("liblinuxcnchal.so.0") +kins = lib("libkinslimits.so.0") + +kins.kinematicsUserInitSparm.restype = ctypes.c_void_p +kins.kinematicsUserInitSparm.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p, + ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p] +for fn in ("kinematicsUserIsRtOnly", "kinematicsUserGetNumTypes"): + getattr(kins, fn).argtypes = [ctypes.c_void_p] +kins.kinematicsUserSetType.argtypes = [ctypes.c_void_p, ctypes.c_int] +kins.kinematicsUserInverse.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Joints] +kins.kinematicsUserForward.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(EmcPose)] +kins.kinematicsUserJacobian.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Jac] +kins.kinematicsUserFree.argtypes = [ctypes.c_void_p] + +comp_id = halc.hal_init(b"kpcheck") +if comp_id < 0: + print("kins-params: FAIL hal_init") + sys.exit(1) +ctx = kins.kinematicsUserInitSparm(module.encode(), joints, coords.encode() if coords else None, sparm, + comp_id, b"kpcheck") +halc.hal_ready(comp_id) +failures = 0 + +def fail(what): + global failures + failures += 1 + print("kins-params: FAIL %s" % what) + +if not ctx or kins.kinematicsUserIsRtOnly(ctx): + fail("%s cannot be evaluated outside realtime" % module) + sys.exit(1) +if ktype and kins.kinematicsUserSetType(ctx, ktype): + fail("%s has no type %d in the block form" % (module, ktype)) + sys.exit(1) + +def pose_of(values): + p = EmcPose() + for n, v in zip("xyzabcuvw", values): + setattr(p, n, v) + return p + +def close(a, b): + return abs(a - b) <= 1e-9 * max(1.0, abs(a), abs(b)) + +def compare(what, ours, theirs): + if not close(ours, theirs): + fail("%s: loader %.12g, realtime %.12g" % (what, ours, theirs)) + +rc_fwd = hal.get_value("paritycheck.rc-fwd") +rc_inv = hal.get_value("paritycheck.rc-inv") +rc_jac = hal.get_value("paritycheck.rc-jac") + +q = Joints(*jnt_in) +qi = Joints(*jnt_in) +J = Jac() +if frompose: + P = pose_of(pose_in) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(P), qi) + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, qi, ctypes.byref(F)) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(P), J) +else: + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, q, ctypes.byref(F)) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(F), qi) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(F), J) + +# the same success or failure on both sides, then the same numbers +for what, ours, theirs in (("forward", r_fwd, rc_fwd), ("inverse", r_inv, rc_inv), ("jacobian", r_jac, rc_jac)): + if (ours != 0) != (theirs != 0): + fail("%s returned %d in the loader and %d in realtime" % (what, ours, theirs)) + +if r_fwd == 0 and rc_fwd == 0: + for n in "xyzabcuvw": + compare("forward %s" % n, getattr(F, n), hal.get_value("paritycheck.fwd-%s" % n)) +if r_inv == 0 and rc_inv == 0: + for j in range(joints): + compare("inverse joint %d" % j, qi[j], hal.get_value("paritycheck.inv-%d" % j)) +if r_jac == 0 and rc_jac == 0: + for j in range(joints): + for a, n in enumerate("xyzabcuvw"): + compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) + +# the caller's tool wins over the module's pin: for a module with a tool +# entry, a length of the caller's must move the inverse, and handing the +# tool back to HAL must return it to what realtime found +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +tool_pin = None +for name in ("tool-offset", "tool-offset-z"): + try: + hal.get_value("%s.%s" % (module, name)) + tool_pin = name + except RuntimeError: + pass +if r_inv == 0 and rc_inv == 0 and tool_pin: + P = pose_of(pose_in) if frompose else F + T = pose_of([0.0] * AXES) + T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 + kins.kinematicsUserSetTool(ctx, ctypes.byref(T)) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the caller's tool") + elif all(close(qt[j], qi[j]) for j in range(joints)): + fail("the caller's tool did not move the inverse") + kins.kinematicsUserSetTool(ctx, None) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the tool handed back") + else: + for j in range(joints): + compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) + +# the two rotaries that orient the tool, told apart by which carries the +# other. The sign of the secondary is what G53.1 P names, so a module that +# gets this wrong sends the machine to the other pose without saying so. +kins.kinematicsUserOrientJoints.argtypes = [ctypes.c_void_p, Joints, + ctypes.POINTER(ctypes.c_int), + ctypes.POINTER(ctypes.c_int)] +primary, secondary = ctypes.c_int(-1), ctypes.c_int(-1) +r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jnt_in), + ctypes.byref(primary), ctypes.byref(secondary)) +class Rot(ctypes.Structure): + _fields_ = [(n, ctypes.c_double * 3) for n in "xyz"] +kins.kinematicsUserToolFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +frame = Rot() +has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jnt_in), ctypes.byref(frame)) == 0 +if r_orient == 0: + got = "%d,%d" % (primary.value, secondary.value) +else: + got = "-" if has_frame else "no-frame" +if got != orient_in: + fail("orientation joints are %s, expected %s" % (got, orient_in)) +else: + print("kins-params: %s type %d orientation joints %s" % (module, ktype, got)) + +# the rotary whose sign G53.1 P1 and P2 name, the first met going from the +# tool to the work: the secondary of two heads, the carrier of two tables, +# the head of one of each +kins.kinematicsUserPoseJoint.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(ctypes.c_int)] +pose_joint = ctypes.c_int(-1) +if kins.kinematicsUserPoseJoint(ctx, Joints(*jnt_in), ctypes.byref(pose_joint)) == 0: + got = "%d" % pose_joint.value +else: + got = "-" if has_frame else "no-frame" +if got != posejoint_in: + fail("the joint naming the pose is %s, expected %s" % (got, posejoint_in)) +else: + print("kins-params: %s type %d pose joint %s" % (module, ktype, got)) + +kins.kinematicsUserFree(ctx) +halc.hal_exit(comp_id) + +if failures: + sys.exit(1) +print("kins-params: %s type %d agrees" % (module, ktype)) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult new file mode 100755 index 00000000000..d3eba1a4da0 --- /dev/null +++ b/tests/kins-params/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c new file mode 100644 index 00000000000..1a3ef4664ab --- /dev/null +++ b/tests/kins-params/paritycheck.c @@ -0,0 +1,148 @@ +/* + * paritycheck: the realtime half of the parameter block parity test. + * + * Loaded after a kinematics module, it evaluates the module through the + * classic entry points once, at load, and publishes the answers on HAL + * pins: the forward pose, the inverse joints and the Jacobian. check.py + * then evaluates the same module through the non-realtime loader, which + * goes through kinsDescribe() and the parameter block, and compares. + * + * Two flows. With frompose=0 the input is a joint set: the pose is the + * forward of it, the joints published are the inverse of that pose, and + * the Jacobian is taken there. With frompose=1 the input is a pose, for + * the parallel machines whose forward wants a seed: the joints are its + * inverse, the forward is run from the pose as seed, and the Jacobian is + * taken there. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + * frompose 0 or 1, as above + * pose up to nine integers, the pose (frompose=1) or the forward + * seed (frompose=0) + * jnt up to sixteen integers, the joint set (frompose=0) or the + * inverse seed (frompose=1) + */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to take the pose as the input"); +static int pose[EMCMOT_MAX_AXIS] = { 0 }; +RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); +static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; +RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); + +static int comp_id = -1; + +static struct { + hal_real_t fwd[EMCMOT_MAX_AXIS]; + hal_real_t inv[EMCMOT_MAX_JOINTS]; + hal_real_t jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + hal_sint_t rc_fwd; + hal_sint_t rc_inv; + hal_sint_t rc_jac; +} *pins; + +static const char letter[EMCMOT_MAX_AXIS] = { 'x','y','z','a','b','c','u','v','w' }; + +static double *coord(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +int rtapi_app_main(void) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double q[EMCMOT_MAX_JOINTS], qi[EMCMOT_MAX_JOINTS]; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + EmcPose P, F, seed; + int a, j, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("paritycheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->fwd[a], 0.0, + "paritycheck.fwd-%c", letter[a]); + } + for (j = 0; j < joints; j++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->inv[j], 0.0, + "paritycheck.inv-%d", j); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->jac[j][a], 0.0, + "paritycheck.jac-%d-%c", j, letter[a]); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_fwd, 0, "paritycheck.rc-fwd"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_inv, 0, "paritycheck.rc-inv"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_jac, 0, "paritycheck.rc-jac"); + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } + + // a switchable module's first forward after load restarts from the + // pose it saved, which is nothing yet; take that call here so the one + // measured starts from the seed like the loader's does + F = seed; + kinematicsForward(q, &F, &fflags, &iflags); + fflags = 0; iflags = 0; + + if (frompose) { + P = seed; + hal_set_si32(pins->rc_inv, kinematicsInverse(&P, qi, &iflags, &fflags)); + F = seed; + fflags = 0; iflags = 0; + hal_set_si32(pins->rc_fwd, kinematicsForward(qi, &F, &fflags, &iflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &P, jac, &iflags)); + } else { + F = seed; + hal_set_si32(pins->rc_fwd, kinematicsForward(q, &F, &fflags, &iflags)); + iflags = 0; fflags = 0; + hal_set_si32(pins->rc_inv, kinematicsInverse(&F, qi, &iflags, &fflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &F, jac, &iflags)); + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->fwd[a], *coord(&F, a)); } + for (j = 0; j < joints; j++) { + hal_set_real(pins->inv[j], qi[j]); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->jac[j][a], jac[j][a]); } + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-params/skip b/tests/kins-params/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-params/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh new file mode 100755 index 00000000000..1e623690fa4 --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,186 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install paritycheck.c >/dev/null + +# The two templates the way a module built out of tree is: renamed, +# compiled by halcompile on their own, and loaded behind the core that +# supplies the shared code. +sed 's/switchkinscomp/oot_switchkins/g' "$EMC2_HOME/src/hal/components/switchkinscomp.comp" > oot_switchkins.comp +sed 's/userkins/oot_userkins/g' "$EMC2_HOME/src/hal/components/userkins.comp" > oot_userkins.comp +${SUDO} halcompile --install oot_switchkins.comp >/dev/null +${SUDO} halcompile --install oot_userkins.comp >/dev/null +rm -f oot_switchkins.comp oot_userkins.comp + +# One hal file per module. paritycheck evaluates the module in realtime +# through the classic entry points and publishes the answers; check.py +# evaluates it through the non-realtime loader, kinsDescribe() and the +# parameter block, and compares. Where they disagree the module keeps +# state its table does not declare. +# ONLY= in the environment runs the entries for that module alone +run() { + local loadrt="$1" setp="$2" parms="$3" ktype="$4" + local module coords sparm joints frompose pose jnt orient posejoint rtparms hal tok + case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac + module=${loadrt%% *} + coords=""; sparm="" + for tok in $loadrt; do + case "$tok" in + coordinates=*) coords=${tok#coordinates=} ;; + sparm=*) sparm=${tok#sparm=} ;; + esac + done + joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + # the two rotaries that orient the tool, "primary,secondary", or "-" + # where the machine has no such pair and "no-frame" where the module + # reports no tool frame at all, which most of the tree still does + orient="no-frame" + # the rotary whose sign G53.1 P names, the same three ways; where the + # entry gives none, what it gives for the pair + posejoint="" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + orient=*) orient=${tok#orient=} ;; + posejoint=*) posejoint=${tok#posejoint=} ;; + esac + done + [ -n "$posejoint" ] || posejoint=$orient + # what the module under test is loaded with: everything but our own words + rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g; s/ posejoint=[^ ]*//g') + hal=$(mktemp --suffix=.hal) + { if [ -n "${CORE:-}" ]; then printf 'loadrt %s\n' "$CORE"; fi + printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" + # halcmd keeps quotes, so an absent value travels as a dash + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" + } > "$hal" + echo "=== $loadrt type ${ktype:-0}" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, a gantry included +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30 orient=-" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20 orient=-" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 orient=-" +run "userkins" "" "joints=3 jnt=10,20,30" +CORE=kins_single_core run "oot_userkins" "" "joints=3 jnt=10,20,30" +CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30 orient=-" +CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30" 1 +run "millturn" "" "joints=4 jnt=10,20,30,40 orient=-" +run "millturn" "" "joints=4 jnt=10,20,30,40" 1 + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads, offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 jnt=10,20,30,0,15,25,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" 1 + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,4 posejoint=4" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=-" 2 + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3" 1 + +# polar +run "rosekins" "" "joints=3 jnt=10,5,30" + +# arms +run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" +run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" +run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" + +# parallel machines, from a pose the forward can be seeded with +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 pose=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" +run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e031f2cd0ff..e426b4d7554 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -7,5 +7,6 @@ block after the switch is planned in the kinematics that runs it, that the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 -here, not 0), and that G13.1 in an ON_ABORT_COMMAND routine does not -swallow the rest of the routine. +here, not 0), that the tool length G43.4 puts in effect reaches the +kinematics with nothing netted to its pin, and that G13.1 in an +ON_ABORT_COMMAND routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 7e999b9aa76..e62800c5cd7 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -76,6 +76,7 @@ def mdi(cmd): said = [] at_switch = None +holding = False strayed = [0.0] * JOINTS w_reached = 0.0 deadline = time.time() + 60 @@ -88,10 +89,25 @@ def mdi(cmd): last = k if k == 1 and at_switch is None: at_switch = now - if k == 1 and at_switch is not None: - for j in CARRIED: - strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) + holding = True + else: + holding = False + # the joints are held over the first stretch in identity, the one the + # W stroke runs in; later on the program applies a tool length in the + # tilted pose, which moves the joints on purpose + if holding: + # only a sample with W out on its stroke counts: the lines after + # it switch kinematics and apply a tool length in the tilted pose, + # which moves the joints on purpose, and they can all be over + # before the first sample that sees W back home + if now[5] > 1e-6: + for j in CARRIED: + strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) w_reached = max(w_reached, now[5]) + # the stretch ends with W back home: a later switch to the primary + # and back can be over between two samples, so do not wait to see it + if w_reached > 10 - 1e-3 and abs(now[5]) < 1e-6: + holding = False said += drain() if s.interp_state == linuxcnc.INTERP_IDLE and time.time() > deadline - 58: break @@ -147,10 +163,41 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- a negative kinematics number is refused ----------------------------- +# ---- the tool length reaches the kinematics with nothing netted ---------- +# +# Motion hands the module the offset G43 puts in effect. The head is +# tilted, so the offset moves the joints, by the tilt term of the length: +# the same length along the tool axis instead of along Z. +import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) +drain() +mdi("G12.1 P0") +mdi("G0 X10 Y10 Z-5 B-22.5 C45") +mdi("G49") +before = mdi("G0 X10 Y10 Z-5") +after = mdi("G43.4 H1") +L = 12.5 # tool 1 in tool.tbl +b, cc = math.radians(-22.5), math.radians(45) +want = [-L * math.sin(math.pi - b) * math.cos(cc), + -L * math.sin(math.pi - b) * math.sin(cc), + -L * (1 + math.cos(math.pi - b))] +got = [after[j] - before[j] for j in (0, 1, 2)] +if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("G43.4 in the tilted pose moved the joints by %s, not %s" + % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) +else: + print("G43.4 moved the joints by the tilt term of the tool length") +# G49 would drop to identity and hold the joints where they are; a zero +# offset without a switch takes the length back out +back = mdi("G43.1 Z0") +if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: + error("a zero tool length did not take the length back out of the joints") +mdi("G49") + +# ---- a negative kinematics number is refused ----------------------------- + drain() c.mdi("G12.1 P-1") c.wait_complete(30) diff --git a/tests/kins-tool-offset/README b/tests/kins-tool-offset/README new file mode 100644 index 00000000000..db4bd42a726 --- /dev/null +++ b/tests/kins-tool-offset/README @@ -0,0 +1,7 @@ +The kinematics module takes the tool offset from motion, not from a net. + +Runs xyzac-trt-kins under motion with nothing connected to its tool-offset +pin, applies a tool length through G43, and checks that the joints move as +the tool length requires. Then connects motion.tooloffset.z to the pin the +old way and checks that nothing changes, and that G49 takes the length back +out through motion alone. diff --git a/tests/kins-tool-offset/checkresult b/tests/kins-tool-offset/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/kins-tool-offset/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/kins-tool-offset/sim.hal b/tests/kins-tool-offset/sim.hal new file mode 100644 index 00000000000..81a0df64444 --- /dev/null +++ b/tests/kins-tool-offset/sim.hal @@ -0,0 +1,20 @@ +# the module under test, with nothing on its tool-offset pin +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 + +# offsets, so that the tool length reaches the joints through a rotation +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +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 J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.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-tool-offset/test-ui.py b/tests/kins-tool-offset/test-ui.py new file mode 100755 index 00000000000..dbd174caa66 --- /dev/null +++ b/tests/kins-tool-offset/test-ui.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The kinematics module takes the tool offset from motion. +# +# xyzac-trt-kins runs with nothing connected to its tool-offset pin. A +# tool length applied with G43 must still reach the joints, since motion +# hands the offset to the module; connecting motion.tooloffset.z to the +# pin afterwards, the old way, must change nothing; and G49 must take the +# length back out again through motion alone. + +import linuxcnc +import hal +import subprocess +import sys +import os +import time + +TOOL_LENGTH = 25.0 +POSE = "G0 X10 Y20 Z30 A30 C45" +AWAY = "G0 X0 Y0 Z0 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + +def joints(): + # the commanded joint positions once the move has settled: in position, + # nothing queued, and the same answer twice in a row, since the in + # position flag can go up a cycle before the last increment lands + deadline = time.time() + 30 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(5)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.1) + error("timed out waiting for the move") + return last + +def same(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +def show(what, j): + print("%-28s %s" % (what, " ".join("%.6f" % v for v in j))) + +# no tool: the pose with nothing applied +mdi("G49", POSE) +base = joints() +show("G49", base) + +# tool applied through motion, the pin still at its default +mdi("G43 H1", AWAY, POSE) +with_tool = joints() +show("G43 H1, pin unconnected", with_tool) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if pin != 0.0: + error("the tool-offset pin reads %g with nothing connected" % pin) +if same(base, with_tool): + error("the tool length did not reach the joints") + +# the table on rotaries at A30 C45: the tool length moves Y and Z joints, +# by a known amount, since the pivot geometry is the module's alone +tool_z = hal.get_value("motion.tooloffset.z") +if abs(tool_z - TOOL_LENGTH) > 1e-9: + error("motion.tooloffset.z is %g, expected %g" % (tool_z, TOOL_LENGTH)) +if abs(with_tool[0] - base[0]) > 1e-6: + error("the tool length moved joint 0, which the A rotation does not touch") + +# the old connection: nothing may change +subprocess.check_call(["halcmd", "net", ":tool-offset", + "motion.tooloffset.z", "xyzac-trt-kins.tool-offset"]) +mdi(AWAY, POSE) +with_net = joints() +show("G43 H1, pin connected", with_net) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if abs(pin - TOOL_LENGTH) > 1e-9: + error("the connected tool-offset pin reads %g" % pin) +if not same(with_tool, with_net): + error("connecting the pin changed the joints") + +# and back out, through motion, with the pin connected +mdi("G49", AWAY, POSE) +without = joints() +show("G49, pin connected", without) +if not same(base, without): + error("G49 did not take the tool length back out") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-tool-offset/test.ini b/tests/kins-tool-offset/test.ini new file mode 100644 index 00000000000..bb7839671ed --- /dev/null +++ b/tests/kins-tool-offset/test.ini @@ -0,0 +1,112 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 2000 +NO_FORCE_HOMING = 1 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = xyzac-trt-kins +JOINTS = 5 + +[AXIS_X] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Y] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Z] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +HOME_SEQUENCE = 0 diff --git a/tests/kins-tool-offset/test.sh b/tests/kins-tool-offset/test.sh new file mode 100755 index 00000000000..a31b772a81c --- /dev/null +++ b/tests/kins-tool-offset/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash -e +linuxcnc -r test.ini diff --git a/tests/kins-tool-offset/tool.tbl b/tests/kins-tool-offset/tool.tbl new file mode 100644 index 00000000000..acb961918d9 --- /dev/null +++ b/tests/kins-tool-offset/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 ;the tool with a length diff --git a/tests/tool-frame/toolframecheck.c b/tests/tool-frame/toolframecheck.c index ceab3acee12..095756589e4 100644 --- a/tests/tool-frame/toolframecheck.c +++ b/tests/tool-frame/toolframecheck.c @@ -23,7 +23,7 @@ #include #include #include -#include +#include MODULE_LICENSE("GPL"); MODULE_DESCRIPTION("tool frame helper checker"); @@ -237,10 +237,29 @@ static int holds(const double *sols, int count, int njoints, return 0; } +/* the joints that turn the work, read off the work frame rather than + declared, so that a caller can hold the table without naming it */ +static void test_work_joints(void) +{ + const double seed[6] = {0, 0, 0, 10, 20, 30}; + unsigned int mask = 99; + + check(toolFrameWorkJoints(xyzacWork, 5, seed, &mask) == 0 && mask == ((1u << 3) | (1u << 4)), + "xyzac: both rotaries carry the work"); + check(toolFrameWorkJoints(identityFrame, 5, seed, &mask) == 0 && mask == 0, + "a head machine: nothing turns the work"); + check(toolFrameWorkJoints(mixedWork, 6, seed, &mask) == 0 && mask == (1u << 3), + "table and head: the table joint alone"); + check(toolFrameWorkJoints(NULL, 5, seed, &mask) == -1, + "no frame function is refused"); +} + static void run_checks(void) { PmRotationMatrix m, r; + test_work_joints(); + /* the supplied constants are usable as declarations */ check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper"); check(toolFrameIsProper(&TOOL_FRAME_FLANGE), "TOOL_FRAME_FLANGE is proper");