kinematics: let a module report where the tool points - #4455
kinematics: let a module report where the tool points#4455grandixximo wants to merge 10 commits into
Conversation
2b21950 to
067ce10
Compare
|
Note that the tool orientation in world coordinates is sufficient for machines with tool side rotation only because the work coordinate cannot rotate away from world orientation. For and example config with mixed roations see: |
|
Actually there is a vtk-vismach sim in the repo above that calculates the work->tool transformation matrix from tracker objects (one for work->world and one for world->tool) embedded in the model. Work, world and tool transformations is also what is used for the 'Tracking' feature: 
|
067ce10 to
03124a3
Compare
|
You are right, and I was handing over the wrong thing. Pushed the split. A renderer cannot use the product. Vismach places the work in one place and the tool in another, both against the machine, and the two halves cannot be recovered from work to tool. Composing is a multiply, decomposing is impossible, so the halves are what the module owes the caller. Now two entry points, It cost nothing, which is what tells me you were right: the trsrn modules were already building the table rotation and the head rotation separately and multiplying them on the last line, so I was discarding a half I already had. Checked that the composition reproduces the previous answer to 2e-16 on both machines across all rotary combinations. One terminology note, since it may read oddly otherwise: in the chapter "world" means the workpiece frame, not the machine, so the single rotation I had was already work to tool with the table in it. The gap was reporting the product rather than the pair. |
03124a3 to
16a4531
Compare
|
While you are already burning along, would it be possible to also somehow provide a function that hands back the possible combinations of rotary angles for a given tool vector? Something that holds the functions derived in the 'Calculating the spindle rotary joint positions for a given tool-orientation vector' section towards the end of this doc: [edit] |
Is there a particular reason for calling the coordinate system for the work piece 'world'? |
A kinematics module reports the controlled point and nothing else, so everything needing the rest of the machine geometry rebuilds it. For xyzacb-trsrn the same chain is written three times: closed-form in xyzacb_trsrn.comp, as homogeneous matrices in the config's remap_funcs_twp.py, and as signed HalRotate calls in the vismach model. The Python copy is kept in step by a comment saying its matrices "must be the same as the ones used to derive the kinematic model". Write down the vocabulary they would need to share: the four frames and which one kinematicsForward() reports in, the rotation sense already stated under Rotational Axes and its ISO 841 equivalent, what conventional-directions costs at its default, and the definition of the tool frame. Tool x is the part worth stating as a rule rather than a formula. The virtual rotation about tool z supplies what a five-axis machine cannot, and the convention is that it leaves tool x parallel to the machine xy-plane; the formula follows from the machine's own secondary rotation matrix, which is why the two nutating configs in tree have different ones. Also anchor the Rotational Axes section so it can be referenced.
kinematicsForward() reports where the controlled point is and nothing about
which way anything faces, so a consumer that needs the geometry rebuilds it
for itself. Add two entry points a module can answer with instead:
int kinematicsToolFrame(const double *joint, PmRotationMatrix *rot,
const KINEMATICS_FORWARD_FLAGS *fflags);
int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot,
const KINEMATICS_FORWARD_FLAGS *fflags);
Each returns the columns of that frame's axes in machine coordinates.
Conventions are in the Kinematics Conventions chapter.
They are reported separately rather than as the single work-to-tool rotation
because the product cannot be taken apart again. A consumer that has to place
both bodies, a simulation model or a tracking display, needs each against
something that does not move; one that wants the tool in workpiece
coordinates, which is what a tilted work plane asks for, composes them with
toolFrameInWork(). Composing is a multiply, decomposing is impossible, so the
halves are what the module owes the caller.
Modules built on switchkins.c export both and dispatch on the current type,
returning -1 for a type that has not supplied them. A type registers with
switchkinsRegisterFrames() from its switchkinsSetup(); leaving it out costs
nothing. Adding it that way rather than as arguments to switchkinsSetup() and
switchkinsRegister() keeps both signatures as they are, so no module has to
change to build.
The tool frame has two live conventions, so a type also declares the rotation
relating its own to the one in use, and the dispatch applies it. That is a
rotation and not a sign: negating the third column alone leaves determinant
-1, a reflection. It is checked once at registration rather than on each
call. The work frame needs none of this, having no tool axis to point the
wrong way.
Identity types answer both the same way whichever module asked for them, so
switchkins.c attaches the identity pair to any type whose forward is the
identity one, and every switchkins module gains correct frames for its
identity type without being touched.
Nothing in motion calls either, so no module is obliged to define them and a
module outside the tree need not know they exist.
Both rotaries carry the work on these machines, so the tool never turns in the machine frame and its frame is the identity. All the rotation is the work's. The forward transform already contains it: the coefficients it applies to a displacement of the X, Y and Z joints are the rotation from machine into work, so the work frame in machine coordinates is their transpose. Checked against the forward transform by central difference over the linear joints at four primary and four secondary angles with both settings of conventional-directions: the composition transpose(work) * tool reproduces the frame to 3e-9, the result is orthonormal with determinant one, and the tool axis is machine z with the rotaries at zero. The check also shows the sign question plainly. With conventional-directions true, A at 90 degrees on an xyzac machine puts the tool axis along -Y in work coordinates, which is +Z turned counterclockwise about +X as the documentation says it should be. With the pin at its default of false the same move puts it along +Y.
trivkins does not build on switchkins.c, so it does not pick up the identity frames the way a switchkins identity type does. Hand them through, since it is the kinematics most machines run and a caller that has to special-case the commonest module has not gained much.
These are the machines the split is for: a rotary carries the work and two more carry the tool, so neither frame is the identity and neither can be recovered from their product. The work frame is the table rotation, written in machine coordinates. The tool frame is the primary rotation about z times the nutating secondary, written as two matrices and multiplied rather than expanded, so it can be read against the matrices in the config's remap_funcs_twp.py. Identity kinematics leaves both square with the machine, and so does tool kinematics: there the world axes are the tool axes by construction, which is what makes a G1 Z move run along the tool, so there is no machine-relative frame to report. Checked against the forward transform of each module at 27 poses. The coefficients the forward applies to a displacement of the linear joints are the transpose of the work frame alone, as they should be, since turning the head does not move the tool tip when a linear joint moves. The tool axis, recovered separately as the direction the tip retreats along when the tool gets longer, matches the third column of transpose(work) * tool. For tool kinematics the same coefficients come out as the transpose of the whole chain including the virtual rotation, which is the statement that the world frame is the tool frame. Agreement is to 3e-9 throughout.
The arm carries the tool and nothing carries the work, so the work frame is the identity and this is the first module whose own tool maths is not in the convention. pumakins builds the ISO 9787 mechanical interface frame, whose z points perpendicularly away from the flange, and it relies on that: it reaches the tool tip by adding PUMA_D6 along the third column. So it answers in its own frame and declares TOOL_FRAME_FLANGE, and switchkins turns it into the convention. Nothing in the module itself flips a sign. Lift the rotation out of the forward kinematics into pumaFlangeRotation() rather than writing it twice, which is the whole point: a second copy of a machine's geometry that has to be kept in step by hand is the thing this work exists to remove. The block moves verbatim and the forward kinematics loses the locals that went with it. At every joint zero the module's own frame is diag(1, -1, -1), a half turn about x, so after the declared half turn it reports the identity: tool axis [0, 0, 1], tool x [1, 0, 0]. A puma at zero and a vertical mill at zero give the same answer, which is right, because both have the tool pointing down at the work.
Pins down the two properties the chapter is about. Relating one tool axis convention to the other is a rotation, not a change of sign: a negated third column is refused because it is a reflection, the declared rotation post-multiplies so it is read in the module's own frame, the half turn keeps tool x and reverses the other two, and applying it twice is the identity. Also checks the pumakins zero pose, whose own frame is a half turn about x, ends up as the identity after the declaration it makes. And composing the two reported frames means transposing the work one: toolFrameInWork() leaves the tool alone when nothing turns the work, gives a proper rotation, and composes a work frame with its own inverse back to the identity. Verified by mutation rather than by passing: making TOOL_FRAME_FLANGE negate only the tool axis fails nine checks, reversing the multiplication order fails four, and dropping the transpose in toolFrameInWork() fails two. Built the way tests/blendmath builds, compiling the source under test directly with the rest garbage-collected by the linker.
16a4531 to
2acf37e
Compare
|
You are right about "world", and I have changed the chapter. It now says machine, work and tool, with one line noting that the interface calls the work frame The angle pairs are in, as the last commit.
A module supplies nothing. The generic implementation searches using the two frame functions, so anything that reports its frames gets the inverse free. Where a closed form is worth having there is Three things your framing does not cover yet: Tool X has to be optional, not implied. Two rotaries cannot pin down the spin about the tool, so on a five axis machine it is left out, and that is the normal case. On a machine with three orientation joints a bare tool axis has a curve of solutions and only adding tool X makes the answer a list. "All pairs" is sometimes the wrong shape. Point the tool along the axis the primary turns about and the primary is free. In that case I report one point of the family, the one nearest where the machine is, with the number of free directions alongside. Handing back samples of a curve as alternatives would tell the operator something false. The vertical singularity needs no special case. Your code returns Tested against a table rotary machine, a nutating head, and one with both, including a module written in radians, since the interface never says the unit. What it does not have is a caller. Nothing in the interpreter can reach the kinematics: no file under |
2acf37e to
0ccacb2
Compare
On a 5axis machine this is done using a virtual frame that rotates around the tool-Z axis (or the spindle rotation). Without this it is not possible to set the orientation of the tool XY coordinates. [edit] |
The frames say where the tool points for a set of joint values. The question a tilted work plane actually asks is the other way round: which joint values point it at the orientation the program wants. Today that is answered outside the kinematics, in per-machine trigonometry in the TWP remap, once per machine and once per pair of rotary letters, which is how the copy in the tree came to check the primary angle against the secondary joint's limits. kinematicsToolFrameInverse() asks the module instead. It reports every set of joint values that reaches the orientation, and nothing else: no joint limits and no preference between the answers, because the caller knows the limits and knows whether the operator asked for the shortest move or for one direction of rotation only. A module that reports its frames needs to supply nothing, the generic search in kins_util.c drives them; a module with a closed form registers it and that is used instead. The request is a tool axis and optionally a tool x as well, and asking for tool x does not require a joint that can reach it. A five axis machine spends both rotaries on the tool axis and the turn about that axis is not a joint, it is the virtual rotation, so the answer comes back in two parts: the poses that reach the axis, and the turn that places tool x, which is zero where a third orientation joint did the job instead. The caller writes one path and the kind of machine becomes a number rather than a branch. That is what the controls do, a Heidenhain PLANE VECTOR block carrying the normal and the base vector together and a Fanuc G68.2 defining where the plane's X points; neither refuses a program for naming both. Where a request still leaves the machine free, at a singular pose or on a machine with a spare orientation joint, one point of the family is reported, the one nearest the seed, with the number of free directions alongside. Returning samples of a curve as though they were alternatives would be worse than saying so. Three details in the search are not incidental. The damping is adaptive, which is what keeps a rank deficient pose from turning finite difference noise into a step of thousands of degrees. The Jacobian is central differenced, because a one sided error is first order in the step and shows up as a spurious singular value, which is exactly what the rank test must not see. And the joint unit is discovered by adding a whole turn and asking whether the frame came back, because every module in the tree takes degrees but the interface does not say so. The unit test drives it through a table rotary machine, a nutating head and a machine with both, and checks the nutating answers against the closed form the remap uses.
0ccacb2 to
cffa81d
Compare
|
Poor wording on my part. I meant the argument, not the quantity: You made me look at what happens when a caller asks for both on a five axis machine, and the answer was "no solutions", which is wrong. Fixed and pushed. It now comes back in two parts. Where the joints can place tool X, on a machine with a third orientation joint, they do and the reported turn is zero. Where they cannot, the joints reach the axis and the reported turn is the virtual rotation that finishes the job. One code path either way, and whether the machine solves tool X with a joint or with a frame is a number that happens to be zero rather than a branch. That is what the controls do, which is the argument for it rather than my preference. Heidenhain |
|
Is this ready and tested? |
|
Tested, and I have just added the test that was missing. The frame cases so far ran machines written for the test. The frame functions a real machine uses were compiled and nothing more, which left the hand-written matrix entries per module unexercised, and those are where a sign or a transposed pair hides. They now run, checked against the forward kinematics of the same module rather than against a matrix copied out of it: column j of the work frame is how work coordinate j responds to the machine axes, measured by central difference, and the tool axis in the work frame is the direction the tool travels when the machine's Z advances. xyzac and xyzbc, both Two things to flag, since they are yours to veto rather than mine to decide. It reaches the module's statics by including Not covered: the two trsrn modules are generated comps and pumakins declares its own The other gap is not testable yet. Nothing calls the kinematics outside motion, so there is no end-to-end path to exercise; that is what the loader in #4374 is for. |
That is a real problem how it was done and may not work properly (in the future). You are including trtfuncts.c (not nice, but well,...). Why are you not just creating a non-RT component and call int comp_id = -1;
int main(void)
{
if((comp_id = hal_init("bla")) < 0) {
error(...);
goto doquit;
}
kparms kp[...] = {... };
if(0 != trtKinematicsSetup(comp_id, "xyz...", kp)) {
error(...);
goto doquit;
}
...
// now you can use
hal_set_real(haldata->x_rot_point, 123,456);
// etc.
// your test
...
doquit:
hal_exit(comp_id);
return 0;
}
You can also do this much more conveniently and much more flexible if you stop including cross-boundary sources. The above hack is a one-way street. If the second load fails (because you returned non-zero from you rtapi_app_main()), then you know it went bad... BTW, you can simply use |
8a28d40 to
2866376
Compare
Both modules build their pins on the first kinematicsType() call and test an is_setup flag that nothing ever sets, so every later call runs the setup again. The second run reassigns haldata to a fresh block and then fails to create the pins that block points at, all of them already taken, so the module is left reading through null references and the next forward or inverse call takes realtime with it. Motion asks twice when num_extrajoints is greater than zero, once in rtapi_app_main and once in init_comm_buffers, so that combination cannot be running today. Latching the flag would stop the repeat, but the setup does not belong in a function whose job is to answer a question. halcompile has a hook for this, used by homecomp for the same reason: EXTRA_SETUP() runs once from the generated setup, before the component is made ready. So the pins exist from the moment the module is loaded, kinematicsType() only answers, and the hal_set_unready() and hal_ready() calls the old placement needed are gone with it.
The frame cases so far run machines written for the test. The frame functions a real machine uses were compiled and nothing more, which leaves the hand-written matrix entries per module unexercised, and those are where a sign or a transposed pair hides. So run them in service: a realtime component loaded after the module under test, reaching it through the same exported entry points motion uses. A failed check fails the load, and a failed load fails the test. The work frame has a tie to the forward kinematics and is checked against it rather than against a matrix copied out of the module: a row of the work frame is how the reported position responds to one machine axis, measured by central difference. The tool frame has no such tie on a machine that carries the work. Its forward reports the rotary joint values, which say how the work is turned and nothing about where the tool points, and the tool length offset moves the reported origin as well as the tip, so it does not isolate the tool axis either. What is checked is that the frame is a rotation, that a spindle the module calls fixed never moves, and that a joint carrying the whole head turns the reported frame about the machine's z and does nothing else. The last one is what catches a frame built for the wrong joint or composed in the wrong order. Every kinematics type a module offers, since the frames are per type, and the geometry checks are asked only of the type where the module models its own machine, which is not always the first. A type that reports no frames is skipped, which the interface allows, so the test also fails a module that reports frames for no type at all. The hal file sets the geometry, which matters more than it sounds: a nutating head defaults to a nutation angle of zero, which leaves the spindle square with the machine and the interesting geometry untested. The helpers in kins_util.c are not exported to a loadable module, and working the answers out in the test is better anyway: nothing the module under test uses is reused to judge it. Checked by mutation, in failed checks: reversing the head composition order in a trsrn module 300, a flipped sign in its work frame 150, one in the xyzac work frame 18, declaring the wrong spindle convention there 25, a flipped sign in the puma flange 72. The tree as it stands fails none.
2866376 to
1e5da0d
Compare
|
@BsAtHome took your second suggestion, thank you. A realtime component loaded after the module under test, reaching it through the exported entry points, with a failed check failing the load. No cross-boundary include, no faked HAL storage. Coverage went from two modules to five, the generated trsrn comps and pumakins being what the include trick could not reach. Two fixes came out of it. Both trsrn components build their pins on the first And my tool-frame check was wrong for a nutating head: it assumed the tool axis is where machine Z drives, which holds only for a fixed spindle. There is no tie from the tool frame to the forward kinematics on these machines, since the forward reports the rotary joint values and the tool length offset moves the reported origin as well as the tip. It now checks that the frame is a rotation, that a declared-fixed spindle never moves, and that the joint carrying the head turns the reported frame about the machine's z and nothing else, which is what catches a reversed composition order. Verified by mutation, in failed checks: reversed head composition 300, trsrn work frame sign 150, xyzac work frame sign 18, wrong spindle convention declared there 25, puma flange sign 72, clean tree 0. |
|
|
||
| ${SUDO} halcompile --install framecheck.c >/dev/null | ||
|
|
||
| # One hal file per module: they all define the same entry points, so |
There was a problem hiding this comment.
Sudo should not be necessary. You should be able to create a local component without installing it. I think you need to look in the tests tree to find a component test (if it isn't there, then my memory is playing tricks).
| rm -f "$hal" | ||
| } |
There was a problem hiding this comment.
Wouldn't you need a full halrun -U unload after a test to start with a clean slate?
3 participants
A kinematics module reports the controlled point and nothing about which way the tool faces, so everything that needs the tool frame rebuilds the machine geometry for itself. For
xyzacb-trsrnthe same chain is written three times: closed-form inxyzacb_trsrn.comp, as homogeneous matrices in the config'sremap_funcs_twp.py, and as signedHalRotatecalls in the vismach model. The only thing keeping them in step is a comment in the Python saying its matrices "must be the same as the ones used to derive the kinematic model".This adds an optional entry point a module can answer with instead, and a chapter saying what it answers in.
The columns are the tool frame axes in world coordinates. Its origin is the controlled point
kinematicsForward()already gives, so the pair is the whole tool pose.Nothing is obliged to change.
switchkinsSetup()andswitchkinsRegister()keep their signatures; a type opts in through a separate call. Nothing in motion calls the entry point, so no module has to define it, and an out-of-tree module need not know it exists. Modules built onswitchkins.cexport it and return -1 for a type that has not supplied one.The two conventions. The tree already contains both senses, and they come from two standards rather than from carelessness. ISO 9787 clause 5.3 puts a robot's flange z pointing perpendicularly away from the mechanical interface, and
pumakinsfollows it, reaching the tool tip by addingPUMA_D6along its third column. Machine tool practice puts z along the spindle, positive away from the work, which is whatG68.2commands. The chapter settles on the machine tool sense for what a module reports, and a module whose own maths is in the other one declares the rotation relating them rather than fixing it up by hand.That matters because it is not a change of sign. Negating the third column gives determinant -1, a reflection, and loses tool x as well. The declared rotation is checked once at registration for orthonormality and determinant +1, so a bad declaration fails at load rather than producing a quietly wrong frame.
Supplied for
trivkins, the identity switchkins types,xyzac-trt-kins,xyzbc-trt-kins, bothtrsrnnutating modules, andpumakins.Verification.
tests/kins-framesloads a realtime component next to each module and checks the frames it reports where they run, through the entry points motion uses. The work frame is checked against the module's own forward kinematics: a row of it is how the reported position responds to one machine axis, by central difference. The tool frame has no such tie on a machine that carries the work, since the forward reports the rotary joint values, so it is checked for being a rotation, for a declared-fixed spindle never moving, and for turning about the machine's z with the joint that carries the head. Every switchkins type, and the nutating geometry set rather than left at the default of zero, which would leave the head square with the machine and the interesting cases untested.Verified by mutation rather than by passing. Failed checks: reversing the head composition order in a trsrn module 300, a flipped sign in its work frame 150, one in the xyzac work frame 18, declaring the wrong spindle convention there 25, a flipped sign in the puma flange 72. The tree as it stands fails none.
tests/tool-framecovers the shared helpers on machines written for the test.One fix rides along, in its own commit. Both trsrn components build their pins on the first
kinematicsType()call and test anis_setupflag nothing ever sets, so a second call reassignshaldataand then fails to create the pins it points at, leaving the module reading through null references. Motion asks twice whennum_extrajoints > 0, so that combination cannot be running today. The setup moves toEXTRA_SETUP(), which halcompile calls once before the component is ready, ashomecompalready does; the pins then exist from load andkinematicsType()only answers. The test is the second caller, which is how it turned up.The
pumakinsrefactor lifts its rotation out of the forward kinematics into one function rather than writing it twice. The moved block is byte-identical.What is not covered: nothing calls the entry point outside these tests, so there is no end-to-end path through the interpreter to exercise. A non-realtime caller is the next piece of this work and is what would close it.
Part of the multiaxis kinematics work in #4374, item 3. Replaces #4454, which was the chapter on its own; that could not merge alone without documenting an API the tree did not have.