Multiaxis kinematics and limits - #4374
Conversation
9e1c9ed to
773c3e9
Compare
773c3e9 to
34ab07b
Compare
|
What is the migration path for people having their own out-of-tree kinematics switch component? |
Their module needs no source change. G12.1 and G13.1 reach a module through the same I checked rather than assumed. I built an out-of-tree component that deliberately does not use switchkins.h or switchkins.c: its own switchkins type, its own with no source change, and no deprecation warning, since the pin is never written. The config keeps working as well. The pin is deprecated, not gone, so an existing M68/M66 config runs as before with one warning per session. Moving off it is a config edit, not code: and the One group does have something to do, and it is not the one you asked about: anybody keeping a copy of switchkins.c in their own tree. Their current build is unaffected, but Announcing it is worth raising at a weekly meeting. Nothing is removed in this PR. |
|
Just a thought: |
|
That matches what switchkins.adoc already warns about: coordinate system offsets are described as generally applicable only to the type 0 startup kinematics, with advice to reset them or update them before switching. So the problem is real, and today it rests on the operator remembering. Can I ask what you switch between, and whether you cut in both types? I am trying to tell two situations apart. If both types leave the axis letters describing the same physical point, then your G54 values are still correct after the switch, and what is missing is having a different one become active. If one of the types is identity, the stored numbers stop describing anything, since X becomes joint 0 and may not even carry the same units, and then a different set does not help either; the values want clearing. Which of these is it for you: does the zero point of the part sit somewhere different once you switch, or is the part zero the same and you simply want a different one of G54 to G59.3 selected? On the mechanism, do you mean an association living in the INI, a coordinate system named per kinematics type? There is nothing like that today. Or do you mean it read at runtime? kins-type is a status pin, so it reports rather than commands, but you could net it into a motion analog input and branch on it with M66 in a sub, which would work now. I am unsure whether that belongs in the core or stays the sort of thing an integrator wires up, and your answer would tell me which problem I am actually looking at. |
Here are two examples, the tool may cut in all kinematic modes. Example1: Serial manipulator arm Example2: Tilted work plane Currently I have two ways of dealing with this:
Maybe a different set of G54...G59.3 values. One set for each kins-type. There are also some other things to think about when working in different kinematic modes ( eg G28 , G30, G53, G92/G52, M6) it might be useful to have way of blocking certain Gcodes / Axiswords in a given mode. |
|
Both examples are clear and they moved my view: I had been assuming identity was mostly a jogging mode, and you cut in all modes. I looked at how the commercial controls handle both cases, and in each of them the closest thing we could do in LinuxCNC turns out not to be an offset set. For the singularity case, the target stays in Cartesian coordinates and only the interpolation changes: a move that runs the inverse once at the endpoint and then interpolates the joints linearly to reach it. Coordinates and units never change, so the offsets keep applying and there is nothing to switch. In tree that is a new move type from canon into the trajectory planner, and the existing KINEMATICS_INVERSE_FLAGS already carry enough to pin which solution branch it lands on. G-code would need a way to ask for it. For the tilted plane, an extra stage in the offset chain rather than new values in it: a frame composed on top of the active G54, with its own cancel, so the blocks in between are programmed in the tilted plane while G54 itself is untouched. That is a stage in rotate_and_offset_pos() plus the modal state to go with it. Nothing is rewritten, so nothing needs restoring and on_abort_with_twp_reset.ngc has nothing left to do. Orienting the tool normal to the plane, your G53.1, does need the kinematics to say where the tool points and to solve for the rotaries, which is item 3 above. On having more of them: that part has plenty of precedent and I would be happy to look at it separately. Fanuc adds G54.1 P1 to P48, or to P300 on the 30i, and Siemens adds G505 to G599. What I cannot find anywhere is a control that ties a set to the active kinematics or transformation. Selection is always explicit in the program, or it belongs to a channel or path. So more coordinate systems yes, but I would rather not bind them to the kinstype. Which leaves me reading one set per kinstype as paying for the two absences above rather than as the thing you actually want. You have run this in production and I have not, so I would rather hear where that is wrong. Would a composing frame cover your case 2, or would it miss something? Your other point stands either way. Per-mode restrictions on G28, G30, G53, G92 and M6 look right to me, and refusing outright beats guessing. |
|
Nice that you have an overview of what other controllers are doing. Makes more sense adopting something that is established rather than coming up with something quirky that falls on our feet somewhere down the road.
That is how I started out, an offset built into the kinematics. The reason I moved away from that was that in it's current form LinuxCNC is totally unaware of what is going on in a custom kinematics. Basically what I'd like is to NOT have to tamper directly with the values in the offset table. |
|
That is the same conclusion from the other side, and it names the dependency. A composing frame is only worth having if motion knows the frame is there; otherwise G53 and G28 keep meaning whatever each module happens to do with them, and we are back to the operator remembering. That is items 3 and 4 above, which is why they come before anything in the offset chain. What "aware" has to mean concretely: the module answers where the tool points and how joint motion maps to world motion, both as plain functions of the joint values and a parameter block, with no state hidden in HAL pins. Once the control can call the kinematics rather than only drive it, G53 resolves in machine axis space, the per-mode restrictions you listed get checked rather than guessed, and a frame composes on top of G54 with nothing rewriting the table. So no per-kinstype offset sets, nothing tampering with the table in the background, and the tilted plane becomes a frame that cancels. Your two examples are the cases I will hold it to. |
|
Sounds great! |
ffd07b8 to
fea53f7
Compare
|
One more thing the tool round trip does not cover, found while giving Both modules add the W coordinate to the pivot to tip distance, so W is already a tool-length route: So on these two modules the tool length has three spellings: the W column with W commanded, the This is the same shape as the rest of items 3 and 4. Each module infers the tool from whatever it happens to be handed, every config picks a different one of the three, and nobody owns which is meant. Once the tool arrives as a parameter at the call site there is one answer instead of three. |
|
I have a set of kinematics that probably cover most 5 axis machine (tool side rotation, work side rotation and mixed) including nutating joints and tilted work plane. I have long wonder if I should add those to the collection: |
|
Yes, and I would take the kinematics first. The nutating ones are what items 3 and 4 need most. A nutating head is where "where does the tool point" stops being obvious, so a weak default every module answers is only worth having if something in tree can break it. Today the only nutating configs are the two On the TWP remap I would rather you decide with item 5 in front of you. I am proposing to move that mechanism into One request if the kins come in: could they take the tool length through a |
|
I'm not in hurry it's been sitting here for a while now. |
d32027c to
87c645b
Compare
|
I had a go at the split on the two trsrn configs in tree, to see how it lands: https://github.com/grandixximo/linuxcnc/tree/twp-remap-split Your generic I ran both configs through eight orientations under Three things I hit in your files:
The commit is authored by me at the moment and the code is yours, so I would set you as author before it goes anywhere. Happy either way: take the branch and send it yourself, or tell me it looks right and I will. |
|
Thanks for looking through this. I'll need to have a good look and report back. |
|
I have found a problem with G13.1/G12.1 when either is in a 'on_abort' call. Testing with This config has [RS274NGC] I have noticed strange behavior with the remapped gcodes after issuing MDI commands that trigger this routine. For example, it looks like the You can get an indication of the issue if you uncomment the first line in the on_abort sub: Now start the config, clear estop and switch the machine on which calls the 
Further testing has shown that having either a |
87c645b to
5bf0103
Compare
The dual rotary table is its TCP inverse differentiated: the rotation matrix for the linear columns, and each term with A or B advanced a quarter turn for the rotary ones. The nutating heads are differentiated the same way, term by term through the secondary angle's r, s and t and the primary angle's sine and cosine. Their TCP inverse reads the rotary angles from the joint argument rather than from the pose, the two being the same numbers once a move is done; the derivative is taken against the pose, which is what a consumer multiplies by, and is the coupling the machine has. The TOOL type takes its angles from pins, so its inverse is linear in the pose and its rows are the coefficients.
…supply the Jacobian A strut or rod changes length by the component of its moving end's motion along it, so the rows of the parallel machines are unit vectors and moments rather than differentiated formulas. The tripod's rows are the strut directions; the linear delta's are the rod directions scaled by the rise; the rotary delta's follow from the foot staying a shin from each knee, so the foot and the knee agree along the leg. The hexapod's rows are the ones its own Newton step already builds, with the rotary columns taken through the matrix that carries roll, pitch and yaw rates to the angular velocity; with a screw lead set, whose correction is a function of the pose too, it falls back to differencing. The pentapod differentiates InvKins() the same way, in effector coordinates.
Both inverses are chains of a few closed form steps, and the derivative follows the chain: for the scara the squared reach fixes the elbow and the bearing less the outer arm's angle fixes the shoulder; for the scorbot the distance to the wrist fixes the isosceles triangle the shoulder and elbow make. Each declines at the poses where its own inverse has no derivative, the arm straight or folded.
compute_jinv() already gives radians of joint per unit of base frame twist. A pose word rate is not a twist: the roll, pitch and yaw rates reach the angular velocity through the matrix of the axes each one turns about, for the RPY convention go_rpy_mat_convert() uses. The Jacobian is that product, with the unit conversions and the unrotate coupling applied in the order the inverse applies them, and the u, v, w pass-through as ones. Having the conversion written once in the module is worth more than the closed form itself, since every consumer would otherwise guess it.
A realtime component loaded after the module under test, reaching it
through the exported entry points, with a failed check failing the load.
Every kinematics module in the tree, every switchkins type, both
conventional-directions settings on the tables.
Two checks, neither reusing the module's own answer. Against the
forward: perturb one joint, difference the forward, multiply by the
Jacobian, and expect that joint's unit vector; the forward is a separate
piece of code from the inverse, so this catches a transposed matrix, a
wrong sign, a wrong column and a wrong unit whichever way the module
answered. Against the inverse: difference it here with a different step
and compare entry by entry, which is the check for the gantry, whose
forward is not one to one.
Verified by mutation, in failed checks, one mutation per module or shared
routine and none passing:
shared differencing, step sign reversed: 3392
shared differencing, matrix transposed: 4224
identity, columns shifted (gantry run): 8
xyzac, one sign in the A column: 40
xyzbc, one entry transposed: 81
5axiskins, one sign in the B column: 30
maxkins, one sign: 15
corexykins, belt difference sign: 3
rotatekins, one entry transposed: 22
rosekins, one sign: 11
matrixkins, one entry transposed: 4
millturn, the turned Y sign: 2
xyzab_tdr_kins, one sign in the A column: 30
xyzacb_trsrn, one sign in the B column: 75
xyzacb_trsrn, one term of the A column dropped: 100
xyzbca_trsrn, one sign in the B column: 100
tripodkins, two components swapped: 8
lineardeltakins, one sign: 12
rotarydeltakins, knee derivative sign: 33
genhexkins, E transposed: 1939
genhexkins, rotary unit scaling dropped: 3456
pentakins, cross product reversed: 560
scarakins, elbow derivative sign: 1500
scorbot-kins, one sign: 32
genserkins, E transposed: 960
genserkins, linear unit scaling dropped: 768
genserkins, unrotate coupling sign: 64
The check against the forward is also a round trip test of each module,
and it found four modules whose forward and inverse did not agree, fixed
in the commits before this one. Two are left as they are and checked in
the way that fits them: maxkins, whose forward and inverse disagree away
from c = 0 and u = 0 and which a separate change addresses, is checked
against its inverse; the nutating heads read their rotary angles from the
joint argument of the inverse, so differencing the inverse about a pose
cannot see the coupling, and they are checked against the forward.
switchkins.c owned rtapi_app_main(), so a module could only use it by
having no main of its own. That ruled out halcompile components, which
is why the switchable kinematics in hal/components each carry a private
copy of the dispatch, the kinstype pins and the switch statement.
Move rtapi_app_main(), rtapi_app_exit() and the coordinates= and sparm=
module parameters to switchkins_main.c, and give switchkins.c a single
entry point:
int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates);
It counts and validates the registered types, creates the pins and
starts on type 0. The caller owns the hal component, doing hal_init()
before and hal_ready() after, so anything that already has a component
can use switchkins by calling this.
The types switchkinsSetup() supplies now reach the arrays through
switchkinsRegister() like any others, rather than being written
directly through its out parameters. One registration path means the
checks apply to every type, so a module that both fills an argument and
registers the same type is refused rather than silently overwriting.
The eight existing modules gain switchkins_main.o in their -objs and
are otherwise untouched.
millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn each carried their own copy of the switchkins dispatch: a private switchkins_type, a kinematicsSwitch() with a hand-written case per type, and a setup routine that had to hal_set_unready() the component again because it ran from kinematicsType(), long after halcompile had called hal_ready(). Four copies of the same thing, none of them sharing the fixes made to switchkins.c. They could not link switchkins.o before, because switchkins.c supplied rtapi_app_main() and so does halcompile. Now that the dispatch is separate from the 'main' program, a component can link it and call switchkinsInit() from EXTRA_SETUP(), which halcompile runs after hal_init() and before hal_ready(). Two build changes make that possible: - the generated per-comp .mak takes a <component>-extra-objs list, so a .comp can name objects besides its own. - switchkins.h is copied to ../include and installed, so <switchkins.h> resolves from a generated component source. Each of the four now registers its kinematics types and calls switchkinsInit(). Their identity type comes from kins_util.c, which gets them the coordinates= module parameter they never had, and a bad motion.switchkins-type is now rejected and leaves the running kinematics alone instead of stranding the module on a type that does not exist. Pin names are unchanged, except that millturn's in/out example pins are gone: they were template scaffolding copied from userkins.comp, unused by the sim config, and a kinematics-type setup routine is where kinematics pins belong now. millturn keeps its fpin pin and fdemo function. The xyzab-tdr, xyzacb-trsrn, xyzbca-trsrn and millturn sim configs give the same positions through the same MDI sequence as before, to four decimals, in every kinematics type.
Nothing stopped an out-of-tree kinematics module from using switchkins except that there was no way to get at the implementation, so anyone writing one reimplemented kinematicsSwitch() and the kinstype.is-N pins, or did without switching entirely. switchkinscomp.comp is the template for doing it properly. It sets TOPDIR to a source tree and includes switchkins.c and kins_util.c, which is how tpcomp.comp and homecomp.comp already reach the trajectory planning and homing sources. The module then registers its kinstypes and calls switchkinsInit() from EXTRA_SETUP(), the same fifteen lines the in-tree components use. That gets an out-of-tree module the kinematics switching, the kinstype.is-N pins, the coordinates= identity mapping and the HAL and G-code controls, all from the one implementation, and it costs no ABI: the sources are compiled into the module, so it is built against one tree and rebuilt when that tree changes. Like tpcomp, the template is not built in tree because it has no kinematics until TOPDIR is set, so it is filtered out of COMPS and its manpage is named explicitly. Renamed to user_switchkins, pointed at this tree and loaded as [KINS]KINEMATICS, it homes, switches to its example kinstype and back, and rejects a kinstype it does not have.
switchkins.h includes kinematics.h, so a module that includes switchkins.h does not need to include kinematics.h itself. switchkins.c had picked up the habit along with genhexkins, 5axiskins, pumakins, scarakins and three21kins, which had it before any of this. Modules that do not use switchkins.h still include kinematics.h directly, as they must.
The kinematics modules are users of switchkins, not part of it, so they take the header the way any other user would. switchkins.c and switchkins_main.c keep the quoted form, being the source itself.
A realtime module cannot link a library, so an out-of-tree kinematics
module has to compile the switchkins implementation itself. Asking it
for the path to a source tree, as the template did, leaves anybody on a
deb install with nothing to point at.
Install switchkins.c and kins_util.c into share/linuxcnc, the way
mesa_modbus.c.tmpl already is, and put that directory on the realtime
include path. The template then reads
#include <switchkins.c>
#include <kins_util.c>
and builds as it stands.
gcodemodule.cc got a raw printf when SELECT_KINS_TYPE was added, so the preview printed a line for every G12.1 and G13.1 in the program. It is the only live printf in the file, every other one having been commented out, and the neighbouring canon stubs are empty. Make this one empty too. saicanon.cc had the same printf. There it should report, since saicanon exists to echo the canonical commands, but through the same macro as the rest of the file so it lands in the canon output with a line number and the argument rather than beside it on stdout.
Alternative to publishing a parameter snapshot in shared memory.
A kinematics module's haldata is a struct of pin handles, and a handle
is an opaque pointer to the value cell hal_get_real() reads. So a
second, non-RT copy of the module can point its haldata at cells
carrying the same values the RT instance reads, and its forward and
inverse then work unmodified, on live values, at any pose asked for.
Per module that comes to:
- export nonrt_attach(), which asks a caller-supplied resolver for
each of its input pins by name, runs the same coordinate parse
setup runs, and returns the existing forward and inverse;
- split that coordinate parse out of setup so nonrt_attach() can
call it without creating pins.
The kinematics math is untouched, and there is no parameter struct to
declare, no shared header that grows once per module, no snapshot to
refresh inside the servo loop, no sequence counter to get right.
Out-of-tree modules can opt in without anyone editing a header they do
not own.
The cells bound to are pins of the caller's own component, not the RT
instance's: a reference into somebody else's pin has a lifetime that
belongs to that component, and rewiring the pin would strand it. The
loader creates one input pin per value and connects it to the signal
the RT pin reads, or, where there is none, to one it makes and removes
again on teardown. Such a pin's reference must live in HAL shared
memory, since that is where HAL rewrites it on connect and disconnect,
so the pins are made against hal_malloc() cells between hal_init() and
hal_ready().
Name lookup lives in the loader, userspace code linked against
liblinuxcnchal and free to call hal_getref_p(). The module is an RT
object, and walking the HAL name space from one is what the HAL
isolation work is removing; it would also risk binding against rtlib's
copy of the same symbols. No HAL change is needed: this patch touches
no file under src/hal.
Bind input pins only, or the two copies write to each other's state.
5axiskins has no output pins and no scratch storage, so its non-RT
haldata is one static struct. trivkins needs no binding at all, only
a statement that joints are axes.
Verified against the struct-snapshot version of this work: for the
same pivot length, kinslimits reports identical caps to the digit.
With 5axiskins.pivot-length set by setp and no motion thread running,
this version reads what was set while the snapshot version reads the
setup default, a snapshot refreshing only when RT calls forward or
inverse. With the pin netted, the bound copy tracks the signal.
The remap carried the geometry of every supported machine inside itself, as branches on the (primary, secondary) joint letter pair in kins_calc_secondary, kins_calc_primary, kins_tool_transformation and kins_calc_tool_rot_c_for_horizontal_x. Adding a machine meant adding a branch to each, and a machine whose maths did not fit that shape could not be added at all. The generic half now lives in remap.py and the machine half in a remap_funcs_twp.py beside each config, pulled in with a plain import. Eleven functions form the interface, the ones the generic side needs to ask a machine: which joint angles reach a tool orientation, how to build the transformation matrix, what the default tool-x direction is, what to write on the module pins. The two configs here supply their own, so the (C,B) and (C,A) branches that were interleaved in one file are now one file each. The generic remap.py and the machine files are David Mueller's, from https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/5axis-twp, where this separation was worked out. His snrtr modules carry exactly the two branches this config pair needs. Adapted here: the ini is read through linuxcnc.ini rather than configparser, the kinematics switch is G12.1 rather than a write to the deprecated motion.switchkins-type pin, kins_set_values converts the two joint angles to degrees because the modules in tree read those pins in degrees, and the pins keep their existing names, so neither module changes and no config has to be rewired. Three behaviour changes come with it, all of them the machine doing what was asked where it previously did not. kins_calc_primary appended its result outside the loop over candidate secondary angles, so only the last candidate ever contributed a primary angle and the solution set was half the size it should be. With the full set, G53.1 P1 and P2 find the positive-only and negative-only solutions they were asking for instead of failing, and P0 sometimes picks a shorter move: on xyzacb-trsrn one of the test orientations is now reached with the primary at -73.87 degrees rather than 130.25, the same tool vector to nine decimal places. Candidate angles were compared in radians against limits read in degrees, so the limit test was meaningless for anything outside plus or minus 57 degrees. On xyzbca-trsrn, G53.6, G68.3 and one G53.3 case reported success while leaving the kinematics in identity with the module pins holding values from whatever ran before. They now activate the tilted work plane. Verified by driving both configs through eight orientations under G53.1 P0, P1 and P2, G53.3, G53.6 and G68.3, and comparing against the same run before the change. Where a different joint solution is chosen the resulting tool vector is identical to within 1e-9. No case fails that used to work.
kins_calc_primary filtered its candidates against the secondary joint's limits. The function declares primary_min_limit and primary_max_limit as globals and then does not use them, so the intent is not in doubt. It is invisible on both configs in tree, where the primary C is the wider of the two: the runs are byte-identical before and after over eight orientations under G53.1 P0/P1/P2, G53.3, G53.6 and G68.3 on each machine, 36 commands and no errors either way. It bites the other way round, on a machine whose primary is tighter than its secondary, where reachable orientations are rejected for exceeding a limit belonging to the other joint.
Asking for a tool vector that lies in a principal plane raises "math domain error" from asin() in kins_calc_primary. The vector reaching that point is a column of a product of rotation matrices, so it is a unit vector only to within rounding, and where one of its components is zero the argument lands on plus or minus one with a rounding error on top and falls outside the domain. Round tripping every reachable orientation, primary and secondary swept in five degree steps, gives 47 failures out of 5184 at the configured nutation of 45 degrees on both machines, and it does not need an unusual nutation angle to appear: 15, 30, 45, 60, 75 and 90 degrees all fail, between 20 and 107 times. Every failure is a tool vector with a component at zero, which is what a G68.2 with I0 or J0 asks for. Clamp the argument where it is within a rounding error of the limit, and leave anything further out to raise, because that is an orientation the machine cannot reach rather than an arithmetic artefact. With the clamp all 5184 orientations solve at every nutation angle tried, on both machines. kins_calc_possible_joint_angles logged such a failure and then fell through to return a variable it had never assigned, so the domain error arrived as an UnboundLocalError over the top of it. Return no solution instead, which is the answer the caller already handles.
78c5727 to
f245857
Compare
A kinematics module reads its geometry from HAL pins it created, keeps its kinematics type and iteration scratch in statics, and so can only answer for the machine as it is now, from inside the module. 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, has to carry a second copy of it. Add the form in which the caller supplies everything: a kins_params block naming the kinematics type, the joint map, the tool and the geometry, a kins_scratch for what an iterative method carries between calls, and a kins_ops table holding the forward, inverse, frames and Jacobian of one type as functions of the two. A module declares its geometry as a table of named entries. In RT the shared code makes one pin per entry, with the names configs already use, copies the pins into the block before every call and the declared outputs back after it, so the maths never touches a pin. Outside RT the caller fills the block from wherever it likes. The classic entry points stay and are supplied once: kins_single.c for a module with one type, switchkins.c for one with several, which now dispatches a type registered with switchkinsRegisterOps() through the block and the older registrations as before, so a module converts one type at a time. Every converted module exports kinsDescribe(), which hands a caller outside RT its table and the ops of each type; a module that does not provide the form keeps working unchanged and reports itself RT-only. Convert trivkins, 5axiskins and the userkfuncs template, and move the non-RT loader onto kinsDescribe(): it binds one input pin per table entry the way it bound the pins nonrt_attach() asked for, fills the block, and evaluates through the same kinsOps functions the RT wrappers use, so both sides get the same answers. nonrt_attach() and its header go, along with the second haldata each module had to keep for it. The loader also takes the tool from motion.tooloffset.* where motion is loaded, and says once when the module's own tool pin disagrees with it, which is a config that lost the tool on the way. jacobian.cc asks the module for its Jacobian instead of differencing at a 0.1 step. switchkinsSetup() is now run by switchkinsRunSetup() in switchkins_setup.c, which a module links only if it defines switchkinsSetup(), so the halcompile components keep linking the core without one. The kparms they build are zeroed, since the struct has grown. identityKinematicsSetup() no longer asks kinematicsType() when built outside RT, where there is no module around it. kinslimits reports the same caps for 5axiskins as before to the digit, now from the closed form Jacobian. tests/kins-jacobian, kins-frames, tool-frame and matrixkins pass unchanged.
…meter block The trt maths becomes two ops tables over one geometry table, TRT_PARAMS, with the joint map read from the block instead of the JX statics and the tool length from p->tool.tran.z, which the shared code fills from the tool-offset entry. The two modules register the tables with switchkinsRegisterOps() in the order sparm decides, and the identity and userk types come from the shared ops. The joint assignment print and the required-letter check that trtKinematicsSetup() did are now the shared code's, so the setup function goes with the haldata. maxkins keeps its fixed joint order and becomes a kins_single.c module: the table declares pivot-length as the HAL_IO pin it was and conventional-directions as before, and the three functions read the block. Pin names and defaults are unchanged throughout.
…s, matrixkins, userkins: move onto the parameter block Each becomes a kins_module description over one ops table and links kins_single.c for the classic entry points. The maths is unchanged; where it read a pin it reads the block, and what it kept between calls it keeps in the scratch: rosekins counts its turns in scratch aux and reports revolutions, theta_degrees and bigtheta_degrees as declared outputs, so each caller counts its own. tripodkins keeps Bx, Cx and Cy as the HAL_IO pins they were. kinematicsHome() goes from corexykins and rotatekins; nothing called it. The two delta modules share their maths with a python module through a common header whose geometry was a set of statics filled by set_geometry(). The geometry is a struct the caller passes now, so the realtime module fills one from the block and the python module keeps its own; the python API is unchanged. matrixkins declared its nine coefficients as HAL parameters; the table makes them pins of the same names, which setp sets the same way. userkins, the template for kinematics built out of tree, includes kins_util.c and kins_single.c by name so halcompile builds it on its own; kins_single.c joins the sources installed in share/linuxcnc, and the in-tree build resolves the same names from emc/kinematics. Its example pins become a declared input and output.
Each arm declares its dimensions as a table, reads them from the block and registers one ops table for its own type, with the identity and userk types from the shared ops. pumakins keeps its flange frame and declares the half turn through the ops table's native rotation, where switchkinsRegisterFrames() carried it before. The setup functions and haldata go; pin names and defaults are unchanged.
…arameter block Each component declares its geometry as a table, writes its types as ops over the block, and supplies switchkinsSetup() like the C modules do; EXTRA_SETUP() runs it through switchkinsRunSetup() and initialises, so the components link switchkins_setup.o too and export kinsDescribe() with the rest. The trsrn TCP type registers its frames and Jacobian in its ops table and the TOOL type the identity frames, as they were registered before. The inverses go on reading the rotary angles from their joint argument, as they always have. Pin names and defaults are unchanged.
The three that build a geometry from many pins now build it from the block on each call: genser its link description, genhex its base and platform points and joint axes, pentakins its base points and effector circles. What they reported through output pins, iteration counts, the strut corrections, the hexapod's fwd-kins-fail and the pose it found for a vismach gui, they report through the scratch, and the running maximum of iterations lives in the scratch too, so each caller keeps its own. genhex and pentakins declare their forward as iterating from the pose it is handed; the switchkins core seeds it with the last answer after a switch as before. genhexkins's six pose pins for the gui were created as inputs and written by the module; they are outputs now, which is the direction they were used in. pentakins declared its geometry as HAL parameters; the table makes them pins of the same names. genserfuncs.c loses the haldata, the global joint copy and the initialised flag, and the userspace test program ugenserkins builds a block itself and calls the ops. Pin names and defaults are otherwise unchanged.
The gui forward for a parallel machine starts from the pose the main forward last found, which the older form saved in lastpose[]. The block path did not save it, so the gui forward of a type registered with switchkinsRegisterOps() started from zero and failed to converge, and genhexkins failed every forward. Save it on the block path too, and call the type's forward directly for the gui, without the seeding that belongs to the main call.
…lazily The loader zeroed the joint array before running a module's inverse and before an iterating forward. Motion hands a module the joints the machine is at, and some read them: a nutating head takes its rotary angles from there, the hexapod starts its forward from the pose it is handed. Zeros put the loader on a different branch from realtime for xyzacb_trsrn. The caller's joints are the seed now, the forward keeps the caller's pose as its seed when the module iterates, and the Jacobian runs its inverse from what the last inverse found. A halcompile component references hal_export_funct() and the rest of what its rtapi_app_main() needs, which only the realtime HAL library provides, so dlopen with RTLD_NOW refused every component. Nothing here calls that main; bind lazily and only what is called has to resolve.
The out-of-tree template declares its geometry as a table, writes its example type as ops over the block and supplies switchkinsSetup() like the in-tree modules, with EXTRA_SETUP() running it through switchkinsRunSetup(). It includes switchkins_setup.c alongside the other two sources, so that file joins those installed in share/linuxcnc. The kparms it built was never zeroed, which the grown struct would have turned into a crash.
The conventions chapter gains a section on the two blocks, the table, the ops table and what the shared code does with them in and outside realtime, and the Writing a Module list gains "no state". The frames and Jacobian sections point at the ops table where they pointed at the register calls. The switchkins chapter's Code Notes describe switchkinsRegisterOps(), the table in kparms, switchkinsRunSetup() and kinsDescribe(), keep the older registration as the older form, and the outline is a module written the new way.
tests/kins-params loads each module, evaluates it once in realtime through the classic entry points (paritycheck.c publishes the forward, the inverse and the Jacobian on pins) and once through the non-realtime loader from python (check.py: kinsDescribe(), the block filled from the module's pins, the same ops), and requires the same success and the same numbers to rounding. 34 runs over the 24 modules, every switchable type that has geometry of its own, the parallel machines from a pose their forward can be seeded with. Checked by mutation: the loader not refreshing the geometry fails 10 comparisons in the first module with a table; the loader seeding the inverse with zeros instead of the caller's joints fails the three translation joints of xyzacb_trsrn, which reads its rotary angles from that array.
A module whose maths needs the tool length read it from a HAL pin the config had to net from motion.tooloffset.z. The offset is controller state, from G43 and the tool table, which motion already holds and only published; the pin was a copy of it, one cycle late, and a missing net gave wrong joints with no error. The parameter block carries the tool, so hand it over directly. Motion calls kinematicsSetTool() whenever the offset changes. It references the symbol weakly, so a module written before the call still loads and keeps its pin. kins_single.c and switchkins.c export it for every module written on the block: after motion has sent anything the tool entry is overwritten with motion's value, and the pin is read only until then, as under halrun with the module alone. A pin left disagreeing with motion for a thousand calls is reported once, since it is a config setting a tool length where the tool table should. The non-realtime loader takes the tool from the caller through kinematicsUserSetTool() where one is given, since a planner knows what a segment runs under better than the machine does, and from motion's pins otherwise as before. tests/kins-tool-offset runs xyzac-trt-kins under motion with nothing on its tool-offset pin and checks that G43 reaches the joints, that connecting the pin the old way changes nothing, and that G49 takes the length out again. tests/kins-params checks that the caller's tool moves the loader's inverse and that handing it back restores realtime's answer.
581bde9 to
8148e82
Compare
3 participants
Draft. Where the multiaxis kinematics work is going, open early so it can be argued with. As pieces are agreed I split them into small PRs, rebase this, and carry on, so nothing stalls behind it.
Plan, in order:
Switchkins: one implementation, used by
.comp,.c, in tree and out of tree. The registration half merged as switchkins: allow more than three kinematics types #4372; the rest is in this branch.Small fixes that stand on their own:
5axiskinsgets a.tool-offsetpin like every other module,maxkinsgets one or a doc note saying it has none, andG53stops ignoring the rotary tool offset on non-wrapped axes.The kinematics interface learns to answer two more questions: where the tool points, and how joint motion maps to world motion. One entry point each, both weak, both defaulting to today's behaviour, and a tool frame convention written down so every module answers the same way rather than each author choosing one.
Those answers, and forward and inverse alongside them, become pure functions of what the caller passes in: joint values, a parameter block carrying the tool and the kinematics type, and no state kept inside the module. Today the maths reads its geometry from HAL pins and keeps scratch in
haldata, so the only way to call it from anywhere else is to write a second copy of it. One implementation, parameters supplied at the call site.Two stages the offset chain is missing. Both came out of the discussion below, and both want 3 and 4 first.
A frame composed on top of the active coordinate system, with its own cancel, so the blocks in between are programmed in the tilted plane while G54 itself is untouched. Fanuc spells this
G68.2; Siemens puts frames above the kinematic transformation for the same reason. In tree that is a stage inrotate_and_offset_pos()plus the modal state to go with it. Nothing is rewritten, so nothing needs restoring, andon_abort_with_twp_reset.ngchas nothing left to do.A move whose target is Cartesian but whose interpolation is in joint space: run the inverse once at the endpoint, then interpolate the joints to reach it. Coordinates and units never change, so the offsets keep applying and there is nothing to switch. That is how the commercial controls cross a singularity without changing coordinate system, and
KINEMATICS_INVERSE_FLAGSalready carries enough to pin which solution branch the endpoint lands on. It needs a new move type from canon into the trajectory planner, and a way to ask for it in G-code.Between them these are the reason not to add a set of offsets per kinematics type. Controls do let you add coordinate systems, Fanuc
G54.1 P1toP48, SiemensG505toG599, and none of them ties a set to the active kinematics or transformation: selection is explicit in the program, or it belongs to a channel. More coordinate systems is a fair request on its own; binding them to the kinstype is the workaround for these two absences.Limits taken from the joints instead of from static per-axis INI values. Joint limits are the physical fact. The region they imply in axis space depends on the tool and on where the rotaries are, and is not a box, which is why a box in the INI cannot describe it and config authors have to pick a conservative one and lose travel.
[AXIS_L]keeps the job it is good for, a restriction the user imposes in axis space, normally set inside what is reachable; the effective bound is the intersection of the two, and on non-trivial kinematics neither one contains the other, so both are checked. Canon asks per move, which gives TP0 and TP1 the result with no planner change and costs 28 us a move on a closed-form five-axis module. Every commercial five-axis control applies its limits in machine-axis space after the transformation; this is the piece LinuxCNC does not have. Asking also makes the limits per kinematics type, which retires a workaround the docs currently prescribe: switchkins.adoc says the INI limits apply to type 0 only and tells you tosetp ini.L.min_limitfrom M-codes at each switch, with the puma sim shipping M128, M129 and M130 to do it by hand. The realtime seam and TP2 after, and only with an analytic Jacobian: differentiating numerically is eighteen inverse calls, 2 us on a closed-form module and milliseconds on an iterative one.This item is travel only.
[AXIS_L]also carries MAX_VELOCITY and MAX_ACCELERATION, which have the same defect a static per-axis scalar always has here: it cannot describe a bound that depends on where the rotaries are. Those come off the Jacobian in 3 rather than off the reachability query in this item, and are a stage of their own.The joint limit check moves earlier and samples inside segments. It exists already, in
inRange(), but only at move endpoints and only once the block has reached motion, so a program that will overtravel says so partway into the cut rather than before it starts. With 4 in place the same check runs at load time, against the tool and the kinematics type each block will actually use. A verify button in a GUI is then a presentation of that, not a subsystem of its own.Two things this needs pinned before it is written. The interpreter has no seam for the query: all 66
GET_EXTERNAL_*accessors take at most an index, an axis, a spindle, a pocket, and never geometry, so one taking a segment is a new shape for that interface. And a load-time answer can go stale, since the interpreter runs ahead andmotion.switchkins-typeis a HAL_IN pin, so until 9 lands the type a block is checked against is not necessarily the type it runs under.Where a module cannot answer, cheaply or at all, the check skips rather than refuses. Refusing a reachable program leaves the user no recourse; missing an unreachable one is still caught by
inRange()at runtime. That asymmetry decides every ambiguous case in 6 and 7.Checking a program that switches type raises a question the tree currently leaves implicit: whether a type's coordinates are geometric. Most are, and for those a scan is one stream of world points with only the inverse changing at the switch.
identityis not, since world becomes the joints, so a datum in millimetres lands on a rotary joint in degrees; nothing notices, which is why switchkins.adoc asks the operator to clear the offsets before switching.A possible direction rather than part of the plan: a kinematics type could declare its space, cartesian or joint. In practice one type is the joint space and it is the one that wants treating specially, with no offsets applied, limits that are the joint limits directly, and possibly its own syntax later rather than borrowing the axis letters. Everything else stays as it is. I have no mechanism to propose yet and would rather hear objections to the idea first.
Per-mode restrictions. G28, G30, G53, G92, G52 and M6 do not mean the same thing in every kinematics type, and today each one quietly does whatever the module happens to leave it doing. Refusing outright beats guessing, and with 4 in place the control knows enough to decide rather than to be told.
Retire
motion.switchkins-type, once the grace period has run. Which release that is has still to be decided.G12.1 and G13.1 are #4414, split out of this branch and standing on their own.
On tool length specifically: the offset is a nine-axis pose from the interpreter, canon adds it into the coordinate, and each rotary kinematics takes the Z component back through a HAL pin and re-applies it along the real tool direction. That round trip is exact when the config wires the pin, and silently wrong otherwise: a missing
netcosts the full tool length, and an X or Y offset is applied in the rotating work frame while the tool is bolted to the machine. The mechanism works; it has no owner.Why 4 is worth the trouble: every other way of answering "will this program overtravel" builds a second model of the machine, in the verification package or in the post, and the two descriptions drift apart. If the kinematics module can be called with parameters the caller chooses, LinuxCNC can check a program against the same code that will run it, with no second model to keep in sync. That is not available to anyone whose kinematics is locked inside the control.
In this branch now
Switchkins becomes one implementation that everything talks to:
rtapi_app_main(), so a module can take its main from anywhere and callswitchkinsInit()itselfEXTRA_SETUP(). The generated per-comp .mak takes a<component>-extra-objslist, and switchkins.h is installedTOPDIRplus#include, the pattern tpcomp and homecomp already use, so no ABIUser visible
coordinates=module parameter, their identity type now coming from kins_util.c.inandoutexample pins, template scaffolding referenced by no config.fpinandfdemostay.Testing
The four comp sim configs give identical positions to master through the same MDI sequence, in every kinematics type.