kinematics: let a module report where the tool points - #4455
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: 
|
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.
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. |
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.
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: Not sure how we would give it the requested tool vector though. |
2 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. The trt and trsrn frames were derived and then checked two independent ways against each module's own forward transform: the coefficients the forward applies to a linear-joint displacement must be the table rotation alone, and the tool axis recovered separately as the direction the tip retreats along when the tool gets longer must equal the third column. Agreement is 3e-9 across 27 poses per trsrn machine and 32 per trt machine, orthonormal, determinant one. The new test covers the shared helpers and was verified by mutation, not by passing: making the flange constant a reflection fails nine checks, and reversing the multiplication order fails four.
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:
pumakinsis not exercised end to end. The claim that it reports the identity at every joint zero is verified by computing its closed form, not by running the module, because there is no caller for the entry point yet. 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.